RRT算法路径优化及仿真验证

刘文光，刘浩伟，罗 通，王志民

(江苏大学 汽车与交通工程学院， 江苏 镇江 212013)

0 引言

无人驾驶技术中，路径规划是重点和难点。为保证无人车的安全行驶，必须解决无人车在行驶过程中的避障问题。无人车避障路径的优劣将直接影响车辆的安全性和实效性。常见的路径规划算法有遗传算法、A*算法、RRT算法、人工势场法、蚁群算法等[1-5]。其中，RRT算法因为不需要对环境进行精准的建模，所以在算法布置和计算效率上具有优势。

RRT算法的核心是基于采样扩展随机生长树节点，生成一条无碰的路径。其主要存在的问题是搜索路径质量不高且非最优、搜索代价过大、收敛慢等。针对搜索代价大、效率不高的问题，李金良等[6]提出安全场的概念，并结合实车数据基于安全距离建立安全场，对RRT算法进行改进，提高了搜索的效率和成功率，但其算法运行过于依赖数据，鲁棒性不够。王海芳等[7]提出同时创建2颗搜索树，交替进行相向搜索，同时以一定的概率对随机点进行偏置，以提高收敛效率，并且对父节点进行重选，增强算法对环境的敏感程度，不过同时也增加了计算量。针对路径非最优及搜索质量不高的问题，叶鸿达[8]提出了一种基于A*引导的RRT算法，其可以更好地引导随机树探索路径。Karaman团队[9]提出了渐进最优RRT算法，基于最优标准来调整生成的随机节点，以提高采样效率，但会花费更多的代价在碰撞检测和重新布线上。Zhou等[10]采用最近邻搜索策略，提高了收敛速度。Zhou[11]结合APF算法对RRT进行优化，加入引力和斥力的作用。而Zhen团队[12]则对RRT算法的采样步长进行基于目标吸引力的调整，提高搜索效率。

基于RRT算法当前存在的问题和研究现状，提出了一种优化的RRT算法，对父节点进行约束范围内的重选，并对随机树进行剪枝，提高搜索效率。同时对生成的路径进行平滑处理，使其满足动力学约束。

1 RRT算法简介

RRT算法是一种基于随机采样且能在多维空间中有效规划路径的算法。该算法以一个初始点作为根节点，再通过随机采样的方式增加树枝节点，进而生成一个随机树，当随机树中的树枝节点生长到了一定数目且包括了目标点，便可以在随机树树枝中找到一条由从初始点到目标点的路径[1]。

RRT算法的执行过程如下：

1) 首先根据起始点，产生一个随机点Xrand。

2) 产生随机点后，计算随机树中的每一个节点与新生成的随机点之间的距离，找出距离此随机点最近的节点，记为Xnear。

3) 定义一个路径长度d,在Xnear与Xrand连线上以Xnear为原点、长度为d处确定新的节点Xnew。

4) 判断Xnew是否满足设定需求，如果不满足，舍弃，重新产生新的随机点Xnew。

5) 循环产生路径，直到Xnew与目标点的距离小于d，也就是新生成的节点到达目标点附近时，则退出循环，生成路径。

由于RRT算法采样具有随机性的特点，所以最终生成的路径往往不是最优路径，故需要对RRT算法进行优化使其能更好地用于实际路径搜寻规划中[2]。

2 避障策略

为了简化问题，路径规划的障碍物选取较为规则的几何形，如圆、多边形等。对于圆形障碍物来说，其边界的判断属于非线性问题，通常将圆形进行线性化处理(转化为多边形)，只需要判断Xnew的横纵坐标是否在圆内。这里规定，当Xnew位于圆形障碍物的外接正方形内，就视为碰撞。如果圆形障碍物的圆心坐标为(X0，Y0)，半径为r，则需要考虑移动机器人的尺寸，对障碍物进行膨化处理，膨胀尺寸为inf，所以碰撞条件为:

X0-r-inf

Y0-r-inf

(1)

因为圆形障碍物的避障策略相对简单，在仿真中并未设置圆形障碍物。在仿真环境中搭建的随机环境，主要选取长方形障碍物。长方形的碰撞机制研究相对复杂一些，具体碰撞机制如下︰在扩展随机树的过程中，由Xnear到Xnew连接的边不能与长方形障碍物的任何一边相交，即将长方形障碍物碰撞检测问题转化为直线与矩形相交问题。直线与矩形相交的判断主要分为两步:

