基于改进RRT算法的冗余机械臂路径规划研究

付东昊, 赵铁军

（沈阳工业大学 机械工程学院，沈阳 110000）

0 引言

机械臂的路径规划问题一直是机械臂领域的研究热点, 机械臂路径规划问题的本质，即在规定的范围内，从路径起始点到路径终止点，为机械臂在运动的过程中寻找到一条最优路径[1]。现阶段机械臂主要的路径规划方法有[2]：人工势场法、搜索算法及随机采样法。人工势场法是在机械臂规划过程中，在目标点与障碍物分别设置引力函数与斥力函数，从而得到一条可行路径[3]。搜索算法解决问题的方法是通过在环境里列举出问题的所有可能结果，并通过高性能的计算机来找出最优解的一种方法，现阶段一般有枚举算法、A*算法等算法。随机采样法有随机路图法（Probabilistic roadmap method, PRM）[4]和快速搜索树（Rapidly-exploring random trees, RRT）[5]。在使用随机采样法进行路径规划时，向外随机采样来规划出一条合适的路径。快速搜索树算法适用于解决处理高维空间中路径规划问题，而随机路图法并不适用，因此，快速随机树算法通常应用在机械臂的路径规划问题上。

近年来，国内外专家学者一直将基于RRT算法的机械臂路径规划作为研究的重点。RRT算法以固定步长向外搜索，具有搜索能力强、适应性高等特点，但是会缺乏方向性，产生过多的无用节点等问题。在此基础上，许多学者对此进行了改进。在2000年Kuffner等[6]提出了从起始点和终止点同时扩展两棵随机树从而实现对工作空间的快速采样，即RRTConnect算法。在2011年Karaman等[7]提出了重选择父节点的方法, 并提出RRT*算法。Gammel等[8]将RRT*算法中引入了椭圆约束采样空间，提出了Informed-RRT*大大缩短了搜索时间，但不适用与动态环境下。Jordan等[9]将双向随机扩展树引入RRT*算法提出了B-RRT*算法。

本文的研究以Sawyer冗余机械臂为例，针对RRT算法在路径规化过程中缺乏方向性，成功率较低等问题，提出了改进的RRT算法，利用改进的算法减少了路径生成时间和无用节点，提高了路径规划的成功率。

1 冗余机械臂分析

在机械臂的工作空间内，非冗余机械臂存在关节限位会导致机械臂的末端执行器在工作空间中的某些点位不可达。为了解决此类问题，冗余机械臂增加了一个自由度[10]。本文所使用的为Rethink公司生产的七自由度机械臂Sawyer，为了在避奇异与空间避障的方面具有优势，增加了关节个数和旋转角度。Sawyer 机械臂如图1所示，D-H参数如表1所示。

2 基本RRT算法

RRT 算法是在工作空间内，以固定步长向全局进行随机采样的全局路径规划方法。其基本原理是在工作空间内，已知起始点Xinit和终止点Xgoal，以初始点Xinit建立树结构并开始向外进行扩展搜索，并向外随机采样生成产生随机点Xrand，遍历随机树找到距Xrand最近的新节点Xnear，由Xnear扩展步长step，得到新节点Xnew。并通过使用碰撞检测函数来判断Xnew是否与障碍物发生碰撞。若Xnew通过碰撞检测则将其纳入树中，否则放弃Xnew。在寻找到目标点之前反复进行以上步骤，并在目标点处设置阈值p，若新节点与终止点的距离小于这个阈值，则判断到达终止点。并在起始点与终止点之间生成一条连续无规则的路径，传统的RRT算法原理如图2所示。

图2 RRT算法原理

3 改进RRT算法

3.1 引入目标偏置策略

根据RRT算法可知，若不限制RRT算法的迭代次数，随机树产生的无用支点会导致随机扩展树产生在整个空间内。只要时间足够，RRT算法总能找到一条有效路径。为解决RRT算法在路径规划过程中，进行无方向性的随机采样，提高选择Xrand的有效性，加快路径的搜索速度。将目标偏置策略引入随机采样的函数中[11]。使在每次采样的过程中，Xrand以一定概率选择Xgoal做为采样点，即Xrand=Xgoal。采样公式如（1）所示：

具体方法为设置参数β为目标偏置概率，并在采样过程中随机产生概率βrand（0＜βrand＜1）。目标偏置策略原理如图3所示。若βrand＜β，则判断Xrand=Xgoal，由点划线连接。在其他情况下则判断Xrand为在空间中随机选择的一点。

图3 目标偏置策略

3.2 双向随机扩展

