一种改进的双向快速搜索随机树算法

徐秉超， 严 华

(四川大学电子信息学院，成都 610065)

随着机器人控制及智能化领域研究的不断发展，作为基础的路径规划研究也日趋深化。路径规划指在障碍物环境下，寻找从初始点至目的点的运行路径，同时保证路径可跟踪，安全无碰撞[1]。常用的路径规划算法分为不同类型，如Dijkstra、A*、D*等组成的前向图搜索算法，以栅格法和可视图法为代表的几何构造规划算法以及在智能控制理论下出现的基于遗传算法[2]和神经网络[3]的规划算法。

快速搜索随机树(RRT)算法由Lavall在1998年提出。相比全局搜索和启发式算法，RRT算法[4]能在保证概率完备性的同时极大降低时间成本。在高维空间里的优秀表现也令它倍受关注。RRT优化性能强大，将机械对象的实际动力学约束加入规划，可以使生成路径可跟踪[5]。该算法无需对地图进行几何划分，可以在动态规划中对局部地图进行再规划以生成局部路径[6],也广泛应用于多目标点规划任务[7]。然而，由于使用全局随机采样，RRT算法存在随机性大、效率低的问题。为此，目标导向RRT[8]和双向RRT[9]等改进方向被接连提出，基于概率的回归机制[10]被用于缓解重复生长问题。文献[11]的变步长双向RRT算法通过目标引力和双向搜索方式，跟踪目标点并扩大连接面积，但同时降低了全局避障能力，额外耗费大量时间。

在变步长双向RRT算法基础上引入预生长、重要障碍斥力和随机点筛选机制策略以缩小问题规模并提高避障过程中的随机性及有效性，优化路径规划的效率。

1 相关工作

1.1 快速搜索随机树

传统 RRT算法主要包括两个阶段：构造随机生长树和反向搜索可行的轨迹[12]。首先以起始点作为树的根节点，在全状态空间中随机采样以生长无碰撞的叶节点，直到有节点进入目标区域或达到目标点。随后以距离目标点最近的点开始反向搜索父节点，生成一条从起始位置到目标位置的路径[13]。

RRT算法中的单次生长如图1所示。首先以起始点Xinit为根节点构造一棵随机搜索树。然后在状态空间中随机取一自由点Xrand，找出随机搜索树中距离Xrand最近的树节点Xnear。接着，从Xnear向Xrand扩展出基础步长为ρ的新节点Xnew。若Xnew与Xnear间的路径没有发生碰撞，则将Xnew加入搜索树。重复上述过程直到节点进入目标区域或到达设置的最大迭代次数。一次完整的规划结果如图2所示。

图1 RRT算法节点生长示意图Fig.1 Schematic diagram of node growth RRT algorithm

图2 RRT寻径示意图Fig.2 Diagram of RRT path searching

由图2可知，传统RRT算法虽然避免了局部最优的陷阱，但因小部分的局部最优陷阱而失去全部导向性，致使在大部分环境中，性能得不偿失。为了解决此问题，需在传统算法中加入适当的导向功能。路径规划有两个要求，一是到达目标点，另一个是避开障碍，需要在目标跟踪过程中加入启发式降低随机性，在避障过程中则需要完全随机从而保证避障的效果。传统RRT没有对生长区域的有效性进行评估，例如在本文研究环境中，一方面为空间形成的区域区分度，即非主路径区域扩展的可能性应稍低，在图2环境中为右上和左下的部分区域；另一方面为时间形成的区域区分度，即已生长区域应有更低的可能性进行扩展，在图2环境中为部分左上区域。

1.2 变步长双向RRT

考虑到RRT算法的诸多不足，研究人员提出了双向生长树和目标引力机制。

双向生长树更多地在实际物理系统限制上进行优化，它从起点和目标点分别扩展生长树，缓和了目标点所在位置对规划的影响，不再需要对邻近边界的目标点进行特殊处理，并且将目标点扩展为点集，提高了搜索树和目标连接的效率，从而降低了形成路径的时间和复杂度。

在人工势场法[14]中，路径前进的方向取决于目标点和所有障碍，给规划提供有效导向，因而实时性较好。在借鉴人工势场法而加入目标引力的RRT算法中，新节点Xnew将使用式(1)得出：

(1)

式(1)中：ρ为基础步长；k为引力系数，即在每一次生长过程中，Xnew相对于Xnear向随机点前进ρ的同时，向目标点前进kρ长度。

目标引力在一定程度上解决了空间形成的区域区分度和到达目标过程中的导向，提高了规划效率，但因为只取用了目标引力，受其影响的避障过程无法实现完全随机，从而降低全局避障能力，对效率造成极大损失。

2 算法的改进

