基于ROS的6自由度机械臂运动轨迹规划*

朱 萌，孟 婥，张 豪，孙以泽

(东华大学机械工程学院，上海 201620)

0 引言

随着传统制造方式不断向智能制造转型，机器人应用领域不断拓展，对机器人控制系统的要求越来越高[1]。然而，工业机器人开发过程面临许多问题。机器人厂商大多开发自身的软件接口，可移植性差；复杂的建模及对大量冗余代码的编写严重影响开发者的工作效率[2]。开源机器人操作系统ROS (Robot Operating System)具有跨平台、多语言支持、代码复用等优点[3]，能很好地解决这些问题。

本文创建工业机器人实体模型并转换为ROS中所使用的URDF格式文件，利用Moveit!提供的C++、Python接口实现了对笛卡尔空间连续轨迹直线与圆弧插补，并在Rviz中进行了仿真。在平面、空间进行路径规划，验证了RRTConnect算法具有更高的搜索速度，完成了复杂环境下无碰撞路径规划，对工业机器人在实际工作中保障运动安全具有一定意义。

1 ROS仿真平台搭建

ROS是Willow Garage公司2010年发布的开源机器人操作系统，它采用分布式架构，通过各功能独立的节点(Node)传递消息实现任务的分层次运行，具有支持多语言、免费且开源、代码易拓展等优点[3]。基于此，本文在Ubuntu18.04上利用ROS-Melodic搭建6自由度机械臂运动规划仿真平台。

1.1 URDF文件获取

URDF (universal robotic description format) 是统一描述机器人仿真模型设定的XML语言[4]，其主要定义了link和joint，包括限位信息limits，碰撞信息collision，惯性信息inertial，可视化信息visual及link间的父子从属关系信息。

本文研究对象为新松6自由度工业机器人如图1所示，在SolidWorks建立机器人模型，其中每个连杆设定相应的coordinate参考坐标系和axis旋转轴，通过ROS官方的SW2URDF插件经顺序定义各关节、父子连杆、限位参数、参考坐标系及旋转轴后生成URDF文件。

图1 新松工业机器人

1.2 Moveit!运动配置

ROS机械臂控制系统主要依赖Moveit!架构及其插件，Moveit!集成了运动学求解、路径规划、碰撞检测三大核心功能，其系统结构如图2所示。通过运动学库选择KDL、TRAC-IK、IK-FAST求解器完成正逆运动学的计算；开放运动规划库OMPL通过随机采样，使机械臂能够避开障碍物，避免自身干涉完成路径规划。

图2 Moveit!控制系统架构

参数服务器加载urdf、config和srdf机器人配置信息，控制系统核心Move Group类向机器人控制器接口传递Joint Trajectory Action关节运动数据信息，同时传感器不断反馈JointState关节信息，以便Move Group调用运动规划算法求正逆运动学解和生成运动轨迹。

为帮助开发者快速配置Moveit!，ROS提供了具有用户界面的设置助手Moveit!SetupAssistant[5]，它可以帮助开发者定制机器人运动规划框架的初始配置，简化了设计编译工作。这些配置包括自碰撞矩阵、虚拟关节列表、规划组定义、机械臂位姿、末端执行器及被动关节列表[4]。依次完成上述配置，其流程如图3所示。

图3 配置流程

2 机械臂路径规划

多自由度机械臂末端到达空间一点时，各关节存在多解，为避免在运动过程中与环境发生碰撞，需对其进行路径规划。基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法[6]，通过对状态空间随机采样，能在复杂约束下快速搜索高维空间，把搜索导向空白区域，从而寻找到从起始点到目标点的规划路径。

2.1 RRT与RRTConnect算法原理

RRT算法从初始位置开始，以树的形式进行探索[7]如图4所示，通过在空间中随机采样对路径进行扩展，直到“树枝”覆盖目标区域，在探索出的状态空间中选取最短路径作为结果输出。规划流程为：①将起始点作为随机树的根节点qinit，并初始化根节点qinit；②在状态空间随机采样一点qrandom；③在已生成随机树各节点中寻找距离qrandom最近的点qnearest；④qnearest向qrandom方向扩展一个搜索步长εq，生长一个新节点qnew；⑤判断新节点qnew是否与障碍物发生碰撞，若发生碰撞则放弃本次生长，反之将qnew添加到随机树中，重复以上步骤直至搜索到目标位置qgoal。

图4 RRT原理图

