一种在线CAD模型驱动的级联式机械臂视觉引导逼近方法与流程

本发明涉及计算机视觉及工业自动化技术领域，具体涉及一种在线cad模型驱动的级联式机械臂视觉引导逼近方法。

背景技术：

机械臂的精确引导一直以来都是工业自动化的一个重要组成部分，但目前很多工业流水线上的机器人只能完成重复单调的动作，在使用机械臂对随机摆放的零件进行接近并执行动作这一方面做得还不够充分，当零件相对于机械臂末端执行器的位姿不确定时，机械臂就无法完成一些精确的工作，因此研究一种可靠的机械臂精确引导逼近方法就显得尤为重要。

目前对于零件进行位姿识别并引导机械臂接近的方法主要分两大类：其一是通过视觉伺服的方法，计算当前图像(或位置)与目标图像(或位置)之间的误差，利用该误差信号作为反馈控制机械臂的运动，这种方法的优点在于控制的精度高，缺点在于运动过程中可能使零件脱离摄像机的视域范围丢失图像特征，致使机械臂无法收敛到期望目标状态；其二是直接通过当前图像与目标图像计算相对位姿，之后控制机械臂完成接近，这种方法的优点在于能够仅依靠起始位置的图像计算得到零件的位姿，将路径规划的部分交由机械臂控制器完成，缺点在于两张图像之间的差别不能太大、不能对路径进行规划，并且其控制方式为开环控制，无法对最后的精度做出保证。

针对目前方法的不足，本发明考虑结合使用这两种方法以避免各自的缺点，并利用双视图三维重建方法引导机械臂按设定路径运动。通过基于模板的位姿匹配定位大致位姿，之后在线生成路径上的零件虚拟图像引导机械臂按指定轨迹接近零件，最后利用视觉伺服方法完成精确引导。如此，既避免了单一使用视觉伺服方法可能使零件脱离摄像机的视域范围丢失图像特征，又避免了单一使用位姿估计方法精度不高的问题，同时使得轨迹可控，满足实际工业应用的需求。

技术实现要素：

本发明为解决上述机械臂引导方法的不足，提出一种在线cad模型驱动的级联式机械臂视觉引导逼近方法，

如图2所示，本发明的技术方案包括以下步骤：

方法实施前，机械臂末端安装摄像机，并进行手眼标定。

步骤1，离线准备阶段：建立被抓取零件的cad模型，使用opengl图形程序接口读取该模型，使用虚拟摄像机对零件的cad模型在不同位姿下拍摄生成一系列模板图像，对模板图像进行边缘检测提取获得模板边缘图像，然后对模板边缘图像进行聚类，构造位姿层次树；

步骤2，位姿预估阶段：将实际拍摄的实际边缘图像和位姿层次树中的模板边缘图像进行相似度计算，以相似度最高的模板边缘图像拍摄时对应的位姿作为预估位姿；

步骤3，接近阶段：以预估位姿设定机械臂的运动轨迹，在运动轨迹上生成多张路径模板图像，通过路径模板图像引导机械臂按运动轨迹接近被抓取零件；

步骤4，精确引导阶段：接近被抓取零件后，用机械臂末端的摄像机实时采集获得实际图像，根据被抓取零件的目标位姿实际图像和实际图像利用基于图像的视觉伺服方法进行最后精确引导，进行抓取或装配操作。

本发明融合了模板位姿估计方法(步骤1+2)、双视图三维重建方法(步骤3)和基于图像的视觉伺服方法(步骤4)进行机械臂视觉引导逼近。

所述步骤1中，opengl图形程序接口中的光源设置应与实际情况相同，使模板图像中的零件不同表面之间的灰度变化与机械臂摄像机拍摄到的实际零件不同表面之间的灰度变化相同。

所述步骤1和2中进行边缘检测提取具体是：对图像采用边缘sobel算子进行卷积(即进行边缘检测)，得到边缘图像，并将边缘图像中低于阈值的像素点全部置0，避免捕捉到圆弧面上由多边形拟合出的错误边缘。

