基于改进RRT算法的快速路径规划

顾子侣，刘 宇，岳 广,2，孙商文，李铜哨，付严宇

(1.中国人民解放军空军航空大学， 长春 130022； 2.中国人民解放军78102部队， 成都 610000; 3.中国人民解放军32072部队， 北京 100089)

1 引言

路径规划是实现无人设备自主导航的关键技术之一，它是指在具有障碍物威胁环境中，按一定评价标准，寻找一条从起点到目标点的无碰撞路径。目前常见的路径规划算法主要有Dijkstra算法、A算法、人工势场法以及包括蚁群算法、遗传算法、粒子群算法等在内的群智能算法。但上述算法存在如下缺陷：Dijkstra算法和A算法需预先对环境信进行栅格化处理，算法计算量随空间范围扩大呈指数增长；人工势场法易陷入局部最优；群智能算法需通过预先确定的代价函数来进行优化求解，强调路径最优性，当规划空间约束较多，代价函数较为复杂时，往往需要较长收敛时间。

快速扩展随机树(rapidly exploring random tree，RRT)算法是Lavalle教授于1998年提出的一种采用增量方式增长的随机采样算法。该算法具有如下优点：通过随机采样点，可搜索整个状态空间，具有概率完备性；扩展速度快，空间搜索效率高；通过碰撞检测方式来使路径避开障碍物威胁，避免对环境空间的建模。基于以上优点，RRT算法被广泛应用于解决复杂路径规划问题中。然而由于RRT算法空间搜索的盲目性，使得算法目标收敛性差、内存计算量大。对此，学者们提出不同改进策略。文献[11]借鉴人工势场中引力场思想，通过给采样点方向和目标点方向分配不同权重来共同决定节点扩展方向，然而由于权重固定，导致算法在复杂障碍物环境中容易长时间陷入局部陷阱中。文献[12]提出一种双向RRT算法，通过同时构建两棵树来加快算法搜索速度，但该算法本质上仍是盲目搜索，算法的随机性并未得到有效降低。文献[13]引入距离约束来决定生成的新节点是否应被添加于随机树中，有效缩短路径长度，但算法规划速度仍存在提升空间。文献[14]提出一种基于概率的目标偏置扩展策略，即通过以一定概率将目标点作为采样点，进而使随机树也以一定概率朝向目标方向扩展，然而由于在每次节点扩展过程中，具体是选择随机扩展方式还是目标偏置扩展方式仍是是随机的，无法针对障碍物，仍存在改善空间。

结合已有研究成果，在RRT算法基础上，通过对随机树中待扩展节点以及扩展方向的选取方式进行改进，实现提高算法规划速度和降低算法内存计算量的目的。另外，通过对初始路径进行冗余点裁剪以及对冗余点裁剪后的路径进行B样条曲线平滑来改善路径质量。

2 RRT算法基本原理

RRT算法基本思想是通过不断扩展叶节点的方式来搜索整个规划空间，直至随机树中的叶节点到达目标区域时，扩展停止。然后通过节点回溯便可找到一条由起点到目标点的可行路径，该算法具体扩展方式如图1所示。

图1 RRT算法节点扩展方式示意图Fig.1 Schematic diagram of RRT algorithm node expansion

其中表示扩展随机树，树上最初只有起点这一个节点，因此也被称为根节点；为随机采样点，在每次节点扩展过程中，由采样函数随机产生；为待扩展节点，传统RRT算法中将树中离随机采样点最近的节点作为，即

(,)≤(,)

式中：为树上由起点开始向外扩展的第个节点；为空间中任意两点欧式距离的计算函数；为生成的新节点，由待扩展节点沿与连线方向扩展一个步长得到，即

如果与连线之间不存在障碍物威胁，则将添加于中，否则，舍弃该节点，重新产生采样点来引导随机树的扩展方向。如此循环往复，直至随机树中的叶节点处于目标区域时，扩展停止，进而找到一条从起点到目标点的无碰撞路径。从算法原理中可以看出，对随机树整体生长方向影响最大因素主要为待扩展节点和扩展方向的选取方式。但在传统RRT算法中，这两者主要由采样点决定，导致算法在路径搜索过程中具有较大盲目性。

3 改进RRT的路径规划算法

针对传统RRT算法路径搜索盲目性较大的缺点，提出基于目标启发的待扩展节点选择策略，并对待扩展节点的扩展方向作出适当改进，使算法具有较强目标启发性的同时，还能在遇到障碍物威胁时，具备充分随机性，实现快速路径规划。另外，通过对初始路径进行冗余点裁剪并对裁剪后的路径再进行B样条曲线平滑来改善路径质量。

