机器视觉主导的机械臂动态抓取策略研究

王校峰，王建文，曹鹏勇，杨云茂

(华东理工大学机械与动力工程学院,上海 200237)

0 前言

机器人技术在生产和生活中的应用越来越广泛，如何利用机械臂对移动目标进行抓取是当前机械臂工作中的一个重要技术问题。通常情况下，机械臂通过示教编程的方式对固定工件进行抓取，而对移动目标进行抓取需要附加传感系统对工件进行识别和趋近操作。在视觉传感器参与构建机器人系统后，配合适当算法使机器人有了一定的自主操作能力。倪鹤鹏等提出一种基于时间与工件位置的图像算法，根据预测工件直线运动到达固定参考点的时间进行抓取。王兆权等利用Kalman滤波算法对目标位置进行预测，通过速度补偿实现机械臂移动过程并完成抓取。在实际应用中，利用视觉技术预测目标位置和定点抓取的方法，同样存在无法适应目标运动速度、运动方向及其他工作环境要素的变化,动态抓取效率不高的问题。

工业机器人的自由度多，控制复杂。为解决传统机械臂控制系统设计难度大、移植性差、算法实现困难的问题，TOSEPH等研究出了广泛为大家接受的机器人操作系统(Robot Operating System，ROS)，为工业机器人避障策略的研究提供了统一的软件平台。该平台实现了控制代码的成组设计和机器系统的模块化设计，提高了机器人设计效率。

本文作者基于ROS框架搭建基于视觉反馈的UR5机械臂动态抓取平台，实现对目标物体的快速识别，并通过视觉测距的方式计算机械臂末端偏移量。设计注视逼近的视觉反馈控制策略，实现机械臂对传送带上移动目标的抓取操作。

1 实验环境搭建

基于ROS机器人操作系统免费且开源、代码易扩展等优点，本文作者在Ubuntu18.04上利用ROS-Melodic搭建动态抓取平台，核心部件包括：传输带、RealSense D415摄像头、UR5机械臂等。摄像头采用Eye-in-Hand方式与机械臂末端连接，用于获取执行端视野。在传动带运行方向侧方，固定UR5机械臂，如图1所示。

图1 UR5机械臂动态抓取系统

1.1 机械臂配置

采用SolidWorks建立六自由度UR5机器人模型,利用sw_urdf_exporter插件，确定机械臂各关节部件的连接关系和坐标关系，自动生成URDF文件及展示用的launch文件。URDF(Unified Robot Description Format)是一种使用XML格式描述机器人模型的文件，供ROS平台使用。

创建一个MoveIt配置的功能包，MoveIt配置助手(setup assistant)针对机械臂模型做一系列的配置并且完成编译，选用KDL(The Kinematics and Dynamics Library)进行正逆运动学求解，完成机器人的配置、可视化和仿真工作。具体步骤如图2所示。

图2 机械臂配置流程

1.2 机械臂运动学分析

对UR5机械臂进行运动学分析，图3所示为UR5机械臂结构图。

图3 UR5机械臂结构图

机械臂正运动学问题是从基坐标依次通过坐标变换的方式求取末端执行器的位姿的过程。UR5机械臂模型的D-H坐标系表达各运动关节间的位姿变换关系，其D-H参数如表1所示。

表1 UR5机械臂D-H参数

确定机械臂的各连杆参数与坐标系后，使用改进的D-H参数法来确定各连杆变换矩阵。将各连杆的变换矩阵相乘就可获得最终的变换矩阵。公式(1)所示为第-1连杆的变换矩阵：

(1)

式中:c=cos；s=sin；c=cos；s=sin，由此可得UR5机械臂的坐标转换公式:

(2)

式中:、、表示末端执行器相对于基坐标的姿态；表示末端执行器相对于基坐标的位置。

2 目标识别及空间定位

2.1 目标识别

在目标定位跟踪前需对目标进行快速准确的识别，本文作者提出一种融合RGB算法和SITF(Scale-Invariant Feature Transform，尺度不变特征变换)特征匹配算法的目标识别算法。

在实验过程中发现SITF算法计算复杂程度高，特征匹配占用大量计算资源，不满足实时性要求。针对这种情况，考虑到实验环境中目标物为简单规则形状，通过特征点主方向夹角的角度差阈值消除误匹配点，降低运算复杂程度。在融合算法的SITF策略中，参考影像与待匹配影像第对特征点的主方向夹角应满足约束条件公式：

|-′|<

(3)

()=|-′|+|-′|+|-′|<

(4)

式中：为参考影像主方向夹角；′为待匹配影像对应主方向的夹角；为单角度差阈值。

若第对特征点的主方向夹角之差大于给定阈值，则认为是错误匹配点并予以舍弃。对小于阈值的特征点对进行下一步计算。通过选择合适的阈值排除主方向差异较大的特征点，减少匹配运算量并保证较高精度。

通过预判断的SITF策略明确了目标的形状特征，然后再通过对目标的颜色特征进行二次匹配来提高匹配精度。

2.2 目标空间定位

确认目标识别算法和中心点后，基于小孔成像技术，采用基于特征点的单目视觉测距方法，将相机采集的图像从三维场景投影到二维成像平面上，通过三角形相似原理估算目标中心与相机的空间位置关系，通过坐标变换引导视觉机械臂对工件进行抓取。单目视觉测距原理如图4所示。

图4 单目视觉测距原理

