基于改进RRT算法的空间机械臂避障路径规划*

柏维华，许行之

(桂林理工大学机械与控制工程学院，桂林 541006)

0 引言

随着自动化技术的快速发展，机器人技术在各个领域的关注度越来越高，应用场景也极大丰富[1]。实际应用中，我们常见到一定数量的关节串联机械臂能够快速准确地完成具有一定复杂度的工作，这对机械臂的运动控制提出了更高的要求，对机械臂进行路径规划也成了控制机械臂运动的重要一环[2-3]。

运动规划问题是机械臂规划过程中的基础。不同的运动规划算法适用于解决不同问题。基于搜索的运动规划方法多针对于二维空间中机器人的运动规划问题，通常使用A*[4]、Dijkstra[5]等算法，虽然其搜索能力强但当目标为机械臂且其关节数增多、自由度较高时，这些算法的复杂度会大幅提升，而基于采样的运动规划方法则巧妙地解决了这些弊端。

基于采样的运动规划方法主要针对高维空间下的机械臂运动规划问题。基于采样的运动规划方法即快速拓展随机树RRT[6-10]，具有概率完整性，快速的探索速度，无需将障碍物从任务空间映射到配置空间的优点。同时，传统的RRT算法也存在一些不足，例如在随机树搜索过程中的收敛速度较低，导向性不稳定，在复杂环境中寻迹速度慢且不必要节点过多以及障碍物约束使其随机采样生成的路径包含许多不必要的断点，从而导致路径不平滑且不连续等。因此，机械臂的运动规划通常是不稳定的。为了解决上述问题，可以在不同层面上做改进[11-13]。本文采用结合目标偏置的RRT算法，通过目标偏置策略在采样过程中引导算法以一定概率向目标点快速拓展实现尽快抵达目标点的任务以减少无用区域搜索，来加速算法收敛并达到探索空间的目的。最终在其规划出来的路径上采用三次B样条函数[14]优化其平滑性，来生成机械臂可跟踪的光滑路径。利用软件仿真得到的数据，并对其进行分析处理，验证了所提算法的快速性以及有效性。

1 机械臂运动学分析

本文的研究对象是一个6自由度的串联机械臂，如图1所示。为避免碰撞检测时，计算机运算量过大，使用机械臂连杆的凸包代替原机械臂连杆的模型。以机械臂底座坐标系作为全局坐标系，如图2所示，机械臂的各个连杆初始坐标系姿态与机械臂底座坐标系姿态相同，其中机械臂底座视为连杆1。

图1 6自由度机械臂及其初始姿态

图2 机械臂底座坐标系 图3 机械臂各关节坐标系示意图

以李群和旋量理论[15]为基础，对各关节的旋量坐标系进行搭建。连杆旋量坐标系如图3所示。惯性坐标系S(x1,y1,z1)设置在机械臂基座上，工具坐标系T(x7,y7,z7)设置在末端执行器上。当每个关节的角度为0时，机械臂的初始位形为：

(1)

各连杆坐标系原点相对全局坐标系位置如表1所示。

表1 各连杆坐标系原点相对全局坐标系位置

各关节螺旋轴原点相对相对全局坐标系位置如表2所示。

表2 各关节螺旋轴原点相对相对全局坐标系位置

(2)

以末端连杆位姿为例，设其初始值为矩阵gst(0)，各关节角变化量为向量θ，则经过正运动学变换后的位姿矩阵为：

gst(θ)=e[ξ1]θ1e[ξ2]θ2…e[ξ6]θ6gst(0)

(3)

2 碰撞检测

机械臂的碰撞检测是机械臂运动规划中的重要组成部分。当随机拓展树向外拓展一个都需要碰撞检测验证节点的有效性，这一过程十分消耗CPU的计算资源，影响机械臂轨迹规划的效率。本文通过导入机械臂的凸包，即能够包含机械臂连杆的最小凸多面体，简化碰撞检测的难度，提升运算效率。在凸多面体碰撞检测的过程中，利用GJK算法[16]，检测凸多面体之间的碰撞情况。

GJK算法通过不断验证单纯形与零点的关系判断两图形是否重叠。计算机想要“看见”两个图形重叠的部分，可以利用自身的计算优势，遍历图形A所有坐标减去图形B的坐标，得到一系列的点。将这些点全部包围起来，如果这个新生成的图形 包含原点，则意味着两个图形发生了重叠，从而判断两个图形发生了碰撞。我们把计算后的点称为闵可夫斯基差(Minkowski difference)，将生成的三角形/线形称为单纯形(simplex)。假定两个凸体A和B，用d(A,B)来表示A与B之间的距离，则距离可以表达为：

(4)

如定义v(C)为C中的离原点最近的一个点，即满足以下条件：

则A、B之间距离用Minkowski和A-B的形式可表示为：

d(A,B)=v(A-B)

