基于3D视觉的机械臂无规则零件抓取系统

朱胜银 肖伯俊 黄世奇

摘要：为解决机械臂抓取零件定位不准或抓取精度不高的问题，提出了一种基于3D视觉机械臂无规则零件抓取方法，该方法主要包括系统空间标定、零件点云定位分割、零件位姿估计算法模块，提出了基于2D图像的点云相机外参标定方法及基于图像语义分割的零件点云分割方法。经试验验证，所提出的标定方法达到1 mm的系统精度误差，而且求解效率高、通用性强；与常用的聚类分割相比，零件点云分割方法精度及效率更高。所设计的二阶段零件空间位姿计算方法能准确估计零件在空间中的6自由度位姿，相比单一阶段匹配方法精度提高约1.8倍。所提出的基于3D视觉的机械臂无规则零件抓取系统精度高、可扩展性强、适应情形广。

關键词：3D视觉 零件抓取 标定 点云分割 位姿估计

中图分类号：U466   文献标识码：B   DOI： 10.19710/J.cnki.1003-8817.20220285

Abstract： In order to solve the problem of inaccurate positioning or low grasping accuracy of parts grasped by manipulator， this paper proposed a method of grasping irregular parts of manipulator based on 3D vision. This method mainly consisted of 3 algorithm modules： system space calibration， part point cloud location and segmentation， and part pose estimation. The external parameter calibration method of point cloud camera based on 2D image and the part point cloud segmentation method based on image semantic segmentation were proposed. Experiments show that the proposed calibration method achieves a system accuracy error of 1 mm， with high efficiency and strong universality. Compared with the commonly used clustering segmentation， the part point cloud segmentation method has higher accuracy and efficiency. The designed 2-stage part spatial pose calculation method can accurately estimate the 6-DOF pose of the part in space， and the accuracy is improved by about 1.8 times compared with the 1-stage matching method. The proposed manipulator irregular part grasping system based on 3D vision has high precision， strong scalability and wide adaptability.

Key words： 3D vision， Part grabbing， Calibration， Point cloud segmentation， Pose estimation

1 前言

在工业4.0时代，工业机器人已成为现代先进制造业尤其是汽车制造业中不可替代的重要设备，基于工业机器人的智能装备应具备更智能化的特点。传统工业机器人普遍采用离线编程的方式，适合固定流水线的工作方式，很难完成复杂场景下的抓取任务。面对乱序堆放工件产品和汽车整装的物流分拣，人工作业方式很难满足新的需求。视觉智能感知技术改变了以往机器人抓取方式[1-3]，带有视觉引导的工业机器人对环境具有较强的感知能力，能够满足高柔性、高智能化工业生产需求[4]。

机械臂抓取是物流分拣、零件装配等生产流程中至关重要的环节[5-6]。然而传统的抓取操作基本上根据人工经验选取抓取位姿，然后编写示教程序控制机械臂运转[7]。这种人工示教的方法在场景目标发生变化时不能实时自动检测目标的位姿，检测目标单一，场景适应能力差[8]。

机器学习抓取方法通过对目标的特征学习从而实现对抓取位姿做出估计[9-11]。该类方法依赖于被抓取物体的表面纹理信息，对于纹理信息缺乏的物体抓取效果不理想。而基于模板匹配的方法则是将被抓取物体的轮廓信息与模板库中的目标轮廓进行匹配，从而根据最佳匹配模板的抓取位姿来估计被抓取物体的位姿[12]，该类方法能够改善纹理缺失物体抓取效果。

传统的基于机器视觉的机械臂抓取方法[13-14]往往只利用了二维信息，而忽视了三维结构信息，如黄启彪等[15]提出一种吸取式机械手物体抓取方法及抓取系统。2D的目标检测方法无法确定目标的三维姿态，因此对于随意放置的目标物难以根据其不同位姿规划出最佳抓取方式[16]。杜学丹等[17]利用深度学习预测机械臂的最佳抓取位置，该方法设计的抓取位置为（x，y，θ），抓取角度θ仅表示坐标系一个轴，不能准确评估抓取目标的空间位姿。舒心等[18]设计了一种基于自监督学习神经网络的机械臂视觉抓取系统及方法，其利用一个打分函数对18类角度进行评判，得出最佳的抓取角度作为抓取的具体位姿，该方法得到的位姿单一，对形状复杂的零件适应性不好。

