ROS环境下机械臂物体抓取技术研究*

王晶航 韩江桂 张文群 孙颙琰

（海军工程大学动力工程学院 武汉 430033）

1 引言

机械臂的自主抓取大多是基于视觉的控制[1～2]。当前，对于复杂环境的非结构化，再加上被抓取模型的不确定性，使得机械臂的自主抓取难度大大提升[3]。解决这一问题的首要任务就是要解决机械臂自身的感知系统或者借助于外部设备实现机械臂的感知。微软研制的Kinect V2 RGB-D传感器能够提供二维和深度图像的信息，因此可以运用Kinect V2来进行研究[4]。当前，国内外不少学者对机械臂自主抓取进行分析研究，丛明等[5]利用机器视觉进行定位导航（SLAM）[6～7]；Kriminger[8]等利用Online active learning进行自主的目标检测[9]；针对机械臂在非结构化的动态环境，Yuan，C[10]等提出了EBG-RRT算法高效地实现了路径重规划[11～12]。

本文借鉴了Kinect V2视觉传感器在机器人系统中的应用，利用其能够同时提供二维图像及深度信息的特性，简化了目标检测的内容，并通过RRT-Connect算法，完成机械臂的运动轨迹规划，从而最终实现了机械臂系统自主完成目标抓取的功能。本文在ROS环境下，将机械臂的仿真模型建立在Rviz和Gazebo环境中，最后完成Rviz和Gazebo联合仿真的实验，对机械臂的物体识别和抓取技术进行研究。

2 机器人系统仿真平台搭建

本文的机器人系统是由Kinect V2 RGB-D传感器、机械臂系统及主控计算机组成[12]，其中机械臂系统又包括机械臂AUBO-i5和Robotiq两指夹爪。整个系统的模型如图1所示。

图1 系统模型

2.1 Kinect V2 RGB-D传感器

系统中的视觉传感器为微软公司研发的Kinect V2 RGB-D传感器。Kinect V2 RGB-D传感器主要由一个普通的RGB摄像头、一个红外发射器、一个红外摄像头以及一组麦克风阵列组成[13]。普通RGB摄像机能够以30帧/s的速率传送640×480分辨率的RGB图像，而红外发射器和摄像机则是用来采集相应的深度信息，可以得到320×240分辨率的深度图像。

2.2 机械臂系统

AUBO-i5协作机器臂是一款高品质、低成本的6自由度协作轻型机械臂。AUBO-i5协作机器臂本体灵活、轻便，可以使用开源的ROS接口对机械臂本体实现运动控制并在基础上进行开发研究。

为了实现机械臂目标抓取，本实验采用Robotiq两指电动夹爪，夹取稳定，而且被夹持物件不容易滑落。同时，该夹爪支持ROS，能够与AUBO-i5机械臂相互配合实现抓取过程。

2.3 仿真平台

ROS全称是Robot Operating System（机器人操作系统），集成了很多的工具、库等，包含三维可视化功能包Rviz、机械臂运动控制功能包Moveit、物理仿真器Gazebo等开发工具[14]。Gazebo是一个三维动态物理仿真器，能够建立测试机器人的仿真场景，通过添加物体库，用来模仿人类世界。同时，能够提供机器人在仿真过程中所需的静力学、动力学环境，从而可以观察研究机械臂在真实环境下的工作状况[15]。

3 路径规划

机械臂的路径规划就是从起始点到目标点的过程中，机械臂能够按照一定的算法策略，规划出其路径，完成这一过程。本文我们选用随机采样算法，目前应用最广的随机采样算法主要有概率路标图法 PRM（Probabilistic Roadmap Methods）和快速扩展随机树法RRT（Rapidly-exploring Random Tree）[16]。PRM参数少，结构相对简单，但是采样过程中采样点大多会偏离目标位置，使得采样过程中计算量变大，路径规划效率降低。因此选择RRT算法。

RRT是一种通过概率遍历全图，在高维空间便于搜索的算法，图2为RRT的生长过程图。算法策略是给定一个起始点qstart，将qstart存储到qnodes中，在空间中全地图随机采样点qrand，寻找qnodes到达qrand中最近的一个点为qnear，在qnear到qrand的方向上以一定的步长δ前进至qnew，在此过程中进行碰撞检测，若未检测到碰撞，则将qnew存储到qnodes中，反之检测到碰撞，则进行重新采样重复上述过程。当 |qnew-qgoal|＜Error视为迅即到目标点，将qnew存入到qnodes中。最后在qnodes根据各节点的父子关系，反向搜索找到规划路径。图3为RRT算法的伪代码。函数中有三个返回值：“Advanced”代表搜索到新的节点但不知其是否在目标点误差区间附近；“Reached”代表新的节点到达目标节点的误差区间内，即完成路径规划；“Trapped”代表在扩展过程中发生碰撞，扩展失败；“Graph”代表生成搜索树路径图。