3.1 基于目标启发的待扩展节点选择策略

传统RRT算法将扩展随机树中与采样点距离最小的节点作为待扩展节点，即待扩展节点完全由采样点所处空间位置来决定。由于每轮循环中，采样点都是随机产生，因此待扩展节点的选取也具有较大随机性。甚至当扩展随机树已经快接近目标点时，由于待扩展节点选取的随机性，导致树中各节点被选取作为待扩展节点的概率是相同的。这会使得随机树向四周等概率进行扩展，导致生成大量无效节点。由于每轮扩展均需要计算随机树中所有节点与采样点的距离来确定待扩展节点，因此也进一步增加算法内存计算量。

通过借鉴A算法中启发距离函数的思想，提出了一种目标启发的待扩展节点选取策略。首先，设随机树上任一节点与采样点距离为()，与目标点距离为()，并令()为两者距离之和，则：

()=(,)

()=(,)

()=()+()

最后，将具有()最小值的节点作为待扩展节点，记为。图2显示了2种方法选取待扩展节点的原理示意图，可以看出，在待扩展节点的选取上，通过增加节点距目标点距离()，可以增大距离目标点较近节点被选取作为待扩展节点的概率，进而达到降低搜索盲目性和提高算法规划速度的目的。

图2 待扩展节点选取方式原理示意图Fig.2 Method for selecting nodes to be expanded

3.2 扩展方向确定策略

传统RRT算法以随机采样点所在位置作为扩展方向，随机性大。为改善RRT算法中扩展方向选取的随机性，受文献[11]提出引力场思想(以该策略进行改进的RRT算法常称为APF-RRT算法)以及文献[14]中基于概率的目标偏置扩展策略(以该策略进行改进的RRT算法常称为hRRT算法)的启发，对节点扩展方向提出如下改进思路：当待扩展节点确定后，优先尝试以目标方向作为扩展方向，若尝试成功，则直接生成新节点；若尝试失败，则以产生的随机采样点方向作为扩展方向，生成新节点，同时将尝试失败的待扩展节点标记为目标方向扩展失败节点，如图3所示。若下次该节点被选取作为待扩展节点时，直接进行随机扩展。

图3 目标方向扩展尝试失败示意图Fig.3 Schematic diagram of failed extension attempt in target

3.3 冗余点裁剪

利用RRT算法规划得到初始路径后，由于算法的随机性，导致生成的初始路径存在诸多冗余点。若两不相邻路径点之间连线不受障碍物影响，则将两点之间的路径点称为冗余点，如图4中路径点+1和+2即为冗余点。

图4 冗余点示意图Fig.4 Schematic diagram of redundant points

因此冗余点裁剪原理为：从初始路径起点(目标点)开始，逐一进行碰撞检测，找出可以无碰撞连接目标点(起点)的中间节点，并将该点作为新一轮起点，再依次寻找能够无碰撞连接的节点，直到每段路径之间不存在冗余点。最后，比较从起点连接目标点与从目标点连接起点这2种方式所得路径长度，将具有较短长度的路径作为冗余点裁剪后的路径。以从起点开始为例，其具体实现步骤：

1) 输入初始路径点序列为{,,…,}，其中为起点位置，为目标点的位置；

2) 构建冗余点裁剪后的路径点序列为，初始为空集；

3) 将起点加入中，并令=1，=；

4) 检测路径点与之间是否存在障碍物，若存在转入步骤5)，否则，转入步骤6)；

5) 若的值等于+1，则令=+1，=，并转入到步骤4)；否则，=-1，并转入步骤4);

6) 将路径点加入到集合中，并令=，=;

7) 若=，则裁剪结束，输出裁剪冗余点后的序列，否则转入步骤4)。

3.4 基于B样条曲线的路径平滑

考虑到路径平滑性要求，需进一步对冗余点裁剪后的路径进行平滑处理。B样条曲线可以将产生的路径节点作为B样条基函数控制点，生成曲率连续的平滑路径。本文采用三次B样条曲线来对初始路径进行平滑处理，三次B样条曲线的表达式为

(1)

式中:+为基函数控制点，,3()为三次B样条曲线的基函数，其表达式为

(2)

令冗余点裁剪后的节点序列集合为，即={,,…}，其中为起始路径点位置，为目标路径点位置。将路径点作为基函数控制点，并与式(2)一同代入式(1)中可得路径点与+3之间的三次B样条曲线段为

3.5 改进算法流程

Step 1：初始化随机树，将起点作为的根节点；

Step 2：在规划空间中随机产生采样点；

Step 3：计算中节点与采样点以及与目标点之间的距离之和()，将具有()最小值的节点作为待扩展节点；

