基于ROS的机械臂结合RGBD相机避障运动规划

郝 奇，许四祥，江天琦，方建中

（安徽工业大学 机械工程学院，马鞍山 243000）

0 引言

目前工业机器人大多数是人工示教，少部分使用离线仿真程序，两者都无法避免因真实工作环境发生变化而发生碰撞的问题。而现有的方法，如加装防护栏和传感器碰撞检测法，前者不利于人机交互且占用作业空间，后者传感器如关节力矩传感器和皮肤压力传感器成本过高且精度有限，而且仍会接触到障碍物[1]。针对这个问题，有人提出了基于ROS的机械臂避障方法[2]，在此基础上，作者提出了一种协作机器人结合RGBD相机使用PCL点云避障的新方法，并在ROS框架下进行了联合避障运动规划的研究。

1 ROS避障系统总体设计

ROS机械臂控制系统基于MoveIt架构，包括控制核心MoveGroup类、参数服务器接口、控制器接口、Python的moveit_commander接口、传感器接口和RVIZ插件接口[3～6]。其中参数服务器加载机器人的config和xacro机器人配置信息；Position控制器接口与MoveGroup类交换JointTrajectoryAction的关节运动数据信息；机器人传感器获得JointState关节信息，便于MoveGroup调用运动规划算法求得正逆运动学解和生成运动轨迹。

Gazebo独立于ROS，它能在三维环境中对多个机器人、传感器及物体进行仿真，产生实际传感器反馈和物体之间的物理响应[7～9]。本研究在Gazebo中使用RGBD相机，把障碍物深度信息传递给MoveIt，并转化为PCL点云信息，通过topic在RVIZ中显示。MoveIt使用避障规划JointTrajectoryAction关节运动数据控制Gazebo中的机械臂运动。其联合控制系统结构如图1所示。

图1 MoveIt和Gazebo联合控制系统架构

2 避障系统配置流程

避障系统配置流程如图2所示。第一步：构建URDF和Xacro，并在其中添加RGBD相机。第二步：使用Setup_Assistant配置MoveIt和Gazebo控制系统功能包，添加统一的控制器和传感器插件。第三步：配置RGBD相机，测试rgb和depth图像，配置MoveIt点云插件，测试点云。第四步：结合RRTConnect算法，启动联合控制系统和Python运动规划程序，分析运动规划结果。

图2 避障系统配置流程

2.1 URDF和Xacro的创建

URDF是统一描述机器人仿真模型设定的XML语言。主要定义了link和joint，其中joint中包含关节名称、关节类型、坐标值、子父系连杆和limit信息，而link中包含inertial、collision和visual信息。通过SolidWorks绘制了UR5及工作环境模型，每个连杆设定如图3所示的coordinate坐标系和axis旋转轴，使用sw_urdf_exporter插件顺序定义关节的子父系link、参考坐标系、旋转轴和限位参数，即可导出URDF文件。

图3 机器人零部件图

Xacro与URDF都是表示机器人模型的XML语言文件。Xacro使用宏定义，能有效缩减代码量，还包含机器人在Gazebo物理仿真世界中的仿真环境、控制器插件和传感器等其他特性。Xacro基于URDF改编而成，其前两行代码指定命名空间，内部参数使用＜xacro:/＞标签进行包含，最后添加transmission传动标签、gazebo_ros_control插件和RGBD相机插件，保证Gazebo功能及相机的正常使用。

使用的RGBD相机为华硕Xtion_Pro_Live，首先需要在模型文件夹中添加相机的sensors文件夹，然后在Xacro文件中添加如图4所示代码：

图4 代码1

其中第一个＜xacro:/＞标签内包含深度相机的模型和参数文件。第二个标签定义了相机相对于base_link坐标系所处的位置及欧拉角信息。

2.2 MoveIt和Gazebo联合系统的建立

MoveIt控制系统功能的实现来源于其配置，在得到合理的Xacro以后，使用Setup_Asssistant来配置moveit和gazebo控制功能包，其具体流程如图5所示。

图5 Setup_Assistant配置流程

其中：碰撞矩阵列出不可能发生碰撞的连杆，以减少运动规划碰撞检测时间。添加由base_link到吸盘gripper_link的arm规划组，选择KDL工具求解正逆运动学姿态。

在Moveit和Gazebo包中分别添加相同的轨迹控制器JointTrajectoryController和位置控制器JointPositionController，其中每个关节对应一个电机位置控制器。

启动系统后的GUI界面如图6所示，左边RVIZ界面显示模拟仿真环境，其中绿色为机器人初始工位的位姿，黄色为最终工位的位姿，机器人上方黑色的小长方体为RGBD相机；右边Gazeb界面显示真实物理环境，表示机器人真实所处的位姿及物理环境；中间是RGBD相机的从Gazebo采集的实时RGB图像，监控机器人运行状况；左下角终端显示加载的控制参数和提示信息。

图6 机器人系统可视化界面

2.3 RGBD相机和PCL点云的配置