图4中,摄像头透镜光心为相机坐标系原点，轴为摄像机的光轴；、′平面分别为摄像机采集图像平面和成像平面；′与轴线交于点，以点(，)在′平面建立图像直角坐标系，、分别表示行和列。通过对目标物体颜色空间的识别，确认在平面中目标水平方向中心点。′为中心点在′中投影点，在′平面以图像直角坐标系表示。

根据小孔成像原理，世界坐标系下的某点(,,)与它在π′平面上投影点′(,)图像坐标的变换关系如公式(5)所示：

(5)

其中:、分别为轴和轴的有效焦距；、分别为摄像机内参数和外参数；为旋转向量；为平移向量。

假定世界坐标系位于平面内，即点的世界坐标(,,0)，=，=[0 0]，式(5)化为

(6)

将平面内矩形沿方向分为份，将第份面积记为，在′上的相应投影面积记为′，则总面积为

图3中：Qe1、Qw分别为热交换前烟气热量、湿垃圾热量；Qe2、Qd、Qg分别为热交换后烟气热量、干垃圾热量、水蒸气热量；Qt为干燥过程中损失的热量；Ve为烟气体积；Ww、Wd、Wg分别为湿垃圾质量、干垃圾质量、水蒸气质量；Te1、Tm1分别为热交换前烟气温度、湿垃圾温度；Te2、Tm2、Tg分别为热交换后烟气温度、干垃圾温度、水蒸气温度；ce1、ce2分别为热交换前后烟气的比热容；cw、cd分别为湿垃圾、干垃圾的比热容；cg为水蒸气比热容。

(7)

即：

(8)

根据上述测距模型，计算目标水平和竖直方向的空间位置相对在、轴方向的偏移量。

如图5所示，光心与点的连线与′平面的交点，即光心到成像平面的距离,为目标中心到成像平面的距离,可得：

图5 x向测量示意

(9)

通过公式(9)可求得目标在水平方向相对于摄像头光轴在轴方向的偏移量，同理可求得目标在竖直方向的相对偏移量，即：

(10)

联合公式(7)可求得点在相机坐标系下的三维空间位置信息：

(11)

综上，求得平面中点相对摄像机光心的、、坐标方向的偏移量。

3 逼近抓取

3.1 逼近策略

考虑目标处于非稳定运动状态，由摄像头的图像坐标投射到三维坐标后通过逆运动学求解的方法比较困难。本文作者通过视觉反馈替代控制UR5机械臂运动，即从图像中获得位置误差，根据误差反馈进行位移控制，然后再利用图像获得目标位置与光心误差，依次循环，直到达到可接受的误差。视觉反馈控制原理如图6所示。

图6 视觉反馈控制原理

图7 机械臂抓取流程

启动UR5机械臂，初始化所有数据和机械臂动作，等待目标进入摄像机视野，若检测到目标存在，则计算各方向误差。

当误差不在允许范围内时，利用MoveIt运行文中提出的融合算法进行轨迹规划，控制机械臂运动弥补误差。一次规划完成后，检测图像并再次对误差进行计算和判断，进行下一次循环。若误差在允许范围内，则控制机械臂进行抓取动作。

3.2 抓取流程

如图8所示，图(a)为相机视野，红色目标在识别后打印出中心坐标；图(b)为机械臂抓取工作状态。机械臂具体工作过程：机械臂初始化运动到预定位置，等待传送带上的目标进入视觉相机的视野范围内，相机对目标进行识别并获取相机坐标系下的目标位姿偏差(目标中心点与相机光心的、、坐标方向位置偏差)，通过视觉反馈引导机械臂逼近目标；随着传送带向前运动，机械臂不断逼近目标，当位姿偏差减小到许可范围内时，抓取目标。

图8 Gazebo动态抓取工作示意

4 实验分析

为测试融合算法的处理效果，利用上述方法在UR5动态抓取平台上设计了两组性能测试实验：视觉抓取成功率测试和抓取偏移量测试。测试现场如图9所示。

图9 动态抓取工作测试现场

根据速度的不同共设置6组自主抓取实验，每次实验中目标方块运动前在初始位置随机摆放，每组实验进行100次完整抓取,结果如表2所示。可知：随着传送带前进速度加快，抓取成功率逐步降低，当传输速度大于等于12 mm/s时，出现漏抓即未成功识别情况。总体目标物体识别成功率达到99.6%，动态逼近抓取成功率达到98.33%以上，基本满足工业应用要求。

表2 动态抓取测试结果

此外，考虑到抓取位置精度问题，在上述速度为0、3、9、15 mm/s的实验中各随机选取20次，目标坐标系轴正方向为运动方向，机械臂末端实际夹取位置与目标中心位置的轴相对偏差如图10所示。可知:机械臂逼近抓取实际位置与预期位置的相对误差随速度增加而增大，抓取位置向轴负方向偏移，误差在许可范围内，验证了抓取算法的精确性。

图10 抓取位置相对误差

5 结论

针对传统工业机器人在移动目标的抓取中表现出的适应性低、柔性差等缺点，提出基于视觉反馈的动态目标抓取方法。设计一套完整的动态抓取策略，使得机器人在移动目标抓取中的自主性和灵活性大大提高。

本文作者提出基于RGB值和特征点匹配结合的新型特征识别定位方法，并设计注视逼近的视觉反馈控制策略进行抓取。通过ROS机器人操作系统搭建平台进行实验验证。结果表明：所提出的动态抓取策略对目标物体识别成功率达到99.6%，抓取成功率达到98.33%以上，抓取位置偏差在许可范围内，基本满足工程实际应用。