基于变步长双向RRT算法，改进算法首先进行预生长，即先在直线方向进行生长的尝试，以接近人类思维的方式，降低前期生长的代价；接着，加入随机点筛选机制，对部分时间形成的区域区分度进行处理；最后，引入重要障碍物斥力，消除引力在避障过程中的影响，降低不必要的效率损失。

2.1 预生长

人类思维下，在无障碍的环境中，使用直线连接起点和目标点具有最高的效率。如图3所示，预生长分别从两棵树的根节点开始，沿着起始点到目标点的直线方向，以2倍基础步长2ρ检测碰撞，以基础步长进行生长直到发生碰撞，并删除最后一个生长的节点。删除操作用于保证搜索树节点与障碍物存在适当距离。与障碍物距离过近的节点在后续生长中将有更大可能性发生碰撞，因而生长失败的概率更大，效率降低。

图3 预生长示意图Fig.3 Diagram of pre-extended

预生长过程模拟了人类的直观思维。在该过程中，除了进行少量碰撞检测外无需其他计算，生长效率大于正常生长流程，在低消耗下快速通过初期的自由空间，减少在前期无障碍区域的时间花费，缩小了需进行规划计算的区域，一定程度上缩小了问题规模。

2.2 随机点筛选机制

目标引力解决了一部分区域区分度问题，却依旧未能对时间形成的区域区分度进行处理。为此，一个基于metropolis接受准则的随机点筛选机制将被用来尝试缓解该问题。metropolis接受准则被用于模拟退火过程，它在一个已有解的基础上，接受优化解的同时以一定概率接受恶化解。随着逐步降低接受恶化解的概率，最终取得近似最优解。该准则可以跳出局部最优，同时保证概率完备性。

具体的，每一次随机生长过程中，分别计算最近邻点Xnear和生长树中的最新节点Xend到目标点的距离。按照式(2)判断随机点是否被接受。与原准则不同，为了提高效率，筛选机制不再使用概率P间接判断解的接受与否，而将趋近近似最优解的过程直接量化在对距离的判断中，原概率P可由式(2)表示。

(2)

(3)

式中：d为计算两个节点间欧式距离的函数。通过比例a，完成metropolis接受准则中概率的功能，实现在生长节点离目标区域较远的时候，改进算法能接受更坏的情况，即距离目标区域比最新生成节点远得多的情况。随着生长节点逐渐接近目标区域，接受坏点的范围逐渐缩小，最终趋近最优解。

通过基于metropolis接受准则的随机点筛选机制，有效对因时间形成的不同生长可能性区域进行划分，一定程度上减少随机搜索树的重复生长，提高生长效率。

2.3 重要障碍物斥力

目标引力在路径规划中的首要任务即寻找目标的过程中加入了导向。但同时，规划过程中占比更高的避障过程也受此影响而无法使用完全随机过程进行处理，从而削弱了避障性能。修改的障碍物斥力将被引入以缓解该问题。

图4对比了仅有目标引力和有较完善的势场两种情况下的理论生长过程。在仅有目标引力的情况下，搜索树生长到节点X3才发现障碍。在经历了数次失败或无效的反向生长后，最终才能到达一个较好的节点X2。然而存在一个更好更快的路径完成这个子任务，即从X1经虚曲线直接到达X2，这就需要一个适当的障碍物斥力机制。

图4 重要障碍物斥力示意图Fig.4 Diagram for repulsion of obstacle

传统人工势场的斥力场计算因考虑所有障碍导致复杂度激增[15]。针对该问题，划分障碍物的权重对于引入斥力极为重要。

在单步生长过程中，最近邻点Xnear到目标点的第一个障碍物是一个典型而优秀的选择，借鉴人工势场法的斥力部分，改进算法斥力体系中的斥力系数krep使用式(4)计算：

(4)

式(4)中：C1为障碍物斥力调整系数；l为起始点到目标点的距离；d为障碍物边缘到最近邻点的距离；C0根据基础步长决定，目的是在障碍距离过远时，精简计算步骤；C2为可接受的反向生长的比例，建议取值在1.0～1.1。设置斥力最大值可以防止在特殊情况下，扩展出过度反向生长的无效节点。

在使用时，将引力系数与计算得到的斥力系数直接相减，可得到合力系数k*，新节点生成公式为

k*=k-krep

(5)

引入重要障碍物斥力，可以缓解路径规划中避障过程因目标引力而产生的效率损失，甚至在避障过程中实现完全随机。虽然在障碍环境极度简单的情况下将增加部分时间成本，但在大部分环境中，寻径效率有较好的提升效果。

3 实验结果与分析

3.1 实验环境

假设目标机器人为理想圆点，使用仿真软件及数据证明改进算法的可行性与搜索效率。实验使用Windows 10，Intel Core 2.5 GHz，8 G内存，编译工具为MATLAB R2018a。

