一种双向目标RRT路径规划算法研究

符 强，蓝星辉，任风华，伍建辉

(桂林电子科技大学，广西 桂林 541004)

随着自动驾驶技术的高速发展，无人机和机器人等智能移动设备的应用越来越广泛，此类智能设备自主探索未知环境的技术正在不断扩展到越来越多的应用领域。进行自主探索的首要任务就是进行路径规划，路径规划的目标是在有大量障碍物的复杂地理环境中，以最快的速度规划出一条连接起点位置和目标点位置的路径，并且该路径能够很好的满足移动设备的运动学约束，使得移动设备能够有效的规避障碍物进行自主探索。目前许多学者们热衷于研究的路径规划算法主要A*算法[1]、D*算法[2]、人工势场法[3]等启发式搜索算法，此类算法在复杂的环境信息下路径规划的效率较高，但是规划出来的路径效果并不理想，路径不能够很好的满足设备的运动学约束，不利于智能设备的行驶；
蚁群算法[4]、遗传算法[5]等智能仿生算法，在搜索环境大且复杂的情况下能够较好的规划出合适的路径，但由于需要不断的迭代计算，时效性不佳；
迪杰斯特拉(Dijkstra)[6]算法能够很好的规划出一条最短路径，但搜索效率会随着地图环境的增大而有所降低。

快速搜索随机树算法(RRT)是一种传统的路径规划算法，它是基于一种树形结构的典型算法，许多学者将其用于路径规划方面。RRT算法具有灵活强大的搜索能力，能够用于许多复杂大环境下的路径规划，但RRT算法也存在一些缺陷，比如采样效率较低、规划所得的最终路径并不是最优、路径存在很多锯齿状的折线路径等问题。针对RRT算法的不足，不少学者也提出了不同的方法对其进行改进。文献[7]提出了基于改进RRT算法的三维路径规划算法，该算法基于重选原有父节点的方式，使得规划的路径能够有一定的缩短，并且引入欧几里得最短距离的思想和启发式搜索函数，提高了算法的效率，但规划的路径曲折；
文献[8]提出了RRT-Connect算法，该算法引入自适应步长调整策略，根据障碍物的密集程度动态调整随机树的探测速度，从而提高搜索效率。文献[9]提出了基于双采样点的B-RRT路径规划算法，该算法改变原有RRT算法单向采样的策略，采用双向采样点的方法提高其搜索速度，并选取各自目标树的节点作为目标点，提高搜索效率，但规划的路径不是最优。文献[10]提出一种专门针对复杂环境下的IB-RRT算法，该算法引入智能样本插入启发式搜索策略提高其搜索速度。文献[11]提出一种改进的B-RRT算法，该算法采用预生长机制提高路径规划的避障效率，但在复杂环境下计算的节点量较大。

上述方法的研究都已经取得一定的成果，但双向RRT算法存在的采样点随机性大、路径曲折、路径过长等问题依然存在，基于上述问题，本文针对原有的B-RRT算法进行改进，提出了融合Dijkstra算法的双向目标RRT算法(goal fusion Dijkstra bi-directional RRT，GDB-RRT)。首先，提出了一种基于目标约束采样和目标偏置扩展的策略改进原始的双向RRT算法，提高路径规划的效率；
其次，融合Dijkstra算法来寻找双向RRT树中完整路径节点之间的最短路径，由于Dijkstra算法只需要对双向目标RRT算法搜索得到的整条路径的节点进行运算，所以能够极大的减小其运算的节点数量，因此运算的效率有很大的提升；
最后采用B样条算法对最短路径进行平滑处理，使得曲线光滑连续且无过大折角，以保证智能移动设备能够按照规划路径前行。

