复杂多障碍物环境下改进的RRT*路径规划算法

余艳碧

（重庆师范大学数学科学学院，重庆 401331）

0 引言

近几年来，路径规划算法在移动机器人领域得到广泛的研究，大量经典的路径规划算法被国内外学者提出。包括基于构建虚拟势场的人工势场法［1］，基于计算智能的蚁群算法［2］、遗传算法［3］、基于图搜索的A*算法［4］，等。然而，上述算法在解决复杂多障碍环境下或高维状态空间下的路径规划问题时，搜索效率都明显降低。

基于采样的路径规划算法被提出，目的是提高算法在复杂多障碍物环境或高维状态空间下搜索能力。快速扩展随机树（RRT）［5］是当前应用最广泛的基于采样的算法，作为概率完备的快速搜索算法，RRT可以弥补上述算法搜索速度慢等缺陷。然而，由于传统的RRT算法是均匀采样的，引导信息不足，从而算法在复杂多障碍物环境下搜索的路径复杂曲折，且耗费时间较长［6］。为了解决基本RRT算法的缺陷，国内外学者提出了许多改进算法。LaValle和Kuffner通过从起始点和目标点两个方向同时向对方扩展形成两棵随机树，提出了Bi-RRT算法［5］，之后在此基础上增加贪婪策略，提出了RRT-connect算法［7］，提高了节点扩展效率。

由于RRT的最大缺陷是没有考虑可行解的成本，没办法搜索出最优路径。Karaman和Frazoli在2011年提出了渐进最优的RRT*算法［8］，该算法在RRT扩展树节点的基础上根据最优标准来调整随机树中的节点。虽然确保了近似最优性，但是该算法仍需对所有状态空间进行采样来搜索路径，增加了不必要的内存和计算。为了提高RRT*的搜索效率，可以通过搜索一条从起始点到目标点的初始引导路径来确定采样区域，比如从智能采样和路径优化两方面改进RRT*的RRT*-smart算法［9］和借鉴RRT-Connect双向扩展树优势的B-RRT*算法［10］，以及Gammell等提出的Informed-RRT*算法［11］，该算法利用RRT*找到初始引导路径，然后生成由起始点、目标点和路径长度决定的椭圆作为采样区域，从而加速收敛以获得最优路径。然而，在复杂的多障碍环境中，Informed-RRT*算法在搜索初始引导路径时容易因为过多的节点导致占用大量内存，搜索效率较低。

综合已有研究成果和存在的问题，本文提出了一种针对复杂多障碍物环境下基于A*引导域的改进RRT*路径规划算法。该算法将A*算法的最优性和RRT*算法快速性结合起来，首先利用A*算法在栅格化后的低分辨率地图中搜索出最优路径，并将该路径作为引导路径生成引导域，限制RRT*算法的采样区域，从而提高其采样效率，然后在引导域中不断迭代搜索，得到一条渐进最优且无碰撞的路径，最后基于B样条曲线的路径优化，使移动机器人能够平稳安全到达终点。通过对RRT*算法与Informed-RRT*算法的仿真比较实验，验证本文算法的有效性和优越性。

1 RRT*算法基本原理

1998年，LaValle教授根据最优控制、随机采样算法等理论提出了一种随机采样增量式运动规划算法，即RRT算法［5］。该算法是从起始点开始在状态空间中随机采样向目标点扩展的一个树状结构，直至找到一条连接起点与终点的路径。RRT算法的搜索方法避免了对整个环境地图的模型构建，加快了算法的收敛，在全局路径规划中有广泛的应用和改进，其算法的有效性和完备性已经得到了充分的验证，但算法仍然存在搜索效率低，路径不优，占用内存过大等问题。

