CN112109072B - 一种大型稀疏特征托盘精确6d位姿测量和抓取方法 - Google Patents

Abstract

本发明公开了一种大型稀疏特征托盘精确6D位姿测量和抓取方法，包括离线校准和示教：利用两个摄像机图像的二维信息可以确定托盘的特征，并将其转换为相机坐标系中的3D坐标，并提前进行手眼标定，在手眼标定工作结束后，也进行了示教过程；在线位姿测量和抓取：机器人将双目视觉安装到一个位置，可以发现物流托盘上的一个特征；移动机器人，可以发现托盘上另一个特征；再移动机器人，发现托盘上第三个特征，并且这三个特征不共线，然后这三个特征在图像上的位置使用基于形状的模板匹配来定位；用双目视觉重建特征点的3D位置；通过3个特征点计算位置和姿态，并根据这个信息进行抓取，本方法降低了硬件的要求并且具有更高的位姿测量精度。

Description

一种大型稀疏特征托盘精确6D位姿测量和抓取方法

技术领域

本发明涉及一种机器人抓取方法，特别涉及一种基于双目视觉机器人系统的抓取方法。

背景技术

随着对定制产品的需求和视觉技术的发展，越来越多带有视觉系统的机器人被开发和实际应用。目标物的精确6D位姿信息对于视觉引导机器人3D操作的成功至关重要。基于视觉的6D位姿测量技术主要分为两类：主动视觉和被动视觉。主动视觉方法主要是针对不同类型的结构化光视觉系统[1]，通过提取投射到目标结构化光的成形条纹，可以构造目标的表面信息，并估计目标的6D位姿。而双目视觉[2]属于被动视觉，它是使用两台摄像机拍摄同一场景的两幅图像以及两个相机的几何关系来重建3D信息的技术。一般在双目视觉下，通过相机拍摄到目标物的所有表面信息，这时会采集到表面不稳定的特征，导致最终测量偏差大。为了提高测量精度和降低所需的硬件设备，选定目标物表面三个稳定且纹理深的特征，在基于双目视觉的机器人系统下，对目标物进行位姿测量和抓取。

[1]张楠.基于图像信息处理的主动视觉跟踪系统研究[D].长春工业大学,2015。

[2]佘银海,王新彦.基于双目立体视觉的草坪植株高度测量[J].江苏农业科学,2020,48(04):215-221。

发明内容

本发明的目的是提供一种大型稀疏特征托盘精确6D位姿测量和抓取方法，提高测量精度。

本发明的目的是这样实现的：一种大型稀疏特征托盘精确6D位姿测量和抓取方法，以下两个过程：

离线校准和示教：

步骤S1：用OpenCV分别校正左相机和右相机，得到他们的内参和畸变系数，找到左右标定图的内角点坐标，加上三维世界坐标P，计算出右相机相对于左相机的平移向量T和旋转矩阵R，来对双目视觉标定；

步骤S2：进行机器人手眼标定，确定机器人末端工具坐标系与相机之间的固定几何关系；

步骤S3：示教，在机器人末端工具坐标系与物体坐标系之间的位姿校准过程，通过抓取和拍照位姿关系，计算并保存目标物与机械手工具坐标系之间的相对位姿；

在线位姿测量和抓取：

步骤S4：使用基于形状模板匹配的方法，生成三个模板，然后基于三个模板分别在图像上定位到对应的三个特征点，并且这三个特征点不共线；

步骤S5：在标定好的双目视觉系统下，确定右相机在左相机的旋转矩阵R和平移向量T；然后经过图像矫正技术，转为理想模型，在根据图像中定位到的三个特征点分别在左相机坐标系中重建3D坐标；

步骤S6：将相机坐标系中的三个特征点坐标转换为机器人基座坐标系；

步骤S7：机器人在双目视觉引导下抓取托盘。

