本文经自动驾驶之心公众号授权转载，转载请联系出处。
1 决策控制与运动规划概述
目前决策控制方法可以分为三类：sequential planning、behavior-aware planning、和end-to-end planning。
- sequential planning：最传统的方法，感知、决策与控制三个部分层次较为清晰；
- behavior-aware planning：相比第一种亮点在于引入人机共驾、车路协同以及车辆对外部动态环境的风险预估；
- end-to-end planning：DL、DRL技术，借助大量的数据训练，获得从图像等感知信息到方向盘转角等车辆控制输入的关系，属于时下最热门的方法之一。
本文将对sequential planning进行介绍，按照整个决策控制顺序讲述自动驾驶汽车的感知控制过程，最后会简要总结一下前文所提到的待解决的问题。
Control architecture for automated vehicles
2 路径规划概述
sequential planning 的过程简要概括为路径规划->决策过程->车辆控制，本文讲述的路径规划属于第一步与第三步。
https://mediatum.ub.tum.de/doc/1523134/74110395763.pdf
在无人车的运动轨迹生成问题上，有直接轨迹生成法与路径-速度分解法两种，相比第一种，路径-速度难度更小，因此更加常用。
2.1 路径规划的类型
路径规划可分为四大类：以PRM、RRT为代表的基于采样的算法、以为A* 、D* 代表的基于搜索的算法、以β样条曲线为代表的基于插值拟合的轨迹生成算法，和以MPC为代表的用于局部路径规划的最优控制算法。本小节将按照上述顺序逐一讲解：
A Review of Motion Planning Techniquesfor Automated Vehicles
2.2 路径规划算法优缺点
3 路径规划方法
3.1 基于采样的算法
3.1.1 基本算法PRM与RRT
(1) PRM
PRM算法（Probabilistic Road Map）。PRM主要包含了两个步骤，一是学习阶段，二是查询阶段。
第一步，学习阶段：对状态空间内的安全区域均匀随机采样ｎ个点，并删除采样落在障碍物上的点，接着对相邻的点进行连接并做碰撞检测，剔除不是collision-free的连线，最终得到一个连通图。
第二步，查询阶段：对于给定的一对初始和目标状态，利用上一步构建好的采样节点及连续，运用图搜索的方法（Dijkstra或者A*）来找出一条可行路径。
完成PRM构建后，就可以用于解决不同初始、目标状态的运动规划问题，但是这个特性对于无人车运动规划而言是不必要的。另外PRM要求对状态之间作精确连接，这对于存在复杂微分约束的运动规划问题是十分困难的。
(2) RRT
RRT (Rapidly-exploring Random Tree)算法。RRT其实代表了一系列基于随机生长树思想的算法，是目前机器人领域运用最广泛、优化变种最多的一类算法.
① 树初始化：初始化树的结点集和边集，结点集只包含初始状态，边集为空；
② 树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展连接；若生成的轨迹不与障碍物发生碰撞，则将该轨迹加入树的边集，该轨迹的终点加入到树的结点集
③ 重复步骤②，直至扩展到目标状态集中，相比PRM的无向图而言，RRT构建的是初始状态作为根结点、目标状态作为叶结点的树结构，对于不同的初始和目标状态，需要构建不同的树。
RRT不要求状态之间的精确连接，更适合解决像无人车运动规划这样的运动动力学问题。
3.1.2 采样法的问题与解决办法
求解效率与是否最优解。PRM与RRT拥有概率完备性的原因在于其几乎会遍历构型空间中所有位置。
(1) 求解效率
在提升求解效率方面，优化RRT的核心思想在于引导树向空旷区域，即尽量远离障碍物，避免对于障碍物处的节点的重复检查，以此提升效率。主要解决方法：
① 均匀采样
标准RRT算法对状态空间均匀随机采样，当前树中结点获得扩展的概率与其Voronoi区域面积成正比，所以树会向着状态空间的空旷区域生长，均匀充满状态空间的自由区域。
RRT-connect算法同时构建两棵分别起始于初始状态和目标状态的树，当两棵树生长到一起时则找到可行解。Go-biaing在随机采样序列中以一定比例插入目标状态，引导树向目标状态扩展，加快求解速度，提高求解质量。
heuristic RRT使用启发式函数增加扩展代价低的结点被采样的概率，计算树中每个结点的代价，但是在复杂环境中，代价函数的定义困难的，为解决这一问题，f-biased采样方法先将状态空间离散化为网格，再使用Dijkstra算法计算每个网格上的代价，这个网格所在区域的点的代价值都等于该值，以此构建启发式函数。
② 优化距离度量
距离用来度量两个构形之间路径的代价，辅助生成启发式代价函数，引导树的走向。但在考虑障碍物的情况下，距离计算的难度高，运动规划中距离的定义采用类似欧氏距离的定义。RG-RRT （rechability guided RRT）可以消除不准确的距离对RRT探索能力的影响，它需要计算树中结点的能达集，当采样点到结点的距离大于到该结点能达集的距离时，该节点才有可能被选中进行扩展。
③ 降低碰撞检查次数
碰撞检查采样法的效率瓶颈之一，通常的做法是对路径等距离离散化，再对每个点处的构形作碰撞检查。resolution complete RRT通过降低靠近障碍物的结点获得扩展的概率，它对输入空间离散化，对于某个结点输入只使用一次；若某个输入对应的轨迹与障碍物碰撞，则对该节点加上一个惩罚值，该惩罚值越高，该节点获得扩展的概率越小。dynamic domain RRT与adaptive dynamic domain RRT限制采样区域在当前树所在的局部空间，以防止靠近障碍物的结点反复扩展失败，提高算法效率。
④ 提升实时性
Anytime RRT先快速构建一个 RRT，获得一个可行解并记录其代价，之后继续采样，但仅将有利于降低可行解代价的结点插入树中，从而逐渐获得较优的可行解。Replanning将整个规划任务分解为若干等时间的子任务序列，在执行当前任务的同时规划下一个任务。
(2) 在解决最优性问题上主要有以下方法：
RGG算法（random geometric graph）：根据随机几何图理论对标准PRM和RRT 改进的具有渐近最优性质的PRM、RRG和RRT 算法，在状态空间中随机采样ｎ个点，并将距离小于r(n)的点连起来，就构成了RGG。
RRT* 算法：在RRG基础上引入“重新连接”步骤，检查新插入结点作为其临近点的父结点是否会使其临近点的代价降低，若降低，则去掉临近点原来的父子关系，将当前插入点作为其父结点，这就是RRT*算法。
LBT-RRT算法：大量的结点连接和局部调整使得PRM和RRT的效率十分低下。LBT-RRT算法将RRG和RRT* 算法结合起来，在获得渐进最优性的前提下，获取更高的效率。
3.2 基于搜索的算法
基本思想是将状态空间通过确定的方式离散成一个图，然后利用各种启发式搜索算法搜索可行解甚至是最优解，该种类别算法比较成熟。
基于搜索的算法的基础是状态格子，状态格子是状态空间离散化，由状态结点和从该结点出发到达相邻结点的运动基元组成，一个状态结点可以通过其运动基元变换到另一个状态结点。状态格子就将原来连续的状态空间转化为一个搜索图，运动规划问题就变成了在图中搜索出一系列将初始状态变换到目标状态运动基元，构建起状态格子后就可以使用图搜索算法来搜索最优轨迹。
3.2.1 基础算法Dijkstra、A*的构建
Dijkstra算法遍历整个构型空间，找出每两个格子之间的距离，最后选择出发点到目标点的最短路径，其广度优先的性质导致效率很低，在该算法的基础上加入启发式函数，即所搜索结点到目标节点的距离，并以此为基础再次进行搜索可避免全局搜索带来的效率低下，这即为A*算法，如下图所示，红色为搜索区域。
图6：A*与Dijkstra算法效果对比图
3.2.2 搜索法的问题与建议
与基于采样的算法相同，这种类别的算法也需要做效率与最优性的优化。
在提升效率上面，A* 本身属于静态规划的算法，针对A* 算法的延申有weighted A* 通过增加启发式函数的权重进一步引导搜索方向向这目标节点进行，搜索速度很快，但是容易陷入局部极小值，无法保证全局最优解。
对于运动的车辆来说，使用A* 的衍生算法D（dynamic A）可大幅度提升效率。同样以动态规划为基础的还有LPA，该算法可以处理状态格子的运动基元的代价是时变的情况，当环境发生变化时可以通过对较少数目节点的重新搜索规划出新的最优路径。在LPA 的基础上开发出D*-Lite可以获得与D*同样的结果，但是效率更高。
在进行最优化解的探寻时，ARA* 是在Weighted A* 基础上发展出的具有Anytime性质的搜索算法，它通过多次调用Weighted A* 算法且每次调用就缩小启发式函数的权重，这样算法可以快速求出可行解，通过引入集合INCONS使得每次循环可以继续使用上一次循环的信息，对路径做出优化，逐渐逼近最优解。
在兼顾算法效率与最优性的问题上，Sandin aine等提出了MHA* 算法，引入多个启发式函数，保证其中有一个启发式函数在单独使用时可以找到最优解，从而通过协调不同启发式函数生成的路径代价，可以兼顾算法的效率和最优性。DMHA在MHA的基础上在线实时生成合适的启发式函数，从而避免局部最小值问题。
3.3 基于插值拟合的算法
基于插值拟合的算法可被定义为：根据已知的一系列用于描述道路图的点集，通过使用数据插值与曲线拟合的方式创造出智能车将行驶的路径，该路径可提供较好的连续性、较高的可导性。具体的方法如下：
Dubins曲线和Reeds and Sheep(RS)曲线是连接构形空间中任意两点的最短路径，分别对应无倒车和有倒车的情况。它们都是由最大曲率圆弧和直线组成的，在圆弧和直线连接处存在曲率不连续，实际车辆按照这样曲线行驶时必须在曲率不连续处停车调整方向轮才能继续行驶。
多项式插值曲线是最常用的一种方法，它可以通过满足结点的要求来设定多项式系数，并且获得较好的连续可导性，四阶多项式常用于纵向约束控制，五阶多项式常用于横向约束控制，三阶多项式也被用于超车轨迹中。
样条曲线具有封闭的表达式，容易保证曲率连续性。β样条曲线可以实现曲率连续性，三次Bezier曲线可以保证曲率的连续性和有界性，并且计算量相对较小。η^3曲线[43]是一种七次样条曲线，它有着很好的性质：曲率连续性和曲率导数的连续性，这对于高速行驶车辆是很有意义的。
3.4 基于最优控制的算法
将基于最优控制的算法归在路径规划中，主要是因为其中的MPC可以进行局部的路径规划以进行避障，除此之外，MPC主要的作用是进行轨迹跟踪，其所考虑的问题除了必要的动力学、运动学约束以外，未来还应考虑舒适性、感知信息的不确定性、车间通信的不确定性，并且在局部轨迹规划时还可以将驾驶员纳入控制闭环。对于以上所提到的不确定性问题与将驾驶员纳入控制闭环将在第四节讨论。关于MPC的学习，主要从优化理论与工程实践两个方面入手。对于前者，推荐Dimitri P. Bertsekas的Convex Optimization Algorithms，James B. Rawlings的Model Predictive Control：Theory, Computation, and Design。中文领域刘浩洋老师的最优化书个人觉得相对清晰易懂。对于后者，首先龚建伟老师的那本无人驾驶MPC书强推了，老版书里的demo有问题，不过都在新版里解决了。
MPC所使用的预测模型有很多种：诸如卷积神经网络、模糊控制、状态空间等等，其中用的最多的为状态空间法。MPC可简要表述为：满足必要的动力学、运动学等等约束的情况下，通过数值手段求解模型的最优解，该最优解即为状态方程的控制量，如方向盘转角等等，并将控制量作用在车模上以获得要求的状态量，如速度、加速度、坐标等等。
通过上述描述可知，MPC的关键在于模型的建立与模型的求解，如何等效简化模型的建立以及提升求解的效率是重中之重。在不同的控制输入下车辆会走不同的轨迹，每一条轨迹都与之对应一个目标函数值，无人驾驶车辆会通过求解算法找出最小目标函数值对应的控制量，并将其作用在车上，如下图所示：
为了降低建模难度，也有使用人工势能场模型进行建模，人工势能场的基本思想类似于电场，道路上的障碍物类似于电场中与场源相异电荷极性的电荷。障碍物（动态、静态）处的势能更高，无人车将向低势能位置前进。
4 开源项目
推荐一个开源项目CppRobotics：
- Path Planning
- Dijkstra
- A Star
- RRT
- Dynamic Window Approach
- Model Predictive Trajectory Generator
- Cubic Spline Planner
- State Lattice Planner
- Frenet Frame Trajectory
5 学习方法
入门新领域的学习脉络是：工程、理论以及视野三驾马车齐头并进，以路径规划为例：
5.1 工程
指的是了解各路径规划算法内容，一边从广度上了解各算法内容，一边从深度上深入学习各算法细节。关于路径规划领域的算法，当前没见全面的教程，但是龚建伟的NMPC运动规划可以参考。
5.2 理论
指的是了解支撑这些算法运算数学原理以及这些算法产生的原因（数学视角）。
- 构建目标函数与约束条件同时求极值来得到最优控制量（路径），属于最优化理论；
- 在求解最优控制量时大家常见的牛顿法、最速下降法等等这些数值求解方法，本质来自于数值求解代数等式方程，属于数值分析；
- 求解过程中所见到的导数雅可比矩阵、判断条件中的向量范数等等，本质就是把一维数值求解放到了高维，属于矩阵理论。
5.3 视野
指的是了解路径规划在科研以及企业的主要应用，手段分别为科研文献以及成果报告等等。
6 小结
本文介绍了当前路径规划的梗概，了解目前路径规划有那些方法。内容很繁杂，很难在没有实际应用导向的情况下下短期全部学通，只能在需要的时候再重点学习。
