基于时域编码结构光的高精度三维视觉引导抓取系统研究

孔令升 崔西宁 郭俊广 宋 展 孙红雨

1(中国科学院深圳先进技术研究院 深圳 518055)

2(中国科学院大学 北京 100039)

3(山东科技大学 青岛 266590)

1 引 言

机械臂抓取是机器人自动化领域的一个重要研究课题，在分拣、装配、上料、焊接、喷涂等领域有着广泛的应用。现有的机械臂抓取技术包括基于 2D 图像与基于 3D 视觉抓取技术。其中，基于 2D 图像抓取技术是抓取技术中较成熟的技术，但由于缺乏 3D 姿态，机械臂只能竖直向下获取抓取位置，故在复杂的环境中，该技术已不能满足空间抓取的应用要求[1-2]。基于 3D 视觉抓取技术由于获取物体目标空间位姿的不确定性、获取点云分辨率低、手眼标定结果误差大、场景的复杂性及遮挡等因素影响，增加了机械臂抓取物体目标的难度。随着工业技术的不断发展，机械臂抓取的需求也愈发向可靠、快速和精确方向发展，对现有机械臂抓取技术提出了严峻挑战。

现有的 3D 点云获取技术包括双目、结构光和飞行时间等[3-5]。在不同的 3D 点云获取技术中，基于结构光的三维扫描方法以其高精度和高效率而被广泛使用。黄会明等[6]提出了一种基于微振镜结构光的三维重建方法。该方法基于多频外差的方法对物体进行三维重建，使用了基于点对特征三维特征匹配的方式对获取的三维数据与基准模型进行匹配和定位操作，能获取任意位置放置的物体在机器人坐标系中的三维坐标和法线信息。与 Kinect 相比，该方法无论是在精度还是在处理时间上都有了很大提升。Li 等[7]提出了一种二维视觉和三维视觉相结合的堆垛目标拾取系统的设计方法。该系统利用结构光的方法获取三维信息，并提取出目标区域，再利用广义霍夫变换方法对目标进行匹配和位姿估计，接着将物体的位姿发送给机械手进行自动抓取，其对散乱物体的捕捉非常有效，且利用二维和三维方法的结合提高了算法的效率。

手眼系统是由摄像机和机械手构成的机器人视觉系统，分为 Eye-in-Hand 式手眼系统[8]和 Eye-to-Hand 式手眼系统[9]。手眼标定的意义在于建立起摄像机坐标系与机械臂末端抓手坐标系之间的联系，能将在摄像机坐标系下的信息通过建立的联系转换成在机械臂坐标系下的信息。Tsai 和 Lenz[10]早在 20 世纪 80 年代就提出了经典的方法，将手眼标定的数学模型归结为求解 AX＝XB 矩阵，即将手眼标定的求解具体化为矩阵的求解。Zhuang 等[11]提出了一种同时计算世界坐标系到机械臂坐标系、机械臂末端手爪坐标系到摄像机坐标系的刚体变换方法，通过运用四元数推导 X 和 Z 的显式线性解求得 AX＝ZB 的齐次矩阵方程。

本文提出一种针对非规则目标的 3D 视觉引导抓取系统，系统可分为体视觉模块、点云处理模块和抓取控制模块。在立体视觉模块中，采用格雷码与线移结合的方法作为结构光系统的编码方案，通过解码实现目标高精度的三维重建[12]并建立 3D 模板库。点云处理模块对获取的点云去噪后，将标准模板与预处理后的场景点云进行匹配。得到匹配参数后，利用手眼标定参数计算出机器手抓取位姿。最后由抓取控制模块完成引导机器手对目标物体的抓取。

2 系统方案设计

2.1 系统硬件设计

基于立体视觉的机械臂抓取系统由工业相机、投影仪、上位机、机器人控制器、电动夹持器和机械臂等部分组成。

本文系统设计图如图 1 所示，对应的硬件功能如下:投影仪与工业相机组成立体视觉系统。具体地，首先由投影仪投出编码图案，工业相机进行拍摄并将采集到的图像传到上位机；随后上位机对图像进行处理，利用标定的参数对投影仪投出的编码图案进行解码，得到三维点云数据后，再进行模板点云获取、点云去噪、点云分割、表面模板匹配等步骤；待获取抓取位置姿态后通过 TCP/IP(Transmission Control Protocol/Internet Protocol)通信传送给机器人控制器，控制机械臂与电动夹持器完成抓取动作并将抓取目标物放置在指定位置。

2.2 系统软件设计

在 Windows 系统环境下，以 VS2015 作为软件开发平台，C＋＋语言作为程序开发环境。系统软件结构运用类设计思想，对应的系统软件结构如图 2 所示，分为立体视觉模块、点云处理模块和抓取控制模块。

(1)立体视觉模块。该部分先对相机、投影仪及相机投影仪之间的旋转平移矩阵参数进行标定，再对投影仪的投影图案进行编码，获得高质量的编码图像后，结合标定参数进行解码获取点云数据，并将点云数据发送至点云处理模块。

图 1 系统设计图Fig. 1 The diagram of system design