作为本发明的进一步改进，步骤S2具体为：采用eye-in-hand手眼标定系统对机器人末端执行器与相机之间的位姿进行校准，机器人基座坐标系、机器人末端工具坐标系和相机坐标系形成一个封闭的链：^BH_O＝^BH_T ^TH_C ^CH_O，其中^TH_C是一个不变和未知的变换矩阵，在机器人移动过程中任意两个位姿，有以下公式成立：

经转换后，可得：

而且在两个过程中

求得^TH_C，确定了机器人末端工具坐标系与相机之间的固定几何关系。

作为本发明的进一步改进，步骤S3具体为：示教过程可以分为两步：对物体的6D位姿进行测量和抓取位姿教学；在测量物体的6D位姿时，建立以下关系：

在抓取教学阶段，建立以下关系：

在位姿测量和抓取教学阶段，物体的位姿是保持不变的，因此

计算出物体坐标系与机器人末端工具坐标系之间的相对位姿转换：

规划出控制器操作的路径，往后机器人在线抓取的每一步都可以按这个路径抓取。

作为本发明的进一步改进，步骤S4具体为：

采用基于形状模板匹配的方法，通过指定特征的ROI，从图像中生成一个模板；然后对模板图像进行canny边缘提取，提取出模板图形的边界点，用sobel算子分别求出模板图像每个点的x方向梯度和y方向梯度，在根据x，y梯度结合和边界点位置信息，求出每个边界点的梯度方向和梯度大小和模板边缘点的重心；在进行匹配时，将原始图像上的点(x，y)转化成对应的梯度向量场值e_x,y＝(v_x,y,w_x,y)^T；其中，v_x,y，w_x,y分别是原始图像上点(x，y)经过sobel算子求出的梯度向量；在模板模型与梯度向量场之间，建立一个相似度量函数s，其中s表达式如下：

其中，n为特征模板模型上所有点的数量，d_i'为经过线性转换的，模板模型上点的方向向量，e_q+p'为原始图像上点的梯度向量。

将模板在带匹配的图像上从左到右、从上往下依次计算s的值，如果s超过给定阈值则代表模板匹配成功，并记录此时模板在图像中的位置和旋转信息；按照以上步骤，基于生成的模板(a)、(b)、(c)分别在图像上定位三个特征点。

作为本发明的进一步改进，步骤S6具体为：

选取托盘上的特征点1作为托盘坐标系的原点，X轴方向为特征点1到特征点2的方向，假设连接特征点1与特征点3确定一个A向量，Z轴方向根据X与向量A的叉乘确定；接着Y轴方向根据Z和X的叉乘确定：Y＝Z×X，因而建立一个托盘坐标系，然后根据方程：^BH_O＝^BH_T ^TH_C ^CH_O，其中^CH_O、^TH_C在前面步骤已被计算，^BH_T在机器人系统中已知，因而建立以下托盘坐标系到机器人基座坐标系的转换，即托盘坐标系在机器人基座坐标系中的位置与姿态；

与现有技术相比，本发明的有益效果在于：本发明首先机器人将双目视觉安装到一个位置，其中那个位置可以发现物流托盘上的一个特征；移动机器人，可以发现托盘上另一个特征；再移动机器人，发现托盘上第三个特征，并且这三个特征不共线，然后这三个特征在图像上的位置使用基于形状的模板匹配来定位；其次，用双目视觉重建特征点的3D位置，用手眼标定信息将相机中特征点的3D坐标转换为机器人基座坐标系；最后，通过3个特征点计算了机器人基座坐标系中物流托盘的位置和姿态，并根据这个信息进行抓取。与现有技术相比，本发明的有益效果在于：本发明借助于工业机器人移动，基于目标物表面稀疏特征对大型物体的6D位姿测量和抓取，因而降低了硬件要求，同时提高了测量精度。

附图说明

图1为本发明中离线校准与在线位姿测量和抓取流程图。

图2为本发明中双目视觉示意图。

图3为本发明中机器人手眼标定图。