双向快速搜索随机树(B-RRT)算法的原理是把起始点和目标点均作为随机树的根节点，分别从两个指定的根节点开始，同时产生2棵各自的随机树Ta、Tb。两棵随机树分别朝着相向的目标树节点进行搜索，每轮通过2次随机采样，每轮产生2个随机采样点，分别进行比较2个候选采样点与各自目标随机树上最近节点的距离，并且判断新生成的随机采样点与最近的节点之间是否存在障碍物，最终选取距离较小的、并且路径之间无障碍物碰撞的点作为最终可行采样点，从而在两棵随机树上分别生成新的枝叶。

双采样点示意图如图1所示，起点和目标点分别生成各自的随机树，两棵随机树相向搜索，同时两棵随机树分别生成2个候选采样点qrand1，qrand2，判断随机点和新的节点qnew之间是否有障碍物存在，如果没有障碍物存在则新的节点添加成功，将qrand更新为qnew，原有的qnew更新为qnear，每次更新之后都会进行一次判断，判断一下起点树的节点坐标是否有等于目标树的节点坐标，即nodestart=nodegoal，当二者节点坐标存在相等情况说明已经搜索到一条完整的路径，立即退出当前搜索。此传统的B-RRT算法相比于原有的单向RRT效率有所提高，但是此搜索策略仍未摆脱原有算法的随机采样，即传统B-RRT算法随机采样随机性太大，不具有目标导向性，存在一些不必要的随机采样点，在原有B-RRT算法基础上还有待改进。

算法的一般步骤如下：

图1 B-RRT算法原理图

1)首先通过MapDate()函数来建立环境数据模型，初始化各项参数并启动秒表计时器；

2)通过while循环来判断两棵随机树的节点坐标是否存在契合点，若不存在，则进入循环，两棵随机树继续相向生长，直到搜索到一条完整的路径，则退出while循环；

3)最后通过ConnectAllPath ()函数获取整条完整路径。

原算法主体程序如下：

RRT＿pathplanning()

1) MapDate()

2) Tree(start，goal)

3) While (pathfound())

4) NODESrand ←SampleRand()

5) NODESstart←Extend(Ta，NODESrand)

6) NODESstart←Extend(Tb，NODESrand)

7) Tree←Add＿tree()

8) endlwhile

9) ConnectAllPath()

3.1 路径规划问题的特征描述

在大量的复杂地形环境中进行路径规划时，使用原有的B-RRT算法进行搜索时存在采样点随机性大、路径过于冗长、最终规划所得路径呈折线折形的问题。由于采样点的生成随机性较大，导致随机树在搜索空间中有过多不必要的搜索，降低了搜索效率；
智能移动设备的能源消耗也是有限的，规划的路径应最大程度达到最短，以减少移动设备的能源消耗；
规划的路径呈现出小范围内的折角转弯路径，根据无人机和机器人等智能移动终端的运动学约束，智能移动设备不能够很好的按照规划的路径前行。基于上述问题以及相关算法的研究，本文提出了一种目标偏置及约束采样的双向RRT算法，具体思路为：为提高采样的有效性，在采样的过程中以合适的偏置概率来平衡目标采样和随机采样的权重比，从而提高随机采样的有效性；
为了提高目标采样的目标导向性，在每生成一个新节点的同时由目标点和采样点共同决定，从而提高采样的目标导向性，以提高搜索的效率。

3.2 目标约束采样

本文为提高随机采样点的目标导向性，结合目标偏置和位置约束两个策略，同时借鉴启发式搜索[12]和人工势场法[13]的思想以提高采样点的目标导向性。在算法中先设置两个目标偏置概率，其中一个为起点随机树向目标点随机树生长的目标偏置概率Pstart-bias，另一个为目标点随机树向起点随机树生长的偏置概率Pgoal-bias。当两个目标偏置概率设定好后，双方的随机树开始朝着各自的目标树进行搜索，然后双方按照均匀概率随机产生一个起点树的概率值和目标点树的概率值分别为Prand-start、Prand-goal。从起点随机树端朝着目标点随机树端开始进行搜索，若起点树的随机概率值Prand-start小于对应的起点随机树的目标偏置概率值Pstart-goal，则将对方目标点随机树的节点坐标Nodesgoal作为随机采样点Nodesrand。如果条件不满足，就在自由空间中随机产生一个采样点，起点树的目标偏置采样公式如式(1)所示

