最小化二维码角点重投影误差的目标物体六自由度定位方法及系统与流程

本发明属于定位技术领域，更具体地，本发明涉及一种最小化二维码角点重投影误差的目标物体六自由度定位方法及系统。

背景技术：

二维码，是一种按一定规律在平面上分布的、黑白相间的、记录数据符号信息的图形，其使用黑白矩形图案表示二进制数据，通过光电设备扫描后自动解读二维码内容并识别二维码位置。二维码相比于一维码，增加了定位点和容错机制，定位点用于定位二维码在图像数据中的位置，容错机制在即使没有识别到全部的条码、或是说条码有污损时，也可以正确地还原条码上的信息。目前二维码的生成、打印、读取和识别已相当成熟，针对一个二维码，现有相机可快速拍摄二维码图像，并读取二维码内容以及定位二维码角点在图像中的位置坐标。

已有基于二维码的定位方法大多是利用二维码本身的结构信息以及采用的二维码的大小尺寸信息进行定位，其定位信息包含三个维度，平面内x、y坐标和z轴方位角，属于三自由度定位，自由度描述不能给出完整的姿态信息，位姿即位置和姿态，位置可用x、y、z坐标描述，姿态可用x、y、z方位角描述，不便于于目标物体的抓取。

技术实现要素：

本发明提供了一种最小化二维码角点重投影误差的目标物体六自由度定位方法，旨在改善上述问题。

本发明是这样实现的，一种最小化二维码角点重投影误差的目标物体六自由度定位方法，所述方法具体包括如下步骤：

s1、相机cm拍摄二维码m图像，获取二维码m角点在像素坐标系{im}下的坐标称为二维码角点像素坐标采集值；

s2、读取二维码m角点在目标物体坐标系{l}中的坐标将二维码m角点从目标物体坐标系{l}转换至相机像素坐标系{im}，获取二维码m角点在像素坐标系{im}下的坐标称为二维码角点像素坐标计算值；

s3、获取与二维码角点像素坐标采集值偏差最小的二维码角点像素坐标计算值及该二维码角点像素坐标计算值下的为最小重投影误差对应的目标物体坐标系{l}相对于定位坐标系{h}的位姿。

进一步的，像素坐标系{im}下的坐标计算公式为：

其中，为二维码m角点在目标物体坐标系{l}中的坐标，为目标物体坐标系{l}相对于定位坐标系{h}之间的坐标转换矩阵，为定位坐标系{h}相对于相机坐标系{cm}之间的坐标转换矩阵，为二维码m角点在相机坐标系{cm}中的坐标，为的z坐标分量，mm为相机cm的内参矩阵；

内参矩阵mm、定位坐标系{h}相对于相机坐标系{cm}之间的坐标转换矩阵为标定参数。

进一步的，针对所有二维码建立优化函数，基于优化函数来获取最小重投影误差对应的目标物体坐标系{l}相对于定位坐标系{h}的位姿优化函数具体如下：

其中，n表示粘贴在目标物体中的二维码数量，j表示二维码的角点序号。

本发明是这样实现的，一种最小化二维码角点重投影误差的目标物体六自由度定位系统，所述系统包括：

目标物体，设于目标物体上的t个二维码，t≥1；

设于目标物体上方的工业视觉系统，工业视觉系统中设有p个工业相机，t个二维码处于p个工业相机的视野中间区域；

与工业视觉系统通讯连接的plc控制单元，plc控制单元基于所述最小化二维码角点重投影误差的目标物体六自由度定位系统来获取最小重投影误差对应的目标物体坐标系{l}相对于定位坐标系{h}的位姿，其中定位坐标系{h}设于机器人末端执行器上。

本发明主要通过在目标物体上布置多个二维码，并通过多个已标定的工业相机对二维码进行拍照和角点检测，根据识别的二维码角点图像坐标，以及在目标物体坐标系下的角点坐标，优化求解目标物体相对于多目视觉系统的空间位置和姿态关系。本发明特别适用于大尺寸目标物体高精度定位，也适用于某一类大尺寸物体的不同个体的识别和定位。

附图说明

图1为本发明实施例提供的最小化二维码角点重投影误差的目标物体六自由度定位系统的结构示意图；

图2为本发明实施例提供的最小化二维码角点重投影误差的目标物体六自由度定位方法流程图；

图3为本发明实施例提供的相机内参标定示意图；

图4为本发明实施例提供的重投影过程示意图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图3为畸变矫正后的工业视觉系统成像原理图，对于一个由工业相机和镜头组成的确定工业视觉系统，设其相机坐标系为坐标系{c}，像素坐标系为坐标系{i}，相机坐标系{c}下的一个三维点p，在像素坐标系下的投影点为p，若设三维点p的齐次坐标为：[x,y,z,1]^t；点p在像素坐标系{i}上的二维投影像素点p的齐次坐标为[u,v,1]^t，则有

其中矩阵

为相机的内参矩阵，可以通过标定相机获得，根据以上公式，给定相机坐标系{c}下的任意点三维坐标，均能计算出其对应的像素坐标系{i}下的二维投影点坐标，但是给定某个点在像素坐标系{i}下的二维投影点，则无法求出其在相机坐标系下的三维点，因为缺乏此三维点在相机坐标系下的深度，也即z轴坐标值。