随机地图仿真实验中，系统随机生成规格为[1 000,1 000]的地图，原点[0,0]为左上角，如图5所示。黑色圆形为设置的障碍物，圆心及半径在设定的一定范围内随机，障碍数量为100。将[25,25]设置为起始点，目标点为[975,975]。

图5 随机地图Fig.5 Random map

在本组实验中，设置基本步长ρ=20，引力系数k=2，C0=2，C1=1/8 000，C2=1.01。

在矩形障碍地图实验及特殊陷阱地图实验中，使用确定的地图进行实验，地图的规格为[500,500]的矩阵，设置[25,25]为起点，右下角[475,475]为目标点。基础步长设置为5，其余参数相同。

3.2 实验结果与分析

首先对改进算法的可行性进行实验，验证算法是否能在给定环境中找到一条可行路径。然后在相同实验条件下对比不同算法的性能，得到实验数据并分析。

3.2.1 可行性实验

图6为一次实验的结果。两棵搜索树分别由左上角的起始点和右下角的目标点出发，图6中展示了寻径过程中产生的节点及连线和最终路径。实验证明了在设定条件下，本文改进算法可以完成路径搜索，输出一条无碰撞的可行路径。

图6 算法可行性验证Fig.6 The verification on the feasibility of algorithm

3.2.2 效率验证

实验将比较改进算法、目标偏向RRT、可变步长的双向RRT算法、人工势场法和A*算法。其中基础的A*算法用于获得最佳路径。其余不同算法使用相对相似的参数在相同环境和地图下进行仿真实验。共进行三组实验，分别使用随机生成环境、矩形障碍环境和一个特殊陷阱地图。设定单次规划时间超过30 s或连续扩展失败100次记作失败。对于存在随机性的RRT系列算法，每组进行50次寻径，统计成功寻径的平均时间、路径长度以及生成的节点数量。

实验首先在随机生成地图中进行，表1为实验结果。人工势场法面对此类环境极易陷入局部最优，无法完成规划任务。RRT系列算法表现较优。改进算法和变步长双向RRT路径规划成功率均为100%，且在各类指标上均优于目标偏向RRT。其中，改进算法相对变步长双向RRT在节点数量和平均规划时间指标上都优化了30%左右。

表1 随机地图仿真实验数据

图7对比了改进算法与变步长双向RRT算法的结果，可变双向RRT在避障过程中，存在不必要的导向性，进行了许多方向明确而重复的生长，降低了规划效率。而改进算法在面对障碍时随机性更充分，从而并没有展现出过强的方向性，避障效率更高。

图7 随机环境实验结果Fig.7 Results of random map experiment

简单矩形障碍环境的实验结果在表2中列出，数据表明人工势场法的路径因为障碍环境不会突变而更为平滑，但由于计算量大，时间方面并没有优势。图8展示了一次路径规划的结果，在该组实验中，预生长的作用尤为凸显。由于预生长的作

图8 矩阵障碍地图实验结果Fig.8 Results of rectangle obstacle map experiment

表2 矩形障碍仿真实验数据

用，实际进行计算规划的区域不到原计算规模的一半，并且降低了节点数目，提高了效率，相比可变步长的双向RRT的规划时间优化了50%左右。

第三组实验使用如图9所示地图，其包含一个典型的陷阱，相比于全局搜索，许多启发式算法面对该类陷阱的表现都较差。从表3的实验数据可看出，基础的人工势场法无法解决此类路径规划问题，而RRT算法由于其概率完备性，存在一定概率完成任务。其中改进算法由于引入障碍斥力和随机点筛选机制，在陷阱中的表现略优于其余比较算法，在成功率和时间指标上都有所提升，但还未达到解决此类问题的程度。其根本原因是对生长可能性的区域区分还不够完善，改进算法中的筛选机制只能划分远离目标点的已生长的区域，而不能划分所有已生长区域，从而较好地避免重复生长，仍有提升空间。

表3 特殊陷阱仿真实验数据

图9 特殊陷阱地图Fig.9 Special trap environment

4 结论

针对RRT算法在规划过程中表现出的部分问题，对可变步长双向RRT算法进行改进，采用预生长、重要障碍斥力和随机点筛选机制三种机制，得到以下结论。

(1)预生长机制可以实现对路径规划的直观处理，缩小实际规划问题规模，提高路径规划速度。

(2)引入的障碍物斥力在不明显提高计算成本的同时，有效解决了目标引力对于避障过程的误导问题。

(3)随机点筛选机制有效减少了重复生长的出现，对于结点数量的指标有较大改善，通过提高生长的成功率改善规划效率。

(4)对于特殊陷阱环境，相比传统方法，改进算法有一定进步，但还无法完全解决重复生长问题，有待深入研究。