1 引言
路径规划是实现无人设备自主导航的关键技术之一，它是指在具有障碍物威胁环境中，按一定评价标准，寻找一条从起点到目标点的无碰撞路径[1]。目前常见的路径规划算法主要有Dijkstra算法[2]、A*算法[3]、人工势场法[4]以及包括蚁群算法[5]、遗传算法[6]、粒子群算法[7]等在内的群智能算法。但上述算法存在如下缺陷：Dijkstra算法和A*算法需预先对环境信进行栅格化处理，算法计算量随空间范围扩大呈指数增长；人工势场法易陷入局部最优；群智能算法需通过预先确定的代价函数来进行优化求解，强调路径最优性，当规划空间约束较多，代价函数较为复杂时，往往需要较长收敛时间。
快速扩展随机树(rapidly exploring random tree，RRT)算法是Lavalle教授于1998年提出的一种采用增量方式增长的随机采样算法[8]。该算法具有如下优点：通过随机采样点，可搜索整个状态空间，具有概率完备性；扩展速度快，空间搜索效率高；通过碰撞检测方式来使路径避开障碍物威胁，避免对环境空间的建模。基于以上优点，RRT算法被广泛应用于解决复杂路径规划问题中[9-10]。然而由于RRT算法空间搜索的盲目性，使得算法目标收敛性差、内存计算量大。对此，学者们提出不同改进策略。文献[11]借鉴人工势场中引力场思想，通过给采样点方向和目标点方向分配不同权重来共同决定节点扩展方向，然而由于权重固定，导致算法在复杂障碍物环境中容易长时间陷入局部陷阱中。文献[12]提出一种双向RRT算法，通过同时构建两棵树来加快算法搜索速度，但该算法本质上仍是盲目搜索，算法的随机性并未得到有效降低。文献[13]引入距离约束来决定生成的新节点是否应被添加于随机树中，有效缩短路径长度，但算法规划速度仍存在提升空间。文献[14]提出一种基于概率p的目标偏置扩展策略，即通过以一定概率将目标点作为采样点，进而使随机树也以一定概率朝向目标方向扩展，然而由于在每次节点扩展过程中，具体是选择随机扩展方式还是目标偏置扩展方式仍是是随机的，无法针对障碍物，仍存在改善空间。
结合已有研究成果，在RRT算法基础上，通过对随机树中待扩展节点以及扩展方向的选取方式进行改进，实现提高算法规划速度和降低算法内存计算量的目的。另外，通过对初始路径进行冗余点裁剪以及对冗余点裁剪后的路径进行B样条曲线平滑来改善路径质量。
2 RRT算法基本原理
RRT算法基本思想是通过不断扩展叶节点的方式来搜索整个规划空间，直至随机树中的叶节点到达目标区域时，扩展停止。然后通过节点回溯便可找到一条由起点到目标点的可行路径，该算法具体扩展方式如图1所示。
图1 RRT算法节点扩展方式示意图
Fig.1 Schematic diagram of RRT algorithm node expansion
其中T表示扩展随机树，树上最初只有起点qstart这一个节点，因此qstart也被称为根节点；qrand为随机采样点，在每次节点扩展过程中，由采样函数随机产生；qnear为待扩展节点，传统RRT算法中将树T中离随机采样点qrand最近的节点作为qnear，即
dist(qrand,qnear)≤dist(qrand,qi)
式中：i为树上由起点开始向外扩展的第i个节点；dist为空间中任意两点欧式距离的计算函数；qnew为生成的新节点，由待扩展节点qnear沿qnear与qrand连线方向扩展一个步长ε得到，即
如果qnear与qnew连线之间不存在障碍物威胁，则将qnew添加于T中，否则，舍弃该节点，重新产生采样点来引导随机树T的扩展方向。如此循环往复，直至随机树中的叶节点处于目标区域时，扩展停止，进而找到一条从起点到目标点的无碰撞路径。从算法原理中可以看出，对随机树整体生长方向影响最大因素主要为待扩展节点和扩展方向的选取方式。但在传统RRT算法中，这两者主要由采样点决定，导致算法在路径搜索过程中具有较大盲目性。
3 改进RRT的路径规划算法
针对传统RRT算法路径搜索盲目性较大的缺点，提出基于目标启发的待扩展节点选择策略，并对待扩展节点的扩展方向作出适当改进，使算法具有较强目标启发性的同时，还能在遇到障碍物威胁时，具备充分随机性，实现快速路径规划。另外，通过对初始路径进行冗余点裁剪并对裁剪后的路径再进行B样条曲线平滑来改善路径质量。
3.1 基于目标启发的待扩展节点选择策略
传统RRT算法将扩展随机树中与采样点距离最小的节点作为待扩展节点，即待扩展节点完全由采样点所处空间位置来决定。由于每轮循环中，采样点都是随机产生，因此待扩展节点的选取也具有较大随机性。甚至当扩展随机树已经快接近目标点时，由于待扩展节点选取的随机性，导致树中各节点被选取作为待扩展节点的概率是相同的。这会使得随机树向四周等概率进行扩展，导致生成大量无效节点。由于每轮扩展均需要计算随机树中所有节点与采样点的距离来确定待扩展节点，因此也进一步增加算法内存计算量。
通过借鉴A*算法中启发距离函数的思想，提出了一种目标启发的待扩展节点选取策略。首先，设随机树T上任一节点与采样点qrand距离为H(i)，与目标点距离为G(i)，并令F(i)为两者距离之和，则：
H(i)=dist(qrand,qi)
G(i)=dist(qgoal,qi)
F(i)=H(i)+G(i)
最后，将具有F(i)最小值的节点作为待扩展节点，记为qnear。图2显示了2种方法选取待扩展节点的原理示意图，可以看出，在待扩展节点的选取上，通过增加节点距目标点距离G(i)，可以增大距离目标点较近节点被选取作为待扩展节点的概率，进而达到降低搜索盲目性和提高算法规划速度的目的。
图2 待扩展节点选取方式原理示意图
Fig.2 Method for selecting nodes to be expanded
3.2 扩展方向确定策略
传统RRT算法以随机采样点qrand所在位置作为扩展方向，随机性大。为改善RRT算法中扩展方向选取的随机性，受文献[11]提出引力场思想(以该策略进行改进的RRT算法常称为APF-RRT算法)以及文献[14]中基于概率p的目标偏置扩展策略(以该策略进行改进的RRT算法常称为hRRT算法)的启发，对节点扩展方向提出如下改进思路：当待扩展节点确定后，优先尝试以目标方向作为扩展方向，若尝试成功，则直接生成新节点；若尝试失败，则以产生的随机采样点方向作为扩展方向，生成新节点，同时将尝试失败的待扩展节点标记为目标方向扩展失败节点，如图3所示。若下次该节点被选取作为待扩展节点时，直接进行随机扩展。
图3 目标方向扩展尝试失败示意图
Fig.3 Schematic diagram of failed extension attempt in target
3.3 冗余点裁剪
利用RRT算法规划得到初始路径后，由于算法的随机性，导致生成的初始路径存在诸多冗余点。若两不相邻路径点之间连线不受障碍物影响，则将两点之间的路径点称为冗余点，如图4中路径点wmi+1和wmi+2即为冗余点。
图4 冗余点示意图
Fig.4 Schematic diagram of redundant points
因此冗余点裁剪原理为：从初始路径起点(目标点)开始，逐一进行碰撞检测，找出可以无碰撞连接目标点(起点)的中间节点，并将该点作为新一轮起点，再依次寻找能够无碰撞连接的节点，直到每段路径之间不存在冗余点。最后，比较从起点连接目标点与从目标点连接起点这2种方式所得路径长度，将具有较短长度的路径作为冗余点裁剪后的路径[15]。以从起点开始为例，其具体实现步骤：
1) 输入初始路径点序列为{wm1,wm2,…,wmn}，其中wm1为起点位置，wmn为目标点的位置；
2) 构建冗余点裁剪后的路径点序列为φ，φ初始为空集；
3) 将起点wm1加入φ中，并令j=1，i=n；
4) 检测路径点wmj与wmi之间是否存在障碍物，若存在转入步骤5)，否则，转入步骤6)；
5) 若i的值等于j+1，则令j=j+1，i=n，并转入到步骤4)；否则，i=i-1，并转入步骤4);
6) 将路径点wmi加入到集合φ中，并令j=i，i=n;
7) 若j=n，则裁剪结束，输出裁剪冗余点后的序列φ，否则转入步骤4)。
3.4 基于B样条曲线的路径平滑
考虑到路径平滑性要求，需进一步对冗余点裁剪后的路径进行平滑处理。B样条曲线可以将产生的路径节点作为B样条基函数控制点，生成曲率连续的平滑路径[13]。本文采用三次B样条曲线来对初始路径进行平滑处理，三次B样条曲线的表达式为
(1)
式中: Vi+j为基函数控制点，Gj,3(t)为三次B样条曲线的基函数，其表达式为
(2)
令冗余点裁剪后的节点序列集合为φ，即φ={φ1,φ2,… φn}，其中φ1为起始路径点位置，φn为目标路径点位置。将路径点作为基函数控制点，并与式(2)一同代入式(1)中可得路径点φi与φi+3之间的三次B样条曲线段为
3.5 改进算法流程
Step 1：初始化随机树T，将起点作为T的根节点qstart；
Step 2：在规划空间中随机产生采样点qrand；
Step 3：计算T中节点与采样点以及与目标点之间的距离之和F(i)，将具有F(i)最小值的节点作为待扩展节点qnear；
Step 4：判断待扩展节点qnear是否存在目标方向扩展失败的历史记录，若存在则转到Step 6；否则转到Step 5；
Step 5：沿qnear与qgoal方向以ε为步长扩展得到qnew，若扩展成功，则将qnew添加于T中，并转到Step 7，否则转到Step 6，并将该节点标记为目标方向扩展失败节点。
Step 6：沿着qnear与qrand方向以ε为步长扩展得到qnew；若扩展成功则将qnew添加于随机树中，否则转到Step 2。
Step 7：判断dist(qnew,qgoal)≤ε，若满足则转到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算法)、基于概率p的目标偏向扩展策略的改进RRT算法(hRRT算法)以及本文算法得到的路径规划结果图，其中APF-RRT算法目标方向扩展权重设为0.5，hRRT算法中目标方向扩展概率p设为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
算法时间扩展节点数规划航迹长度/m初始航迹长度经冗余点裁剪和平滑后长度基本RRT算法4.13751 111.3908.8APF-RRT算法3.2282915.8859.1hRRT算法1.171941.7866.5本文算法0.750890.1859.6
表2 复杂障碍物环境下算法路径规划性能实验数据
Table 2 Performance comparison of algorithm path planning in complex obstacle environment
算法时间扩展节点数规划航迹长度/m初始航迹长度经冗余点裁剪和平滑后长度基本RRT算法5.23661 064.6978.9APF-RRT算法4.5292949.6899.2hRRT算法3.22211 114.7919.7本文算法1.11101 193.3906.1
从表1、表2中可看出，不管是在简单障碍物威胁还是复杂障碍物威胁环境下，本文算法的规划时间和节点扩展数均是是最少的。且相比于hRRT算法，规划速度在简单威胁环境中提升36%左右，节点扩展数减少29%左右；在复杂威胁环境中，规划速度提升65%左右，扩展节点数减少50%左右。另外，初始路径经冗余点裁剪以及进一步的B样条曲线平滑处理后，路径长度得到明显缩短。
5 结论
1) 将RRT算法应用到路径规划时，针对原有算法规划速度慢、内存计算量大，对算法中随机扩展树的待扩展节点以及扩展方向的选取上作了一定改进，使得算法在具备目标启发性的同时，又能在障碍物威胁环境下具备较大随机性。
2) 本文提出的改进策略不仅可以提高算法的路径规划速度，还降低了算法的内存计算量，存在的不足是规划得到的初始路径长度较长、弯曲度较大。然而，通过对得到的初始路径进行冗余点裁剪后的路径进行B样条曲线平滑，可以有效提高路径质量。
[1] 徐晓苏,袁杰.基于改进强化学习的移动机器人路径规划方法[J].中国惯性技术学报,2019,27(03):314-320.
Xu X S,Yuan J.Path planning for mobile robot based on improved reinforcement learning algorithm[J].Journal of Chinese Inertial Technology,2019,27(03):314-320.
[2] 程凝怡,刘志乾,李昱奇.一种基于Dijkstra的多约束条件下智能飞行器航迹规划算法[J].西北工业大学学报,2020,38(06):1284-1290.
Cheng N Y,Liu Z Q,Li X Q,et al.Path planning algorithm of dijkstra-based intelligent aircraft under multiple constraints[J].Journal of Northwestern Polytechnical University,2020,38(06):1284-1290.
[3] Sun J,Tang J,Lao S.Collision avoidance for cooperative UAVs with optimized artificial potential field algorithm[J].IEEE Access,2017(05):18382-18390.
[4] Wu X X,Wei G L,Song Y,et al.Improved ACO-based path planning with rollback and death strategies[J].Systems Science & Control Engineering,2018,6(01):102-107.
[5] 谢凯利,杨海涛,谢海平.智能航迹规划算法研究现状与展望[J].兵器装备工程学报，2020,41(12):8-13,34.
Xie K L,Yang H T,Xie H P.Research status of intelligent track planning algorithm[J].Journal of Ordnance Equipment Engineering,2020,41(12):8-13,34.
[6] Yan S K,Pan F.Research on route planning of AUV based on genetic algorithms[C]//IEEE International Conference on Un-manned Systems and Artificial Intelligence (ICUSAI),2019:184-187.
[7] Lim H S,Fan S S,Chin C K H,et al.Constrained path planning of autonomous underwater vehicle using selectively-hybridized particle swarm optimization algorithms[J].IFAC-PapersOnLine,2019,52(21):311-322.
[8] LaValle S M.Rapidly-exploring random trees:A new tool for path planning[R].Computer Science Department,Iowa State University,1998.
[9] 陈秋莲,蒋环宇,郑以君.机器人路径规划的快速扩展随机树算法综述[J].计算机工程与应用,2019,55(16):10-17.
Chen Q L,Jiang H Y,Zheng Y J.Summary of rapidly-exploring random tree algorithm in robot path planning[J].Computer Engineering and Applications,2019,55(16):10-17.
[10] 阮晓钢,周静,张晶晶,等.基于子目标搜索的机器人目标导向RRT路径规划算法[J].控制与决策,2020,35(10):2543-2548.
Ruan X G,Zhou J,Zhang J J,et al.Robot goal guide RRT path planning based on sub-target search[J].Control and Decision,2020,35(10):2543-2548.
[11] 张伟民,付仕雄.基于改进RRT*算法的移动机器人路径规划[J].华中科技大学学报(自然科学版),2021,49(01):31-36.
Zhang W M,Fu S X.Mobile robot path planning based on improved RRT* algorithm[J].Journal of Huazhong University of Science and Technology(Nature Science Edition),2021,49(01):31-36.
[12] Kuffner J J,LaValle S M.RRT-connect:An efficient approach to single-query path planning[C]//IEEE International Conference on Robotics and Automation，2000:995-1001.
[13] 尹高扬,周绍磊.基于改进RRT算法的无人机航迹规划[J].电子学报,2017，45(07):1764-1769.
Yin G Y,Zhou S L.An improved RRT algorithm for UAV path planning[J].Acta Electronica Sinica,2017,45(07):1764-1769.
[14] Urmson C,Simmons R.Approaches for heuristically biasing RRT growth[C]//2003 IEEE/RSJ International Conference on Intelligent Robots & Systems,2003:1178-1183.
[15] 王海芳,张瑶,朱亚锟,等.基于改进双向RRT*的移动机器人路径规划算法[J].东北大学学报(自然科学版),2021,42(08):1065-1070,1142.
Wang H F,Zhang Y,Zhu Y K,et al.Mobile robot path planning based on improved bidirectional RRT*[J].Journal of Northeastern University(Natural Science),2021,42(08):1065-1070,1142.