图4为本发明中机器人示教图。

图5为本发明中基于形状的模板模型图。

图6为本发明中托盘坐标系图。

图7为本发明中机器人抓取现场图。

具体实施方式

下面结合具体实施例对本发明做进一步说明。

在实施例中，工业机器人为ABB IR6700，其最大工作范围为3200mm，最大载荷为150KG。双目视觉系统由两台Basler ACE-3800 1000万像素灰色相机组成；托盘大小为1200mm×1200mm。夹持器是专门用来抓取物流托盘的，两侧各有两个爪，中间区域设有6个气动吸盘。

如图1所示，一种基于双目视觉机器人系统的大型稀疏特征托盘精确6D位姿测量和抓取方法，包括离线校准和示教与在线位姿测量和抓取：

离线校准和示教：

步骤S1：用OpenCV分别校正左相机和右相机，得到他们的内参和畸变系数，找到左右标定图的内角点坐标，加上三维世界坐标P，求出右相机相对于左相机的平移向量T和旋转矩阵R，来对双目视觉标定(附图2)。表1是左右两相机的内参标定结果，表2为左右两相机的相对位姿标定结果；

表1左右相机内参标定结果

相机	f/mm	k/mm	Sx/um	Sy/um	Cx	Cy
							左相机	12.2442	-515.782	1.67	1.67	1918.05	1288.57
右相机	12.2594	-510.88	1.66967	1.67	1919.07	1329.35

表2左右相机相对位姿标定结果

步骤S2：进行机器人手眼标定时，对于机器人移动过程中任意两个位姿，有以下公式成立：

经转换后，可得：

而且在两个过程中

因而求得^TH_C，确定了机器人末端工具坐标系与相机之间的固定几何关系(附图3)。手眼标定结果如表3所示；

表3手眼标定结果

步骤S3：示教看作是机器人末端工具坐标系与物体坐标系之间的位姿校准过程，通过抓取和拍照位姿关系，计算并保存目标物与机械手工具坐标系之间的相对位姿(附图4)。

在线位姿测量和抓取：

步骤S4：使用基于形状模板匹配的方法，生成模板(a)、(b)、(c)，然后基于三个模板分别在图像上定位到对应的三个特征点，并且这三个特征点不共线(附图5)；

本实施例中，在1200mm×1200mm物流托盘上选出3个特征，用于托盘位姿测量。基于这3个特征，用模板匹配的方法生成3个模板模型。然后用相似度量函数s，来表明模型与图像之间的匹配程度。

步骤S5：在标定好的双目视觉系统下，确定了右相机在左相机的旋转矩阵R和平移向量T：

和T＝T_r-RT_l。然后经过图像矫正技术，转为理想模型，在根据图像中定位到的三个特征点分别在主相机(左相机)坐标系中重建3D坐标；

本实施例中，已校准好的两个Basler ACE-3800相机被安装距离大约200mm，而且它们的光轴几乎是平行的。相机拍到特征点1，在主相机中进行特征点三维重建；然后移动机器人，拍摄到另一个特征点2，在进行特征点三维重建；最后拍特征点3，进行三维重建。

步骤S6：将相机坐标系中的三个特征点坐标进一步转换为机器人基座坐标系，以便抓取(附图6)；

步骤S7：在抓取过程中，以下方程是正确的：^BH_O＝^BH_T ^TH_O，而在示教阶段，^TH_O已经被校准；^BH_O在步骤6中已被计算，因此得到机器人末端工具坐标系的相对位姿：^BH_T＝^BH_O(^TH_O)^-1，然后机器人在双目视觉引导下抓取托盘(附图7)。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。

Claims (1)

1.一种大型稀疏特征托盘精确6D位姿测量和抓取方法，其特征在于，以下两个过程：

离线校准和示教：

步骤S1：用OpenCV分别校正左相机和右相机，得到他们的内参和畸变系数，找到左右标定图的内角点坐标，加上三维世界坐标P，计算出右相机相对于左相机的平移向量T和旋转矩阵R，来对双目视觉标定；