为解决上述技术问题，本文设计了一种3D视觉引导的机械臂无序零件抓取方法，一是针对现有零件点云定位分割方法的不足和存在的问题，发现和设计了一种基于零件2D语义分割深度学习模型与零件3D点云相融合的零件三维空间定位分割方法。与传统基于距离点云分割方法相比，点云分割法精度更高、效率更快。二是设计自动标注的方法制作零件语义分割的标签数据集。该方法可以替代传统手动标注，极大地提高零件分割模型训练的效率。三是设计零件点云特征匹配及两点建立坐标系方法准确估计零件空间6自由度位姿。该点云特征匹配方法可以准确地估计无规则零件抓取点位及空间三维x轴、y轴、z轴角度。该方法对于任意放置的零件均能准确计算其空间6自由度位姿，远比二维平面角度的垂直抓取方式适用情形更广。从立体位姿角度方面考虑，本方法获得的零件位姿比由平面位姿加景深图融合而得到的立体位姿更加准确，同时可以标记2点快速确定零件抓取点位及抓取坐标系。四是设计了一种基于2D图像的激光点云3D相机外参标定方法。传统3D点云相机通过目标物体的点云数据计算3D相机在机械臂底座的位姿，而本系统通过左相机采集的2D图像数据计算其位姿。该方式具备机械臂操作方便、数据获取便捷、2D图像求解效率高的优点。

2 机器人视觉抓取系统搭建

针对机械臂视觉抓取任务中系统空间标定、位姿计算两大主要问题[19]，进行了机器人视觉抓取系统搭建。整个系统主要分为机械臂与相机系统空间标定模块、2D深度学习语义分割零件定位模块、零件空间位姿计算模块。系统整体技术路线如图1所示。

在机械臂与相机系统空间标定中，激光点云相机固定在工作空间正上方，工作空间中放置装有各种类别零件的料框，三手指抓手固定在机械臂末端，系统硬件设备方案如图2a所示。机械臂结合视觉进行抓取时，通过相机获取了物体在空间中的位姿信息，但此时的位姿信息为基于相机坐标系，并不能直接使用，需将该位姿转化为机器人可执行的机械臂底座坐标系下的位姿[20]。3D点云相机采集到的零件场景点云经本文提出的点云定位分割算法得到单个零件点云，然后该单个零件点云经本方案位姿计算模块得到其在相机坐标系下的位姿，最后将该位姿经系统标定模块确定的位姿转换关系得到机械臂下的最终目标位姿，执行抓取操作。

本系统方案视觉信息由3D激光相机和双目相机集成于一体视觉设备采集，如图2b所示。该设备可以实时输出点云图像与2D图像，点云图像包含了目标零件的景深信息。系统选取埃尔森AT-S1000-01A-S1型号相机，该相机适用于远距离、大视野、大景深场景，且具备高级别抗冲击、稳定可靠的优点。根据实际料框零件场景，本系统确定的相机工作距离为1.5 m。

3 视觉系统标定原理及方法

根据抓取场景需求，本文所设计的抓取系统要求相机固定安装在机械臂外，并将其安装在固定支架上，机械臂底座固定于地面上，在标定时将标定板固定在机械臂上并伴随机械臂移动。

3.1 相机内部参数标定

相机内部参数在求解像素坐标系与相机坐标系的转换关系中起着重要作用，相机内部参数标定即是求解出相机（Eye）的焦距（fx，fy）、主点（x0，y0）、畸变参数[21]。

3.2 基于最小二乘法优化Tasi手眼标定算法

手眼标定是为了求解机械臂底座坐标系与相机坐标系间的空间位姿关系，从而将相机坐标系下的位姿转化到机械臂底座坐标系下的位姿。

在机械臂运动阶段，任意2个时刻都满足如下关系：

式中，H2D和H3D分别为2D相机位姿矩阵、3D点云相机位姿矩阵。

按照原有相机固定在机械臂外的情形可直接构造方程组AXc2b=Xc2bB，利用Tasi算法直接得出Hc2b，而本文先求解出Hc2g，优化计算过程如下：