Karaman和Frazzoli于2011年提出渐进最优性（最短距离）的RRT*算法［8］，该算法基本结构和RRT算法相似，都将初始状态作为根节点增量式地扩展随机树，在避开障碍物的同时，当树的节点到达预设目标状态附近区域则结束对状态空间的搜索。随机树T最初只有一个根节点qinit［图1（a）中节点0］，每次迭代都会在状态空间中随机采样一个点的qrand，并计算随机树T中距离qrand最近的节点qnear［图1（a）中节点5］，如果从最近邻节点qnear向qrand扩展一定预设步长移动可达即无碰撞，那么这个可达的点记为新节点qnew［图1（a）中节点9］，并将qnew加入随机树T中，此时qnear成为qnew的父节点，RRT算法便是重复上述过程最终规划出路径。但RRT*算法在这里会启动重选父节点过程，即qnear不一定是最后的父节点。当确定qnew后，根据近邻准则找出qnew周围一组近邻节点［图1（b）中节点4、5、7、8］，并计算Qnew中每一个近邻节点到根节点qinit的累积成本与该近邻节点到qnew的距离成本的和，取这些和的最小值所对应的近邻节点作为qnew的新父节点q′near［例如图1（a）中，qnew经过节点5索引到qinit的路径为9-5-2-0，累积成本为9；qnew经过节点7索引至qinit的路径为9-7-2-0，累积成本为7，代价最小，故节点7为qnew新的父节点q′near，如图1（b）所示］，并以q′near作为父节点将qnew加入随机树T中。接下来优化Qnew中每一个近邻节点，具体是让qnew代替每一个近邻点的父节点，如果此时近邻点到根节点qinit的累积成本比qnew代替前更小，则放弃原先的父节点而把qnew作为该点的父节点重新连接到近邻节点上［如图1（c）中，节点8以qnew为父节点时索引至qinit的路径为8-9-7-2-0，累积成本为8，而不以qnew为父节点时索引至qinit的路径为8-5-2-0，累积成本为10，故修剪随机树T，将节点8的父节点改为qnew，如图1（d）所示］，从而对随机树T实现逐步优化。

图1 RRT*算法修剪节点示意图

2 A*算法

2.1 A*算法基本原理

A*算法是一种基于深度优先算法和广度优先算法的启发式融合搜索算法，基于深度优先算法能以最快速度找到一条从起始点到目标点的路径，但不能保证最优性，基于广度优先算法则必然能找到最短路径，但需要遍历所有结点，计算复杂。而A*算法融合了这两种算法的优点，基于启发函数构建了代价函数，考虑了新结点距离初始点和目标点的代价［12］。

2.2 低分辨率环境地图下的A*算法

A*算法是在栅格化后的环境地图中搜索，搜索过程中将每一个栅格作为一个状态，当栅格尺寸越大，即地图分辨率越低，则环境地图分解成的状态数就越少，使得当起始点和目标点坐标不变时，A*算法执行的时间会变短，规划出来的路径也会发生相应变化［6］。图2是大小为128×128的环境地图，起始点坐标（11，11），目标点坐标（100，100）。对于图中复杂的障碍物环境，本文分别进行128×128，64×64和43×43的栅格化，A*算法在三种不同分辨率下的规划结果如图2（a）、（b）、（c）所示。

图2 三种分辨率下A*算法规划结果

在三种不同分辨率的环境地图下，A*算法规划时间分别为477 ms、64 ms和23 ms，由此可以看出栅格地图分辨率越小，A*算法的规划时间大幅减少，更重要的是，A*算法在不同分辨率下规划出来的路径趋势是一致的。

3 基于A*引导域的RRT＊算法

3.1 A*引导域

记A*算法规划路径经过的第i个栅格中心点坐标为A i（x i,y i），则引导域上界点集定义为：

下界点集定义为：

k为单个引导路径点横纵坐标移动长度，再将上界点集和下界点集分别按顺序连接成一条线得到上界和下界，最后连接上下界形成封闭引导域。图3为引导域示意图，蓝色线条为A*算法规划出的引导路径，红色线条内的区域为生成的引导域。

图3 A*算法生成的引导域

3.2 采样策略

由于RRT*算法采样范围大且随机性强，缺乏一定的引导性，因此本文在3.1节引导域的基础上，采用目标偏向策略，以一定的概率p选择目标点作为采样点进行随机树扩展，提高随机树向目标点扩展的概率，以达到减少采样点的个数，加速随机树的形成。

