基于双目视觉定位的机械臂智能抓取控制研究

陈晗彬

（福建省特种设备检验研究院国家特种机器人产品质量检验检测中心（福建），福州 350028）

0 引言

近年来，随着互联网、人工智能、智能制造的迅速发展，再加上“中国制造2025”发展战略的推出，传统的通过单一示教执行单一指令的机器人已经难以满足复杂的生产需求，越来越多的机器人厂家希望机器人能够适应更加复杂的生产环境，例如在机械臂需要抓取物体大小、位置、形状发生改变时，会出现丢失目标无法抓取等问题，需要人为进行干预[1-2]。为了让工业机器人更加智能、高效地适应工业生产的绝大部分应用场景，应用基于双目视觉的机器人是一个很好的解决方案。使用双目视觉相机作为其外部传感器，赋予其感知能力，不仅可以识别二维图像信息，还可以通过三维点云图获得物体的轮廓和位置信息，在视觉识别方面有着广泛应用[3-4]。利用双目视觉传感器设备在复杂环境下识别待抓取物体，并获得物体的尺寸、位置属性和周围环境信息，再由ROS 机器人操作系统进行信息处理、判断和模拟仿真确定机械臂的行径轨迹，最后与机械臂进行数据沟通实现现实环境下的抓取操作[5-6]。这样机械臂就被赋予了判断和决策的能力，变得更加智能化、自主化[7-8]。

本文主要对基于双目视觉的机械臂进行识别定位及抓取控制研究，主要包括机器视觉的手眼标定、机械臂D-H 方法描述，ROS 系统上的模拟仿真、现实环境下的识别抓取实验等内容，为后续机械臂能更好地适应复杂工作环境以及为产业化发展提供理论参考[9]。

1 双目视觉定位原理介绍

1.1 双目视觉相机介绍

为在视觉方面获得更加丰富和清晰的信息，采用的视觉输入设备为Intel RealSense D435 双目深度相机，其主体外观和各相机功能位置如图1所示。

图1 Intel RealSense D435双目深度相机

该相机具有全局快门，可以捕捉快速移动的物体，双目视觉分辨率最高为1 280 像素×720 像素、RGB 分辨率最高为1 920像素×1 080像素、相机的深度识别距离为0.2～10 m，而且相机内置了一个6 自由度的IMU 单元，可检测刚体在三维空间中的前后左右上下平移，以及旋转、俯仰、摇摆等动作。

1.2 眼在手上相机标定方法介绍

在实际的视觉定位抓取实验中，双目视觉相机在获得目标的坐标位置后，通过手眼标定设置好的转化矩阵将物体在相机中的坐标转化到机械臂的坐标系中，然后根据机械臂空间坐标系物体所在的位置进行路径规划得出每个关节的运动情况，从而控制机械臂完成抓取操作[10]。手眼标定是确定像素坐标系和空间机械臂坐标系的坐标转化关系，标定的精度严重影响机械臂抓取操作的准确程度。

眼在手上（Eye-in-hand）的手眼标定方法指的是将相机固定在机械臂的末端夹具上进行标定，目的是找到相机与机械手末端的坐标关系。手眼标定方式如图2所示。

图2 眼在手上标定方式示意

在求解过程中，会使用到机械臂基坐标系（base）、机械手坐标系（tool）、相机坐标系（cam）、标定板坐标系（cal）。首先需要使用齐次变换矩阵来表示在标定过程中不变的坐标系间的相互关系量：相机与夹具坐标系的转换关系、标定板与机械臂基坐标系转化关系，和2 个变换但可求解已知的关系量：夹具与机械臂基坐标系的转化关系、相机与标定板坐标系的转化关系。其中，相机与机械臂末端的坐标关系为手眼标定最终想要求解得到的未知量[11]。

在经过机械臂多次的姿态变换后，根据以上公式可得式（2）～（3）：

式（6）～（8）可化简为式（9）：

可设两次机械臂运动的夹具与机械臂基座之间的位姿变换矩阵的乘积为A、相机与标定板之间的位姿变换矩阵乘积为B，所以可得式（10）：

最终简化为一般表达式（11）完成求解。

式（11）一般采用李群李代数方法替换转换为线性方程，运用“两步法”先对旋转矩阵进行求解，再得平移向量[6]，或者采用时对偶四元数，用对偶四元数表达旋转和平移，进行统一求解，最终求解出X。求解完成后可以得到相机与机械臂基坐标系的变换矩阵关系[12-14]。

1.3 相机安装方式选择

本文综合考虑到以下几点原因选择相机的安装方式。

（1）眼在手外的相机视野单一，在复杂情况下容易被遮挡导致识别失败，相机在手上可以随着机械臂末端的移动改变视野位置从而避开遮挡进行识别。

（2）如果想要为机械臂加上移动底盘，让机械臂能够在不同地点进行移动，眼在手上的相机可以随着机械臂一起移动，其识别面积就不像眼在手外仅有一个区域。