a. 间接构造AXc2g=Xc2gB；

b. 根据Tasi方法解算AX=XB方程组；

c. 求解20组Hc2b，Hc2b=Hg2b×Hc2g；

d. 通过20组Hc2b做最小二乘拟合出一个真实值。

其中，Xc2b为相机到机械臂底座的转换矩阵；Hc2g为相机到抓手的转换矩阵；Hg2b为抓手到机械臂底座的转换矩阵。具体优化求解流程如图3所示。

这样通过多组数值进行拟合可以减小一次求解过程中的计算误差，提高标定精度。

4 基于语义分割的零件点云定位分割

本模塊设计了一种基于零件2D语义分割深度学习模型与零件3D点云相融合的零件三维空间定位分割方法，该方法可实现零部件图像的快速定位和像素位置分割，最终得到较为准确的零件点云。

4.1 零件语义分割样本集制作

试验操作中选取了物流试验室4种常见的机械部件，将这些零件按照不同位置任意摆放在料框内，用相机采集500组零件2D图片及对应点云，其中200组作为测试图片，300组作为训练图片。采集的零件图片如图4所示。

4.2 零件标签自动标注

利用深度学习进行自动定位与检测，通常需要大量的标签样本，目前行业内主流是利用Labelimg或Labelme标签工具手动对图片做标签，传统人工手动标记需要大量时间和人力。本自动标注技术利用图像处理算法提取到目标的区域及轮廓，输出区域坐标、类别的标签信息。自动标注原理如图5所示。

4.3 零件点云定位分割

零件2D语义分割深度学习模型采用LinkNet网络结构，通过采用Fine-Tuning训练方法来减少因新增零件类别而重复训练时间和资源消耗。

2D图像与有序点云映射关系如下：

[fx，y→PointCloud（x，y）]              （7）

式中，x、y为原始图像行列索引；PointCloud为相机采集到的有序点云；f（x， y）为2D图像在（x， y）处的像素值。

零件点云定位与分割的详细步骤如下，对应原理流程如图6所示；

a. 对制作好的2D零件图片及标签样本进行语义分割模型训练；

b. 采用Fine-Tuning模型调优技术对新增零件类别进行模型更新；

c. 对实时采集的零件图片进行在线模型预测，得到包含Prob信息的Mask分割结果；

d. 将Mask分割结果根据公式（7）进行3D点云映射，删除非目标点云，确定零件点云图。

5 零件空间位姿计算

零件点云特征匹配及2点建系的方法可以准确估计零件空间6自由度位姿。该点云特征匹配方法可以准确地估计无规则零件的抓取点位及空间三维x轴、y轴、z轴角度[23]。

5.1 两点建立抓取坐标系

根据点云模板上选定的2点自动建立抓取坐标系的具体过程如下：

a. 根据第1点确定坐标系原点及该点处的点云法向量n（与垂直向下向量成锐角），并将其作为抓取坐标系Z轴；

b. 根据该法向量求取切平面，记为XOY；

c. 将第2点所在切平面投影点与原点组成的向量作为X轴；

d. 在XOY面上求取X轴的法向量记为Y轴。

XOY平面方程为：

A（x-x0）+B（y-y0）+C（z–z0）=0                （8）

该平面法向量n=（A，B，C），其中A、B、C为平面法向量3个分量。利用法向量与X、Y轴垂直，即：Xn=0，Yn=0，可求解出所建立的坐标系。

5.2 点云特征匹配

目标点云和模板点云具体特征匹配的过程如下：

a. 根据2点建坐标系法在模板点云上建立抓取坐标系；

b. 将模板点云与场景目标点云按照Fpfh（点特征直方图）、SAC-IA（采样匹配一致性）特征匹配方法得到匹配位姿；

c. 将步骤b得到的位姿作为初始位姿，利用迭代最近点（Iterative Closest Point，ICP）方法得到较精确的目标位姿R、T。粗匹配结合精匹配计算零件抓取位姿的流程如图7所示。

6 试验验证

6.1 相机内部参数标定试验

相机内部参数标定试验步骤如下：

a. 把标定板放在3D相机左眼下方；