通过如上的转换，就可以将A、B两凸体间的相交检测转化为更为简单的形式。当A、B碰撞发生时，A、B交集不为空集，此时单纯形(A-B)中存在零点。碰撞未发生时，单纯形(A-B)中不存在零点。通过迭代的方式，不断寻找单纯形(A-B)中离原点最近的点，当误差小于设定好的上限时，停止迭代。

(a) 凸体A和B的位置 (b) 单纯形(A-B)和零点的位置图4 发生碰撞时零点在A-B内部

(a) 凸体A和B的位置 (b) 单纯形(A-B)和零点的位置图5 未发生碰撞时零点在A-B外部

3 RRT算法及其改进

3.1 基本RRT算法

RRT 算法以采样的方式进行路径搜索,由于性能良好，被广泛地应用在各种机器人的运动规划场景中。从给定起点开始搜索，并将节点存储在随机树中。随机采样获取采样点，在随机树中选取距离采样点最近节点在两点连线方向扩展一个步长，并产生新节点。在扩展出的节点到达目标点之前，不断进行以上操作。在环境M中由起点xinit到目标点xgoal构建随机树T的伪代码如表3所示。

表3 随机树T的伪代码表

图6 RRT基本算法示意图

3.2 RRT算法改进

3.2.1 目标偏置策略

传统的 RRT 算法在复杂环境中存在搜索时间长、采样点随机性高等问题，本文采用基于概率的RRT算法。根据概率P的值来选择树的生长方向是随机生长(xrand)还是朝向目标位置(xgoal)生长，引入像目标生长的机制可以加速路径搜索的收敛速度。算法伪代码如表4所示。

如图7所示，与传统RRT算法不同的是，采样点不一定是由Sample()函数生成的随机节点。采样点为随机节点和目标点的概率分别是P和1-P，通过改变采样点的采样方式提升规划效率。

图7 基于概率的RRT算法示意图

3.2.2 渐进最优机制

RRT算法效率相对较高，但是不能在运行过程中优化路径。RRT*算法可以随着采样点的增加，对路径渐进优化。RRT*算法与RRT算法的区别主要在于两个针对新节点xnew的重计算过程，分别为：重新为xnew选择父节点的过程；重布线随机树的过程。

图8 RRT*算法节点拓展

算法实现过程如下：

步骤1：通过基本RRT算法的得到新节点xnew和树枝(xnear,xnew)；

步骤2：以xnew为中心，ri为半径，在树上搜索节点；

步骤3：找出潜在的父节点集合xpotential_parent，其目的是要更新xnew，看看有没有比它更好的父节点；

步骤4：对每一个潜在的父节点xpotential_parent进行检测，找出能够使从xinit到xnew路径代价最小且未发生碰撞的父节点xparent；

步骤5：删除树枝(xnear,xnew)，添加树枝(xparent,xnew)；

步骤6：对潜在的父节点集合xpotential_parent的剩余节点进行检测，假设以xnew作为父节点替换原先的父节点，计算xinit到xpotential_parent路径代价是否小于原先的路径代价且未发生碰撞；

步骤7：如果是，则更改父节点为xnew；如果不是，则保留原先父节点。

3.2.3 局部极小值脱离机制

RRT算法本身不能辨别已搜索的区域，这使得规划器在扩展区域内重复扩展。目标偏好RRT算法与路径规划中的人工势场方法[17]的一些特点相同。通常，简单的加速展开法会在某些局部区域，产生局部极小值问题。某些已被搜索过的区域会被不断重复搜索，这使得随机树很难快速逃离局部最小区域，甚至会降低算法运行成功率。因此，本文提出了一种结合碰撞信息检测机制[18]的扩展点选择机制来逃避局部最小区域，弥补了原算法仅使用欧氏线性距离来选择节点机制来选择下一个扩展方向，缺乏对局部约束的考虑，并能尽快扩大以避开障碍。

图9 向目标点扩展的节点与其父节点的碰撞概率 图10 向随机点扩展的节点的碰撞概率

记录碰撞概率算法实现过程如下：

步骤1：随机树扩展过程中发生碰撞；

3.2.4 针对机械臂改进采样环境

机械臂每个关节角相同的变化量对机械臂位姿的影响差别很大。将关节空间的每一个方向设置一个权重，可以大幅提升机械臂轨迹规划的效率。

表5 机械臂关节权重

3.3 改进后的RRT算法流程

步骤1：初始化随机树；

步骤2：生成一个(0,1)的随机数rand，比较该随机数与目标偏置概率的大小。若rand小于等于目标偏置概率，选取采样点为xgoal；若rand大于目标偏置概率，选取采样点为xrand；

步骤3：在碰撞概率小于pthreshold的点的集合中选择距离采样点最近的节点xnear；

步骤4：以固定步长在xnear和采样点连线方向上扩展新节点xnew；