所述的步骤1中，对模板边缘图像进行聚类构造位姿层次树具体是：对模板边缘图像按相似度进行层次聚类，每完成一层的聚类，对模板图像进行向下采样，再次进行边缘检测及聚类，直至类别数达到设定数量，即完成了位姿姿态树的构造。具体包括：

1.1针对所有图像，相似计算每两个图像之间的相似度，将相互之间均相似的多张图像归为一类，从而进行分类；

1.2先对模板边缘图像采用步骤1.1进行处理获得第一层分类；

1.3再针对第一层分类后的每一类，取位姿位于中心的模板边缘图像作为该层该类的中心图像，接着将该层的所有中心图像采用步骤1.1进行处理

1.4获得下一层分类，再迭代重复步骤1.3直至类别数达到预设数量阈值，完成位姿姿态树构建。

从第一层到最高一层的图像大小作逐渐减小处理，使得能够快速完成层次聚类。

所述的步骤2具体为：用机械臂末端的摄像机实时采集获得实际图像，并进行边缘检测提取获得实际边缘图像，将实际边缘图像和位姿层次树中的模板边缘图像进行相似度计算，并使用终止条件加速相似度计算，获得相似度最高的模板边缘图像，以模板边缘图像拍摄时对应的位姿作为预估位姿。

未加特殊说明的情况下，所述位姿均为机械臂末端执行器与零件坐标系的相对位姿，包括位置和姿态。

所述的预估位姿目的是便于进行后续的路径规划，并非仅依靠该方法引导机械臂全程的运动。

更具体地：先将实际边缘图像与位姿层次树中最高层的模板边缘图像进行相似度计算，再根据最高层相似度结果将实际边缘图像与位姿层次树中第二高层中包含最高层匹配成功的模板图像的那一类中的模板边缘图像进行相似度计算，以此类推直到与第一层的模板边缘图像进行相似度计算后，以相似度最高的模板边缘图像拍摄时对应的位姿作为预估位姿。

所述的相似度采用以下公式计算：

其中，为一图像中的任意一点p处的梯度向量，为另一图像中点p’处的梯度向量，点p与点p’在各自图像中的坐标相同，n为图像中的所有像素点的总数，i代表计算的是第i对点，s为两张图像之间的相似度；

若相似度大于相似阈值tm，则认为两张图像相似；否则不相似；

并且在相似度值计算过程中，为了加快模板边缘图像与实际边缘图像之间相似度计算的速度，第k对点时的相似度值sk满足条件时，则立即终止计算，认为两张图像不相似，以加速整体的计算速度。

第k对点时的相似度值sk采用以下公式计算：

若此时即便之后所有点对的两个边缘向量的内积均为1，最终的相似度s也将小于tm。因此在满足条件时，则认为两张图像不相似，以终止计算、加速整体的计算速度。

所述的步骤3具体为：以预估位姿和目标位姿之间的连接直线作为机械臂的运动轨迹，在运动轨迹上设置多个位置点，用虚拟摄像机沿轨迹运动在每个位置点生成零件的路径模板图像，利用路径模板图像控制机械臂进行逐位置点移动；在每次需要移动到下一个位置点时，将实际图像与下一个位姿的路径模板图像进行特征点匹配，计算两张图像之间的本质矩阵，进而计算旋转和平移向量来移动机械臂，如此逐位置点移动直至机械臂末端到达目标位姿。

所述的步骤3包括：以预估位姿和目标位姿的位置之间的连接直线作为机械臂的运动轨迹，在连接直线上均匀取n个位置点，在预估位姿和目标位姿的姿态(朝向方向)之间插入n个姿态(这里的每个姿态，其视线方向均为由当前位置点指向零件坐标系原点，其上方方向可以在始末两个姿态的上方方向之间均匀插值得到)，n个姿态分别作为n个位置点的姿态，用虚拟摄像机在每个位置点以对应的位姿拍摄获得n张路径模板图像；

