基于改进RRT算法的七自由度机械臂路径规划

沈业全，刘 霞

（江汉大学 智能制造学院，湖北 武汉 430056）

0 引言

近年来，随着各国工业战略的提出，工业机械臂已经被广泛运用于金属加工、汽车、军工、医疗等领域。路径规划是机械臂完成任务中的一个重要的环节，由于普通机械臂在末端位姿固定的情况下，机械臂关节角度的解只有有限的几个，空间中总有一些位姿普通机械臂不能到达，在作业环境复杂的情况下，可能会造成任务的失败。七自由度机械臂在2、3关节之间又增加了一个自由度，相比于普通机械臂其逆解的可能性更多、灵活性更高。

路径规划是指已知机械臂的起始位置和终点位置，找到从起点到终点的无碰撞路线，主要有耗时、路径代价等评价标准［1］，目前主要使用快速扩展随机树（rapidly-exploring random tree，RRT）算法来进行机械臂路径规划，RRT算法不需要提前构造环境地图而以随机采样的方法进行线路扩展，但传统RRT算法存在路径代价大、耗时长等问题。陈肇星等［2］提出的DSSP-RRT算法通过动态采样与贪婪策略减少了迭代次数，但是存在可能陷入局部最优的情况；闫明亮等［3］提出的双采样点的双向RRT算法加快了算法的收敛速度，但还是存在路径代价大的问题；赵惠等［4］在RRT算法基础上加入了概率目标偏向与引力思想，增强了算法的启发性；Strub等［5］提出了Informed-RRT*算法，通过限制随机采样的区域来加快算法的收敛速度，但是存在规划不成功的情况。

本文在已有研究的基础上，针对RRT算法收敛速度慢、路径代价大的问题，提出了一种改进的RRT算法进行七自由度机械臂的路径规划，以起始点和目标点为初始点同步构建两棵树，在扩展时采用贪婪策略；同时为进一步提高算法的收敛性，提出了动态步长与新节点生成优化策略；最后利用MATLAB进行二维仿真，ROS中进行三维仿真，验证了该算法的可行性。

1 七自由度机械臂运动学原理

机械臂研究的一个基本问题是其正运动学与逆运动学［6］。正运动学是计算机械臂末端执行器的位置和姿态的几何问题，简而言之，假设给定一组关节角的大小，计算末端执行器坐标系相对于基坐标系的姿态。逆运动学是给定机械臂夹爪位姿，计算所有可以到达目标点位置的姿态的关节角大小。

Denavit和Hartenberg［7］在1955年提出一种描述相邻连杆之间坐标关系的方法（D-H法），根据机械臂的构造，建立机械臂的D-H坐标系如图1所示，其中xi与zi为第i(i=1,2,…,7)个关节X轴与Z轴。D-H坐标系主要参数见表1。

表1 D-H参数表Tab.1 D-H parameters

图1 机械臂模型与D-H坐标系的建立Fig.1 Mechanical arm model and establishment of D-H coordinate system

表1中，ai为连杆长度；di为连杆偏距；θi为两杆夹角；αi-1为连杆扭转角。d1=0.333 m；d3=0.316 m；d5=0.384 m；d7=0.107 m；a4=0.082 5 m；a5=-0.082 5 m；a7=0.088 0 m。连杆坐标系{}i相对于连杆坐标系{ }

i-1的变换矩阵可以表示为

将连杆1到连杆7之间的变换矩阵相乘就可以得到机械臂末端的位姿即机械臂的正运动学方程，

式中，r11,r12,…,r33为3×3的矩阵用来表示姿态；px,py,pz为位置描述。

机械臂的逆运动学与正运动学刚好相反，已知末端坐标系相对基坐标系预期的位姿，计算出满足期望位姿的其他关节角［8］。常用的求解方法有几何法、数值计算法，本文选择数值计算法求解机械臂的逆运动学方程［9］，可得到8组不同的逆解，在实际解的选取上根据具体的外界环境选取关节变化角度最小的解作为最优解。

