﻿ 无人车运动规划算法综述
 文章快速检索
 同济大学学报(自然科学版)  2017, Vol. 45 Issue (8): 1150-1159.  DOI: 10.11908/j.issn.0253-374x.2017.08.008 0

### 引用本文

YU Zhuoping, LI Yishan, XIONG Lu. A Review of the Motion Planning Problem of Autonomous Vehicle[J]. Journal of Tongji University (Natural Science), 2017, 45(8): 1150-1159. DOI: 10.11908/j.issn.0253-374x.2017.08.008.

### 文章历史

1. 同级大学 汽车学院，上海 201804;
2. 同济大学 智能型新能源汽车协同创新中心，上海 201804

A Review of the Motion Planning Problem of Autonomous Vehicle
YU Zhuoping1,2, LI Yishan1,2, XIONG Lu1,2
1. School of Automotive Studies, Tongji University, Shanghai 201804, China;
2. State Intelligent Car of New Energy Resources Collaborative Innovation Center, Tongji University, Shanghai 201804, China
Abstract: The motion planning problem of unmanned autonomous vehicle (UAV) is reviewed. UAV operates in both structured road and unstructured field with differential constraints. The problem of trajectory generation can be simplified with the differential flatness of Ackerman steering vehicle. Compared to direct trajectory generation method, path-velocity decomposition method is more popular. Clothoids, splines and polynomial spirals are used for path generation. The two major planning algorithms of great practical significance are rapidly random tree(RRT) in the name of sampling-based and A* in the name of search-based methods.
Key words: unmanned autonomous vehicle    motion planning    sampling-based method    search-based method

1 问题定义

 图 1 车辆模型和规划算法的关系 Fig.1 Relationship between the vehicle model and planning algorithm

 $\begin{array}{l} \dot x = v{\rm{cos}}\;\theta \\ \dot y = v{\rm{sin}}\;\theta \\ \dot \theta = v \kappa\\ \dot v = a\\ \dot \kappa = \frac{{\dot \phi }}{{b{\rm{co}}{{\rm{s}}^2}\varphi }} \end{array}$ (1)
 图 2 车辆模型 Fig.2 Vehicle model

2 轨迹生成 2.1 轨迹生成与非线性控制

2.2 轨迹生成方法

(1) 直接构造法是指直接构造车辆后轴中心R坐标关于时间的函数。文献[14]针对非结构化环境使用5次多项式来构造满足初始和终止位置、速度和加速度的轨迹，而且5次多项式使得加速度变化率最小，有利于车辆的平稳行驶.但是这一方法需要多次调整轨迹的时间区间，来保证整条轨迹上速度、加速度、曲率和曲率变化率的有界性.文献[15]在[14]的基础上针对结构化环境中的轨迹生成方法作出了改进，计算道路中线的Frenet标架下轨迹的坐标，可以适应各种道路环境下的轨迹生成问题.

(2) 路径-速度分解法[16]最初是为了解决有移动障碍物的环境下运动规划的问题而提出的，先构造一条避开静态障碍物的路径，再在路径上规划速度，避开移动障碍物.对于轨迹生成而言无需考虑障碍物，只需要考虑无人车的运动学和动力学约束.直接轨迹生成法要一次性满足速度、加速度、曲率和曲率导数的有界性，难度较大；而路径-速度分解法先生成一条曲率连续有界的路径，然后在路径上生成连续有界的速度，同时保证有界的加速度和有界的曲率导数，降低了轨迹生成的难度.

(a)路径生成

Dubins曲线[17]和Reeds and Shepp (RS)曲线[18]是连接构形空间中任意两点的最短路径，分别对应无倒车和有倒车的情况.它们都是由最大曲率圆弧和直线组成的，在圆弧和直线连接处存在曲率不连续，实际车辆按照这样曲线行驶时必须在曲率不连续处停车调整方向轮才能继续行驶.

(b)速度生成

3 基于采样的算法

3.1 基本算法PRM/RRT 3.1.1 PRM的构建

(1) 预处理阶段：对状态空间内的安全区域均匀随机采样n个点，每个采样点分别与一定距离内的邻近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图.