b. 旋转标定板，标定板在相机下方变换不同位姿（每个位姿均在相机视野范围）；

c. 相机记录（拍照）下不同位姿的标定板；

d. 重复步骤b和步骤c过程，每个位姿记录一组图片，旋转标定板20个位姿得到20组数据信息。标定采集的图片如图8所示（1 Pixel=0.00 586 mm）。相机内参标定的结果如表1所示，其中Fx、Fy为相机焦距，x0、y0为相机主点，K1、K2、K3、P1、P2为相机畸变因数。

6.2 手眼标定试验

2D相机在机械臂底座的空间关系标定试验步骤如下：

a. 将标定板固定在机械臂上；

b. 旋转机械臂把标定板移动到相机左眼下；

c. 旋转机械臂，使标定板在相机下方变换不同位姿；

d. 相机拍照保存步骤c中标定板多个位置图片；

e. 机械臂同时记录下不同位姿对应的机械臂末端在底座下的位姿信息；

f. 重复步骤c～步骤e过程，得到30对数据信息。需要将标定板置于图像中心到四角各个视场拍照。

将上述30对数据信息随机分为2组，每组各15对数据信息，分别利用2组数据计算相机在机械臂底座的结果矩阵，结果数值及偏差如表2所示。

标定位置（x，y，z）平均波动值为0.573 mm，3个方向的角度偏差<1°。

基于标定结果进行实际空间点验证，以衡量标定相机外部参数的准确性。标定结果验证步骤如下：

a. 确定标定板上一个角点A为验证对象；

b. 根据标定板拍图及上一步得到的标定结果矩阵，可以得到角点A的全局坐标；

c. 将步骤b的计算结果和机械臂记录读取的实际坐標进行比对，结果如表3所示。

基于标定结果预测的标定板一角空间位置平均误差为2.06 mm。

6.3 零件空间抓取位姿验证

6.3.1 零件点云定位分割试验

表4中基于聚类的点云分割方法除了将零件分割出来外，还保留了料框边界区域，并且不同种类零件间不能进行类别区分，因此无法定位目标。本文所采取的分割方法可精确确定每个零件点云区域和种类。

6.3.2 零件点云位姿计算试验

表5展示了1阶段、2阶段的零件匹配效果，基于Fpfh特征匹配与ICP结合的二阶段匹配目标点云与模板点云几乎完全重合，效果明显优于一阶段匹配。图9中a为制作的模板，c为实际待检的点云图，b为匹配检索的结果。结果显示，在零件点云场景中本文的二阶段匹配算法可准确定位到零件位姿。

试验中在每个模板零件上选取标志点，采用本文的位姿匹配算法计算场景中目标零件的标志点位置作为预测值，同时记录目标零件点云上标志点的真实位置，并把预测值与真实值的差统计在表6中。结果数据表明，本文的二阶段匹配算法位姿计算平均误差为2.58 mm，精度提高约1.8倍。从匹配目视效果上，目标点云与模板点云几乎重合。

零件实际抓取效果如图10所示。对于所设计系统的抓取位姿计算误差，需要指出的是抓手坐标系的标定也存在误差（约0.5 mm），这也会导致实际抓取位姿计算误差。

7 结束语

本文设计了一种基于3D视觉的机械臂无规则零件抓取系统。该系统利用深度学习分割模型准确分割定位零件点云，在二阶段特征匹配算法下精确估计零件空间6自由度位姿，并且经过机械臂、3D相机空间系统标定得到高精度抓取位姿。本方法在机器人试验室下对多种任意摆放的无规则料框零件及标准件进行抓取验证，系统标定精度约为1 mm。试验结果表明，所设计的系统抓取精度高、操作便捷、场景适用范围广。

参考文献：

[1] 张文， 丁雨林， 陈咏华， 等. 基于外部视觉与机载IMU组合的爬壁机器人自主定位方法[J]. 清华大学学报（自然科学版）， 2022， 62（9）： 1524-1531.

[2] 倪涛， 张泮虹， 李文航， 等. 基于关键点预测的装配机器人工件视觉定位技术[J]. 农业机械学报， 2022， 53（6）： 443-450.