2 七自由度机械臂路径规划算法

2.1 基础RRT算法

RRT的思想是快速扩张一棵树，用最快的速度来探索整个机械臂的工作空间然后找到可行的路径［10］。随机是RRT的一大特点，虽然开始时不知道从初始点到终点的路径，但是随着采样点的不断增加，通过反复的试探找到终点的概率就越来越大，主要步骤如下：

1）将起始点Xinit初始化为扩展树的根节点。

2）在地图中随机产生一个采样点Xrand。

3）计算树中每个节点与Xrand间的距离，找出距离Xrand最近的节点Xnearest。

4）沿着Xnearest与Xrand的连线方向增加固定步长，产生新的节点Xnew。

5）检测节点Xnew与Xnearest的连线是否会与障碍物发生碰撞，如果发生碰撞就停止本次迭代，返回步骤2），否则将节点Xnew加入树中。

6）判断新节点与终点距离是否小于设定的阈值，如果是，则说明找到可行路径。

RRT快速扩展随机树的扩展原理如图2所示。

图2 RRT扩展原理Fig.2 RRT expansion principle

基础RRT算法虽然能够找到从起始点到终止点的路径，但由于它采用完全随机采样，寻找路径时间长且路径差异大，算法收敛性差。

2.2 改进的RRT算法

2.2.1 双向扩展与动态步长本文改进的RRT算法与传统RRT算法相比，以起始点和目标点为初始点构建两棵树并交替进行扩展，同时在扩展时加入了贪婪思想，即起始点构建的扩展树始终以目标点为方向进行扩展，目标点构建的树以起始点扩展的新节点Xnew为扩展方向。

在扩展时为了进一步提高算法的收敛性，提出了动态步长的概念。RRT算法步长是固定的，在改进的RRT算法中加入动态步长函数，具体做法为：扩展树在开始时以固定步长生成，如果没有遇到障碍物就以2倍固定步长进行扩展，直到遇到障碍物或者两树相连就将步长换为原始步长。

2.2.2 采样点优化由于算法采用贪婪思想与动态步长策略，在遇到障碍物时扩展线路容易陷入局部最优，为避免这种情况，本文对遇到障碍物后生成新节点产生的范围进行了设定，以遇到障碍物的节点为圆心、固定步长为半径生成一个圆，在圆上范围选取下一节点，如图3所示。

图3遇到障碍物新节点生成图Fig.3 Generation of new nodes when encountering obstacles

图3 中红色圆为障碍物，P(px,py)是在扩展时遇到障碍物的节点，虚线圆表示下一个新的节点可能在的位置，虚线圆的半径为扩展步长；虚线圆上的无碰撞节点（如点X1,X2,N）都是下一步可以规划成功的节点。与障碍物相交的区域表示下一次扩展还会发生碰撞（如点X3）。所以下一个节点生成就必须满足：①新节点必须在圆上；②节点Xnew不会与障碍物碰撞；③Xnew可以和P(px,py)相连接。

2.2.3 算法描述本文改进的RRT算法具体步骤：

1）首先构造T1和T2两棵树，分别初始化为Xinit和Xgoal。

2）T1先开始扩展，以目标点为方向进行扩展，如果扩展成功就将步长进行增大直到遇到障碍物或者搜寻到目标点停止。

3）当T1在扩展时与障碍物相碰，再以Xnew为圆心、初始固定步长为半径生成一个新的节点，并且把该节点加入到T1中。

4）调换T2进行扩展，以T1产生的新的节点为目标进行扩展。

5）在T2遇到障碍物时与步骤3）相同，直到两棵树相遇。改进的RRT算法扩展原理见图4。

图4 改进的RRT算法扩展图Fig.4 Expansion diagram of improved RRT algorithm

改进RRT算法的伪代码如下：

