基于改进RRT＊算法的自动驾驶车辆路径规划研究

宋若旸，阙海霞，马宗钰，兰海潮

（长安大学 汽车学院，陕西 西安 710064）

前言

在科学技术不断发展的今天，传统汽车在不断地走向智能化，并且逐步具备复杂环境感知、智能决策、协同控制和执行的功能，而最终的目标就是实现自动驾驶[1]。国际自动机工程师学会（SAE）将自动驾驶的过程依据人工参与驾驶的情况分为五级来逐步完成[2]。自动驾驶汽车在感知外部环境后，进行路径规划与自身决策控制。

路径规划是在有障碍物的环境中生成无碰撞路径，并根据一定的准则对其进行优化[3]，是智能驾驶汽车完成自动驾驶行为的必要保证[4]。常见的路径规划算法大致可以分为以A*算法为代表的基于搜索的规划算法、以 RRT为代表的基于采样的规划算法和以遗传算法为代表的基于启发式的规划算法。遗传算法（Genetic Algorithms简称GA）由John Holland与20世纪60年代末创建，它源于达尔文的进化论和孟德尔、摩根的遗传学理论，通过模拟生物进化的机制来构造人工系统，常用于解决传统算法不能快速求解的复杂问题[5]A*算法利用将出发点到此处的状态函数和由此处到目标点的预期函数组成估值评价函数来进行搜索，该算法保证了最优路径的生成[6]。文献[7]描述了在 A*算法上改进的 Hybrid A*算法，它在包括车辆行驶方向 θ的分散的四维环境中描述车辆状态，生成可行的平滑轨迹。快速搜索随机树算法（RRT）由Lavalle于1998年提出，通过采样生成拓展树的路径规划算法。该方法搜索速度较快且便于进行约束，但是该方法生成的不是最优路径[8]。文献[9]描述了Sertac Karaman和Matthew R.Walter等人提出的一种改进的RRT算法，称之为RRT*算法。

1 原始RRT＊算法分析

现将标准RRT*算法进行分析[9]：X为全局配置空间，令障碍物Xobs分布在障碍物区域，Xfree=X/Xobs为无障碍物区域。xstart和xgoal都在全局配置空间内。运动规划所要解决的问题即是产生一条可行的路径 x(t)⊂ Xfree，从初始点x(0)=xstar到达目标点区域 x (t)∈ xgoal。

图1 RRT＊算法采样图

（1）如图1所示，首先无障碍物区域随机采样即Xrand∈Xfree。

（2）搜索随机树T上距离随机点Xrand距离最近的节点 Xnearest，并拓展一个步长生成新的节点 Xnew，检测节点Xnearest与节点Xnew的连线间是否有障碍物，如没有障碍物则将节点Xnew添加到搜索树T上，并计算其价值函数。

（3）在生成的新节点规定的邻域 R范围内搜索节点集合Xnear，计算将集合内点Xnear-i作为Xnew父节点Xparent的价值函数，并与之前以Xnearest作为父节点Xparent的价值函数相比较。如果以Xnear-i作为Xnew父节点Xparent的价值函数小于原价值函数 cost，并且检测连线后无碰撞，则将 Xnearest与节点 Xnew的连线断开，连接以 Xnear-i与Xnew，并更新价值函数。

（4）遍历集合Xnear所有内点Xnear-i，更新价值函数cost，并将此时价值函数cost所对应的节点Xnear-i作为父节点Xparent添加至随机树T。

（5）重复以上过程，直至搜索树 T存在一点与目标点Xgoal之间的距离小于所规定的阈值，停止搜索。

（6）倒叙找路，生成完整的RRT*路径。

2 交叉路口环境下改进的RRT＊

2.1 环境分析

