作者 | 黄浴 编辑 | 汽车人
原文链接：https://zhuanlan.zhihu.com/p/59089908
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【规划控制】技术交流群
后台回复【规划控制综述】获取自动驾驶、智能机器人规划控制最新综述论文！
以前提到过规划中的行为模型和行为预测，其实是规划中的一部分，也是最新的部分。也提到过端到端的机器学习做规划以及做控制（无论输入是原始感知器数据，还是感知的输出），前者输出的是车轨迹，后者就直接输出车的线控信号，即brake，throttle，steering。
那么，这里回顾一下整个自动驾驶的规划控制的经典框架和方法。
1 S. Dixit et al.,“Trajectory planning and tracking for autonomous overtaking: State-of-the-art and future prospects“，Annual Reviews in Control，March 2018。
这篇综述是讨论自动超车涉及的轨迹规划和跟踪（控制）的技术和前景。重点是讨论高速场景超车的两个关键方面：1）车辆动力学和环境约束；2）环境和周围障碍物的精确知识。
超车是一个复杂驾驶任务，涉及要超的车的侧向和纵向运动，同时要避免和慢运动车辆（前车）的碰撞。一个超车操作包括：
换道到超车道；
通过前车；
换道至原车道。
超车中的换道可以是可商酌的（DLC），也可以是强制的（MLC）。前者意味着快道的交通状况好于当前道，而后者是交通规则强制的结果，如停滞的车辆，必须走的车道等等。同理，返回原车道也是这样的。下图是是超车动作的框图表示。
超车动作可以分成四种类型：
正常：该车接近前方车辆准备超车；
飞：该车不调整纵向速度，可直接超过前车；
随着：该车跟着另外一个车一起超过前车；
2+：该车一次超过两个或更多的前车。
车辆周围环境被以下特征填充：
永久的（道路和车道限制）；
慢速变化（临时速度限制，道路网络，交通密度）；
快速变化（周围车辆的速度，位置和方向）。
下图是一个要超越前车（TV）的自动驾驶车（SV）带有的传感器配置：雷达和双目相机。而要完成的感知任务包括：目标分类，跟踪静止和运动障碍物，确认安全驾驶车道等等。车联网V2X，还有高清地图HD Map，能提供附加的环境信息。
运动规划分成：物理特性的Physics-based，操作特性的Manoeuvre-based，交互特性的Interaction-aware。
避开运动车来的轨迹规划技术：
车辆轨迹跟踪的控制技术：
2. W Schwarting , J Alonso-Mora，D Rus，“Planning and Decision-Making for Autonomous Vehicles“, Annual Review of Control, Robotics, and Autonomous Systems, 1，2018
见上图，这篇综述会涉及规划中和其他交通参与者的交互行为建模，特别提到新概念，interactive planning和end-to-end learning，讨论相关的安全和可靠性的问题。另外，感知和规划集成的工作，以及behavior-aware planning的方法，重点做了介绍，也提出了系统验证和安全的问题。最后还讨论了自动驾驶车队的挑战性。
一般运动规划被分为三种方法：
离散空间如lattice planner,
随机规划如RRT(rapidly exploring random trees),
带model predictive control (MPC)的constrained optimization。
基于NN的感知方法对不确定性缺乏反馈信息，所以深度学习和贝叶斯理论结合可以给出不确定性的估计。
将感知和规划集成，构成了end-to-end planning。不过，对自动驾驶而言，这种方案风险比较高。仿真模拟对于驾驶行为的学习作用大，特别是强化学习的采用。
Behavior-aware planning要求交互和合作的决策过程。但不确定性会爆炸般增大，所以作者提到了3个方法解决：
动态环境的更好描述，包括动态障碍物；
交互和合作的条件建模；
采用联合分布建模。
一般概率方法，即POMDP (partially observable Markov decision process)模型。
而机器学习方法，数据驱动型，IRL（inverse reinforcement learning）可以是最合适的框架。
3 S D Pendleton，et al. “Perception, Planning, Control, and Coordination for Autonomous Vehicles”，Machines 2017, 5, 6
感知和车联网部分就跳过了。
规划分成三个层次：任务规划，行为规划，运动规划。
任务规划经典的算法主要是Dijkstra’s 和 A*，行为规划多半是一个FSM的决策过程。运动规划计算复杂度高，有著名的维度困扰。一个好的想法是离散化，将连续空间模型变成离散空间模型。这里包括两个途径：combinatorial planning和sampling-based planning。前者如Voronoi-diagrams，后者有Rapidly-exploring Random Trees (RRT)和Probabilistic RoadMaps (PRM)。
决策中，优先观察者（precedence observers）将按照车辆当前位置有关的规则检查是否允许其前进，而间隙观察者（clearance observers）将检查“碰撞时间TTC”——检测的障碍物进入指定感兴趣区域ROI的最短时间——以 确保其他交通参与者的安全通行。
比较难的是动态环境的规划，一个障碍物运动的环境，还有系统的不确定性。防撞和避开障碍物是主要的任务。经典的解决方法是Partially Observable Markov Decision Processes (POMDP)。在时-空维度上规划需要考虑障碍物的位置，速度和轨迹。
下图是时空中轨迹预测的系统空间可视化展示：不确定性造成维度随时间增长。
文章还提到了Incremental Planning和Replanning问题。增量规划需要一种增量生成子目标的方法，或者基于一些启发从一组可能的轨迹中选择最佳轨迹。迭代的重规划生成新的解决方案轨迹，提供了一个潜在机会，将先前规划迭代得到的知识，迁移到后续规划迭代中。
运动控制方法：PID和MPC
Proportional-Integral-Derivative (PID) 控制
比例-积分-微分（Proportional-Integral-Derivative，PID) 控制是根据偏差量计算变量值的算法，偏差是指计划值与实际值之间的差异（如上图），其一般表示为
其中的三个元素如下：
P 与误差当前值e成正比。例如，如果误差e大且为正，则考虑到增益因子“K”，控制输出将成比例地大且为正。单独使用比例P控制会导致设定值和实际过程值之间出现误差e，因为它需要一个误差来生成成比例的响应。如果没有错误，则没有纠正响应。对于给定的误差变化，高比例增益会导致输出发生较大变化。如果比例增益太高，系统可能会变得不稳定。相比之下，小增益会导致对大输入误差的小输出响应，以及响应较慢或敏感度较低的控制器。如果比例增益太低，则在响应系统扰动时控制动作可能太小。调整理论和工业实践表明，比例项应该贡献大部分的输出变化。
I 说明误差e的过去值，并随时间推移进行积分产生 I 项。例如，如果在比例P控制后存在残余 误差，积分项将添加误差e累积值的控制效应以消除残余误差。当误差e消除后，积分I项将停止增长。这将导致比例P效应随着误差的减小而减小，但会被不断增长的积分效应所补偿。积分项加速了向设定点移动的过程，并消除纯比例控制器出现的残余稳态误差。但是，由于积分项响应的是过去累积误差，因此可能导致当前值超过设定值。
D 是基于当前变化误差delta-e未来趋势的最佳估计。它有时被称为“预期控制”，因为它施加误差变化产生的控制影响来有效地寻求减少误差的影响。变化越快，控制或阻尼效果就越大。理想的微分不是因果关系，因此 PID 控制器的实现包括微分项额外的低通滤波，以限制高频增益和噪声。然而，微分项在实践中很少使用。
通过在控制器中包含一个前馈项，可以将另一个自由度添加到控制器中，其中该控制器架构如上图所示。在控制器中添加前馈项有助于克服反馈控制的局限性。前馈项被添加到控制信号中，而不考虑受控系统的任何测量。然而，前馈项可能涉及干扰的测量等。设计前馈控制需要更完整地理解物理系统，因此，模型参考通常用于前馈控制器。在控制器中组合前馈和反馈项的方法也称为两自由度（2-DOF）控制器。
前馈用于生成参考轨迹，而反馈用于补偿干扰和误差。状态空间控制，通常称为现代控制，是一种尝试通过检查系统状态来控制系统整个矢量作为一个单元的技术。
Model Predictive Control (MPC) 模型预测控制
一种基于模型的闭环优化控制方法（如图所示），其表示为如下优化问题：
与最优控制公式类似，成本函数表示由阶段成本（stage cost）L（x, y, u）和终端成本（terminal cost）F（x）组成的性能目标。其中t是离散时间索引。向量v（h | t）表示基于直到t的信息、以时间t做参考在h时间步长预测的v值。在上面f(x, u)和h(x, u)表示的系统动力学离散时间模型中， x∈Rn是系统的状态，u∈Rm是控制输入，y∈Rp是系统输出。优化器是控制输入序列U（t）=（u（0 | t），···，u（N-1 | t）），其中N是预测范围。
在任何控制周期t，针对一般问题的MPC策略如下操作：测量系统输出并估计状态x（t）。以上公式最小化的状态估计限制于初始化x（0|t）。一旦解决了MPC优化问题并获得最佳输入序列U * （t），那么将最优输入序列的第一个元素代入系统u（t）= u*（0 | t）。在随后的周期中，用新获取的状态估计重复该过程，从而得到反馈。
已经开发了MPC以集成最优控制的性能和鲁棒控制的鲁棒性。通常，预测是在称为预测范围的短时间范围内执行的，其中模型预测控制器的目标是计算该预测范围内的最优解。该模型以及控制器可以在线更改以适应不同的条件。
MPC最大的优点在于显式处理约束的能力, 原因是其基于模型对系统未来动态的预测, 通过把约束加到未来的输入、输出或状态变量, 这样就把约束显式表示在一个在线求解的二次规划或非线性规划方程中。
模型预测控制优点是控制效果好、鲁棒性强，能有效地克服不确定性、非线性和并联性，并能够轻松处理被控变量和处理变量中的各种约束条件。
模型预测控制虽然非常强大，但很难实现。
轨迹产生和跟踪方法：基本是两类。
合并法 Combined Trajectory Generation and Tracking
分离法 Separate Trajectory Generation and Tracking
轨迹产生方法：可以分为两种。
基于传感器
基于动力学
轨迹跟踪方法：两个路径。
几何法
模型法
几何法：提到两个方法。
1 Pure pursuit path tracking
其中车的转角
2 The Stanley method（Stanford）
其中车的转角
模型法：基于MPC的三种方法。
Path Tracking Model Predictive Controller：路径跟踪MPC
Unconstrained MPC with Kinematic Model：运动学模型的无限制MPC
MPC Trajectory Controller with Dynamic Car Model ：动力学模型的MPC轨迹控制器
4. González, David, et al. "A Review of Motion Planning Techniques for Automated Vehicles." IEEE Trans. Intelligent Transportation Systems 17.4 (2016): 1135-1145.
这个综述把运动规划分成几类：
图搜索法：Dijkstra algorithm，A-star algorithm (A*)，State Lattice；
采样法：Probabilistic Road Maps (PRM) ，Rapidly-exploring Random Tree (RRT)；
曲线内插法：Lines，Circles，Clothoid，Polynomial，Bezier，Spline；
数值优化：Function Optimization。
5. Paden, Brian, et al. "A survey of motion planning and control techniques for self-driving urban vehicles." IEEE Transactions on intelligent vehicles 1.1 (2016): 33-55.
运动学（kinematic）模型按照数学公式的描述进行车辆驾驶，形成车辆的运动和轨迹。缺点是这种模型允许瞬时方向角改变，但当运动规划模块在这种瞬时变化中生成解决方案会造成问题。
动力学（dynamic）模型考虑了动力学原理，并以此计算施加车辆的外力，比如空气阻力，车辆重量，重力，离心力和车轮摩擦力等。
实际应用中，车辆的的常见基本模型是包括2个刚性连接轮子的“自行车模型”，并且限制在一个平面运动。有时也叫作“运动学模型”，或者“单轨道模型”，如图：其中pr 和pf 是前后轮在路面的接触点，θ 是车辆的前向角度，pr 和pf 的时间微分受限于不完整约束，δ 是前轮的转向角。
以车子后轮着地点为例，得到的微分约束方程是：
同样，以车子前轮的着地点为例，其微分约束方程是：
一般对这个模型而言，规划控制问题就是如何选择转向角δ∈ [δmin,δmax]和前向速度vr ∈ [vmin,vmax]。
图是四轮车辆最基本的Ackerman模型：假设两个前轮转向会有一些区别，满足如下Ackerman条件
其瞬时旋转中心（ICR）可以单纯地用运动学方法计算，如下图所示，ICR纵向位置 ICRx 和横向位置ICRy 满足以下约束：
其中vox 和 voy 是车辆质心的纵向和横向速度。
相比单轨道模型，如此四轮模型的运动学比较复杂，特别是考虑轮胎的滑动特性时候。以下为简化分析，主要采用自行车模型讨论。当车速相当快的时候，轮胎和路面不打滑的假设就不成立了。所以，更准确的车辆模型应该是一个满足基本动量（momentum）准则的刚体，这时候加速度正比于路面加在轮胎上的力量。
如图，设pc 车辆的质心, 那么车辆的运动符合公式：
其中Fr 和 Ff 是通过路面-轮胎的交互该路面加在车上的力, m 是车的总质量, Izz是惯性在eˆz方向关于质心的的极点惯性距。
假设路面平整，可以得到如下的运动学方程：
设sr和sf是轮胎与地面接触点相对地面的运动速度，这些也可看做是轮胎打滑速度。总得讲， sr和sf区别于pr和pf的地方是轮子的角速度，其运动学关系如下：
而轮子角速度为
其中
如下图所示是后轮运动学（前轮类似）示意图。可以算出来后轮的牵引力（前轮的推导一样）为
其中
运动规划
分成path planning（静态环境）和trajectory planning（动态环境，考虑时间元素）两种。
Path planning分成四类：
几何法
变分法
图搜索法
步进搜索法
Trajectory planning
可以采用Path Panning的类似方法。
变分法只做局部规划，可以分成直接法和间接法。
图搜索法可以做全局规划。建立这个图是需要对车的配置空间离散化，方法有多种：
手工车道线图
几何推导
控制/配置的采样
下图是手工车道线图：
几何方法有vertical cell decomposition, generalized Voronoi diagrams, visibility graphs。
采样方法通过两种途径探索自由的配置空间：steering和collision checking。前者包括 Random steering，Heuristic steering，Exact steering，Optimal exact steering。
步进搜索法比较灵活，可以逐步地提供一个可行的方案，如expansive spaces tree (EST) ， Rapidly-exploring Random Trees (RRT) ，Stable Sparse Tree (SST) 等等。
运动控制
其目标是实现path stabilization或者trajectory stabilization，有如下方法：
Path Stabilization for the Kinematic Model: pure pursuit;
Trajectory Tracking Control for the Kinematic Model: control Lyapunov-based design;
Path Stabilization for the Kinematic Model
许多车辆控制法，包括这里讨论的几个，都使用了反馈法则
其中反馈是参考通路（path）上最近点的函数。一个重要问题是闭环向量f(x, u(x)) 不是连续的。如果通路在某一点上是自相交（self intersecting）或不可导，则f(x, u(x))的不连续性将直接落在通路上。如果执行的轨迹遇到不连续性，这会导致不可预测的行为。
许多情况下，分析轨迹的稳定性可以变成确定时变系统中原点的稳定性问题。应该注意的一个问题是，控制器通常用时间趋于无穷大的渐近跟踪误差（asymptotic tracking error）表示。在实践中，参考轨迹是有限的，因此还应该考虑系统的瞬态响应。
纯跟踪控制（Pure Pursuit）
纯跟踪控制（Pure Pursuit）是最早的通路跟踪方法，其思想是基于当前车辆后轮中心位置，在车前面距离L（预瞄距离）处，选择参考路径上的一个点，然后如图拟合一个弧线，其曲率为
给定车速vr，前向角速度为
那么，计算出车身与弦L之间的夹角为
其中(xr , yr )是车辆后轮中心坐标，(xref , yref ) 是预瞄点坐标，θ 是车辆在全局坐标系下的横摆角。
R与α，L之间的关系
在车辆到通路的距离大于L的情况下，控制器未定义输出。另一个考虑因素是参考通路的曲率变化可能导致汽车偏离参考轨迹。这对于沿着道路（road）行驶可能是可接受的，但是对于跟踪泊车操作（parking maneuvers）可能是有问题的。最后，当车速增加时，航向速率ω对反馈角α变得越来越敏感。此问题的常见解决方法是对L相对车速缩放。
后轮位置反馈控制
后轮位置反馈控制是通过后轮中心的跟踪偏差来进行控制量计算。如下图，先要找到参考通路上和后轮距离最近的点：
然后求这个点的切线如下：
而车辆和切线之间的角度差
得到的运动学方程是
然后设计的控制器如下
前轮位置反馈
前轮位置反馈又称作Stanley 控制（斯坦福大学自动驾驶采用），其思想是，通过前轮中心的通路跟踪偏差，来计算控制量。如下图所示，横向位置误差的时间微分是
可以看出它可以直接通过前轮转角 控制，当设
求解前轮转角为
对于有限域内的指数收敛，可以放宽到局部指数收敛，即变成
斯坦福大学的这个方法，在变曲率通路且路径连续可导的的条件下，局部指数收敛到通路。只是倒车比较差，不适合泊车场景。
Trajectory Tracking Control for the Kinematic Model
基于控制Lyapunov函数的控制
基于控制Lyapunov函数的控制设计是在固定到汽车的坐标系中定义跟踪误差。配置误差可以通过使用参考轨迹的惯性坐标系的基础变化和速度（xref，yref，θref，vref，ωref）来表示，
配置误差的动态变化
其中控制分配
闭环误差动态变化写成
对于
其稳定性通过以下Lyapunov方程验证
其中半负定时间导数为
局部分析表明，控制法则提供了局部指数稳定性。但是，对于时不变系统，ωref和vref是常数。
输出反馈线性化
对更高车速的车辆，把转向角限制为连续运动是合适的。添加该状态后，从简单的几何考虑设计控制器变得更加困难。这种情况下，一个不错的选择是对系统进行输出线性化。
使用前轮或后轮位置不容易实现这一点。一种简化的反馈线性化，选择车辆前方任意距离不是零的点，与转向角对齐。
将系统的输出记为xp = xf + dcos（θ+δ）和yp = yf + dsin（θ+δ）。用输出的导数代替前面微分方程的动力学模型，产生
定义公式右边为辅助控制变量，即
从ux和uy，通过逆矩阵恢复原始控制vf和vδ，即
通过上述的输入-输出线性系统，局部轨迹稳定（local trajectory stabilization）的控制实现如下：
Model Predictive Control（MPC）
MPC是一种通用的控制设计方法，可以非常有效地解决高保真的复杂控制问题。从概念上讲，该方法是在较短的时间范围内解决运动规划问题，对产生的开环控制采取较短的间隔，并将其应用于系统。在执行时，MPC重新解决运动规划问题，为下一个时间间隔找到合适的控制。计算硬件以及数学编程算法的进步使预测控制在无人驾驶车辆中的实时变得可行。
控制定律记做：
得到近似线性模型如下：
加扰动的一阶扩展，得到线性时变 linear time varying (LTV)模型如下
这样得到二次优化目标如
一些MPC的变型方法包括 Unconstrained MPC with kinematic models，Path tracking controllers和Trajectory tracking controllers等。
线性化输出反馈控制 Linear Parameter Varying Controllers（LPV）
对于较高的车速，适当的是将转向角限制为连续运动。已经有了状态估计，仅考虑简单的几何关系设计控制器变得更加困难。在这种情况下，一个很好的选择是输出线性化（output-linearize）系统。这种控制仅仅用前轮或后轮位置不容易实现。
以前面的后轮位置反馈控制为例，即横向误差动力学模型
假设跟踪误差足够小，引入一个新控制变量，则
在更高速度情况下，极点进入复平面，导致振荡响应。相比之下，小的 增益会导致低速响应不佳。针对这一挑战，一个非常直观且广泛使用的补救措施是增益调度gain scheduling。该技术属于LPV模型的控制设计类，增益调度是此类控制器设计的经典方法。
最后，以上控制方法总结如下表：
6. Katrakazas, Christos, et al. "Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions." Transportation Research Part C: Emerging Technologies 60 (2015): 416-442.
如上图所示，运动规划分成：
1) route planning
2) path planning
3) manoeuvre choice
4) trajectory planning
目前规划主要是搜索状态空间或者配置空间，而空间的离散化是根本。
RRT方法介绍
Lattice planner方法介绍
整个图的搜索比较费时，所以局部搜索流行，比如某个几何曲线或者该曲线的多个横向移位组合。下图展示的是分别在动作空间和状态空间的局部搜索方法优缺点比较：
Manoeuvre planning方法划分为两类:
强调运动建模和障碍物预测；
关心基于交通建模的决策模块。
Trajectory planning方法中轨迹的表示不同：arcs，clothoids，Nelson polynomials， polynomial spirals, spline curves，Bezier curves。
7. Lefèvre, Stéphanie, Dizan Vasquez, and Christian Laugier. "A survey on motion prediction and risk assessment for intelligent vehicles." Robomech Journal 1.1 (2014): 1.
如上图，综述把运动建模和预测的方法分成：
Physics-based motion models：最简单的模型，基于车动力学，适合短时（1秒以内）预测；
Maneuver-based motion models：除了考虑车体运动之外，还有驾驶意图因素；
Interaction-aware motion models：考虑各个车辆机动之间的相关性。
高斯过程（GP）很适合描述在公路交通中的车体运动模式。不过，GP不能考虑车的物理限制，计算量大。
另外一种方法是先估计驾驶意图，后估计状态。最大的好处是不需要将一个之前观测的轨迹和行驶轨迹匹配。
上下文和启发式可用于确定机动行为，另外一个普遍做法是将机动分成一系列小事件，并用HMM（hidden Markov model）表示这个序列。
不过，不考虑车之间的交互关系也会造成运动预测的错误。这种交互模型表示为样板轨迹，或者采用动态贝叶斯网络。局限性是计算复杂，无法实时性。
除此以外，运动建模和预测还需要风险评估，就是能预测碰撞的可能性。一般是两步：
预测潜在的轨迹；
检测碰撞，推导风险的估计。
紧张也会造成风险，未知行为的预测可以降低这种紧张。两类方法：
检测不支持事件；
检测冲突性的机动。
（一）视频课程来了！
自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、多传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）
（扫码学习最新视频）
视频官网：www.zdjszx.com
（二）国内首个自动驾驶学习社区
近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！
（三）【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；
添加汽车人助理微信邀请入群
备注：学校/公司+方向+昵称