基于B样条曲线的无人驾驶车辆Informed RRT*算法研究

刘梦奇，王维强，田良宇

（武汉科技大学 汽车与交通工程学院，武汉 430065）

0 引 言

汽车产销量的持续增长，使得城市交通控制任务面临严峻形势，而城市交通系统中有关交通拥堵、安全和环境污染等问题也不容小觑。随着计算机云计算等新技术的快速普及，无人驾驶技术得到了重视与发展。无人驾驶车辆不仅可以降低事故的发生率，还可以提高汽车的出行效率。作为自动驾驶核心体系中的路径规划，一直都是无人驾驶领域的热点与难点。

传统的路径规划算法主要分为4类，分别是：基于图搜索的算法，如A算法，D算法等；基于插值拟合的轨迹生成算法，如β样条曲线等；以MPC为代表的用于局部路径规划的最优控制算法；基于采样的算法，如PRM，RRT算法等。

其中，RRT是一种在完全已知的环境中通过采样扩展搜索的算法，相较于其它的算法，最突出的优点就是快，而且其搜索过程不需要构建显式的任务空间，计算简单，且具有概率完备性。但同时也有着比较明显的缺点，比如常常得不到最优解、规划的路径并不平滑等，因而后续提出了很多对RRT算法的改进策略。如，贺伊琳等人针对RRT算法盲目采样的缺点，基于目标偏向策略，限制采样区域，以一定的概率将目标点作为随机点进行随机树扩张；针对RRT算法缺乏稳定性和收敛速度慢的问题，提出了一种改进的双向搜索路径规划算法；Bormann等人提出了一种渐进最优版本的RRT改进算法RRT，该算法探索状态空间的过程与RRT相似，但增加了父节点重新选择和重新布线的步骤来保证最优性，且在可行解找到后，扩展过程并不停止，而是继续不断迭代找到更优的解，当迭代次数越来越多时，RRT会产生最优解或者几乎接近最优的解。刘猛等人将RRT算法与三次曲线规划相结合，通过三次曲线规划使路径趋向平滑，减少汽车自主泊车时的横向移动。

为了解决RRT产生大的采样空间且采样时间长的问题，则随即提出了Informed RRT算法。该算法在利用RRT找到初始解后，立即生成由起点、目标点、当前路径长度决定的椭圆状态空间区域，之后进行启发式直接采样，即把采样范围仅控制在这个能改善当前可行解的启发式椭圆子集区域内，从而加速收敛到最优解。但Informed算法产生的路径会产生明显的拐角，使生成的路径不平滑、质量差，本文拟进一步采用B样条曲线函数来拟合生成的路径点，从而生成曲率连续的平滑路径。

1 相关算法与原理

1.1 RRT算法原理

RRT算法是由美国爱荷华州立大学的LaValle教授于1998年提出的，是基于概率采样的运动规划算法，能够快速有效地搜索高维空间。算法运算流程如图1所示。下面将阐释分析RRT算法的核心操作。

图1 RRT算法流程图Fig.1 Flow chart of RRT algorithm

（1）初始化。由于RRT需要知道完整的环境地图，所以在初始化时要提供环境地图，以进行后续的碰撞检测，再将起点作为树的根节点进行存储。

（2）随机采样。确定起点和地图后，在整个地图中进行随机采样。

（3）树节点扩展。寻找距离采样点最近的树中的节点，将其作为新扩展的节点的父节点，再以该父节点为基础，向采样点的方向延伸一定距离，将延伸后的节点作为新节点，并加入树。

（4）终止条件。在树节点扩展的方法基础上，可以将扩展的新节点是终点作为终止条件。

1.2 RRT*算法原理

RRT算法的规划结果通常不会最优，RRT就是针对RRT的规划结果并非最优的局限性提出来的。在原始RRT的基础上，RRT主要有2点改进，即：为新节点选择代价最小的父节点、重布线。

RRT算法重选父节点和重布线示意图如图2所示。图2中，首先，当采样节点X加入节点树后，以采样点为圆心加一个小圆，考虑该圆圈内是否有更近的父节点使起点到该节点的距离最近；若存在更合适的父节点，就将其连接起来，并去除原来的连线，然后判断圈内是否存在某种节点，该节点经过X节点到初始节点的代价比该节点到初始节点原路径的代价小，若存在该类型节点，则将X节点作为该节点的父节点，并断开该节点与原父节点的连接，最后重新进行连线。

图2 RRT*算法重选父节点和重布线示意图Fig.2 Schematic diagram of RRT*algorithm for reselecting parent nodes and rewiring

1.3 Informed RRT*算法的原理

Informed RRT是在RRT的基础上对采样空间进行了优化，大大缩短了搜索时间。其伪代码参见算法1。黑体部分为改进的地方，其中表示起点到终点的距离，C表示规划路径的长度，初始状态下C为0。图3为Informed RRT算法的采样示意图，将已经搜索到的最短路径作为C，C作为椭圆的2个顶点2，作为椭圆的2个焦点，将采样空间限制在椭圆内，以此来构建椭圆，如图4所示。随着路径长度的不断缩短，逐渐缩小该椭圆形区域。

