一种机器人3D视觉导引抓取方法

本发明涉及机器视觉，具体涉及一种机器人3d视觉导引抓取方法。

背景技术：

1、在中国工业转型升级的时代背景下，利用机器视觉与工业机器人对诸多行业进行智能技术改造升级已是大势所趋。在机器视觉的理论领域与机器人的应用领域，手眼标定、视觉识别检测和机械臂自动抓取是核心的研究对象。

2、机器人系统中的手眼标定问题最早可追溯到20世纪末。自从上世纪80年代，shiu和ahmad第一次提出了手眼标定问题中矩阵等式的求解方法之后，手眼标定问题便受到了世界各国学者的强烈关注。面向单一或多样应用场景的手眼标定方法层出不穷。tsai于2002年提出了一种新的手眼标定解析算法，并首次给出了该种方法下手眼标定数据集的筛选原则。fassi和legnani提出了一种从几何角度分析手眼标定矩阵公式的求解算法，同时研究了算法中过度限制和奇异的情况。其他手眼标定算法还包括但不限于欧几里得群理论方法、四元数方法、对偶四元数方法、矩阵直积法和螺旋运动方法等。此外，也有一些新的优化方法如凸优化方法、全局优化方法等用于手眼标定矩阵的求解。上述算法首先需要通过机器人末端在空间中的轨迹移动获取标定所需数据，随后将采集获得的标定数据集代入标定算法进行求解。

3、机械臂自动抓取通常被分为二维平面抓取与六自由度抓取。在机械臂的二维平面抓取中，物体被放置在平面上，末端执行器直接从垂直于平面的单一方向完成抓取操作，该类抓取方法仅需要感知物体的二维平面坐标与末端执行器的旋转角度，并通过机械臂与物体放置平面之间的相对位置关系确定抓取时的高度与方向。然而，二维平面抓取只适用于放置在传送带、桌面等平面上的物体，应用场景有很大的局限性。六自由度抓取方法适用于三维空间中任意姿态下的物体，可以在物体交互、人机协同等更加广泛的场景下使用，正在逐渐成为机械臂抓取相关研究的主流。六自由度抓取的目标为物体坐标系在机械臂坐标系下的6d位姿，因此，精确感知空间中物体的6d位姿，正在成为机械臂抓取研究中的关键问题。

技术实现思路

1、针对上述问题，本发明提出了一种机器人3d视觉导引抓取方法。首先，对机器人系统进行手眼标定环节，在得到手眼之间的标定矩阵后，对实时的场景点云进行采集，接着转换点云坐标系并进行点云匹配的环节，在得到匹配结果后，即可对最终机器人抓取位姿进行计算。

2、本发明解决其技术问题所采用的技术方案是：一种机器人3d视觉导引抓取方法，包括以下步骤：

3、步骤1)手眼标定；在整个三维空间中分别进行“眼在手上”和“眼在手外”的标定；机械臂位姿通过机械臂输出的6d位姿数据的欧拉角转换为旋转矩阵得到，相机位姿求解使用了张氏相机标定和pnp算法；在得到上述二者位姿数据后，代入求解方程a·x＝x·b进行求解，其中，a为机械臂位姿，b为相机位姿，x为手眼标定问题待求解的矩阵；

4、步骤2)场景点云信息采集；

5、步骤3)点云坐标系变换；在rgbd相机获得并传输场景点云后，变换当前点云所在的坐标系，利用算法将该点云和示教拍照位姿下的点云进行坐标系的统一；

6、步骤4)点云匹配；首先，将待抓取的模型点云匹配到实际场景下的物体点云处，识别并获得二者之间的坐标转换关系，再将模型点云代替场景点云进行后续抓取位姿的计算；最后，利用迭代最近点算法对点云匹配的位姿结果进行微调；

7、步骤5)机械臂抓取位姿计算；将最终抓取所需的机械臂6d位姿结果分为三个自由度的位移与三个自由度的空间旋转，并分别计算。

8、其中，步骤1)手眼标定的具体操作步骤如下：

9、引入在有限感受野下的多相机合作方案，具体为：将一台工业相机固定于机械臂抓手处，另一台辅助定位的相机固定于机械臂外的空间中某一固定点位；根据手眼之间的相对位置关系可将上述两手眼标定场景定义为“眼在手上”和“眼在手外”，二者特征与区别具体如下：

10、(1)眼在手上

