基于双向随机树改进的智能车辆路径规划研究*

施杨洋,杨家富,梅 淼,朱林峰,2

(1.南京林业大学机械电子工程学院，江苏 南京 210037；2.北方信息控制研究院集团有限公司，江苏 南京 211106)

1 引言

车辆的路径规划[1 - 4]指的是已知车辆的起始点位置、目标点位置和环境中的障碍物分布，按照一定的标准，规划出一条与障碍物不相碰撞的路径。近年来新的路径规划算法不断涌现，在路径规划领域，最具有代表性和最常见的路径规划算法主要有基于地图的路径规划算法、基于仿生学的路径规划算法以及基于采样的路径规划算法。

基于采样的路径规划算法有概率图算法和快速扩展随机树算法。快速扩展随机树算法是由LaValle等[5,6]提出的一种路径规划算法。其优点在于不需要对规划的空间进行建模，是一种随机采样的算法，同时考虑了无人车客观存在的约束，因此得到了广泛应用[7]。基本的快速扩展随机树算法在进行路径规划时存在以下缺点：(1)随机生成路径，路径具有偏差性；(2)随机树在搜索过程中无导向性；(3)收敛速度迟缓，搜索效率低。

针对基本快速扩展随机树算法的不足，国内外研究者进行了大量的改进研究。偏向搜索和双向扩展的快速扩展随机树算法提高了算法的收敛速度和搜索效率[8,9]，但是并未克服随机树生成节点时的随机性；动态步长的快速扩展随机树算法[10]改善了算法的不确定性，提高了避障能力，但针对不同的障碍物步长无法确定；快速扩展随机树算法中引入了启发式函数[11]，使得搜索树在搜索过程中更具有导向性，但该算法在进行路径规划时容易陷入死循环。

本文基于快速扩展随机树提出了一种改进的智能车辆路径规划算法，采用循环交替迭代搜索方式生成新节点，双向随机树同时扩展，增加车辆的转弯角度约束，对生成的节点进行优化，对路径进行平滑处理，改善了快速扩展随机树算法在路径规划时的不足。

2 基本快速扩展随机树算法

基本快速扩展随机树算法的扩展示意图如图1所示。以初始点xinit作为随机树的根节点，搜索自由空间，选取随机采样点xrand，在已知的随机树上，搜索选择一个距离随机采样点xrand最近的节点xnearest，连接节点xnearest和节点xrand，以一定的步长ρ从xnearest出发生成一个新节点xnew，如果从xnearest到xnew的扩展过程中与障碍物无碰撞发生，则将这个新节点xnew加入到随机树中，生成一个随机树，当随机树中的子节点包含目标点xgoal时，便可以在随机树中生成一条由初始点xinit到目标点xgoal的路径。反之若发生碰撞，则舍弃这次扩展。

Figure 1 Expansion of rapidly-exploring random tree algorithm图1 基本快速扩展随机树算法的扩展

3 车辆转向模型

智能车辆在行驶的过程中，不仅要保证车辆的行驶安全，还要保证车上乘客的乘坐舒适性，因此智能车辆在进行路径规划时，既要保证车辆规避所有的障碍物，又要保证规划路径的平滑性。

根据牛顿第二定律以及转向的几何关系可以推导出智能车辆进行稳态转向的方程。为了便于分析智能车辆的转向状态，采用如图2所示的两轮模型。

Figure 2 Two-wheel steering model图2 两轮转向模型

图2中δ为前轮轮向角度，当前轮发生转向时，车身产生离心力，在前后轮上产生的侧向反作用力为Fy1和Fy2，并产生相应的侧偏角αf和αr。设车辆的质心和转动的瞬心分别为O和P，则2点间的距离R即为转弯半径。r为横摆角速度，则质心处的速度为vc=rR。β为质心处的侧偏角，即质心处车辆行驶方向与X轴之间的夹角。lf为前轮到质心O的距离，lr为后轮到质心O的距离，l为前轮到后轮的距离。前轮速度为vf，后轮速度为vr。

假定汽车只作平面运动，无垂直方向以及绕X轴和Y轴的侧倾运动和俯仰运动。汽车速度为一个定值，并忽略空气阻力和切向力对车辆的影响。以前轮转角作为唯一输入，忽略转向系统对车辆的影响，不考虑左车轮由于载荷变化引起轮胎特性变化和回正力矩的作用，则vc在X轴上的分量为：

u=vccosβ

(1)

车辆在转弯过程中的β很小，得到cosβ≈1,因此：

u=vc=rR

(2)

vc在Y轴上的分量为：

v=vcsinβ

(3)

由此得到质心的加速度ac为：

(4)

从力和力矩平衡方程式可导出微分运动方程式为：

(5)

(6)

其中，m为整车质量；Iz为车身绕Z轴的转动惯量。

前后轮的侧偏力分别为：

Fy1=Cfαf

(7)

Fy2=Crαr

(8)