在每次需要移动到下一个位置点时，提取当前位置位姿下的实际图像和下一位置位姿下的路径模板图像的特征点，根据特征点对两幅图像的特征点进行匹配得到匹配点对，用匹配点对采用求解计算获得本质矩阵e，然后对本质矩阵e进行奇异值分解得到旋转矩阵r和平移向量t，使机械臂末端按照旋转矩阵r进行旋转并按照平移向量t进行平移。

本发明仅在机械臂末端执行器接近零件之后使用基于图像的视觉伺服方法，并非在机械臂全程的运动使用。

本发明的有益效果是：

1)本发明结合使用基于模板的位姿估计、基于双视图的三维重建方法及基于图像的视觉伺服以对零件进行视觉引导逼近，避免了单一使用视觉伺服方法可能使零件脱离摄像机的视域范围丢失图像特征，又避免了单一使用位姿估计方法精度不高的问题，同时使得轨迹可控。

2)本发明通过cad模型生成模板图像和路径模板图像进行位姿估计与路径引导。

通过使用cad模型生成模板图像和路径模板图像，避免实际操作中需要拍摄大量模板图像的问题，简化了工作的流程，并且提高了模板图像的精度。

通过单目图像信息与cad模型生成的模板图像进行相似度计算可以获得预估位姿信息，在机械臂接近零件的过程中，位姿的误差随着距离的缩短而减小，能够更精确地完成引导任务。

3)本发明仅通过单目视觉信息并结合已知模型的cad信息就能完成视觉引导逼近。本发明引导精度高，轨迹可控，避免了单一使用视觉伺服方法可能使零件脱离摄像机的视域范围丢失图像特征和单一使用位姿估计方法精度不高的问题，同时使得轨迹可控，可满足实际工业应用的需求。

附图说明

图1为本发明实施例的机器人组件设备示意图；

图2为本发明方法流程图；

图3为实施例四种可能的摄像机相对位姿示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

图1显示了能够使用本发明的实施方式的机器人组件设备。系统包括六轴机械臂1，单目摄像机2及末端执行器3，需要抓取的零件为4。其中单目摄像机2固连在末端执行器3上。

方法实施前，对机械臂末端安装的摄像机使用棋盘格标定板通过tsai-lenz方法进行手眼标定。

第一步离线准备。建立被抓取零件的三维cad模型。为了获得三维cad模型的二维图像，本发明优选地使用opengl读取该模型，但不限于该图形程序接口，可以使用诸如directx等其他的图形程序接口。视线方向对准零件坐标系原点，保证零件处于图像中心，均匀改变视点位置(3个参数)及绕视线旋转角度(1个参数)共4个参数的值，保存在这些位姿下所获得的零件二维图像,即模板图像。采用sobel算子对模板图像进行卷积(即进行边缘检测)，获得模板图像在水平方向及竖直方向的梯度值，也即提取出了边缘。由于在opengl中的零件表面是由面片组成的，因此如圆弧之类的表面也是由许多面片来近似的，所以圆弧表面相邻面片之间的灰度值会略有不同，这就会被边缘算子所捕捉到。因此之后需要对获得的梯度值进行阈值处理，将低于阈值的梯度全部置0，因为圆弧面上相邻面片的灰度值相差不大，所以经过卷积得到的值也很小，阈值处理就可以将圆弧面上的错误边缘除去，经过处理后的该图像称为模板边缘图像。

为加速单目摄像机2拍摄到的实际图像提取边缘后的实际边缘图像与模板边缘图像的匹配过程，需要对模板边缘图像构造位姿层次树，减少搜索的时间。位姿层次树构造方法如下：首先任取一张模板边缘图像，计算位姿相邻的另一张模板边缘图像与其的相似度，若该相似度大于相似阈值tm，则将其归为一类。

计算与该类中的模板的位姿相邻(还没有被划入其他类)的另一个模板边缘图像与类内所有图像的相似度，若所有相似度均大于tm，则将其加入该类；如此往复，直至与该类中的模版的位姿相邻的模板边缘图像均不能满足前述的条件。然后再任取一个还未被分类的模板边缘图像，重复上述过程，直至所有位姿的模板边缘图像均被分类完毕，完成第一层的分类。