(2) 查询阶段：对于给定的一对初始和目标状态，分别将其连接到已经构建的图中，再使用搜索算法寻找满足要求的轨迹.

3.1.2 RRT的构建

(1) 树的初始化：初始化树的结点集和边集，结点集只包含初始状态，边集为空.

(2) 树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展(或连接).若生成的轨迹不与障碍物发生碰撞，则将该轨迹加入树的边集，该轨迹的终点加入到树的结点集.

(3) 重复步骤(2)，直至扩展到目标状态集中.

3.2 提高算法效率和求解质量的措施

(1) 均匀采样：标准RRT算法对状态空间均匀随机采样，当前树中结点获得扩展的概率与其Voronoi区域面积成正比，所以树会向着状态空间的空旷区域生长，均匀充满状态空间的自由区域.盲目的扩展伴随着大量的碰撞检查，严重影响程序效率.

(2) 距离度量：距离用来度量构形空间(状态空间)中两个构形(状态)之间路径(轨迹)的代价，这个代价可以理解为路径的长度、消耗的能量或是花费的时间.不同于欧氏空间，车辆的构形空间或状态空间中距离没有闭形表达式，其计算难度等价于轨迹生成的难度；如果考虑障碍物的话，距离的计算难度是等价于整个运动规划的难度的.因此，运动规划中距离的定义是采用类似欧氏距离的表达式.文献[34]给出构形空间SE(3) 中加权距离表达式：

 $\rho \left( {{q_0}, {q_1}} \right) = {\omega _{\rm{t}}}\left\| {{\mathit{\boldsymbol{X}}_0}-{\mathit{\boldsymbol{X}}_1}} \right\| + {\omega _{\rm{r}}}f\left( {{R_0}, {R_1}} \right)$ (2)

 $\begin{array}{l} \rho \left( {{x_1}, {x_2}} \right) = {\omega _p}{\left\| {{p_1}-{p_2}} \right\|^2}{\rm{ + }}{\omega _q}{\left( {1-\left| {{q_1} \cdot {q_2}} \right|} \right)^2}{\rm{ + }}\\ \;\;\;\;\;\;\;\;\;{\omega _v}{\left\| {{V_1}-{V_2}} \right\|^2} + {\omega _w}{\left\| {{w_1} - {w_2}} \right\|^2} \end{array}$ (3)

(3) 障碍物：碰撞检查是基于采样的算法的效率瓶颈之一，通常的做法是对路径等距离离散化，再对每个点处的构形作碰撞检查.文献[36]提出RC-RRT(resolution complete RRT)来降低靠近障碍物的结点获得扩展的概率，具体做法是对输入空间离散化，对于某个结点，其某个输入只能使用一次；若某个输入对应的轨迹与障碍物碰撞，则对该节点加上一个惩罚值，该惩罚值越高，该节点获得扩展的概率越小.文献[37]结合RG-RRT和RC-RRT的优点，提出了EG-RRT(environment guided RRT)使算法更适合处理有狭窄通道的障碍物环境.文献[38]与文献[39]分别提出DD-RRT(dynamic domain RRT)与ADD-RRT(adaptive dynamic domain RRT)，限制采样区域在当前树所在的局部空间，以防止靠近障碍物的结点反复扩展失败，提高算法效率.文献[40]中提出了T-RRT算法，它作用于代价地图上，在进行搜索和扩展时，引入了过渡测试(transition test)，利用Metropolis准则来判断新结点的代价是否能够满足扩展的要求，同时控制无效扩展，从而提高算法效率并提高路径的质量.

(4) 实时性：anytime RRT[41]是一种实时性较高的算法，它先快速构建一个RRT，获得一个可行解并记录其代价.之后算法会继续采样，但仅将有利于降低可行解代价的结点插入树中，从而逐渐获得较优的可行解.Replanning[42]是另一种实时规划算法，它将整个规划任务分解为若干等时间的子任务序列，在执行当前任务的同时规划下一个任务.