其中，Cf、Cr分别为前后轮胎侧偏刚度；αf、αr分别为前后轮侧偏角。

由几何关系可求得：

(9)

(10)

代入式(5)和式(6)可得：

(11)

(12)

由式(3)得到β≈sinβ=v/vc,代入式(11)和式(12)整理得到：

(13)

(14)

(15)

(16)

由式(15)和式(16)联立并消去v，可得：

(17)

由式(17)可得：

(18)

稳定性因素K是影响转向稳定性能的重要指标。

4 改进快速扩展随机树算法

4.1 循环交替迭代搜索

快速扩展随机树算法采用基于采样的搜索方式，具有很强的随机性。针对基本快速扩展随机树算法搜索效率低，搜索过程无导向性，收敛速度迟缓等缺点，本文对基本快速扩展随机树算法进行优化。

在生成新节点xnew时采用循环交替迭代的搜索方式：(1)快速扩展随机树在扩展过程中首先采用随机采样方式生成随机采样点xrand，将该随机采样点xrand作为随机树子节点生成的生长目标点；(2)接着直接将目标点xgoal作为随机树子节点的生长目标点。2种搜索方式循环交替迭代搜索，直到搜索到可行路径，或者达到设定的迭代次数阈值，退出搜索，搜索路径失败。改进快速扩展随机树算法的流程图如图3所示。

Figure 3 Flow chart of improved rapidly-exploring random tree algorithm图3 改进快速扩展随机树算法流程图

4.2 双向快速扩展随机树算法

针对基本快速扩展随机树算法的缺点，采用双向快速扩展随机树算法进行搜索，双向快速扩展随机树算法在自由空间中定义了2棵随机树，分别以起始点和目标点为随机树根节点，双向扩展，直到2棵树相遇为止，即找到了搜索的路径。

以起始点为根节点的随机树搜索自由空间建立随机树的同时，以目标点为根节点的随机树也开始建立，2棵随机树交替生成新节点，检测该新节点与另一棵随机树节点的欧氏距离是否小于设定的阈值。当2个节点的距离小于设定阈值时，将2个节点连接，即2棵随机树合并成1棵随机树，生成路径。

4.3 增加角度约束

由车辆的转向模型可知，稳定性因素K是影响转向稳定性能的重要指标。K的取值分为3种情况：中性转向(K=0)，不足转向 (K>0)，过多转向(K<0)。以中性转向为基础，由式(18)可得：

(19)

根据以上分析，车辆的前轮转向角受转弯半径的影响，为了保证转向的平稳顺利进行，在快速扩展随机树进行路径规划时，新节点需要满足一定的角度约束，使生成的路径更加贴近车辆的运动需求。

设快速扩展随机树在生成新节点xnew的角度约束为φ，如图4所示，当xnew、xnearest和xinit之间角度φ1小于约束的值φ时，则舍弃该生成的新节点，当φ2大于或等于约束的值φ时，则保留该新节点，并将新节点加入到随机树中。

Figure 4 Expansion of rapidly-exploring random tree with angle constraints图4 增加角度约束的快速扩展随机树扩展示意图

5 节点优化

快速扩展随机树算法以一定的步长在地图中扩展，生成的路径节点过多，路径弯曲度较大，无法满足车辆的实际行驶条件，因此对生成的路径可行点进行优化，删除多余的节点，以优化生成的路径。

如图5所示，数组A存放着通过快速扩展随机树算法搜索得到的可通行路径的可行点，1表示起始点xinit，n表示目标点xgoal，2～(n-1)表示从起始点到目标点的可行点。

优化节点的思想如下所示：

(1)将初始点xinit加入到新的数组B中。

(2)判断节点n和节点n+1之间有无障碍物，若没有，则跳过节点n+1，判断节点n和下一个节点n+2之间有无障碍物，若有障碍物，则将节点n+2加入到数组B中，以n+2为新的节点往后搜索，依此类推。

(3)当搜索到目标点xgoal时，结束搜索，将目标点加入到数组B中。数组B为优化后的节点集合。

Figure 5 Node optimization图5 节点优化

6 路径平滑处理

本文选择B样条曲线对快速扩展随机树规划生成的路径进行平滑处理。

通常给定q+n+1个顶点Pi(i=0,1,2,…,q+n),则可定义q+1段n次的参数曲线，B样条曲线的表达式为：

(20)

其中，0≤t≤1,Pi+k(i=0,1,2,…,q;k=0,1,2,…,n)为控制顶点,n为规定的基函数的次数，Fk,n(t)为n次B样条曲线基函数。

Fk,n(t)定义如下：

(21)

B样条曲线是分段定义的。如果给定q+n+1个顶点Pi(i=0,1,2,…,q+n)，则可定义q+1段n次的参数曲线。

B样条曲线具有几何不变性、凸包性、保凸性、变差减小性等优点，且控制点的个数与函数的阶数相关性小。因此，对于路径的平滑处理具有良好的效果。