在RRT算法中，从起始点生成一棵树向终止点进行随机扩展搜索。若工作空间较小，障碍物较少则路径规划较容易，产生的无用支点较少。若障碍物较多，工作空间较大，RRT算法会在空环境中产生过多的无用节点，增加路径规划的时间，降低了算法的搜索效率。

针对上述问题，本文提出在起始点与终止点中随机生成一点Xmid，并从起始点与终止点分别同时向Xmid生成随机搜索树，进行随机采样，此时Xmid将作为此次路径的Xgoal，而原本的Xgoal将作为Xinit2。在起始点与终止点结束路径搜索后将两条路径整合，得到一条最终路径。改进算法的原理如图4所示。

图4 改进RRT算法原理

4 仿真验证

为了验证改进的RRT算法可以在工作空间中具有更好的搜索效率，本文分别在Matlab与ROS中进行三维路径规划与冗余机械臂路径规划的仿真实验。

4.1 三维路径规划仿真

使用Matlab建立路径规划的三维仿真环境。设置仿真环境的采样空间为500 mm×500 mm×500 mm，起始目标点坐标为（0，0，0），终止目标点为（500，500，500）。随机设置黑色障碍物的形状与位置。RRT算法搜索路径的扩展步长step为30 mm，阈值p为20。改进的RRT算法中目标偏置概率为0.5，设置算法迭代次数为10 000，若在迭代次数内无法到达目标点，则判定路径搜索失败。RRT算法与改进的RRT算法路径规划结果如图5和图6所示。

图5 RRT算法生成路径

图6 改进RRT算法生成路径

由图2与图3可知，RRT算法在路径规划过程中在无障碍物区域产生过多的无用节点，而改进的RRT算法明显减少了无用节点的产生。通过30次的仿真实验，RRT算法与改进算法的各项数据如表2所示。

表2 实验结果平均值

由表2中数据可知，改进的RRT算法对比RRT算法在三维环境中，各项数据均少于RRT算法。生成节点数减少了约89.3%，平均路径规划时间减少了约91.1%，成功率提高了约31.6%。改进的RRT算法有效提高了路径的搜索效率。

4.2 冗余机械臂路径规划仿真

本文通过使用机器人操作系统ROS进行机械臂可视化的仿真实验，并通过可视化平台Rviz中设置仿真环境，通过Moveit！对机器人进行运动规划。通过搭建障碍物，利用冗余机械臂来对比在使用RRT算法与改进RRT算法时机械臂的运动情况。为模拟真实机械臂的运动情况，在机械臂的末端附加一个立方体，以表示机械臂完成抓取后，进行避障。

实验一：简单环境冗余机械臂避障。每种算法在相同环境下各进行20次实验。在简单环境中机械臂使用RRT算法与改进的RRT算法路径规划分别如图7、图8所示。平均时间如表3所示。

表3 简单环境平均时间

图7 简单环境下RRT算法机械臂路径

图8 简单环境下改进RRT算法机械臂路径

通过20次简单环境的实验，随机选择冗余机械臂避障路径的仿真结果，可以看出改进算法所规划的路径要优于RRT算法规划的路径，平均的路径规划时间也减少了大约84%。验证了改进算法应用在机械臂简单环境避障的有效性。

实验二：狭窄环境避障。为了验证冗余机械臂在狭窄环境的避障能力，将增加障碍物。与简单环境相同，各进行20次实验。在狭窄环境中机械臂使用RRT算法与改进的RRT算法路径规划分别如图9、图10所示。平均规划时间如表4所示。

表4 狭窄环境平均时间

图9 狭窄环境下RRT算法机械臂路径

图10 狭窄环境下改进RRT算法机械臂路径

由仿真结果可知，复杂环境由于障碍物较多，而且在设置障碍物之间的距离较为狭窄的情况下，路径长度与时间要多于简单环境。冗余机械臂在狭窄环境中使用改进RRT算法的所生成的路径要明显优于RRT算法生成的路径。而在平均规划时间上，改进的RRT算法减少了约85%。

综合两次实验，证明了改进的RRT算法在路径规划上提高了路径搜索效率，减少了算法的计算量。

5 结论

本文以冗余机械臂Sawyer为研究对象，在简单环境与狭窄环境下对冗余机械臂进行多次仿真实验。通过实验结果可以看出，改进的RRT算法在两种环境下的规划时间均减少了约85%，在路径的选择上也有明显的改善。并且机械臂在狭窄环境下也可以有效地避免障碍物发生碰撞。改进的RRT算法在机械臂的路径规划问题上具有一定的参考价值。