图2 RRT的生长过程图

图3 RRT算法伪代码

RRT算法对于机械臂路径规划来讲，有些路径不够光滑，可能会包含棱角，通常也远离最优路径，效率较低。所以本文采用RRT-Connect算法，伪代码如图4。在起始状态点qstart和目标状态点qgoal同时生长两颗快速扩展随机树tree1和tree2，这样扩展效率更高。搜索空间这两个扩展的方式与基础的RRT算法一样，是基于整个空间随机采样进行扩展，先扩展完tree1的第一个节点qnew.1，随后第二棵树tree2朝着qnew.1的方向进行扩展，得到qnew.2，在这个过程中，若没有碰撞，则进行下一步扩展，并一直重复上述过程。直到发生碰撞扩展失败或者 |qnew.1-qnew.2|＜Error实现两棵树连接起来，即RRT-Connect算法实现。

图4 RRT-Connect算法伪代码

4 识别与抓取

4.1 物体识别

从Kinect V2传感器获取图像，标定机械臂各关节，经过世界坐标系与图像坐标系的转换，得到关节点在空间中的坐标，通过几何运动学解算出此时各关节角的角度；其次，计算目标角度与当前角度的误差，使用反馈控制来减小误差[17]；最后，由于本文是以Rviz与Gazebo联合仿真实验，所以可以不必进行Kinect V2内参和外参的标定。对于物体识别的过程，采用find_object_2d快速实现物体检测，识别物体后能够通过KinectV2传感器获得物体在空间的位置，并将返回值赋给机械臂末端夹爪。

4.2 物体抓取

在4.1节物体识别的过程中，已经得到机械臂各关节的坐标和待抓取物体的空间位置，对不同颜色的物品进行分类放置于不同的盒子中，再使用Moveit中的 KDL（Kinematics and Dynamics Library）插件对整个过程进行逆运动学求解，完成运动规划及避障，从而实现物体的识别与抓取，即实现了非结构环境下的物体抓取过程。

5 仿真与分析

统一机器人描述格式文件URDF（Unified Robot Description Format）是ROS中通用的机器人描述文件，能够描述机器人的结构，主要由连杆（Link）和关节（Joint）两种标签构成，连杆标签用于描述机器人连杆的几何特性、运动学特性以及动力学特性，关节标签则用于描述机器人关节的相关特性。同时，这两个标签还有描述几何信息和物理信息的子标签。在建立好机械臂的URDF文件后，将URDF文件在Rviz中打开，建立的机械臂仿真模型如图5所示。

图5 机械臂仿真模型

为了验证机械臂在非结构环境下目标识别及抓取的有效性。在ROS环境下，采用Rviz与Gazebo联合仿真。以Aubo-i5协作机械臂为研究对象，通过Kinect V2传感器来采集环境信息，在Gazebo中设置抓取环境。在Rviz中显示的点云信息如图6（a），当物体被抓取离开摄像机视野后如图6（b），Rviz实时检测更新环境。同时Kinect V2也可以向Rviz返回RGB图像信息并利用如图7所示。

图6 点云信息

图7 RGB图像

图8为Gazebo仿真器中机械臂在抓取过程中的位姿变化。其中图8（a）是机械臂的零点位置，设为抓取过程的起始点和终止点。图8（b）显示机械臂正在抓取桌上物体，并逐个将其分配到盒子中如图8（c）。图8（d）完成将物体分开放置在两个盒子中回到零点位置。

图8 Gazebo仿真器中机械臂位姿变化

在机械臂一次的运动抓取过程中，通过订阅6个关节的角位置话题，6个关节的角度变化如图9所示。机械臂通过Kinect V2传感器识别后对目标进行规划抓取，按照RRT-Connect算法规划得到的路径，能够成功地将物体放置在盒子中。从起始零点位置到抓取结束回到终止零点位置过程中，机械臂能够平稳运行，各个关节能实现平滑的轨迹过渡且角度未发生突变。

图9 机械臂关节角度变化曲线

6 结语

本文在ROS环境下搭建了机械臂识别抓取仿真系统，采用KinectV2 RGB-D传感器来采集环境信息，通过Rviz和Gazebo联合仿真，从而实现目标抓取，运用RRT-Connect算法对机械臂进行路径规划，Gazebo来模拟机器人在真实条件下的运动，验证了机械臂在非结构环境下路径规划的有效性。