对于快速扩展随机树搜索生成的可行路径，各个可行节点连接时生成折点，为了避免用B样条曲线处理的路径与障碍物发生碰撞，在折点的两端分别生成局部端点，如图6所示，An-1、An和An+1为路径的可行点，在折点A2的两端分别生成局部端点Bn1和Bn2。

Figure 6 Inserting local endpoints图6 插入局部端点

由于折点所在的角度不同，采用自适应的方式选取局部端点Bn1和Bn2，通过点An-1和点An确定一次函数y(x)：

y(x)=A·x+B

(22)

其中，A和B为常数。

则Bn1的横坐标满足：

(23)

其中s为自适应系数。

将Bn1的横坐标x(Bn1)代入式(22)得到y(Bn1)，求出点Bn1坐标，再求出点Bn2坐标。将局部端点Bn1和Bn2加入到可行点中，对路径进行平滑处理。

7 仿真及分析

改进的快速扩展随机树算法是否符合要求，完成路径规划，智能车辆是否能够准确到达设定好的目标位置，需要通过软件来进行验证与分析。本节利用Matlab软件搭建仿真实验平台来对改进后的快速扩展随机树算法的正确性进行验证分析，同时与基本的快速扩展随机树算法进行了对比验证。仿真地图为500*500的二维空间。

7.1 增加角度约束仿真结果

未增加角度约束的随机树扩展示意图如图7所示，增加角度约束的随机树扩展示意图如图8所示。

Figure 7 Simulation result of rapidly-exploring random tree expansion图7 基本快速扩展随机树扩展仿真结果示意图

Figure 8 Simulation result of random tree expansion with angle constraints图8 增加角度约束的随机树扩展仿真结果示意图

分析图7和图8可知，未增加角度约束的快速扩展随机树算法，生成的可行点之间的角度大小无法确定，过小的角度无法满足车辆的运动学模型；增加了角度约束的快速扩展随机树，在扩展过程中可行点之间的角度更加平滑，增加的可行点之间的角度满足约束要求，生成的路径更加符合车辆的实际需求。

7.2 基本快速扩展随机树算法仿真结果

基本快速扩展随机树算法仿真结果如图9所示。

Figure 9 Simulation result of rapidly-exploring random tree algorithm图9 基本快速扩展随机树算法仿真结果

如图9所示，起始点为(10，10)，目标点为(480，480)，基本快速扩展随机树算法在搜索过程中无导向性，随机生成新节点，随机性大，生成的路径质量差，具有偏差性。

7.3 改进快速扩展随机树算法仿真结果

改进快速扩展随机树算法的仿真结果如图10所示。

Figure 10 Simulation result of improved rapidly-exploring random tree algorithm图10 改进快速扩展随机树算法仿真结果

节点优化仿真结果如图11所示。

Figure 11 Simulation result of node optimization图11 节点优化仿真结果

路径平滑仿真结果如图12所示。

Figure 12 Simulation result of path smoothing图12 路径平滑仿真结果

分析图9和图10可知，改进的快速扩展随机树算法采用循环交替迭代搜索方式，大大改善了基本快速扩展随机树算法的随机性，减少了算法的迭代次数。分析图10和图11可知，图10中未对节点进行优化，虽然生成了可行的路径，但可行点较多，路径长度较长，不是最优路径；图11中加入了节点优化，缩短了路径的长度，提高了路径的可行性。分析图11和图12可知，在折点两端插入局部端点，使路径折点处更加平滑，提高了快速扩展随机树算法的准确性，大大改善了路径的质量，更加符合车辆行驶的实际条件。

改进的算法与基本快速扩展随机树算法在步长一致的前提下，分别仿真50次，计算仿真实验的平均运行时间、平均路径长度和平均迭代次数，结果如表1所示。分析表1可以得出，本文算法降低了基本快速扩展随机树算法的随机性，路径长度较短，减少了算法的迭代次数，加快了算法的收敛速度。改进的快速扩展随机树算法新生节点都朝向目标点生长，避免了全局搜索，提高了无人车运动的实时性，而且较易获得最优路径，改善了基本快速扩展随机树算法的偏差性。

Table 1 Data comparison

8 结束语

本文通过采用循环交替迭代的搜索方式生成新节点，采用双向随机树同时搜索，建立车辆转向模型，增加车辆的转弯角度约束，改进了基本快速扩展随机树算法，解决了基本快速扩展随机树算法随机性大、收敛速度慢和偏差性的问题。基于车辆转向模型的算法，更加符合车辆行驶的实际情况，提高了可行性。对生成的路径进行节点优化，缩短了路径的长度，对路径进行平滑度处理，使生成的路径更加符合车辆的行驶条件。但是，无论是快速扩展随机树算法、粒子群算法等还是改进的快速扩展随机树算法都是一种模型驱动，有一定的局限性，需要进一步研究，结合数据驱动来进行智能车辆的路径规划及避障，最终使智能车辆的技术更加成熟。