3.3 最优性

 $r\left( n \right) = \gamma {\left( {\frac{{\log \left( n \right)}}{n}} \right)^{1/d}}$ (4)

RRT*与标准RRT一样具有盲目探索整个状态空间的特点，其计算效率还有很大的提升空间.文献[49]提出了Informed RRT*算法，来避免RRT*对整个状态空间的不必要的探索，其思路是先用RRT*找到一个解，然后把采样区域限制到一个以初始、目标状态为焦点并包含当前解的椭球内，继续优化当前解.文献[50]提出了BIT*(batch informed trees)算法，其思路是先构建一个满足渐进最优条件的RGG，然后利用启发式搜索构建一个从初始状态到目标状态的树，之后反复构造更稠密的RGG，并在已有的树结构基础上作优化.经试验验证，BIT*可以获得比RRT*、FMT*和Informed RRT*更高的效率.

4 基于搜索的算法

4.1 状态格子

4.2 搜索算法

Dijkstra[55]算法是经典的最短路径搜索算法，然而对于给定的一组初始、目标状态，其广度优先的性质将会导致太多无关节点的搜索，效率很低.A*[56]是一种启发式最优搜索算法，其框架与Dijkstra基本一致，不同点在于引入了对当前结点到目标结点最低代价的估计函数.A*算法维护两个集合：OPEN和CLOSED，OPEN是存储待扩展节点的优先队列，根据通过当前结点的路径总代价来排序，CLOSED存储已经扩展过的节点.算法每次从OPEN中取出最优先的节点，进行扩展，并将该节点插入到CLOSED中，该节点所扩展到的节点如果不在CLOSED中，就插入到OPEN中，这样循环下去，直到扩展到目标结点.可见A*算法通过启发式函数引导靠近目标结点的节点获得优先扩展的机会；但在格子数量很多时，A*处理的节点数目过多.

Weighted A*[57]通过增加启发式函数的权重进一步引导搜索方向向这目标节点进行，搜索速度很快，但是容易陷入局部极小值，无法保证全局最优解.ARA*[58]是在Weighted A*基础上发展出的具有Anytime性质的搜索算法，它通过多次调用Weighted A*算法且每次调用就缩小启发式函数的权重，这样算法可以快速求出可行解，通过引入集合INCONS使得每次循环可以继续使用上一次循环的信息，对路径做出优化，逐渐逼近最优解.

A*及其变种算法Weighted A*、ARA*可以有效解决确定性环境中的搜索问题，但对于环境复杂多变的情况，环境中的代价分布会发生变化，简单地重复使用A*虽然可以重新搜索出最优路径，但是计算效率较低.LPA*[62]可以处理状态格子的运动基元的代价是时变的情况，当环境发生变化时可以通过对较少数目节点的重新搜索规划出新的最优路径；D*[63]适合处理复杂多变的环境，能够根据所获取环境信息快速重新规划从当前状态到目标状态的最优轨迹，适合机器人在行进过程中探测出新的障碍物时重新选择路径；D*-Lite[64]是对LPA*的发展，可以获得与D*同样的结果，但是效率比D*高.

5 算法实现

SBPL(search-based planning library)是专门针对基于搜索的运动规划算法而开发的程序库，几乎涵盖了第4.2节所列举的所有搜索算法，使用C ++实现.

6 发展趋势

(1) 与车辆动力学结合

(2) 与状态参数估计结合

(3) 与机器学习结合

6 总结

(1) 采用具有阿克曼转向性质的车辆模型，可以利用其微分平坦性质简化轨迹生成问题.

(2) 直接轨迹生成法相对路径-速度分解法而言还不够成熟，使用多项式螺旋线生成路径对速度生成和碰撞检查都有利，应用比较成熟.

(3) 缺少准确的距离度量是基于采样的运动规划算法的主要缺陷，相关改进算法的效率有待验证；各种最优化算法虽然不断提高效率，但离实际应用还有距离；Anytime-RRT/RRT*是实用性比较高的算法.

(4) 基于搜索的运动规划算法在非结构化环境和结构化环境中无人车运动规划问题上都有成功的应用，是一类具有很大实用意义的方法.