图3 Informed RRT*采样示意图Fig.3 Schematic diagram of Informed RRT*sampling

图4 Informed RRT*搜索过程示意图Fig.4 Schematic diagram of the Informed RRT*search process

Informed RRT(X，X)

2 B样条曲线拟合路径点

用B样条规划路径可以满足规划路径时对局部路径进行修改而不改变整个路径形状的要求，本文正是应用B样条曲线的这些优点来完成路径点的后期拟合，以生成光滑路径。

设一共有1个控制点，这些控制点用于定义样条曲线的走向、界限范围，则阶B样条曲线的定义为：

其中，B（）是第个阶B样条基函数，与控制点相对应，≥1；是自变量，基函数有以下推导式：

一个非减的实数序列［，…，］称为节点向量，B样条基函数B（）在区间［u，u］之外为0，对所有，和都为非负。

一般来说，B样条曲线分为均匀B样条曲线和准均匀B样条曲线，本文选择准均匀B样条曲线，因为这是两端节点具有重复度、中间节点非递减的序列，故而有着更为广泛的应用。还需指出的是，B样条曲线公式中的值代表曲线的平滑度，值越大，曲线的平滑度越高，但计算复杂度也越高；值过小，曲线的平滑度就差。为兼顾无人车轨迹的平滑度与计算复杂度，本文最终选择三阶B样条曲线，即3，将起始点和目标点都设置为3重节点。

3 仿真结果与分析

为了验证算法改进的有效性，拟基于Matlab软件进行路径规划仿真实验。实验仿真环境为800 m*800 m的矩形区域，设置起始坐标（0，0），目标点（700，700），在Matlab中对RRT、RRT、Informed RRT、基于B样条优化的Informed RRT四种算法进行仿真。仿真实验对比的评价指标包括平均采样的数目、平均规划时间和平均路径长度等。算法生成随机树上的采样节点越少，规划时间越短，则说明算法规划路径的效率越高，规划的优越性更好。

3.1 RRT、RRT*、Informed RRT*三种算法的仿真分析

图5为RRT、RRT、Informed RRT三种算法在障碍物环境下的仿真实验规划结果对比图。

图5 3种算法仿真结果实验图Fig.5 Experimental diagram of simulation results of three algorithms

3种算法50次实验的平均采样时间、平均采样长度、平均采样节点数的数据对比见表1。

表1 算法对比指标数据Tab.1 Indicator data comparison of three algorithms

由表1可知，基本RRT算法的平均采样时间，平均采样长度和平均采样节点较大，RRT算法和Informed RRT算法在各项评价指标上则有较大的改进，特别是在采样时间和采样节点上均有显著的下降。与RRT算法相比，Informed RRT算法的大多数分支集中在起始点和终止点的空间，反复实验过程中，最终路径并无太大变化，而RRT树支基本遍布整个空间，每次运行生成的路径都不相似，随机性较大。同时，其采样时间提高了大约28%，采样长度和平均采样节点并无太大变化。

3.2 基于B样条曲线的优化仿真

进行B样条曲线的优化仿真，图6为基础的Informed RRT仿真结果图，图7为加入B样条曲线平滑后的路径对比，图8为局部优化对比图，图9为基于三次B样条曲线轨迹优化后路径的曲率变化图。

图6 Informed RRT*仿真结果Fig.6 Simulation results graph of Informed RRT*

图7 B-Informed RRT*平滑路径对比图Fig.7 Comparison of B-Informed RRT*smooth path

图8 局部优化图Fig.8 Local optimization diagram

图9 基于三次B样条曲线的轨迹优化曲率变化图Fig.9 Trajectory optimization curvature change diagram based on cubic B-spline curve

由图6～图9可知，加入了B样条曲线拟合后的路径平滑性更高，经B样条曲线拟合后，所得曲线能够沿给定趋势变化，剔除了控制曲线中的尖点及曲率半径较小处的点，曲线的平滑度有了一定程度的提高。其中，三次B样条拟合曲线的曲率半径更小，尖点处的过渡更为平滑，从车辆最大转弯半径求得允许最大曲率为0.14，优化路径的最大曲率小于0.12，经过对比可以发现B样条曲线优化后的曲率连续且满足车辆的曲率范围，适用于车辆规划轨迹。

4 结束语

综上所述，通过对比RRT、RRT、Informed RRT算法，本文提出的B-Informed RRT算法生成的路径平均曲率降低，路径平滑点增加，同时采样时间和采样节点的降低，使得轨迹的生成速率有了较大的提升，从而提高了轨迹规划的实时性，能够在一定程度上满足车辆在高速行驶时的轨迹规划要求；而且加入了三次B样条曲线，在无人驾驶汽车轨迹规划中具有较强的实用性。