11、该场景下，相机固定在机械臂抓手处，标定物放置于相机视野内一固定点位上；以机械臂基座到相机两个坐标系间的变换为目标描述量可以得到如下变换关系：basehgripper·gripperhcam＝basehcal·calhcam；该式中，base指代机械臂基座坐标系，gripper指代机械臂抓手坐标系，cam指代相机坐标系，cal指代标定物世界坐标系；basehgripper表示机械臂基座坐标系到机械臂抓手坐标系的齐次变换矩阵，gripperhcam表示机械臂抓手坐标系到相机坐标系的齐次变换矩阵，basehcal表示机械臂基座坐标系到标定物世界坐标系的齐次变换矩阵，calhcam表示标定物世界坐标系到相机坐标系的齐次变换矩阵；该场景下，手眼标定问题求解的量为固定量——机械臂抓手到相机坐标系的变换矩阵gripperhcam；由于场景中标定物与机械臂基座的相对位置不变，机械臂基座到标定物世界坐标系的变换矩阵basehcal为固定量，因此可以由两组位姿关系消掉固定量basehcal建立方程：basehgripper0·gripperhcam·(calhcam0)-1＝basehgripper1·gripperhcam·(calhcam1)-1，经整理可得(basehgripper1)-1·basehgripper0·gripperhcam＝gripperhcam·(calhcam1)-1·calhcam0，其形如a·x＝x·b；

12、(2)眼在手外

13、该场景下，标定物固定在机械臂抓手处，相机放置于空间内视野可涵盖标定物移动的一固定点位上；以机械臂基座到标定物两个坐标系间的变换为目标描述量可以得到如下变换关系：basehgripper·gripperhcal＝basehcam·camhcal；该场景下，手眼标定问题求解的量为固定量——机械臂基座到相机坐标系的变换矩阵basehcam；由于场景中标定物与机械臂抓手的相对位置不变，同理可以通过两组位姿关系联立方程并整理得到方程：basehgripper1·(basehgripper0)-1·basehcam＝basehcam·camhcal1·(camhcal0)-1，此方程亦符合a·x＝x·b的形式；

14、因此，手眼标定问题可以转换为矩阵方程a·x＝x·b中，对4*4齐次矩阵x的求解；

15、机械臂位姿可通过机械臂输出的6d位姿数据的欧拉角转换为旋转矩阵表示得到，相机位姿求解使用了张氏相机标定和pnp(perspective-n-point)算法；

16、具体地，确定相机位姿的算法流程为：使用的标定物为棋盘格标定板；首先进行相机内参的标定，提取角点并将角点坐标细化至亚像素精度，使用张氏标定算法得到相机内部参数与畸变系数；在对图像的畸变校正过程中，使用相机标定的结果对图像进行去畸变，重新提取亚像素精度的角点坐标，即2d点坐标；将世界坐标系定义在标定板上，m行n列的角点对应的3d坐标为(m*object_size.width,n*object_size.height,0)；已知点集的3d坐标、对应2d坐标以及相机内参，使用pnp算法，建立透视投影模型，可求解得到世界坐标系到相机坐标系的旋转与平移矩阵，即相机位姿；

17、在得到机械臂位姿与相机位姿后，代入求解方程a·x＝x·b，使用opencv:calibratehandeye算子求解；对于求解得到的手眼矩阵，通过计算重投影误差来评估矩阵精度：重投影误差的计算需要两组位姿数据，以其中一组为真值，估计另一个相机位姿，将位姿和3d点集进行重投影得到像平面的2d点集坐标，计算投影点坐标与角点坐标之间的mse误差值。

18、其中，步骤2)场景点云信息采集包括：在手眼标定步骤之后，需使用rgbd相机进行物体图像的点云采集，在此过程中，可同时获得rgb彩色图像与depth深度图像信息，并且获得场景的点云信息。

19、其中，步骤3)点云坐标系变换包括以下步骤：

20、在rgbd相机获得并传输场景点云后，需将该点云和示教拍照位姿下的点云进行坐标系的统一，对场景点云进行坐标系的变换的算法如下：首先，将眼在手上的手眼标定矩阵左乘示教拍照时的位姿，便可获得示教拍照时相机的位置到机械臂基座的转换矩阵，接着，对此矩阵取逆，得到机械臂基座到示教拍照点的位姿转换矩阵1；同理，将眼在手上的手眼标定矩阵左乘拍照场景点云时的位姿，便可获得实时拍照时相机的位置到机械臂基座的转换矩阵2；将上述的矩阵2左乘矩阵1，最终便可将场景点云的坐标系变换至示教拍照时的坐标系。

21、其中，步骤4)点云匹配包括以下步骤：

22、在完成对场景点云的坐标系变换的前提下，选用基于点对特征的匹配方式进行识别与位姿估计，它会将待抓取的模型点云匹配到实际场景下的物体点云处，识别并获得二者之间的坐标转换关系，再以模型点云代替场景点云进行后续抓取位姿的计算，最后，利用迭代最近点算法对位姿结果进行微调；

23、点对特征是该算法的基础，它是由两个带有法向量的空间点计算所得，点对特征是由四个特征量组成的特征向量；

24、已知组成点对的两个点空间坐标分别为m1和m2，法向量分别为n1和n2，则点对特征的定义为：f(mi，mj)＝[||d||2，∠(n1，d)，∠(n2，d)，∠(n1，n2)]；其中，||d||2表示两点之间的欧式距离，形如∠(，)表示两个法向量之间的夹角，且∠(a，b)∈[0，π]；