（3）考虑到外界移动碰触的因素，在实验室的环境下，眼在手外的相机很容易被移动导致需要重新标定，而眼在手上的相机只需标定一次就可以一直使用。

最终选择将双目视觉相机放在机械臂末端，选用眼在手上的手眼标定方式，相机的安装方式如图3所示。

图3 双目视觉相机安装方式

2 ROS仿真平台介绍

利用ROS 系统在进行机器人开发时，最大的一个优点就是ROS 拥有丰富的功能包，在对机械臂运动学仿真时，ROS 提供了几类常用工具，如Rviz、Moviet！、Rqt、Gazebo 等。Gazebo 和Moveit！提供了仿真功能的实现、Rviz 和rqt提供了调试与可视化功能。对于机器人开发而言，仿真是机器人控制的基础，因为机器人造价高昂，为降低机器人学习、开发、调试的成本，需要通过仿真来完成大量前期工作。进行模拟仿真时，首先要对机械臂进行建模，描述和表达出机械臂的关节和姿态。在ROS 中，主要使用urdf 文件和xacro 文件对机械臂模型进行描述，这2 种文件都采用了XML 文件格式，可以直观地通过各类标签了解机械臂的相关描述属性，直接阅读也十分方便。机器人模型文件主要根据D-H 参数法[15-16]进行建模，在urdf 文件中，使用＜link＞标签格式描述机器人上连杆的大小、颜色、碰撞参数等外观和物理属性；使用＜joint＞标签格式描述2 个连杆之间的运动学和动力学属性；使用＜robot＞标签格式描述整个机器人。这3 类标签组成了完整的机器人模型描述[17]。

3 机械臂智能抓取仿真试验

3.1 摄像头手眼标定

在给机械臂添加视觉模块时，为确定相机位置，调高机械臂抓取精准度，需要对相机进行手眼标定。标定过程采用了camera＿calibration 功能包，功能包使用棋盘格图案作为标定靶，在标定过程中标定靶与机械臂基座固定，机械臂末端上的相机会对棋盘格进行拍照，通过检测棋盘格中的边角识别坐标，之后机械臂各关节会进行转动，调节末端位置，但每次移动末端相机视角均会指向标定靶[18]。

功能包标定界面如图4所示。界面中x、y、Size表示标定时相机的左右、上下和前后移动，Skew 表示相机倾斜转动，在右上角的进度全部完成后，就可以得到标定结果，最后将数据结果进行发布，等待ROS调用即可。

图4 标定功能包界面

3.2 自主物体识别定位

视觉部分将采用OpenCV 开源库实现图像的处理工作，使用OpenCV 库可以更简便高效地完成视觉方面的应用开发，在OpenCV 与ROS 进行数据传输时需要使用cv＿bridge 功能包将OpenCV 和ROS 的图像数据相互转化和通信，OpenCV与ROS的集成框架[19]如图5所示。

图5 ROS与OpenCV集成框架

OpenCV 开源库的使用使相机能够进行基础的图像识别和ROS 通信，但这些功能远远不能满足精准的物体识别与物体定位功能。为实现这些功能，采用ROS 的一个强大的物体识别框架——ORK 功能包，功能包提供了模型训练和模型数据库配置的工具，以及多种物体识别的算法。ORK 兼容性良好，识别精准率高，使用较为简单，是很好的三维识别定位软件。

ORK 功能包进行物体识别的算法思路是模板匹配，首先需要建立待识别的物体三维模型，接着在数据库中对模型进行数据训练，生成匹配模板，训练完成后就可以在ROS 中根据模板进行三维物体识别，识别效果如图6 所示。由图可知，相机已经识别出了物体，并用白色区域将可乐罐的面积覆盖，并在可乐罐处建立坐标系，这样识别物体的位置和姿态都可以获得，最终识别到的物体的位置信息和模板匹配信息会在终端打印并进行发布，为之后抓取做好准备。

图6 三维物体识别效果

3.3 机械臂障碍规避

机械臂在环境复杂的情况下进行抓取，单单只识别到物体是不够的，如果忽视了障碍物，会使得机械臂产生碰撞造成危险。为了让机械臂能够更加智能地避开障碍物，需要使用双目视觉相机对障碍物生成点云信息。

待抓取物体周围环境的点云信息可以由双目视觉相机获取，获得的清晰点云图信息如图7所示。在ROS中，实验使用MoveIt！的OctoMap 插件将双目视觉相机识别到的点云图环境信息以栅格化的形式生成八叉树（Octree）去描述，这样大大减轻了系统对环境信息处理的负担，可以更快知道物体在场景中的位置。最后，使用点云图完成MoveIt！中的障碍物建立后，需要将待抓取物体进行标记以忽略其作为障碍物的识别，在Rviz 可视化窗口显示的障碍物识别情况如图7所示，图中红色的物料为待抓取的物体，其点云图并不会被生成为八叉树，而两边的水杯以及桌面就会被识别为障碍物，以栅格化的形式标注障碍物体积，等机械臂进行抓取规划时，会使用RRT路径规划算法来计算能够抓取物体且可以完成避障的路径[20]。