使用的Xtion_Pro_Live相机在正面有三个摄像头，左边和右边分别是IR红外线发射器和接收器，用来感测深度值，中间的则是RGB传感器，用来撷取一般的彩色影像，两边的小孔是Audio麦克风。如图7所示。

图7 RGBD相机

在相机xacro文件中定义camera参数和发布图像话题的插件。首先定义相机工作角度为58°，图像格式为RGB8，分辨率为真实相机分辨率249×329，工作范围为0.05～4m。最后定义相机发布rgb图像和depth图像的topic信息插件，代码如图8所示。

图8 代码2

启动联合避障系统，使用rostopic list命令列出发布的话题，出现rgbd_camera相关的topic就说明相机已经正常运行[9～11]。通过终端spawn_model命令在Gazebo的工作台上添加两个可乐瓶模型，在新终端中输入rosrun image_view和topic命令显示相机的rgb和depth深度图像，如图9所示。

图9 rgb图像和depth深度图像

PCL点云环境是三维重建的过程，需要得到RBGD图像每个像素点的三维坐标。RGBD相机扫描得到的每一帧数据是由彩色RGB图像和灰色的depth图像组成[10,11]。灰色的depth图像每个像素点包含一个灰度值，这个值是PCL点到深度相机所在垂直平面的距离，这个值Z通过结构光编码技术测得。像素坐标（u，v）和世界坐标（X，Y，Z）的映射关系如图10所示。

图10 像素坐标和世界坐标映射图

由相似原理：

PCL点的三维坐标：

其中：fx和fy为像素表示的焦距长；cx和cy为图像坐标系原点在像素坐标系的坐标。

在MoveIt传感器插件中定义PointCloudOctomapUpdater插件，相机图像的像素点在RVIZ环境中进行三维重建，显示为PCL点云。定义插件订阅的点云话/rgbd_camera/depth/points，发布的点云话题为output_cloud。重新启动系统，如图11所示。右边Gazebo相机视野下的两个可乐瓶在左边MoveIt的可视化插件RVIZ中转化为绿色的点云图。

图11 Rviz可视化和Gazebo物理世界仿真

3 RRTConnect算法结合PCL避障规划实现

3.1 RRTConnect避障规划算法

ROS中的OMPL开源运动规划库是采样规划算法库，相比传统规划算法需要在一个确定空间对障碍物进行建模，采样规划算法适合解决多自由度机器人在复杂环境的规划[12,13]。原始RRT算法只有一棵树从初始点扩展搜索整个状态空间，本文使用的RRTConnect算法称为连结型双树RRT算法，是从初始点和目标点同时生长两棵RRT树，能够快速有效地搜索状态空间。RRTConnect算法流程如图12所示。

图12 代码3

RRTConnect算法构建过程：首先对起点和目标点的两棵树初始化，然后建立循环，随机抛出一个点Prand，第一棵树T1计算最近点Pnearst，最近点沿着Prand扩展一个步长ε，没有碰撞则得到新节点Pnew。新节点作为第二棵树T2扩展的方向，扩展一个步长得到P'new，没有碰撞则同向扩展一次；每次迭代后进行比较，选择节点少的树扩展；经过N次迭代，直到P'new=Pnew，两棵树连结在一起，整个算法结束。

3.2 RRTConnect算法结合PCL的避障规划

启动MoveIt和Gazebo联合避障系统，运行 Python运动规划程序，UR5末端执行器进行左工位至右工位的机器人取放操作任务，在无障碍物的环境中运动规划路径如图13所示。

图13 MoveIt无障碍运动轨迹

终端使用spawn_model命令，在Gazebo中添加障碍物遮挡原机器人工作路径，通过RGBD相机，MoveIt的RVIZ立刻建立障碍物面向相机两个面的PCL点云模型,结合RRTConnect算法，机器人末端执行器吸盘绕过障碍物从左工位移动到右工位。如图14所示，左边的RVIZ显示运动规划路径；左下角终端提示算法用时0.056s计算出规划路径；右边的Gazebo环境和RGB图显示真实环境机器人最终到达的状态。

图14 MoveIt避障规划轨迹及Gazebo结果

终端使用rqt_plot命令，添加/joint_states话题数据，实时监控机器人各个关节角度随时间变化曲线，如图15所示，避障运动过程用时约4.5s，机器人6个关节在避障工作下能实现平滑的轨迹过渡。

图15 机器人各关节角度随时间变化曲线

4 结束语

通过在ROS中构建URDF和Xacro模型、添加RGBD相机、设计配置MoveIt和Gazebo联合控制系统、配置RGBD相机及点云、结合RRTConnect算法进行避障规划。通过上述工作流程，在ROS中实现了机器人在工作环境发生变化的情况下，能立刻避开障碍物平稳运行。结果表明，该避障系统便捷有效，机械臂实现了在障碍物环境中自动避障，解决了离线仿真机械臂因环境变化而出现的碰撞问题，对工业机器人在ROS下的应用研究有着重要的科学意义。