重投影误差。如图4所示，假定已知视觉系统的内参矩阵，对于相机坐标系{c}下的某一点p，通过视觉系统成像得到图像数据，其图像数据内也即在像素坐标系{i}下的投影点为p，自然如果已知点p在相机坐标系{c}下的坐标，那么根据内参矩阵也可以计算出点p在像素坐标系{i}下的投影点p'，如果输入的点p的三维坐标存在误差，那么在像素坐标系{i}下投影点p和投影点p'将不重合，实际投影点p与计算投影点p'之间的误差称为重投影误差。

本发明方法包含三个重要过程，其一，标定过程，标定出二维码角点在目标物体坐标系下的准确位置，标定出所有相机的内参矩阵，标定出相机坐标系与定位坐标系之间的关系；其二，检测过程，通过相机拍摄目标物体上所有二维码，并在图像数据中检测二维码角点的像素坐标；其三，建立优化目标，也即最小化重投影误差。通过此三个步骤即可实现目标物体空间六自由度定位。

图1为本发明实施例提供的最小化二维码角点重投影误差的目标物体六自由度定位系统的结构示意图，为了便于说明，仅示出与本发明实施例相关的部分。

1需要进行六自由度空间定位的目标物体，2为目标物体坐标系{l}，3、4、5和6为粘贴在目标物体上的二维码1、二维码2、二维码3、二维码4，7为定位坐标系{h}，8、9、10和11分为别相机1、相机2、相机3和相机4，12为相机1自身的相机坐标系{cm}，{im}为相机cm对应的像素坐标系。

图1中是以四个工业相机拍摄四个二维码为例进行说明，在本发明实施例中，二维码的数量不限于四个，但目标物体上粘贴至少一个二维码，一个二维码存在四个角点，二维码粘贴的位置根据对应工业相机成像视野确定，尽可能粘贴在工业相机成像视野的中间区域，以使多目视觉系统检测范围更广，工业相机的数量也不限于四个，一个工业相机的视野中间区域存在至少一个二维码，也可以存在多个二维码。

二维码至少包含二维码的编号信息、及编码信息关联的二维码各角点在目标物体坐标系中的坐标信息，二维码的四个角点按照指定的顺序记性编号，例如顺时针或逆时针编号，设定二维码编号的目的是，便于系统通过二维码内容查询其角点在目标物体坐标系中的坐标息，在二维码编码中也可添加对应目标物体的其他信息，以便区分不同目标物体；

图2为本发明实施例提供的最小化二维码角点重投影误差的目标物体六自由度定位方法流程图，该方法具体包括如下步骤：

s1、通过测量设备，如激光跟踪仪等精密仪器，准确测量出四个二维码的角点在目标物体坐标系{l}下的准确坐标，如第i个二维码第j个角点在目标物体坐标系{l}下的坐标

s2、对多目视觉系统的多个工业相机，调整好镜头焦距后分别进行标定，获得其内参矩阵，如对第m个相机获得其内参矩为mm：

s3、通过工业相机拍摄二维码图像，识别二维码并检测二维码角点在像素坐标系下的坐标，设第i个二维码第j个角点在像素坐标系{im}下的坐标为称为二维码角点像素坐标采集值；

s4、计算二维码角点在像素坐标系{im}下的坐标称为二维码角点像素坐标计算值。

假如给定目标物体坐标系{l}相对于定位坐标系{h}之间的坐标转换关系根据相机标定参数，即可求出对应二维码角点在像素坐标系{im}下的坐标，

即第i个二维码第j个角点在第m个相机的相机坐标系{cm}下的坐标

为二维码mn角点在目标物体坐标系{l}中的坐标，为目标物体坐标系{l}相对于定位坐标系{h}之间的坐标转换矩阵，为定位坐标系{h}相对于相机坐标系{cm}之间的坐标转换矩阵。

根据第m个相机的成像模型，得到第i个二维码第j个角点在像素坐标系下的坐标

为二维码mn角点在相机坐标系{cm}中的坐标，为的z坐标分量，mm为相机cm的内参矩阵.

若给定的目标物体坐标系{l}相对于定位坐标系{h}之间的坐标转换关系与真实值一致，那么通过相机成像模型计算的第i个二维码第j个角点的像素坐标应该与实际像素坐标一致，也即：

s5、基于以上分析，建立优化目标：

其中，优化变量为即需要定位的目标物体坐标系{l}相对于定位坐标系{h}的转换矩阵，为二维码m角点在目标物体坐标系{l}中的坐标，为定位坐标系{h}相对于相机坐标系{cm}之间的坐标转换矩阵，为二维码m角点在相机坐标系{cm}中的坐标，为的z坐标分量，mm为相机cm的内参矩阵，本发明中的定位坐标系{h}可以理解为机器人末端执行器上的坐标系；

只有求得的目标物体坐标系{l}相对于定位坐标系{h}的位姿关系矩阵足够精度，才能使计算的二维码角点图像坐标与实际图像中二维码角点坐标足够接近。通过求解采用最常用的非线性最小二乘方法即可实现优化求解，优化求解的包含的目标物体坐标系{l}相对于定位坐标系{h}的位置和姿态信息，也即实现了目标物体的空间六自由度定位。

上述优化过程通过搜索法、迭代线性最小二乘法、非线性最小二乘法均可实现，本发明仅以非线性最小二乘法为例进行了相关说明。

本发明主要通过在目标物体上布置多个二维码，并通过多个已标定的工业相机对二维码进行拍照和角点检测，根据识别的二维码角点图像坐标，以及在目标物体坐标系下的角点坐标，优化求解目标物体相对于多目视觉系统的空间位置和姿态关系。本发明特别适用于大尺寸目标物体高精度定位，也适用于某一类大尺寸物体的不同个体的识别和定位。

并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。