步骤S2：进行机器人手眼标定，确定机器人末端工具坐标系与相机之间的固定几何关系，步骤S2具体为：采用eye-in-hand手眼标定系统对机器人末端执行器与相机之间的位姿进行校准，机器人基座坐标系、机器人末端工具坐标系和相机坐标系形成一个封闭的链：^BH_O＝^BH_T ^TH_C ^CH_O，其中^TH_C是一个不变和未知的变换矩阵，在机器人移动过程中任意两个位姿，有以下公式成立：

经转换后，可得：

而且在两个过程中

求得^TH_C，确定了机器人末端工具坐标系与相机之间的固定几何关系；

步骤S3：示教，在机器人末端工具坐标系与物体坐标系之间的位姿校准过程，通过抓取和拍照位姿关系，计算并保存目标物与机械手工具坐标系之间的相对位姿，步骤S3具体为：示教过程可以分为两步：对物体的6D位姿进行测量和抓取位姿教学；在测量物体的6D位姿时，建立以下关系：

在抓取教学阶段，建立以下关系：

在位姿测量和抓取教学阶段，物体的位姿是保持不变的，因此

计算出物体坐标系与机器人末端工具坐标系之间的相对位姿转换：

规划出控制器操作的路径，往后机器人在线抓取的每一步都可以按这个路径抓取；

在线位姿测量和抓取：

步骤S4：使用基于形状模板匹配的方法，生成三个模板，然后基于三个模板分别在图像上定位到对应的三个特征点，并且这三个特征点不共线，步骤S4具体为：

采用基于形状模板匹配的方法，通过指定特征的ROI，从图像中生成一个模板；然后对模板图像进行canny边缘提取，提取出模板图形的边界点，用sobel算子分别求出模板图像每个点的x方向梯度和y方向梯度，在根据x，y梯度结合和边界点位置信息，求出每个边界点的梯度方向和梯度大小和模板边缘点的重心；在进行匹配时，将原始图像上的点(x，y)转化成对应的梯度向量场值e_x,y＝(v_x,y,w_x,y)^T；其中，v_x,y，w_x,y分别是原始图像上点(x，y)经过sobel算子求出的梯度向量；在模板模型与梯度向量场之间，建立一个相似度量函数s，其中s表达式如下：

其中，n为特征模板模型上所有点的数量，d_i'为经过线性转换的，模板模型上点的方向向量，e_q+p'为原始图像上点的梯度向量；

将模板在带匹配的图像上从左到右、从上往下依次计算s的值，如果s超过给定阈值则代表模板匹配成功，并记录此时模板在图像中的位置和旋转信息；按照以上步骤，基于生成的模板(a)、(b)、(c)分别在图像上定位三个特征点；

步骤S5：在标定好的双目视觉系统下，确定右相机在左相机的旋转矩阵R和平移向量T；然后经过图像矫正技术，转为理想模型，在根据图像中定位到的三个特征点分别在左相机坐标系中重建3D坐标；

步骤S6：将相机坐标系中的三个特征点坐标转换为机器人基座坐标系，步骤S6具体为：

选取托盘上的特征点1作为托盘坐标系的原点，X轴方向为特征点1到特征点2的方向，假设连接特征点1与特征点3确定一个A向量，Z轴方向根据X与向量A的叉乘确定；接着Y轴方向根据Z和X的叉乘确定：Y＝Z×X，因而建立一个托盘坐标系，然后根据方程：^BH_O＝^BH_T ^TH_C ^CH_O，其中^CH_O、^TH_C在前面步骤已被计算，^BH_T在机器人系统中已知，因而建立以下托盘坐标系到机器人基座坐标系的转换，即托盘坐标系在机器人基座坐标系中的位置与姿态；

步骤S7：机器人在双目视觉引导下抓取托盘。

Images