第一步，判断Xnew与Xnear是否在矩形某一条边的一侧。如果Xnew与Xnear在矩形任意一条边的同侧，则不用进行后续判断，Xnew与Xnear连线必定不与矩形相交。这里不存在两点位于矩形内部的情况，因为Xnew由Xnear产生，而Xnear必位于矩形外侧，如果Xnew与Xnear位于某—条边的两侧，则进行第二步判断[3]。

第二步，当Xnew与Xnear在矩形任意一边的不同侧时，分为2种情况:第一种情况是Xnew位于矩形内部，则Xnew与Xnear连线必定与矩形相交。第二种情况是两点均在矩形外部。在这种情况下并不能保证两点连线不与矩形相交，两点位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交。在这种情况下，利用直线与矩形的性质进行避碰计算。

3 RRT算法优化

尽管RRT算法相对于其他算法比较高效，可以较好地解决障碍物复杂场景路径的规划问题，具有很多优势，但是由于RRT算法取点的随机性，并不能保证所得出的路径最佳，所以须对RRT算法进行优化以便更好地解决路径优化的问题[4]。现根据RRT算法在路径规划过程中，路径非最优，提出这样2个优化策略：约束范围内重选父节点，基于重选的父节点修剪随机树树枝。

3.1 重新选择父节点

在最新生成的节点Xnew附近以一定半径范围作为约束搜寻相邻的节点，使之作为替代Xnew父节点的候选节点。

计算约束范围内的附近节点到起点的路径距离和Xnew到每个附近节点的总路径距离，具体过程见图1。图1(a)中表达的是扩展随机树过程中的某一时刻，节点的数字表示本节点产生的顺序，0是起点，10是最新产生的节点Xnew，节点6是节点10这一时刻的父节点Xnear，节点间连线上的数字表示两节点间的距离。

图1 重选父节点过程示意图

在为节点10重新选择父节点过程中，以节点10为中心，以某一半径作为约束，对范围内的节点10的相邻节点作为父节点的候选点，也就是点5、7、9。计算其路径代价如表1所示。

由表1可知，候选点5作为节点10新的父节点的总路径距离是最小的，所以把节点10的父节点改成6，重新生成的路径随机树如图1(b)所示。

表1 候选点路径代价

3.2 修剪随机树

在重新选择父节点的基础上，通过修剪随机树来调整随机树树杈的数目，减少不必要的树枝生成，进而缩短路径距离。

修剪逻辑为：如果新生成的Xnew可以作为相邻节点的父节点，并且缩短路径距离，则进行修改，过程示意图如图2所示。

图2 修剪随机树过程示意图

如图2(a)所示，节点10为新生成的节点Xnew，相邻的节点为6、7、9，如果用新生成的节点10代替他们各自的父节点0、5、6，则路径代价如表2所示。

表2 修剪后的路径代价

如果节点10作为原节点6的父节点，原路径由0-6变成0-1-5-10-6，代价大于原路径，舍弃本方案；同理候选点9结果也是如此。

如果节点10作为节点7的父节点，原路径由0-6-7变成0-1-5-10-7，代价相比较于原路径较少，则使用本方案。

修剪随机树的意义在于通过新生成的节点来替换老节点的父节点，修改随机树的树枝，去掉多余的树枝路径，缩短路径距离。

如图2(b)所示，重新选择父节点使新生成节点连成通路后的距离缩短，修剪随机树则使新生成节点后的树枝精简，减少路径长度代价，但新路径的建立必须要保证新节点和父节点相连通且不触及障碍物的条件下,重新选择父节点和修剪随机树2个方法各司其职，相互配合，能够进一步缩短路径距离和搜索代价。

4 路径处理

由于RRT算法是基于随机采样的，导致算法搜索到的路径比较曲折、不平滑，路径还包含了许多非必要的节点，特别是在复杂的多障碍物环境下，曲折点更多，由于汽车转弯半径有限，不利于汽车的行驶，故现提出2个路径处理方法。

4.1 基于最小转弯半径约束的路径修改