选取十字形道路交叉口图 2，作为典型车辆道路交通环境来改进RRT*算法的无人驾驶车辆轨迹研究。其中，s0、s1、s2、s3、s4为车辆行驶道路；p1、p2、p3、p4分别为四条行驶道路外部的中点；p0为s0的中点；其余为障碍物；整个地图尺寸为100×100m，车辆行驶道路的宽度为10m。

图2 交叉路口环境图

2.2 改进的目标偏向策略

带偏向的RRT通过以一定的概率P将目标点作为随机点来进行RRT的拓展，是提高搜索效果的一种有效的方式。如果环境中障碍物密集，则应该降低概率 P；相反，在障碍物稀疏的地图中应提高概率P的值。不管障碍物密集与稀疏，通常认为取P为10%较为合理。

目标偏向策略可以有效地将随机树向目标点引导，当随机点与父节点之间有障碍物且生成新节点不碰撞的情况下，拓展的新节点将会靠近下障碍物。此情况经常出现在车辆需要进行转向时，此时路径曲率增大，对拓展树造成负担，同时过于靠近障碍物会影响车辆的安全行驶。

2.3 概率采样策略

如图2车辆行驶的起点为p1，当车辆直行时取目标点为p3。按照原始的 RRT*定义，此环境中采样区域应为S=s0+s1+s2+s3+s4，但是当车辆进行直行行驶时，区域s2和s4的期望采样概率为0，即对区域s2和s4的采样是无效的采样，不仅增加了搜索时间，而且任何一个不必要方向的引导，都会对曲线的质量造成影响。概率采样策略则解决了这一问题，以车辆进行左转弯为例来说明概率采样策略：

首先，车辆在直线行驶时满足在区域s2和s4的期望采样概率为0，即在改进的RRT*算法中区域s2和s4的采样点为零。进而，在期望采样概率非零的区域平均采样即：

Psij，i=0，1，3表示在区域si，i=0，1，3中任一点j的采样概率。计算区域面积占比，即在任一区域生成随机采样点的概率：

ti，i=0，1，3表示区域si，i=0，1，3的区域面积占比；Ssi，i=0，1，3表示区域si，i=0，1，3的面积。

随机采样点的区域分布情况如R所示：

考虑目标偏向策略后，对公式（2）进行修正，改进的RRT*算法在期望采样概率非0的区域平均采样：

randp为概率修正系数，randp=1-P；Ti，i=0，1，3表示在区域si，i=0，1，3的区域生成采样点的概率。

3 仿真结果与分析

本文基于Matlab/Simulink软件版本为2019b，搭建十字形交叉路口环境，对车辆直行驾驶进行仿真验证与分析，在相同环境下将标准RRT*算法和改进的RRT*算法进行比较。如图3中（a）和（b）分别采用的原始方法和改进方法，当车辆使用标准RRT*算法进行路径规划时，虽然标准RRT*算法里存在着价值函数使得搜索时遵循最短路径原则，但是在非直行区域的采样会使路径向道路两旁偏移，从而导致生成的路径变长且不是预期的直线行驶。在改进的RRT*算法中，一方面目标偏向策略将路径向直行的方向引导，另一方面采用概率采样策略使得路径不向两侧偏移，最终导致路径生成长度变短，并且减少搜索时间，生成的路径可以更好地使车辆以直线的方式向目标点行驶。其中使用原始 RRT*算法进行路径规划时，规划路径长度为104.35m，仿真时长为5.96s；使用改进 RRT*算法进行路径规划时，规划路径长度为100.35m，仿真时长为5.71s。

图3 车辆直行行驶结果图

4 结束语

本文针对无人驾驶车辆在交叉路口环境下驾驶行为，在原始 RRT*算法的基础上改进了目标偏向策略并提出了一种新的概率采样策略，改进的 RRT*算法保障了车辆的安全行驶，并且可以快速地生成合理的路径。本文提出的在交叉路口环境下改进的 RRT*算法未考虑在折点处的平滑策略，因此未来将进一步平滑曲线，进一步地保障车辆行驶的操纵稳定性与平顺性。