?

2.3 二维仿真实验与结果分析

为验证算法的通用性，本文在不同环境下将RRT与改进RRT算法进行对比。两仿真实验地图分别为狭窄通道的栅格环境与障碍物随机分布环境，地图大小均为10 cm×10 cm，起始点在地图左上角，目标点在地图右下角。仿真使用的软件为MATLAB 2019a，计算机配置为：Intel Core i7处理器、显卡为NVIDIA GeForce RTX 2060。为避免算法的偶然性，分别在这两种环境下重复进行100次仿真实验，并取数据的平均值为结果。

如图5和图6所示，黑色区域代表为障碍物，白色区域为可扩展区域，蓝色线条代表最后生成的路径。表2和表3分别为RRT与改进RRT算法找到目标点的规划时间以及路径长度对比。

图5 栅格环境下算法仿真图Fig.5 Algorithm simulation in the grid environment

图6 障碍物随机分布环境下算法仿真图Fig.6 Algorithm simulation in obstacle random environment

从表2算法的参数对比可知，在狭窄通道的栅格环境下RRT算法的耗时为3.62 s、路径长度为320.1 mm。本文改进的RRT算法耗时为1.21 s、路径长度为162.6 mm，本文改进的RRT算法相对于基础RRT算法规划时间减少了66.6%，路径长度减少了49.8%。由表3障碍物随机分布环境下算法参数对比可以看出，本文改进的RRT算法比较稳定，耗时基本都在1.0 s左右，路径长度也比较理想。

表2 栅格环境下算法参数对比Tab.2 Comparison of algorithm parameters in the grid environment

表3 障碍物随机分布环境下算法参数对比Tab.3 Comparison of algorithm parameters in obstacle random environment

由以上仿真结果可知，不管是狭窄通道的栅格环境还是障碍物随机分布环境，本文改进的RRT算法相比于RRT算法避障路径规划效果均得到显著改善。

3 七自由度机械臂运动规划

为验证本文改进的RRT算法在加入真实机械臂后的仿真效果，在ROS机器人系统下，利用Moveit及其插件接口实现机械臂在三维空间下的避障运动规划，并通过RViz显示［11］。机械臂避障仿真框架如图7所示，碰撞检测选择FCL插件中的OBB检测方法，在KDL运动求解器中设置正、逆运动学方程，OMPL路径规划器［12］采用基本RRT算法和本文改进的RRT算法进行仿真对比实验。

图7 机械臂避障仿真框架图Fig.7 Frame diagram of obstacle avoidance simulation of mechanical arm

机械臂避障仿真物场景如图8所示，图中黄色为机械臂的初始位姿，白色为机械臂目标位姿，绿色为场景中的障碍物，仿真在RViz中进行显示。机械臂初始关节角度θ1和目标关节角度θ2分别为

图8 障碍物场景图Fig.8 Obstacle scene

在图8的场景下，用RRT和本文改进的RRT算法分别进行50次实验，统计每次规划的时间、路径长度以及规划成功率，求取平均规划时间与平均路径长度作为结果（见表4）。图9为RRT和本文改进的RRT算法某次运行轨迹对比。

表4 仿真参数对比Tab.4 Comparison of simulation parameters

图9 机械臂仿真结果图Fig.9 Simulation results of mechanical arm

从图9中可以看出，本文改进的RRT其路径长度比RRT算法短。由表4可知，本文改进的RRT算法相对于RRT算法规划时间缩短了65.1%，路径长度缩短了50.6%，达到了预期的效果。

4 结语

本文研究了七自由度机械臂的路径规划算法，提出了一种改进的RRT算法，对RRT算法加入双向搜索、贪婪思想、动态步长和新节点生成等优化策略，令机械臂能够快速找到一个可行的路线。仿真结果表明，该算法可以适用于七自由度机械臂的路径规划，能够缩短算法的规划时间和路径长度。