图 2 系统软件结构Fig. 2 Structure of system software

(2)点云处理模块。该方案采用模板匹配方案对场景中物体进行抓取。首先，在工作距离下获取标准模板，并获取标准模板下的抓取位姿与放置位姿；然后，对获取场景点云进行分割，并将分割后的点云与标准模板进行匹配，获取抓取位姿；最后，通过 TCP/IP 通信将数据传送到抓取控制模块。

(3)抓取控制模块。抓取控制模块主要包括机械臂控制模块、电动夹持器抓取控制模块和路径规划模块。机械臂控制模块通过实时获取目标位姿，并计算机械臂控制参数来控制机械臂移动。当机械臂到达目标位姿后，由电动夹持器抓取控制模块进行抓取，确保抓取成功。为避免抓取后与平台发生碰撞，根据抓取位姿与放置目标位姿合理规划路径，确保目标物能准确放到指定位置。

3 系统实现的关键技术

本文提出的抓取系统实现的关键技术大致分 3 步:(1)标定结构光系统内外参数与机械臂手眼标定参数，通过解码编码图像完成点云重建，基于此完成模板库的建立；(2)进行场景点云与模板库中标准模板点云匹配，找出匹配率最高的匹配参数作为最终结果，获取转换矩阵；(3)基于标定参数与匹配得到的转换矩阵，获取抓取位姿，引导机械臂完成抓取。

3.1 系统标定与点云获取

图 3 系统标定原理图Fig. 3 System calibration schematic diagram

其中，[R, T] 为转换矩阵。

由公式(1)～(3)使用格雷码与线移结合进行编解码[13-14]，可解出摄像机坐标系下的空间点坐标，具体如公式(4)所示。

图 4 手眼标定原理图Fig. 4 System calibration schematic diagram

基于上述方案获取点云后，用三维制图软件创建匹配模板。构建 3D 模板库时，需获取不同位姿下的物体点云构建模板库，以解决实际场景中遮挡和堆叠情况下获取点云的多姿态问题。

3.2 三维点云匹配

本文采用点对特征的三维点云匹配算法[18]。点对特征分为全局特征与局部特征，其中全局特征匹配恢复三维姿态计算量巨大[19]，而直接在点云上进行关键点、面片等局部特征提取易出现误匹配[20]。在本文 3.1 小节中，完成了 2D 图像到 3D 点云的转换，通过转换矩阵的逆矩阵可将 3D 点云转换为 2D 图像，随后在模板图像中设置关键点，可生成 128 维关键点描述子[21]。在局部对模型图片和场景图片进行匹配时，选取匹配率最高的 3 对像素点映射到 3D 点云中构建点对特征。而对匹配结果进行验证时，选取匹配误差最小的特征作为点对特征，从而恢复模型在场景中的全局姿态。点对特征可用来描述两个点的相对位置及法线方向[22]。假设对于任意两个点 m1、m2以及它们各自的法矢量 n1、n2，令矢量差 d＝m2－m1，并定义特征 F 为:

其中，设分量 F1为 m1和 m2之间的距离；F2和 F3为法线与两点定义的向量夹角；F4为两法线夹角。点对特征 F 的 4 个分量如图 5 所示。

图 5 特征描述Fig. 5 Feature description

点云匹配识别率由相关度大小决定匹配程度。在匹配过程中，堆叠物体会匹配多个模板，且由于不同模板在场景中遮挡程度不同，以及模板本身特征的差异，最终生成多组匹配数据。因此，取匹配率最高的一组数据作为最终识别结果。由于场景中总会有一目标处于无遮挡或接近于无遮挡状态，使用该方案在复杂的堆叠情况下也能有效识别。

3.3 机械臂抓取

4 实验结果与分析

4.1 实验设备

本文实验平台由工业相机、投影仪、UR5 机械臂和 RG2 夹持器组成。其中，海康工业相机(MV-CA013-20UM)分辨率为 1 280×1 024，像元尺寸为 4.8 μm，最高帧率为 170 fps；镜头为 OPT(奥普特)工业定焦镜头(OPT-C1214-2M)，光圈为 1.4～16，焦距为 12 mm，C 接口镜头；投影仪采用德州仪器公司的投影仪(DLP4500)，分辨率为 912×1 140，像素尺寸为 7.64 μm；机械臂选用丹麦优傲机械臂(UR5)，有效负载为 5 kg，有效工作半径为 850 mm，自由度为 6 个旋转关节；夹爪选用丹麦优傲夹持器(RG2)，有效载荷为2 kg，夹持力为 3～40 N，总行程为 110 mm，行程时间为 950 ms/200 ms。实验平台搭建如图 6 所示。

4.2 实验结果与分析

