基于ROS的机械臂运动规划研究

屈力刚，高凯，邢宇飞,张丹雅

(沈阳航空航天大学机电工程学院，辽宁沈阳 110136)

0 前言

随着机器人平台与硬件设备日益丰富，机器人系统变得更加复杂，软件的编写难度也在不断增加。当前机器人技术面临的问题有：不同软件之间难以互通互联；硬件不断丰富，软件代码复杂繁琐；不同机器人之间软件代码不能复用和模块化[1]。为解决以上问题，科学工作者研发了多种优秀的软件框架：ROS(机器人操作系统)界面简单易操作，适配多种机器人模型，采用统一的格式进行描述，可以控制多种型号机器人，系统设计结构简单，支持多种语言；OpenRTM机器人平台是面向组件的开发，通过为各功能元素创建组件来构建系统；Orocos是用于机器人控制的C语言库，允许使用C语言编写实时组件；其中ROS具有多语言支持、架构精简，集成度高、组件化工具包丰富、免费开源等多个优点被广泛使用[2-3]。

本文作者基于ROS实验平台，采用ABB irb-2400型号机器人为研究主体，进行正逆运动学分析。随后将机器人的三维实体模型设置为URDF文件，采用Setup Assistant配置包，根据URDF模型文件完成机器人的配置和Rviz可视化，并对机器人进行笛卡尔空间规划，最后进行避障仿真。

1 模型配置

ROS使用统一的机器人描述格式(Unified Robot Description Format，URDF)来构建机器人模型，可以直接控制移动或者程序驱动机器人以及添加其他环境信息[4]。URDF文件采用XML标签，以连杆(link)和关节(joint)为基本结构，描述了机器人的各个部件及其位置关系。并且可以储存机器人的基本物理属性，以及机器人的运动学、动力学属性。

URDF文件主要由连杆(link)和关节(joint)2个基本结构组成。如图1所示：连杆分别通过visual描述外观参数，inertial描述惯性参数，以及collision描述碰撞属性；关节的作用是连接2个连杆，称为父连杆和子连杆。复杂的机械臂模型可以简化为若干连杆，通过关节按照固定顺序连接起来。每个连杆和关节的属性都是固定的，因此，只要确定基座的坐标系就可以推出整个模型的坐标关系[5]。

图1 URDF关节、连杆结构

依据上述原理创建好ABB irb-2400型号机器人完整的URDF文件后，使用MoveIt Setup Assistant生成irb-2400机器人的MoveIt配置功能包，从而完成机器人运动规划的初始配置、可视化和仿真等工作[6]。

随后运行其中的launch文件，通过Rviz实时显示机器人模型以及状态，以便于直观地配置和修改模型文件。机器人模型如图2所示[7]。

图2 机器人模型

2 运动学分析

2.1 正运动学求解

机器人的正向运动学分析是根据机械臂各关节类型，相邻关节尺寸及运动量大小来确定末端执行器的位置和位姿关系。运用D-H参数法对irb-2400型号机械臂进行正运动学求解，即通过坐标转换把末端执行器的位姿在坐标系里表示出来。该参数法由DENAVIT和HARTENBERG提出[8]，现已成为常用的机器人连杆建模方法[9]。根据D-H参数法建立的irb-2400 机器人的坐标系如图3所示。

图3 ABB irb-2400机器人连杆坐标系

图3中基坐标系即为x0y0z0构成的坐标系，经过坐标变换得到末端执行器坐标系即为x6y6z6所构成的坐标系。机器人连杆长度和关节转角范围的相关参数描述可以通过查阅URDF文件获取。根据相关参数建立机械臂D-H参数，如表1所示。其中，i表示连杆，ai表示连杆长度，αi表示连杆扭曲角，di表示连杆偏置，θi表示变量关节角度[10]。

表1 ABB irb-2400机器人D-H参数

在建立各个关节的坐标系以后，根据D-H法则经过2次旋转和2次平移后确立相邻两连杆i-1和i之间的相对关系:

(1)

(2)

2.2 逆运动学求解

在实际情况中，大多数是已知机器人末端执行器的目标位姿矩阵，求解各关节的角度变化，即求θ1、θ2、θ3、θ4、θ5、θ6的值。通过代数法对机器人进行逆运动学求解，ABB irb-2400机器人具有6个自由度，其运动学方程可写成：

(3)

令矩阵方程(3)两端元素2、4行对应相等得:

-sinθ1px+cosθ1py=d2

(4)

利用三角代换得：

(5)

式中正负号对应于θ1的2种可能解。选定其中一个解后，再令式(3)两端元素1、4行和3、4行分别对应相等，得到2个方程，继续使用三角代换可解出θ3。以此类推求出其余每个关节的解。

3 轨迹规划

机器人在进行连续作业时，为保证末端执行器按照既定轨迹进行运动，则需要在笛卡尔空间中进行轨迹规划。笛卡尔空间轨迹规划就是已知机器人末端的初始点位置和终止点位置，将轨迹划分为有限个点，通过插补算法求解出中间点的位置，然后使用逆运动学求解出各中间点所对应的关节角度值。

3.1 直线轨迹规划

对于直线轨迹规划，已知机器人末端执行器起始点Q0(x0,y0,z0)和终止点Q1(x1,y1,z1)，设插补次数为N，则中间插补点坐标为