图7 障碍物识别情况

在ROS 系统中MoveIt！的运动规划库OMPL 支持避障规划。由如图8 所示的机械臂末端路径曲线可知，在机械臂的仿真环境中可以在机械臂的行进路径中添加一个障碍物，之后进行路径规划，此时机械臂会绕过添加的障碍物到达目标点位，机械臂即完成避障规划。

图8 机械臂障碍物规避规划

3.4 机械臂控制连接

实际机器人控制在ROS 端的环境配置与联合仿真相类似，而在实时系统的控制、与ROS 的通信和开发的接口方面，每一家机器人公司的配置都不尽相同。本次实验的机械臂驱动包使用HSR—Co605 协作机器人配套的HIROP 软件包，该软件包集成了完善的通信框架和功能模块，能实现机器人硬件和ROS软件层的搭建。

配置完成软件包，即可以在终端中输入指令完成机械臂的连接与控制工作，通过HIROP 提供的各类函数库和功能包文件，自定义特有的视觉抓取应用，可以执行的功能有识别牛奶盒、构建清除点云、抓取目标、放置目标、移动到目标点、打开合上夹爪等，通过整合单独功能，机械臂最终可在现实情况下完成基于视觉的抓取操作，如图9所示。

图9 机械臂最终完成视觉抓取

图10 识别抓取流程

3.5 智能识别抓取实验

视觉机械臂实现识别抓取的流程图如10所示。为保证视觉机械臂可以在不同环境下进行有效识别抓取，设计实验对视觉机械臂进行测试。图11所示为识别抓取实验场景，场景1：待抓取物体牛奶盒被放置在高度适中的桌面，距离机械臂距离适中且周围没有障碍物遮挡；场景2：待识别物体离机械臂距离较近周围没有障碍物遮挡；场景3：待识别物体与机械臂距离适中，周围有较少障碍物，物体正前无障碍物遮挡；场景4：待识别物体与机械臂距离适中，周围有较多障碍物，物体正前有障碍物遮挡。

图11 视觉抓取实验场景

将实验所得数据和结果汇总至表1，表中（X，Y，Z）表示物体实际底部中心点坐标，（X’，Y’，Z’）表示视觉系统识别到的坐标，坐标单位为mm。

表1 不同场景下识别抓取实验

经过相同场景的多次试验后，对实验现象和结果进行分析，结论如下。

场景1：双目相机可以识别到物体，且完成桌面的点云图构建，机械臂完成桌面避障，对牛奶盒进行抓取，验证了视觉机械臂识别抓取功能的完善性和稳定性。

场景2：双目视觉相机在多次实验中未完成对牛奶盒的识别，通过数据分析可知，相机在此视野下只能识别到牛奶盒的顶端一小部分，面对这样的识别视野，ORK 没有足够的信息，不足以进行模板匹配，所以识别失败。对此，可以使机械臂转动，改变相机的识别姿态，从而完成复杂角度的识别。

场景3：在较为复杂的环境下，相机可以成功完成识别定位，机械臂在进行抓取操作时，夹爪会夹住牛奶盒的上半部分，表示已成功完成障碍物识别和规避，保证了机械臂成功完成抓取，实验验证了在复杂情况下机械臂对物体位姿状态的识别和障碍物规避功能的可行性。

场景4：在识别物体下半部分有遮挡这种更加复杂的环境下，相机也可以完成识别，由识别坐标可知，坐标系被建立在物体上方，避免了在抓取时发生碰撞的危险。但是机械臂没有完成运动规划，通过错误排查得知，由于障碍物识别生成的栅格化八叉树像素体积过大，使得抓取行径路线完全被遮挡，导致运动规划失败。对此，可以使用算力更强的显卡，以生成高像素八叉树，使得障碍物的体积描述更加准确，从而面对理论上可以避障的环境时不会受障碍物识别的影响。

4 结束语

本文基于双目视觉相机和HSR-Co605 协作机械臂以及ROS仿真平台对智能抓取控制进行了仿真研究和试验。

（1）在ROS 仿真平台中完成模拟仿真代码的编写，实现机械臂仿真情况下的碰撞检测、运动规划和轨迹检测并验证了其可行性。

（2）对双目视觉定位技术进行了试验研究，使用手眼标定建立相机和机械臂之间的坐标联系，使用Cvbridge 和ORK 功能包对物体进行识别、定位以及姿态显示并实现各部分配置和连接。

（3）在不同场景下进行了视觉抓取实验，特别是目标相对于相机太近时，需要转动机械臂，改变相机的识别姿态才能更好地完成目标识别。在其他具有障碍物遮拦的场景下，距离适中时，也能很好地完成目标识别任务。通过大量的试验验证了这一技术的可行性。