Step 4：判断待扩展节点是否存在目标方向扩展失败的历史记录，若存在则转到Step 6；否则转到Step 5；

Step 5：沿与方向以为步长扩展得到，若扩展成功，则将添加于中，并转到Step 7，否则转到Step 6，并将该节点标记为目标方向扩展失败节点。

Step 6：沿着与方向以为步长扩展得到；若扩展成功则将添加于随机树中，否则转到Step 2。

Step 7：判断(,)≤，若满足则转到Step 8，否则转到Step 2；

Step 8：跳出循环，回溯得到初始路径；

Step 9：对初始路径进行冗余点裁剪；

Step 10：对裁剪后路径进行B样条曲线平滑，得到最终路径。

4 计算结果及分析

为验证改进算法可行性，在MATLAB R2016b中进行算法仿真实验。实验所用计算机配置如下：处理器为Intel Core-i5-7200U，主频为2.71 GHz，内存为12 GB，操作系统为64位Windows 10。并设置2种不同的地图环境，地图大小为800*800(无量纲)，如图5所示。另外，起点和目标点坐标分别设为(100，700)和(700，100)，扩展步长为20。

图6、图7分别表示在简单和复杂障碍物威胁环境下利用基本RRT算法、引入目标引力策略的改进RRT算法(APF-RRT算法)、基于概率的目标偏向扩展策略的改进RRT算法(hRRT算法)以及本文算法得到的路径规划结果图，其中APF-RRT算法目标方向扩展权重设为0.5，hRRT算法中目标方向扩展概率设为0.5。图中黑色实线为随机树的扩展路径，蓝色实线为得到的初始路径，红色实线为经冗余点裁剪和B样条曲线平滑后的最终路径。

图5 路径规划空间示意图Fig.5 Route planning space

图6 简单障碍物环境下4种算法路径规划示意图Fig.6 Schematic diagram of route planning with four algorithms in simple obstacle environment

图7 复杂障碍物环境下4种算法路径规划示意图Fig.7 Schematic diagram of route planning with four algorithms in complex obstacle environment

从图6和图7可以看出，在离障碍物威胁较远区域，本文算法中随机树的路径扩展分支最少，RRT算法最多，其他2种算法次之。这是由于本文算法在对随机树的待扩展节点以及扩展方向的选取上具有较强的目标启发性，因此相比于其他3种算法而言，随机树在离障碍物威胁较远区域可以更迅速地朝向目标区域生长；另外，在障碍物威胁区域附近，本文算法产生了大量不同方向的路径扩展分支，这是因为将目标扩展失败的节点及时转变为随机方向进行扩展，使得本文算法在障碍物威胁附近随机树的扩展方向作出了巨大的改变，以此来使随机树的生长快速跳出障碍物威胁的影响，但这也导致了规划所得的路径距离较长、且整体路径较为曲折震荡，如图中蓝色实线所示。

为弥补此不足，对得到的初始路径进行冗余点裁剪并将裁剪后的路径进行B样条曲线平滑，得到图6、图7中红色实线所示的最终路径，从图中可以看出路径质量得到有效提高。另外，为更好的验证算法性能，分别对4种算法在2种障碍物环境下进行200次仿真实验，得到的实验数据如表1、表2所示，其中各统计参数均为平均值。

表1 简单障碍物环境下算法路径规划性能实验数据Table 1 Performance comparison of algorithm path planning in simple obstacle environment

表2 复杂障碍物环境下算法路径规划性能实验数据Table 2 Performance comparison of algorithm path planning in complex obstacle environment

从表1、表2中可看出，不管是在简单障碍物威胁还是复杂障碍物威胁环境下，本文算法的规划时间和节点扩展数均是是最少的。且相比于hRRT算法，规划速度在简单威胁环境中提升36%左右，节点扩展数减少29%左右；在复杂威胁环境中，规划速度提升65%左右，扩展节点数减少50%左右。另外，初始路径经冗余点裁剪以及进一步的B样条曲线平滑处理后，路径长度得到明显缩短。

5 结论

1) 将RRT算法应用到路径规划时，针对原有算法规划速度慢、内存计算量大，对算法中随机扩展树的待扩展节点以及扩展方向的选取上作了一定改进，使得算法在具备目标启发性的同时，又能在障碍物威胁环境下具备较大随机性。

2) 本文提出的改进策略不仅可以提高算法的路径规划速度，还降低了算法的内存计算量，存在的不足是规划得到的初始路径长度较长、弯曲度较大。然而，通过对得到的初始路径进行冗余点裁剪后的路径进行B样条曲线平滑，可以有效提高路径质量。