(1)

从目标点随机树端朝着起点随机树端开始搜索，若该目标点随机树的随机概率Prand-goal小于对应的目标点随机树的偏置概率Pgoal-bias，则将起始点随机树的节点坐标Nodesstart作为采样点Nodesrand。如果条件不满足，就在自由空间中随机产生一个采样点。目标点树的目标偏置采样公式如式(2)所示

(2)

两式中RandSimple()为随机采样函数。

采样点的生成是随机的，在进行路径搜索的过程中需要对生成的随机点进行筛选，即需对生成的随机采样点进行位置约束，去掉一些不满足条件的随机点。约束思想是每随机生成一组随机采样点，即对其各随机点的位置进行判断。判断该采样点在x方向或者在y方向上比前一个采样点更加接近目标树的节点，若满足上述条件则选取该组采样点中最靠近目标树的节点作为新节点，否则继续循环采样，直到满足上述条件为止。将两棵随机树的起点分别设置为第一个参考采样点，之后不断更新，约束采样的示意图如图2所示。

图2 采样约束示意图

3.3 目标偏置扩展

本文为使得随机点不断朝着目标树节点的方向扩展，借鉴粒子群算法[14]的思想，通过给采样点方向和目标树节点方向分配不同的权重，在采样点和目标树节点之间生成一个新的节点，该节点按照一定的权重偏向于目标点，使得新点的扩展方向不再单纯由随机采样点决定，而是由采样点和各自目标树的节点来共同决定。通过设置合适的权重就能够使得新的随机点不断朝着目标点的方向扩展，使得每一次扩展都能够有效的接近目标点，从而加快路径的搜索速度，目标偏置扩展的示意图如图3所示。

新节点偏向目标的生成公式为：

图3 目标偏置扩展示意图

Nodesnew=Nodesnearest+xstep·vj·Ngoal+(1-vj)·Nrand

(3)

式中：xstep为扩展步长，vj为目标点方向权重，取值范围为[0，1]；
Ngoal为目标点方向单位矢量，Nrand为采样点方向单位矢量。

当目标点方向权重为1时，采样点径直的朝着目标随机树的方向扩散，当目标点方向权重为0时，采样点不具有目标导向性。

双向目标RRT算法结合Dijkstra策略是在双向目标RRT算法搜索到完整路径后进行最短路径优化的一个处理过程。在双向目标RRT搜索完成后，将多余的不必要枝叶节点进行修剪，保留整条完整的主干路径节点，通过Dijkstra算法对整条完整路径中Nodes节点坐标进行迭代运算找到一条最短路径。

4.1 Dijkstra算法原理

Dijkstra算法是经典的最短路径搜索算法。它的主要的思路是，从起点坐标开始通过不同的路径逐渐地向外层扩展搜索，搜索到终点坐标则停止搜索，最后得到一条最短路径。Dijkstra算法求解最短路径问题的基本步骤如下：

1)首先设立一个节点数组PathFound，通过该数组保存所有待访问的节点坐标，其次设立另一个节点数组PathThrough记录已经访问过的所有节点坐标的集合，数组PathFound初始化为只有起始点节点坐标，数组PathThrough初始化为空集合。

2)从节点数组PathFound中的起点坐标开始，寻找出距离起始节点最近的节点坐标，将寻找到的节点坐标放入数组PathThrough中进行存储，两节点之间的距离求解公式为：

(4)

3)将起点到所有可行路径的节点距离进行迭代运算，寻找出起点到路径中各个节点的最短路径，这一步操作称为松弛操作，同时把这些最短路径的相邻节点坐标加入集合PathFound中等待下一次的运算。

4)判断节点数组PathThrough中的所有节点坐标是否已经全部访问，如果没有则返回步骤2)直到PathThrough数组中的点坐标均已经访问完成，此时，节点数组PathThrough中包含了网络中从起始节点出发可以到达的所有节点，通过上一个步骤的松弛操作可以最终寻得一条最短路径。