RRT每次搜索只从初始点生长快速扩展随机树来搜索状态空间，与RRT相比基于双向扩展平衡的连结型双树RRTConnect算法[8]在目标点区域建立第二棵树与第一棵树同时进行扩展如图4所示，直到扩展失败或与第一棵树相连，整个算法结束。

2.2 算法测试

在平面路径规划测试中，本文利用MATLAB编程，输入像素尺寸为500×500的地图，定义起始点source=[10,10]，目标点goal=[490,490]，迭代步长Stepsize为20，最大迭代次数MaxAttempts为10 000次，使用RRT与RRTConnect算法搜索出无碰撞路径，测试结果如表1所示。

表1 测试结果对比

在空间路径规划测试中，为提高路径规划效率，本文在ROS中的OMPL运动规划库，选用RRTConnect算法替代RRT算法进行路径规划，在Rviz中添加MotionPlanning插件，通过导入Blender生成的.dae文件添加障碍物，同时设定机器人运动起始点home与目标点goal，运行C++源文件执行机器人无碰撞路径规划如图5所示，RRTConnect算法与RRT算法规划分别耗时0.083 s和0.035 s。

(a) 起始点位置 (b) 目标点位置 (c) 路径规划过程图5 无碰撞路径规划

平面与空间路径规划的仿真结果表明，与RRT相比RRTConnect通过朝向对方快速扩展，这种带有启发性的扩展使树的扩展更加明确，能显著提高搜索速度、搜索效率，有利于机械臂在多约束复杂环境下尽快完成路径规划，实现避障。

3 笛卡尔空间机械臂轨迹规划

机械臂在进行连续轨迹作业时，要保证末端轨迹的准确定义[9]，使末端执行器避开障碍物，必须在笛卡尔空间进行轨迹规划，并把规划的路径划分为有限个过渡点，通过逆运动学求解，实时转换为每个关节的角度值。本文主要研究新松SR7CL工业机器人在笛卡尔空间中的直线轨迹规划和圆弧轨迹规划[10]。

3.1 空间直线轨迹规划

(1)

在ROS中利用Moveit!提供的Python接口，完成轨迹点之间的直线插补，如图6所示。

图6 笛卡尔空间直线规划

3.2 空间圆弧轨迹规划

设笛卡尔空间不共线的3点为P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)，机械臂末端以圆弧从P1经P2运动至P3，这三点所确定的圆弧就是机械臂末端的运动轨迹。

在笛卡尔空间圆弧轨迹规划中，为计算简便，在圆弧所在平面新建一个以圆弧中心为原点的坐标系，把圆弧上任意两个始末点弧长计算转化为两点对应圆心角增量的计算，计算出圆弧的各插补点在新坐标系中的值，通过运动学关系将这些值映射到参考坐标系中。

(1)确定圆心P0(x0,y0,z0)及半径r

由三点确定过圆弧的平面方程为：

(2)

易得过P1P2中点且与P1P2垂直的平面方程和过P2P3中点且与P2P3垂直的平面方程，与式(2)联立，可求圆心P0(x0,y0,z0)以及半径r。

(2)以P0为原点建立新坐标系B并进行坐标变换:

图7 笛卡尔空间坐标变换示意图

图7为笛卡尔空间坐标变换示意图，U轴位于P0P1连线上，其单位向量为:

(3)

W轴方向与圆弧所在平面垂直，其单位向量为：

(4)

V轴由右手定则通过U、W轴确定，其单位向量为：

v=u×w

(5)

由A坐标系到B坐标系的位姿变换矩阵为：

(6)

设笛卡尔空间圆弧上一点为AP=(x,y,z)，∠PP0P1=θ，BP=(rcosθ,rsinθ,0)，则笛卡尔空间圆弧轨迹可通过坐标变换关系转化成平面圆弧，即:

(7)

笛卡尔空间圆弧规划把对圆弧长度的插补转换为对圆弧所对应圆心角θ的插补，生成B坐标系下圆弧上一系列点由式(7)映射为基坐标系下一系列路径点，在ROS中利用Moveit!提供的Python接口编程，实现了笛卡尔空间圆弧规划如图8所示。

图8 笛卡尔空间圆弧规划

4 结论

本文基于ROS搭建了机械臂仿真平台，创建URDF文件完成了6自由度机械臂的建模。为保证机械臂实际作业中避免与环境碰撞，在平面、空间分别进行算法测试，验证了RRTConnect算法具有更高的搜索速度，完成了复杂环境下无碰撞路径规划，保证了机械臂运动安全。利用Moveit!进行笛卡尔空间下直线与圆弧轨迹规划，实现了插补运算，保证了对连续轨迹的精确定义。