(6)

在MoveIt中采用前面的直线轨迹规划进行运算，完成轨迹点之间的插补，运动实验如图4所示，实体为机器人起始状态，虚体为结束状态，给定起始点和终止点坐标，运行程序。

图4 直线轨迹规划

3.2 圆弧轨迹规划

机器人末端从Q1(x1,y1,z1)经过第二点Q2(x2,y2,z2)到达终点Q3(x3,y3,z3)，此圆弧即为机械臂末端的运动轨迹。由Q1、Q2、Q3三点所构成的平面方程为

Ax+By+Cz+D=0

(7)

取直线Q1Q2和Q2Q3垂直平分线，交点即为圆心O(x0,y0,z0)坐标，进而求得圆弧半径R。

以圆心O为坐标原点，构建新坐标系o-uvw。令Q1O连线方向为u轴方向，Q1Q2连线垂直方向为w轴方向，v轴方向通过u、w轴向量的叉乘得到[11]。则可建立起基座标系到新坐标系的变换矩阵为

(8)

设圆弧上有一点Qi在新坐标系下坐标为(ui,vi,wi),设Q0O和Q1O夹角为θ，则Q0坐标为(Rcosθ,Rsinθ,0),将插补结果得到坐标映射至基座标系上。设点Q0在基坐标系的坐标值为(xi,yi,zi),则有：

(9)

在MoveIt中对机器人进行圆弧轨迹规划，运动轨迹如图5所示。

图5 圆弧轨迹规划

3.3 自定义线性插补规划

move_group是MoveIt中的核心节点，主要是将各功能包和插件集成起来为用户提供动作指令和服务。自定义规划算法的实现如图6所示，将设计好的算法与MoveIt标准接口组成一个插件，添加注册文件使得move_group能够感知算法，之后通过yaml、launch文件调用算法，从而使机器人在运动规划过程中采用自定义算法进行轨迹规划。

图6 自定义规划算法实现原理

线性插补是机器人在运动规划过程中从起始点到终止点各轴均保持线性均速移动。对于线性插补规划，已知起始点Q0(x0,y0,z0)和终止点Q1(x1,y1,z1)，设插补次数为N，各轴中心点初始坐标为bi，插补后坐标为Zi(i=1,…,6)，斜率为k，则中间各轴坐标满足：

Zi=(k×N+1)bi

(10)

将算法插件接入MoveIt，运行ABB机械臂launch文件，打开Rviz界面后，即可看到运动规划器已换成自定义算法。给定起始点和终止点，点击运行，机器人即完成线性插补。

将运行过程中各轴插补点坐标导入Excel表格，结果如图7所示，各轴均保持线性运动，证明该算法的可行性。

图7 线性插补过程各轴位置

4 避障规划

在很多应用场景下，机器人的工作环境中有其他物体，在机器人运动过程中这些物体可能成为其运动规划的障碍物。为避免与物体发生碰撞，运动规划需要考虑避障问题。MoveIt中的OMPL是包含许多运动规划算法的开源采样运动规划库并且支持避障规划，文中采用其中的RRT-Connect算法对机器人进行避障仿真分析。

传统RRT(Rapidly Exploring Random Tree)算法是一种快速搜索算法，从起始点开始，以树状分支的路径形式快速扩展来填充空间的大部分区域，直到寻找到目标区域，原理如图8所示[12]。RRT-Connect算法在RRT的基础上进行改进，在目标点区域建立第二棵树，2棵树不断朝向对方交替扩展，显著提高了搜索速度和效率[13]。

图8 RRT算法原理

通过在MATLAB编程，加载障碍物地图，设定起始点[10,10]和目标点[490,490],迭代次数5 000，迭代步长为20，使用RRT和RRT-Connect算法搜索出到达目标点的无碰撞路径，所用时间和路径长度如图9所示。

图9 RRT算法与RRT-Connect算法结果对比

可见，RRT-Connect算法所用时间和规划路径长度均短，因此，为规划提高效率，在仿真实验中采用RRT-Connect算法进行路径规划。

在终端启动配置的irb-2400机器人文件，进入Rviz界面，选择RRT-Connect算法，在Motion Planning里将机器人末端运动轨迹显示出来，以便于分析和观察。令初始位置为起点位置，设定机器人的终点位置。另起一个终端，将编写的Python程序运行，可以看到障碍物添加到环境中。将障碍物发布于环境，机器人在运动过程中会躲避障碍物到达终点。当机器人从终点返回到起点时，仍然会规划一条避障路径，运动轨迹如图10、图11所示。

图10 机械臂到达终点 图11 机械臂返回起点

Rqt_plot是ROS平台的二维数值曲线绘制工具，可以将需要显示的数据用平面坐标图描绘。在终端输入Rqt打开界面，输入/joint_states/position，即可得到实时关节变化曲线。如图12所示，在机器人进行避障规划过程中，各关节平稳运行。

图12 关节变化曲线

5 结语

文中基于ROS平台对机器人进行运动规划研究，包括轨迹规划及避障仿真实验。通过各关节位置变化曲线可以看出：机器人运动过程轨迹平滑连续，稳定性强。既可以按照既定直线和圆弧轨迹运动，又可以灵活进行避障，并且可以根据实际需求自定义运动规划算法，对机器人的研究具有一定的参考价值。