4.2 松弛操作

松弛操作属于一种遍历的方法，其主要思想是在所有的可行的节点空间树中，从起点出发探索整个空间树的节点，比较相邻节点的路径消耗来确定其子节点，如此不断探索下去，直到找到最终节点。具体算法的实现流程如下图所示。

图4 松弛操作流程图

图5中的模型表示的是从起点到路径中其余各个可行节点的最短路径，起始节点为1号点，目标节点为6号点，现在需要求的是从起始节点到目标节点的最短路径；
首先将模型中的顶点和边长的数值在二维数组N中进行存储，初始值如表1所示。

图5 Dijkstra算法路径搜索模型

表1 二维数组N存储的节点之间的距离关系

还需要用一个一维数组dis来存储1号顶点到其余各个顶点的初始路程，可以称dis数组为“距离表”，如下：

123456dis0112∞∞∞

此时的dis数组中的值称为最短路径的估计值。

要得到从随机树1号起点到随机树其余各可行路径节点的最短路程，就得通过松弛得方式进行遍历计算，通过遍历计算，最终得到一条到达终点的最短路径，下面就从1号点开始模拟一下松弛的过程：首先从根节点开始。找到一个距离1号起点最近的可行节点，通过观察数组dis可知当前离随机树根节点的1号节点最近的是2号节点，当选择了2号节点后，dis[2]的距离值就已经从估计值变为了一个确定值，即随机树起点1号节点到2号节点的最短路程就是当前的dis[2]值。当2号点已经确定，接下来寻找2号顶点的可行的路径，从模型图中可以看出有2→3和2→4这两可行路径。首先讨论通过2→3这条可行路径，其路径长度为N[2][3]=9，然而到随机树节点3的路径还有1→3这可行路径，经运算其路径长度dis[3]=12，接下来比对比两种行走方式那条路径更短，通过对比发现dis[3]>dis[2]+N[2][3]，因此将dis[3]更新为10，并确定其路径为1→2→3这样的一个过程称之为松弛。类似的讨论2→4可知dis[4]>dis[2]+N[2][4]，因此dis[4]更新为4。以上就是对2号顶点所有的可行路径进行了松弛，松弛完毕之后的dis数组为

123456dis0110 4∞∞

完整的松弛过程如下：

123456dis0112∞ ∞∞↓123456dis0110 4∞∞↓123456dis01 8 41719↓123456dis01 8 41319↓123456dis01 8 41317↓123456dis01 8 41317

本文为获得最短路径，充分利用Dijkstra算法搜索最短路径的优势，将双向目标RRT算法搜索得到的完整路径的节点进行多路径连接操作，即将相间隔的可行节点进行连接，并且连接路径中无障碍物产生碰撞，由此产生多路径通道。当多路径通道模型建立，采用 Dijkstra算法进行松弛运算，得到一条最短路径。

1946年Schoenberg首次提出样条理论，1972年Gordon等从外形设计的需求出发，对Bezier曲线进行发展，提出B样条曲线[15]。B样条曲线是一种变化灵活的曲线，曲线的局部形状受相应顶点的控制，如果在曲线的转折点一定范围内选择合适的控制顶点，可以在该段折线范围内得到满足一定要求的光滑曲线，其数学表达式为

(5)

式中，0≤t≤1；
i=1，2，…，m，分别表示第i段B样条曲线；
n表示样条曲线为n次参数曲线；
Pi+k表示第i段B样条曲线的第k个控制点，Fk，n为n次B样条的基函数，其表达式为

(6)

式中，0≤μ≤1，k=0，1，2…，n。

由式(7)可知，当曲线的阶次确定下来后，在本文中的阶次采用3阶曲线的基函数也随之得到确定，可通过改变曲线路径中转折点附近控制点的选取来控制曲线的形状。因此采用B样条理论通过改变折线路径中转折点附近的采样点的选取，进而通过这些控制点将转折点两端的线段进行平滑处理，最终得到平滑的可飞行路径。