在采样过程中，为了判断采样点是否在引导域外，即判断采样点qnew和其父节点qnear连线与引导域边界是否发生碰撞，需进行碰撞检测。如果发生碰撞，则放弃此次采样重新选择采样点。如图4，检测是否碰撞，即判断线段qnearqnew和线段p1p2是否相交可以利用向量的叉乘。先固定线段qnearqnew，然后以qnear为轴，计算与是否异号；再固定线段p1p2，然后以p1为轴，计算是否异号。当上述的叉乘都异号的时候，说明两条线段相交，发生碰撞，则重新选择采样点。

图4 碰撞检测

3.3 基于B样条曲线的路径优化

由于RRT*算法随机采样的特性，导致算法最终搜索出来的路径一般是曲折、不平滑的，不利于移动机器人进行路径追踪。而B样条曲线具有连续性、局部可调整性等优点，使得该曲线在路径规划中被广泛的使用［13］，因此本文提出了基于三次B样条曲线的路径优化处理，与算法相结合生成一条平稳光滑的路径，确保移动机器人运动的连续性和平稳性。

Riesenfeld等人在1972年构造了B样条曲线，将Bernstein基函数用B样条基函数来代替。B样条曲线是分段组成的，每一段的参数区间为[0,1]，因此克服了贝塞尔曲线改变任意控制点其曲线上所有点也改变的缺陷，使得样条曲线具有局部修改的特性［14］，这种优点使得B样条曲线广泛应用于路径规划中。

设有n+1个控制点P0,P1,…,P n，则k阶B样条曲线的数学表达式为：

其中，B i,k（u）是第i个k阶B样条基函数，u是自变量。

基函数具有如下Cox-deBoor递推式

式（4）和式（5）中u i是一组被称为节点矢量的非递减序列的连续变化值。

4 仿真实验与结果分析

为验证本文提出的复杂多障碍物环境下基于A*引导域的RRT*算法搜索路径的有效性和快速性，本文在复杂环境地图下对该算法的规划结果与RRT*、Informed-RRT*两种算法的规划结果进行仿真对比，最后给出所有算法仿真实验的结果与分析。

本文将仿真地图大小设置为128×128，起始点坐标（11，11），目标点坐标（100，100），障碍物由多个矩形和若干线条组成，分别对本文算法，RRT*算法和Informed-RRT*算法进行10次仿真，最大迭代次数为1000次，扩展步长为5，目标采样率为10%，并将三种算法找到初始路径时的初始路径平均长度和初始平均搜索时间以及到达最大迭代次数后的最终路径平均长度进行对比分析。图5为三种算法最终规划出的路径示意图，其中红色曲线为搜索的最终路径。

图5 三种算法的最终规划结果

表1为仿真实验数据对比。从表中可以看出，本文算法搜索出的初始路径平均长度和初始平均搜索时间比另外两种算法的都要少，与Informed-RRT*算法的初始路径长度和初始搜索时间对比分别减少了5.4%和46.01%，搜索时间大幅下降，这说明本文算法的收敛速度更快。而从达到最大迭代次数后后的最终路径平均长度上来看，本文算法也是优于另外两种算法的，并且对比本文算法的初始路径长度和最终路径长度，在大部分情况下，本文算法搜索出的初始路径已经趋于最优。以此证明了本文算法的有效性和优越性。

表1 仿真实验数据对比

5 结语

针对RRT*算法和其一些改进算法存在的搜索效率低、占用内存过多等不足，本文提出了一种复杂多障碍物环境下基于A*引导域的改进RRT*路径规划算法。该算法有效地将A*算法的最优性和RRT*算法快速性结合起来，第一步利用A*算法在栅格化后的低分辨率仿真地图中搜索出最优路径，并将该路径作为引导路径生成引导域，限制RRT*算法的采样区域，从而提高其采样效率，然后在引导域中不断迭代搜索，得到从起始点到目标点的渐进最优且无碰撞的路径，最后基于B样条曲线的路径优化，生成一条平滑且曲率连续的优化路径，使移动机器人能够平稳安全到达终点。最后，通过对RRT*算法与Informed-RRT*算法的仿真比较实验，验证本文算法的有效性和优越性。