步骤5：若生成的新节点与环境M发生碰撞，则扩展失败并记录碰撞概率；若生成的新节点与环境M未发生碰撞，则扩展成功，将节点和树枝记录到随机树T中；

步骤6：利用渐进最优机制，重新为xnew选择父节点的过程并重布线随机树，从而降低路径代价；

步骤7：判断xnew和xgoal之间的距离是否小于等于误差e。若是，则停止搜索，并将目标点添加到随机树T中；否则，重复步骤2～步骤6，直到随机树扩展成功；

步骤8：回溯随机树T，得到规划路径。

4 仿真实验验证

为验证改进算法，在二维/三维空间中运用不同的算法进行路径搜索，并以6自由度机械臂作为仿真实验对象进行仿真。实验在Intel(R) Core(M) i5-4210H CPU @2.90 GHz 2.90 GHz，8.00 GB内存，实验所用的电脑操作系统为Win10。使用MATLAB在二维和三维空间中对点状机器人进行仿真，对于6自由度机械臂的仿真则在Mathematica中进行。

4.1 二维仿真

设置800×800的场景1，将起点设为(1,1)，目标点设为(750,750)，设置目标偏向概率P=0.15，步长为5，令固定的最大节点数为1000，最大迭代次数为5000。

如图11所示，不同的算法进行路径搜索得到的随机树以及规划出的路径。

(a) 基本RRT算法 (b) RRT*算法 (c) 改进后的RRT算法图11 二维场景中测试结果

经过100组重复实验，实验结果如表6所示。可以看出，大多数情况下在该场景中都可以成功完成路径搜索。本文算法相对其它算法在路径搜索更有方向性同时也大大减少了不必要的探索节点，从而节省时间提升了搜索效率。

表6 二维场景中各算法性能对比

4.2 三维仿真

设置600×400×400的三维场景，将起点设为(50,50,50)，终点设为(450,350,300)。经过20组重复实验，实验结果如图12和表7所示。从图12和表7中可以看出，在具有障碍物的三维环境中，基本RRT算法以及RRT*算法在达到最大迭代次数时并不一定能成功搜索出从起点到终点的路径。相对基本RRT算法，RRT*算法搜索出的路径代价更小而改进后的RRT算法在每次实验中都能在较短的时间内找到路径代价相对较小的路径。

(a) 基本RRT算法 (b) RRT*算法

表7 三维场景中各算法性能对比

4.3 6自由度机械臂仿真

在MATLAB中完成了二维/三维空间的路径搜索仿真，并对比了不同算法之间的性能。接下来本文在Mathematica中，运行改进后的RRT算法，可以生成一条较为平滑且不产生碰撞的机械臂运行路径。

由于我们生活在三维空间，没有办法看到六维关节空间，为了将随机树的搜索以更加直观的方式表达出来，将六维关节空间拆分成两个三维空间，分别对应前3个关节和后3个关节。为了提高轨迹规划的效率，机械臂的关节空间在不同维度上赋予了不同的权重。如图13所示，不同算法在关节空间中进行路径搜索。

(a) 基本RRT算法

经过20组重复实验，实验结果如表8所示。由表8分析可知，在进行机械臂运行路径规划时，由于关节空间维度较高，经常存在规划失败的情况。相较于RRT和RRT*算法，改进后的RRT算法进行规划时，平均运行时间显著缩短，成功率大幅提高。同时，改进后的RRT算法平均路径节点数更少，这意味着机械臂运行过程中的位姿变化量更小。

表8 机械臂轨迹规划过程中各算法性能对比

利用改进后的RRT算法进行运动规划后得到的关节角变化曲线如图14所示。

图14 机械臂关节角变化数据 图15 三次B样条插值后机械臂关节角变化数据

由于RRT算法规划出的路径是相邻的点以线段的方式连接形成的，是一条折线，其中一个特征便是路径不够平滑。利用三次B样条曲线对得到的一串机械臂关节角进行插值，可以平滑机械臂关节角变化速率，如图15所示。

利用改进后的RRT算法结合三次B样条曲线对机械臂关节角变化数据进行插值，可以得到一条较为平滑且路径代价较小的机械臂运行轨迹。机械臂末端运行轨迹如图16所示。

图16 机械臂末端运行轨迹

5 结论

本文以RRT算法为基础，利用目标偏置提升规划效率，使得算法在运行过程中具有导向性。同时为了降低算法陷入局部极小值的几率，提出利用碰撞信息检测机制降低随机树向局部极小值扩展的概率。

在不同实验场景下，改进后的算法具有以下几个优势：①运行效率更高；②占用的计算资源更少；③更好地避开障碍物，同时消耗的路径代价也相对较小。

在机械臂仿真实验中，改进后的RRT算法能够快速有效地生成可以运行的机械臂运动轨迹。同时利用三次B样条函数对运行轨迹进行处理，保证机械臂在运动过程中保持平稳。