[3] 刘红帝， 吕睿， 田林雳， 等. 用于引导机器人定位的汇聚式双目视觉算法实现[J]. 机械工程学报， 2022， 58（14）： 161-169.

[4] 张青春， 姚胜， 郭振久， 等. 基于AR技术机械臂物料抓取系统设计[J]. 中国测试， 2021， 47（11）： 41-46.

[5] 王勇， 陈荟西， 冯雨齐. 基于改进CenterNet的机械臂抓取检测[J]. 中南大学学报（自然科学版）， 2021， 52（9）： 3242-3250.

[6] 洪诚康， 杨力， 江文松， 等. 基于多深度相机融合的机械臂抓取系统[J/OL]. 计算机集成制造系统： 1-18[2023-02-22]. http：//kns.cnki.net/kcms/detail/11.5946.tp.202201

24.1920.004.html.

[7] 余玉琴， 魏国亮， 王永雄. 基于改进YOLOv2的无标定3D机械臂自主抓取方法[J]. 计算机应用研究， 2020， 37（5）： 1450-1455.

[8] 张建民， 许志辉， 龙佳乐， 等. 三维立体视觉机械臂智能抓取分类系统的开发[J]. 计算机工程与应用， 2019， 55（15）： 235-240.

[9] 王鹏， 曹现刚， 夏晶， 等. 基于机器视觉的多机械臂煤矸石分拣机器人系统研究[J]. 工矿自动化， 2019， 45（9）： 47-53.

[10] TRI WAHYU UTOMO， ADHA IMAM CAHYADI， IGI ARDIYANTO. Suction-based Grasp Point Estimation in Cluttered Environment for Robotic Manipulator Using Deep Learning-based Affordance Map[J]. International Journal of Automation and Computing， 2021， 18（2）： 277-287.

[11] 张云洲， 李奇， 曹赫， 等. 基于多层级特征的机械臂单阶段抓取位姿检测[J]. 控制与决策， 2021， 36（8）： 1815-1824.

[12] 谢宇珅， 吴青聪， 陈柏， 等. 基于单目视觉的移动机械臂抓取作业方法研究[J]. 机电工程， 2019， 36（1）： 71-76.

[13] 苏杰， 张云洲， 房立金， 等. 基于多重几何约束的未知物体抓取位姿估计[J]. 机器人， 2020， 42（2）： 129-138.

[14] 薛腾， 刘文海， 潘震宇， 等. 基于视觉感知和触觉先验知识学习的机器人稳定抓取[J]. 机器人， 2021， 43（1）： 1-8.

[15] 先驱智能机械（深圳）有限公司. 吸取式机械手的物体抓取方法及物体抓取系统： CN106003119A[P]. 2016-10-12[2022-09-04].

[16] 张波， 王文军， 成波. 基于人脸3D模型的驾驶人头部姿态检测[J]. 汽车工程， 2016， 38（1）： 53-60+71.

[17] 杜学丹， 蔡莹皓， 鲁涛， 等. 一种基于深度学习的机械臂抓取方法[J]. 机器人， 2017， 39（6）： 820-828+837.

[18] 中国科学院电子学研究所. 基于自监督学习神经网络的机械臂视觉抓取系统及方法： CN109702741A[P]. 2020-12-18[2022-09-04].

[19] 陈友东， 刘嘉蕾， 胡澜晓. 一种基于高斯过程混合模型的机械臂抓取方法[J]. 机器人， 2019， 41（3）： 343-352.

[20] 陈璐， 关立文， 刘春， 等. 基于结构光三维视觉测量的机器人制孔姿态修正方法[J]. 清华大学学报（自然科学版）， 2022， 62（1）： 149-155.

[21] 孙典奇， 段慧仙， 裴浩东， 等. 基于TOF相机的空间非合作目标位姿测量方法[J]. 光学学报， 2021， 41（22）： 103-112.

[22] 呂睿， 彭真， 吕远健， 等. 基于重定位的叶片机器人磨抛系统手眼标定算法[J]. 中国机械工程， 2022， 33（3）： 339-347.

[23] 杨旭升， 王帅炀， 张文安， 等. 基于二次关键点匹配的药盒位姿估计方法[J]. 计算机辅助设计与图形学学报， 2022， 34（4）： 570-580.