25、当分别在模型点云和实际拍摄的场景点云中找到特征值一样的点对时，便可由两个点对计算出点对之间的空间变换关系；对于点对特征中的四个值进行离散化，表示为设置两个参数ddist和dangle＝2π/nangle,其中nangle为角度的离散个数；

26、完整匹配分为离线训练阶段与在线运行阶段；在训练阶段，匹配算法会在模型点云上提取上述特征，进行量化，并存储于一个哈希表中；运行阶段，将会在场景点云中提取上述特征向量，并在哈希表中进行查找，在找到对应的特征点对后，计算位姿；

27、在已有上述四维特征的基础上，将三维点云再进行两层分辨率的采样，并将稀疏、稠密两层分辨率的特征进行级联，共形成六维特征；模型哈希表是关联模型点云和场景点云的关键；对于计算得到的六维点对特征，采用murmurhash3算法计算哈希表的关键码值，然后在相应关键码值下存储或查询点对信息；

28、应用投票的机制，将位姿估计结果离散化，对不同点对计算得到的位姿结果进行投票累积，并将得票结果最高的量化位姿值作为最终结果；在投票机制中，mr为场景点云中提取点对特征的一个参考点，αi为离散化后的位姿变量；在两组点对间计算位姿时，利用点对特征上各个向量的几何约束构建一个中间坐标系，将六自由度的位姿计算简化为1个角度αi来表示；

29、在上述算法进行粗匹配得到初步位姿之后，将采用迭代最近点的方式进行精匹配过程，便可获得精确的位姿结果；具体而言，迭代最近点算法用于三维点的匹配，通过寻找两个点集的对应配准点之间的关系，计算满足给定的收敛精度的点集间的变换参数，即旋转和平移关系；其基本步骤为，分别从待配准的两组点云中，选定初始迭代点对，使用初始对应点对估计刚体变换，将模板点云中的点变换到目标点云坐标系中，得到变换后的点云，重新搜索目标点云中与变换后的点云最近的点，更新点云的对应关系；其中，通过估计刚体变换可使得模板点云与目标点云的重合度最大化；重复上述步骤，直至满足停止准则；其中，停止准则可以是迭代次数达到设定阈值或者刚体变换的变化量小于设定阈值；获取当停止迭代时的最终得到的刚体变换，结合粗匹配的物体位姿，计算出使误差函数最小的旋转矩阵r和平移向量t，便为最佳匹配参数，它描述了模板点云与目标点云之间的最佳匹配关系。

30、其中，步骤5)机械臂抓取位姿计算包括以下步骤：

31、在得到了点云匹配的结果后，对机器人最终抓取位姿进行计算；将6d位姿结果分为两部分(三个自由度的位移和三个自由度的空间旋转)并分别计算；

32、三个自由度的位移计算过程为：示教抓取点在相机坐标系下的坐标左乘待抓取点在相机坐标系下相对模版的平移矩阵，之后再左乘相机在基坐标系下的位姿矩阵，表示为basehcam·templatettarget·camtteach pose，其中，basehcam表示相机在基坐标系下的位姿矩阵，templatettarget表示待抓取点在相机坐标系下相对模版的平移矩阵，camtteach pose表示示教抓取点在相机坐标系下的坐标；

33、三个自由度的空间旋转则是通过：示教抓取点在机械臂坐标系下的旋转矩阵左乘机械臂旋转部分变量的逆，之后左乘待抓取点在相机坐标系下相对模版的旋转矩阵，最后左乘机械臂旋转部分变量来求得，表示为basercam·templatertarget·(basercam)-1·baserteach pose，其中，basercam表示机械臂旋转部分的变量，templatertarget表示待抓取点在相机坐标系下相对模版的旋转矩阵，baserteach pose表示示教抓取点在机械臂坐标系下的旋转矩阵。

34、本发明采用以上技术方案与现有技术相比，具有以下有益效果：

35、1、针对手眼协作在工业场景下的应用，本发明引入了在有限感受野下的多相机合作方案。在完成两次手眼标定的基础上，眼在手外的工业相机可起到扩大视野、辅助定位、提高精度的作用。此外，考虑到标定过程需要标定板在视野内尽可能大范围移动，但视野边缘成像极易产生明显畸变，本发明增加了对图像的畸变校正算法流程。

36、2、相较于用传统四维点对特征进行哈希表的构建与投票机制的计算，本发明扩展了点对特征的维度，级联上了基于多分辨率的哈希网格的3d点云特征，并将此六维特征用于计算。经验证，该匹配算法与传统点云匹配算法相比，在速度和精度上均有着突出优势。

37、3、本发明通过对物体进行实时感知检测，并拍摄物体点云，利用点云信息及时获取物体位置信息，根据点云匹配的结果对机器人进行引导定位，使机器人准确完成抓取工作，避免出现偏差。