为验证路径平滑后的效果，选取经过最短路径优化后的路径作比较，其对比结果如图6所，图中蓝色点为起点，红色点为终点，红色的线条为经过Dijkstra算法优化后的最短路径，绿色线条为经过3次B样条优化后的路径。从图中可以看出，最短路径在经过3次B样条处理后路径变得光滑，折角被平滑处理，整体路径的走势没有被改变。

GDB-RRT算法的详细流程图如图7所示。双向目标RRT算法能够提高随机采样点的目标导向性，扩展的路径也朝着目标点进行生长，加快搜索的速度，提高其搜索效率。融合Dijkstra算法缩短路径的长度，通过样条曲线理论改善了路径曲折的问题，最终保证路径的可行性。GDB-RRT算法的完整代码结构主要包括：目标约束采样函数、目标偏置扩展函数、路径缩短函数、平滑路径函数四部分。

图6 路径平滑前后对比

图7 GDB-RRT算法整体实现流程图

为验证GDB-RRT算法的效果，本文针对传统B-RRT算法和GDB-RRT算法进行仿真分析。选取同一尺寸不同复杂环境下的实验结果作对比。仿真平台及配置为：matlab R2020a，64位Windows10操作系统，处理Intel(R) Core(TM) i5-9500，CPU主频3GHz，内存8GB。

6.1 仿真研究

为了验证本文所提出的GDD-RRT算法具有更好的性能，将B-RRT和GDB-RRT算法在3种二维仿真环境下进行仿真对比实验，环境地图尺寸大小均为[500，500]。仿真中的所有起点均标注为红色点，坐标为(10，490)，终点均标注为蓝色点，坐标为(490，10)。由于RRT算法类均具有随机性，因此在每个环境中，对B-RRT和GDB-RRT算法分别执行100次重复实验，最后给出平滑后的可飞行路径，并统计

图8 B-RRT和双向目标RRT搜索结果对比

各个指标的平均值。图8中的(a)图(b)图分别为B-RRT和双向目标RRT的搜索结果对比，图9为双向目标RRT路径经过最短路径优化前后的对比。图10～12为不同环境下B-RRT算法和GDB-RRT算法路径规划效果对比。

图8中(a)图为原始B-RRT搜索过程，(b)图为双向目标RRT搜索过程，图中从蓝色起点出发的线条是起点随机树，从红色起点出发的线条是目标随机树，从两图的对比中可以明显看出，由于原始B-RRT采样的随机性，随机树在没有障碍物的空间随机扩展，生长较为发散。由于双向目标RRT算法引入了约束扩展和目标偏置策略，使得随机树不再单纯的朝着无碰撞空间进行扩展，而是朝着目标树有方向性的生长，能够快速的通过狭窄区域进行搜索，减少其在不必要的空间中进行探索，相比于原有B-RRT算法，双向目标RRT算法采样的效率有所提升，搜索的时间也变得更短。图9中的蓝色线条为双向目标RRT搜索得到的路径，红色线条为经过Dijkstra算法优化后得到的最短路径。从图中可以看出经过优化后的路径明显变短。

图9 最短路径优化前后对比

三种不同环境下的路径规划结果如图10～12所示，图中，蓝色实线代表初始搜索路径，红色实线代表简化缩短的路径，绿色实线代表经过平滑处理后的最优路径。图10的环境1中为存在大量不规则随机障碍物的环境，图11中的环境2为狭窄通道较多的复杂环境，图12中的环境3为非常具有挑战性的单通道迷宫环境。

综合以上三种复杂环境的规划结果可知：相比于原有的B-RRT算法，GDB-RRT算法的路径搜索得到的最优路径更短且平滑，此算法搜索得到的最优路径更加符合智能移动设备的运动平稳性要求，且为设备的运动节省更多的能源消耗。

图10 环境1规划结果

6.2 占用内存分析