如图3(a)所示，在生成的路径中，节点3处的夹角小于汽车的最小转弯半径所要求的最小路径夹角，因此，在路径规划结束后再对路径进行检查修剪，在路径夹角不符合要求时进行修剪。如图3(b)所示，由于原节点3处的夹角过小，不符合要求，所以在节点3的附近插入一个新的节点4，使其满足转弯条件。

图3 最小转弯半径约束示意图

4.2 基于三角形原则的路径修改

在算法规避障碍物生成路径的过程中，可能会出现如图4(a)所示的情况，由于RRT算法的随机性，生成的路径虽然避开了障碍物，但是比较曲折，存在多余的路径。现根据三角形两边和大于第三边的原则[13]，对路径进行修改。

图4 三角形原则修剪过程示意图

在图4(b)中，点1、2、3构成三角形，现把路径1-2、2-3消除，生成1-3路径，进而缩短路径距离，但是也必须保证新生成的1-3路径与障碍物存在一定距离。点3、4、5构成的三角形也进行相同的处理，生成3-5新路径。路径最终由1-2-3-4-5变为1-3-5，在缩短了路径的同时，还一定程度上使生成的路径更加平滑。

5 Matlab仿真实验

为验证改进RRT算法相比于经典RRT算法的改进效果，现将其布置到同一张地图上进行仿真实验，观察其优化效果。同时，为了与其他优化算法做对比，这里选取A*引导RRT算法[2]参与仿真实验，进行横向对比。

现将改进的RRT和A*引导RRT算法及经典RRT算法分别在Matlab生成的随机地图上进行重复仿真实验50次，分析其生成途径的总长平均值，用来评判改进后的RRT算法和A*引导RRT算法各自相对经典RRT算法的优化程度如何，同时记录平均迭代时间，评判算法迭代效率如何。

为了验证改进后的RRT算法优化效果的鲁棒性和普适性，分别在3幅不同的随机地图上进行仿真实验，其效果如图5—13所示。

图5 Map1(基于RRT生成的路径)

图6 Map1(基于A*引导RRT生成的路径)

图7 Map1(基于改进RRT生成的路径)

图9 Map2(基于A*引导RRT生成的路径)

图10 Map2(基于改进RRT生成的路径)

图11 Map3(基于RRT生成的路径)

图12 Map3(基于A*引导RRT生成的路径)

在3幅地图，9个实验组中，均设置起点为(0,0)，终点为[(25,-25),(26,-25),(26,-26),(26,-25)]包围的区域内，每个实验组共进行50次实验，得出的数据如表3所示。

表3 Map1仿真结果

表4 Map2仿真结果

表5 Map3仿真结果

由3组仿真实验可以看出，A*引导RRT算法及改进的RRT算法(本算法)相较于原RRT算法在路径长度上均有不错的优化效果，在不同地图特征下优化度略有差异，不过总体优化度接近，但改进的RRT算法通过对约束下的父节点重选和对随机树剪枝的过程，大大提高了迭代收敛的效率。可以看到，在3组实验中，改进的RRT算法迭代时长均比A*引导RRT算法短，可见，本算法在提高路径优化度的同时还保持了较高的搜索效率。图7、图10、图13均是改进后的RRT算法生成的路径图。从图中可以看出，相较于原RRT算法生成的图5、图8、图11路径图来说，改进后RRT算法生成的随机树具有更好的发散性，而原RRT算法生成的随机树则比较曲折凌乱。故此，在最后生成的路径中可以比较得出，优化算法生成的路径较为平缓，更符合车辆动力学规律，保障汽车能平缓行驶。

图13 Map3(基于改进RRT生成的路径)

6 结论

RRT算法在经过约束后对父节点进行重选，使生成的随机树树枝更加简洁，指向性也更加明确，使找到目标点后生成的路径距离缩短，但由于重新选择父节点可能会导致生成的路径更加曲折，所以还需要对路径进行进一步的优化，即对随机树进行剪枝，通过修剪随机树修剪多余的随机树树枝，减少冗余通路，能有效地减少路径的长度。这两步主要的功能是对规划的路径距离进行缩减，而后续的基于对最小转弯半径约束的路径优化和基于三角形原则的路径优化，则是对生成的路径进行优化，使路径更加平滑，使生成的路径更贴合实际汽车的行驶要求并满足动力学约束。