在基于结构光的机器人抓取实验中，首先需要重建出散乱摆放系列物体的三维数据，且重点在三维数据的获取速度和精度上。因此，本文选用国际象棋和金属两种物体进行实验。其中，国际象棋表面形状复杂、没有颜色特征，且比较小，使用其他方式很难获取高精度 3D 模型。金属物体由于高反区域及黑暗区域使重建模型容易造成缺失。本文国际象棋和金属物体三维数据获取结果如图 7 和图 8 所示。其中，使用结构光扫描设备投射条纹，结果如图 7(b)和图 8(b)所示。根据编解码图案和标定数据得到的物体三维重建结果如图 7(c)和图 8(c)所示。通过象棋与金属物体进行泊松重建(如图 7(d)和图 8(d)所示)，验证了重建结果的高精度。使用如图 7(c)和图 8(c)所示模板库中的模板点云在如图 7(h)和图 8(h)的三维场景数据上进行匹配，找到所有匹配到的物体在场景点云中的旋转平移矩阵，匹配得到的物体用黑色显示，如图 9 所示。完成匹配矩阵计算后，即可通过计算获取抓取点完成抓取。图 10 为将匹配结果进行分析计算获取抓取位置姿态后完成抓取动作的过程。

本文使用标准国际象棋进行抓取实验，其任务是在散乱放置的象棋中，抓取匹配到的模板象棋，并以一定姿态放置于指定位置上。其中，象棋采用人工放置，且随机摆放。模板匹配得到匹配参数后，系统按照第 3.3 小节中的程序抓取象棋，抓取实验过程如图 10 所示。

为分析抓取系统的性能，验证精度时将机械臂移动到测量位置，使用示教器读取机械臂在世界坐标系下的标准抓取坐标与姿态。基于 3D 视觉引导抓取系统同样可得实际抓取坐标与姿态。实验设置计算两组坐标之间的距离作为位置精度，取 Z 轴方向的旋转角误差作为角度误差，相机获取图片到位姿输出的时间作为处理时间(未包括机械臂运动时间)。实验共采集 100 组数据比较以确定其精度，其中 5 组数据如表 1 所示。

图 6 实验平台搭建 Fig. 6 The experimental setup

图 7 国际象棋的结构光重建Fig. 7 3D reconstruction of chess model with structured light

图 8 金属的结构光重建Fig. 8 3D reconstruction of metal model with structured light

图 9 模板匹配结果Fig. 9 Template matching results

图 10 抓取的关键步骤Fig. 10 Key steps of picking

表 1 精度与处理时间的比较Table 1 Comparision of accuracy and processing time

从实验结果来看，象棋的准确抓取验证了抓取系统的精度。此法位置精度在 0.05 mm 内，绕 Z 轴的旋转角度误差在 5°以内，处理时间在 0.2 s 左右，散乱象棋识别率为 100%，机器人成功抓取率可达 97%。进行金属零件抓取时，由于重建点云噪点过多，识别率可达 94%，机器人成功抓取率可达 86%。黄会明等[6]提出基于微振镜结构光的方式可获得 0.1 mm 左右的重建精度，获取位置信息处理时间约为 0.4 s，但其并未进行姿态精度计算。Li 等[7]提出一种二维视觉和三维视觉相结合的堆垛目标拾取系统的设计方法，抓取精度达 1 mm。Lin 等[25]提出一种基于深度学习的机器人视觉目标识别与姿态估计系统的设计方案，抓取平均平移和旋转误差分别小于 5.2 mm 和 3.95°。Le 和 Lin[26]基于二维像素值提取三维点云数据，抓取平均平移和旋转误差分别小于 0.23 cm 和 2.26°，抓取一次时间约为 0.9 s，不适合小物体抓取。Wang 等[27]提出端到端的深度学习方法估计六自由度物体姿态，机器人成功抓取率仅为 73%，且训练模型时间长，故不适合在工业任务中使用。Wang 等[28]提出一种求取给定类别中不可见对象实例 6D 位姿的方法，在散乱场景中识别率为 94.7%。Park 等[29]提出一种新的 6D 姿态估计框架 Pix2Pose，正确估计姿态百分比为 72.4%，训练集所需数据量庞大，且系统框架复杂。Park 等[30]提出一种多任务模板匹配(MTTM)框架，该框架预测分割率为 78.8%、位姿预测正确率为 52.9%，完成抓取小型物体任务不稳定。目前，虽然基于深度学习的识别与位姿估计方案无需提前准备 3D 模板即可完成抓取，但在工业应用中仍有识别率过低、位姿估计鲁棒性差等问题亟需解决。与现有的方法相比，本文的抓取系统可作为高精度工业抓取切实可行的解决方案，但所提方案基于 3D 模板库限制了使用场景，仍需进一步研究。

5 结 论

针对可靠、快速和精确机械臂抓取工作，本文搭建结构光三维重建系统，利用结构光系统高精度、快速重建的优势，在 10 ms 内获得了物体的高精度三维信息。为精确计算目标的姿态，先对点云进行滤波处理，再采用点对特征的三维点云匹配算法对场景和模型点云进行精确匹配。通过坐标系变换确定目标位姿，以便机器人正确地抓取目标。该系统能在散乱物体中以准确位姿抓取目标，可靠、快速和精确地完成机械臂抓取工作，可应用于分拣、装配、上料等工业自动化领域，尤其是小目标抓取领域，以提高现有生产线的性能。