对于B-RRT而言，有效节点数越多，随机节点数越少，节点的利用率就越高，算法消耗的内存就越少，因此以路径搜索后的节点利用率来衡量算法所占用内存的多少。2种算法在不同环境下路径搜索的节点平均使用情况如表2所示。

表2 不同环境下路径搜索的节点使用情况

图11 环境2规划结果

图12 环境3规划结果

由表2可知：三种环境下B-RRT算法的平均扩展节点有259个，GDB-RRT算法的平均扩展节点有137个，相比之下减少了47%的扩展节点。B-RRT算法的平均有效节点有27个，平均有效节点的利用率为10%，GDB-RRT算法的平均有效节点有32个，平均有效节点利用率为20%。综上数据，GDB-RRT的节点利用率是B-RRT节点利用率的2倍左右，节点利用率有所提高，占用的内存减少近一半。

6.3 路径规划效率分析

一般而言，算法运行的时间越短，那么路径规划的效率也就越高，因此通过算法的运行时间来衡量规划效率的高低。3种环境下算法运行的平均时间如表3所示。

表3 不同环境下算法运行时间

由表3可知：B-RRT算法在三种环境下的平均运行时间为3.672s，而GDB-RRT算法的平均运行时间为2.461s，平均运行时间缩短了33%。因此，相比B-RRT算法，GDB-RRT算法的规划效率更高，能够更快的搜索出完整的优化路径。

6.4 路径优化效果分析

在路径规划中，规划所得的路径长度也是用来衡量算法对路径优化效果的一个重要指标，其次路径平滑的程度也能够作为一项评价指标，由于平滑程度无法量化，所以只对路径长度进行量化分析，2种算法在不同环境下所规划路径的平均长度对比如表4所示。

表4 不同环境下算法规划的路径长度

由表4可知：B-RRT算法在3种环境下所得路径的平均长度为1066m，而GDB-RRT算法所得路径的平均长度为981m，相比较之下GDB-RRT的算法比原算法规划的路径缩短了85m，平均缩短7.9%。同时，结合图7～9可知：GDB-RRT算法得到的路径总体上比B-RRT更加平滑。综上数据对比可知GDB-RRT算法的路径规划效率有所提高，最终得到的路径更短而且更加平滑。

本文提出的GDB-RRT算法的路径规划算法在以下三方面有所提高：首先采用目标采样和目标偏置策略使得采样点具有目标导向性，提高采样点的利用率，使得搜索效率有所提高；
其次融合Dijkstra算法使得规划的路径变得更短，减少移动设备的能源消耗；
最后采用样条优化策略对算法得到的最短路径进行优化，使得路径变得更加平滑。

通过在三种不同环境下与B-RRT算法的对比仿真，结果表明GDB-RRT算法在节点利用率方面提高了1倍，扩展节点减少了49%，内存占用更少，搜索时间缩短33%，路径缩短7.9%，最终的路径也更加平滑，优化效果更好。证明本文所提算法具有一定的优越性和有效性。

猜你喜欢样条偏置起点基于40%正面偏置碰撞的某车型仿真及结构优化汽车实用技术(2022年15期)2022-08-19一元五次B样条拟插值研究安徽师范大学学报（自然科学版）(2022年3期)2022-07-14基于双向线性插值的车道辅助系统障碍避让研究中国信息化(2022年5期)2022-06-13弄清楚“起点”前面有多少小学生学习指导(低年级)(2018年10期)2018-10-13起点音乐天地(音乐创作版)(2018年2期)2018-05-21三次参数样条在机床高速高精加工中的应用制造技术与机床(2017年7期)2018-01-19三次样条和二次删除相辅助的WASD神经网络与日本人口预测软件(2017年6期)2017-09-23我的“新”起点小学生作文(中高年级适用)(2017年3期)2017-07-07基于样条函数的高精度电子秤设计计算机测量与控制(2017年6期)2017-07-01一级旋流偏置对双旋流杯下游流场的影响北京航空航天大学学报(2016年6期)2016-11-16

推荐访问:双向 算法 路径