将处于每一类中心位置的模板边缘图像和模板图像作为该类的模板边缘图像和模板图像。随后将每一类的模板图像进行向下采样(图像大小缩小为原先的四分之一)，获得第二层的模板图像，之后提取边缘获得第二层的模板边缘图像，以同样的方式对第二层的模板边缘图像进行分类。如此往复，直至某层的类别数量减小至设定的阈值。

第二步位姿预估阶段。获取机械臂起始位置处拍摄到的实际图像，对实际图像进行若干次向下采样操作(该操作次数取决于之前生成的位姿层次树的层数)，对这些图像进行边缘提取，获得若干实际边缘图像，将这些实际边缘图像与位姿层次树内的模板边缘图像进行相似度计算，从上层至下层逐步搜索。例如：预先构造完毕的位姿层次树有2层，第一层有30张模板边缘图像(100*100大小)(共分为5类，每类6张模板)，第二层有5张模板边缘图像(50*50大小)。则对实际图像(100*100大小)首先进行1次向下采样，得到50*50的图像，再对这两张图像提取边缘。首先使用50*50的实际边缘图像与第二层的5张模板边缘图像计算相似度，假设与第一张匹配，则之后再使用100*100的实际边缘图像与第一层中包含第二层第一张模板边缘图像的那一类中的图像计算相似度，假设与第三张图像匹配，则返回该图像所对应的位姿信息作为预估位姿。

第三步接近阶段。

首先按照路径最短的条件规划出轨迹：在第二步中估计出的与零件的相对位置与预设的最终的与零件之间的相对位置之间沿直线均匀取n个位置点；

在第二步中估计出的与零件的相对姿态与预设的最终的与零件之间的相对姿态之间插入n个姿态(这里的每个姿态，其视线方向均为由当前位置点指向零件坐标系原点，其上方方向可以在始末两个姿态的上方方向之间均匀插值得到)，分别作为之前n个位置点的姿态，这样就获得了n个相对零件的位姿，在计算机中将虚拟摄像机的位姿设定为这n个位姿，就获得了n张路径模板图像。

利用基于尺度不变、旋转不变的特征提取算法，提取当前位姿下摄像机实际图像和第1张路径模板图像中的特征点，根据特征点的描述信息对这两幅图像的特征点进行匹配，得到若干对匹配点对。

为了提高最后的计算得到的本质矩阵e的精度，可使用ransac方法进行优化。本发明使用opencv自带的findessentialmat()函数进行本质矩阵e的求解，findessentialmat()函数中使用ransac方法进行优化求解过程。

在得到本质矩阵e之后，对它进行奇异值分解得到两个旋转矩阵r和两个平移向量t，由此可以组合出4种情况，如图3所示。图中，a和b分别代表摄像机的两个位置，“t”字型符号的横线代表成像平面，竖线代表摄像机的光轴方向，竖线的下端点代表摄像机的光心位置。可以发现，除了(a)以外，物体均会出现在某个(或两个)摄像机的后方，据此，可以排除三种错误的情况，获得唯一的r和t的解。在本发明中通过调用opencv中的recoverpose()函数得到正确的r和t。

之后使机械臂末端按照矩阵r进行旋转并按照向量t进行平移，即可运动至第1张路径模板图像拍摄时所处的相对位姿。之后再读取第2张路径模板图像，进行类似的动作，如此往复，直至接近阶段完成。

第四步精确引导阶段。将最终位姿时刻机械臂末端摄像机拍摄到的实际图像作为目标图像(手动获得第一个零件的目标图像，由于机械臂接近的对象是若干相同的零件，所以该图像可以用于之后零件的引导，因为机械臂末端执行器在执行动作时相对于每个零件的相对位姿都是相同的)，利用基于图像的视觉伺服方法完成最后的精确引导，并执行动作。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。