自动驾驶车辆必须执行的安全性关键任务包括:通过与其他车辆和行人共享的动态环境规划动作，以及通过反馈控制来实现其可靠的执行。本文的目的是介绍在城市环境下无人车的动作规划和控制算法。本文对所提出的技术的选择进行了综述，并对其有效性进行了讨论。 本文提炼于A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles
在最高水平上，车辆的决策系统必须从其当前位置到所请求的目的地选择一条路线。通过将道路网络表示为一个有边权值的有向图，与穿越路段的成本相对应，可以将这条路线描述为在路网图上寻找最低成本路径的问题。然而，表示道路网络的图形可以包含数百万条边，使得经典的最短路径算法如Dijkstra或A*不实用。交通网络中有效路径规划的问题引起了交通科学界的极大兴趣，这导致了一个算法家族的发明，在一个一次性的预处理步骤之后，在一个大陆规模的网络上，以毫秒为单位返回最优路径。
图I.1:决策过程层级的说明。目的地被传递给路径规划器，它通过道路网络生成路径。一个关于环境的行为层原因，并生成一个运动规范来沿着选定的路线前进，然后运动规划器解决了实现该规范的可行的运动。反馈控制调整执行变量以纠正在执行引用路径时的错误。
在找到路线计划后，自动驾驶车辆必须能够在选定的路线上行驶，并根据驾驶惯例和道路规则与其他交通参与者进行互动。给定一个指定路线的路段序列，行为层负责根据其他交通参与者的感知行为、道路状况和基础设施的信号，在任何时间点选择合适的驾驶行为。例如，当车辆到达十字路口前的停车线时，行为层将控制车辆在路口停车，观察其他车辆、自行车和行人在十字路口的行为，并让车辆在转弯后继续行驶。 然而，现实世界的驾驶，尤其是在城市环境下的驾驶，以其他交通参与者的不确定性为特征。还研究了其他车辆、自行车和行人的未来轨迹的意向预测和估计问题。提出的解决方案技术包括基于机器学习的技术，例如高斯混合模型，高斯过程回归，在谷歌的自动驾驶系统中用于意向预测的学习技术，以及基于模型的直接估计传感器测量意图的方法。
当行为层决定在当前环境中执行的驱动行为时（例如：巡航, 变道，或右转），所选的行为必须被转换成可以由低级反馈控制器跟踪的路径或轨迹。所产生的路径或轨迹必须是动态可行的，对乘客来说是舒适的，并且避免与车载传感器检测到的障碍物发生碰撞。寻找这样的路径或轨迹的任务是运动规划系统的责任。 对运动规划问题的精确解在大多数情况下是难以计算的。因此，数值逼近方法通常在实际应用中得到应用。最常用的数值方法是将问题作为函数空间中的非线性优化的变分方法，用图形搜索方法来构造车辆状态空间的图形化离散化，并利用图搜索寻找最短路径以及增量树的方法，增量地从车辆的初始状态构建可到达状态树，然后选择这种树的最佳分支。第三节详细讨论了有关自动驾驶的运动规划方法。
针对运动规划系统提供的参考运动，提出了许多有效的反馈控制器。在第四节中详细讨论了相关技术的调查。
在本节中，我们将调查最常用的汽车类车辆的移动模型。这些模型被广泛应用于控制和运动规划算法中，以接近车辆的行为，以响应在相关操作条件下的控制动作。高保真度模型可以准确反映车辆的反应，但增加的细节可能会使规划和控制问题复杂化。这在选择的模型的准确性和决策问题的难度之间进行了权衡。本节概述一般的建模概念和用于运动规划和控制的模型的调查
在最基本的实际应用模型中，汽车由两个由刚性连杆连接的车轮组成[48]-[52]。假设车轮在接触点处不滑动，但可以自由旋转其转动轴。前轮有一个额外自由度，它可以绕轴向运动平面旋转。这是转向模型。这两个造型特征反映了大多数乘客在没有同时向前移动的情况下无法进行横向位移的体验。更正式地说，可操作性的限制被称为非全域约束。非完整约束是表示为对汽车运动的微分限制。这个表达式根据坐标系统的选择而变化。这种模型的变化被称为运动学模型，或单轨模型。
下面是几种常用的配置坐标系中的微分约束的推导。在图II.1引用,向量Pr和Pf表示在静止的或惯性坐标系与基向量(eˆx,eˆy,eˆz)的前后车轮的位置 。θ角度描述车辆面临的方向。定义为向量eˆx 与 Pf 之间的夹角
图II.1 单轨模型的运动学。Pr和Pf分别是后胎和前胎的地面接触点。θ是车辆航向。Pr和Pf的时间导数受到蓝色箭头所指方向的非完整约束的限制。δ是前轮的转向角。
点pr和pf的运动必须与车轮方向共线，以满足无滑动假设。作为方程表示，后轮上的约束是:
(p ̇r·eˆy)cos(θ)−(p ̇r·eˆx)sin(θ)=0
对于前轮:
(p ̇f •eˆy)cos(θ+δ)−(p ̇f •eˆx)sin(θ+δ)=0
运动学模型适用于低速(例如停车机动和城市驾驶)的规划路径，与无滑动假设的限制相比，惯性效应较小。该模型的一个主要缺点是，它允许瞬时转向角的变化，如果运动规划模块通过这种瞬时变化产生解决方案，就会产生问题。
当车辆的加速度足够大时，轮胎与地面之间的无滑动假设失效。在这种情况下，更精确的模型是作为一个刚体满足基本动量原则。也就是说，加速度与轮胎上地面产生的力成正比。以Pc为质心的车辆中心，以及配置的坐标，车辆的运动是受下面控制：
其中Fr和Ff是通过地面轮胎相互作用对车辆施加的力，m是车辆的总质量，而Izz则是在质心的eˆz 方向上的惯性极矩。在以下推导我们默认忽略的Pc在eˆz方向的运动并且假设道路水平,悬架是刚性和车辆仍在路上。
Fr和Ff的表达式根据建模假设的不同而变化，但是在任何情况下，表达式都是可推导的。详细的推导过程见文首提到的外文链接。
本节讨论的模型经常出现在关于无人驾驶汽车运动规划和控制的文献中。它们适用于本调查所讨论的运动规划和控制任务。然而，较低水平的控制任务，如电子稳定控制和主动悬架系统，通常使用更复杂的底盘、转向和驱动列车模型。
运动规划层负责计算一个安全、舒适、动态可行的轨迹，从车辆的当前配置到决策制定层次的行为层提供的目标配置。根据情况不同，目标配置可能不同。例如，目标位置可能是当前车道的中心点，在前方数米的方向。 在行进中，在下一个十字路口的停车线的中心，或下一个想要的停车位。运动规划组件接受关于车辆周围静态和动态障碍物的信息，并产生一个无碰撞的轨道，满足车辆运动的动力学和运动约束。通常，运动规划器也将给定的目标函数最小化。除了旅行时间外，目标函数还可能对引起乘客不适的危险动作或动作进行处罚。在典型的设置中，运动规划器的输出被传递给本地反馈控制层。反过来，反馈控制器会产生一个输入信号来调节车辆跟随这个给定的运动计划。
在接下来的两部分中，我们给出了路径规划和轨迹规划问题的正式问题定义，并对这两种公式的主要复杂度和算法结果进行了讨论。
路径规划问题是要在配置空间的车辆(或更一般的,一个机器人)，找到一个路径σ(α):[0,1]→X,开始在初始配置,达到我们的目标区域在满足给定的全局和当地的约束。根据解决方案路径的质量是否被考虑，使用可行的和最优的条件来描述这条路径。可行路径规划是指在不关注解决方案质量的情况下，确定满足给定问题约束的路径的问题;而最优路径规划是指在给定约束条件下，找到一条优化某些质量准则的路径的问题。
最优路径规划问题可以正式表述如下：假设X是车辆的配置空间,让Σ(X)表示所有连续函数的集合[0,1]→X。车辆的初始配置x init ∈ X。最终所需的路径是在目标地区X goal ⊆ X。车辆的所有允许配置的集合称为自由配置空间，表示Xfree。通常，自由配置是那些不会导致与障碍冲突的，但是自由配置集也可以表示路径上的其他完整约束。路径上的微分约束由一个谓词D(x,x'',x'''， . . . .)表示，并可用于对车辆的路径进行一定程度的平滑水平，如路径曲率或曲率的速率。先看一个最优路径规划问题：
已知问题IV.1中所表述的完整和微分约束的最优路径的问题是。这意味着它至少和解决任何NP完全问题一样困难，因此，假设P = NP，就没有有效的(多项式时间)算法来解决所有的问题。此后，研究的重点一直是研究近似方法，或研究一般运动规划问题的子集。
最初的研究主要集中在可行的，在多边形/多面体环境下的完整车辆模型的非最优路径规划。也就是说，这些障碍被假定为多边形/多面体，并且在结果路径上没有微分约束。众所周知，在具有多边形障碍的二维环境中，没有已知的多项式时间算法来寻找像无人车的最短路径。
由于对于自动驾驶的大多数问题，精确的算法与实际的计算复杂度是不可用的，所以不得不求助于更一般的、数值求解方法。这些方法一般都找不到精确的解，但试图找到一个令人满意的解决方案或一系列可行解，收敛于最优解。这些方法的效用和性能通常是由它们所适用的问题类别和它们对收敛到最佳解决方案的保证所量化的。路径规划的数值方法大致可分为三大类:（后面有详细地介绍）
1、Variational methods：利用非线性连续优化技术对矢量参数进行优化，得到了有限维向量的函数参数化路径。
2、Graph-search methods：车辆作为图形，其中顶点表示车辆配置的有限集合，边缘表示顶点之间的转换。通过在这样的图中搜索最低成本路径，可以找到所需的路径。
3、Incremental search methods：增量搜索方法示例配置空间并且增量地构建一个可达性图(通常是树)，它维护一个独立的可访问的配置集和它们之间的可行转换。一旦图形足够大，以至于至少有一个节点在目标区域中，通过跟踪从开始配置中指向该节点的边来获得所需的路径。与更基本的图形搜索方法相比，基于采样的方法是增量式的。
在动态的环境或有着动态约束的运动规划问题可能更适合放在轨迹规划框架解决，在这个框架中这个问题是一个轨迹，即时间的参数化函数：π（t）：[ 0，T]→X及时地对车辆配置的进行演变。
复杂性:由于动态环境中的轨迹规划是静态环境中路径规划的一般化，因此问题仍然存在PSPACE-hard。此外，动态环境中的轨迹规划已经被证明比路径规划更困难，因为在动态环境中考虑类比问题时，在静态环境中易于处理的某些变量变得难以处理。对于自动驾驶中出现的非平凡轨迹规划问题，无法得到精确的算法，使数值方法成为任务的热门选择。轨迹规划问题可以通过直接在时域上的一些**变分方法（Variational Methods）**来进行数值求解，或者将轨迹规划问题转化为具有附加时间维度的配置空间中的路径规划。
在非线性连续优化的框架下，我们将首先讨论轨迹规划问题。在这种情况下，问题通常被称为轨迹优化。在本小节中，我们将采用轨迹规划公式，并理解这样做不会影响通用性，因为路径规划可以在单位时间间隔内作为轨迹优化来制定。为了利用现有的非线性优化方法，需要将轨迹的无限维函数空间投影到有限维向量空间。
变分方法的两个子类:直接法和间接法。
直接法中的数值逼近方案已被证明是一个非线性规划的轨迹优化问题。我们在这里提到了两个最常见的方案:Numerical integrators with collocation、pseudospectral methods。
- Numerical integrators with collocation：
用搭配和欧拉方法或Runge-Kutta方法来构造非线性程序比其他方法更直接，使其成为一种常见的选择
- Pseudospectral Methods:
积分技术利用时间间隔的离散化，并在配置点之间进行插值函数。伪谱近似方法在此基础上，利用该方法进一步表征插值函数。在配置点之间插值的典型基函数是勒让德或契比雪夫多项式的有限子集。这些方法通常在基本配置方法上提高了收敛速度，在选择配置点和基函数的自适应方法在时尤其如此.
间接方法：Pontryagin的最小原理[89]，是最优控制的著名结果，它提供了解决问题IV .2的最优条件。间接方法，顾名思义，通过寻找满足这些最优性条件的解决方案来解决问题。这些最优性条件被描述为一个常微分方程(ODEs)的增广系统，控制状态和一组共态。然而，该系统在两个点的边界值问题上产生了问题，难以用数值求解。一种技术是改变问题的自由初始条件，并将系统整合进搜索初始条件，从而产生期望的终端状态。这种方法被称为射击方法，该方法的一个版本已被应用于计划一个停车演习。间接方法的优点，如在拍摄方法中，是将优化问题的维数降低到状态空间的维数。 变分方法的主题非常广泛，因此，以上只是选择方法的简要描述。关于这个主题的专门调查，请参见J. T. Betts, “Survey of numerical methods for trajectory optimization,” Journal of Guidance, Control, and Dynamics, vol. 21, pp. 193–207, 1998
尽管在许多情况下有用，但变分方法的适用性受限于局部极小值的收敛性。在本节中，我们将讨论通过在路径空间的离散化版本中执行全局搜索来减轻问题的这一类方法。这些所谓的图形搜索方法将车辆的配置空间X离散化，并以图形的形式表示，然后在这样的图形上搜索最小代价路径。
我们将讨论三种常见的策略:1）手工绘制的lane图 2）从几何图形中提取的图形 3）由控件或配置抽样构造的图形
- Lane Graph: 当路径规划问题涉及到在结构化道路网络上行驶时，一个充分的图离散化可能由表示汽车在每条巷道内的路径和横截面的路径的边组成。路道图通常是由算法生成的。
下面有一个行驶路径的手工图
虽然大多数时候，自动车辆可以按照道路车道图中编码的路径行驶，但有时它必须能够绕过在设计道路网络图时没有考虑到的障碍物，或者在没有被图形覆盖的环境中。例如，一个有缺陷的车辆阻塞了车辆计划穿越的车道——在这种情况下，必须使用更通用的运动规划方法，以在检测到的障碍物周围找到无碰撞的路径。
-
Geometric Methods :在本节中，我们将重点讨论与障碍物几何表示相关的路径规划方法。我们将首先关注无差异约束的路径规划，因为这是一种有效的精确路径规划算法。
-
Sampling-based Methods（基于采样的方法）:在自动驾驶中，一般不直接使用Xfree的几何模型，从原始的sensoric数据中构建它的成本太高。此外，对结果路径的要求通常要比简单的最大曲率约束复杂得多。这可能解释了基于samplon的技术的流行，这些技术并没有强制执行自由配置集和动态约束的特定表示。基于抽样的方法不是通过几何表示来进行推理，而是利用转向和碰撞检查例程来探索自由配置空间的可达性。
有了转向和碰撞检查功能，主要的挑战是如何构造一个离散化，它可以很好地逼近Xfree的连接性，而不能访问其几何的显式模型。我们将从文献中回顾基于采样的离散化策略。
一种简单的方法是选择一组运动原语(固定的机动)并通过递归地应用它们从车辆的初始配置xinit开始生成搜索图，例如，使用算法1中的方法。对于没有微分约束的路径规划，运动原语可以是一组具有不同方向和长度的直线。对于类似汽车的车辆，这样的运动原语可能是由一组弧线表示汽车所遵循的不同的转向值。各种各样的技术可以用来为无人驾驶车辆生成运动原语。一种简单的方法是对一些控制输入进行采样，并利用车辆模型模拟远期，以获得可行的运动。为了有连续的曲率路径，有时也会使用一些回旋分段。原始的运动也可以通过记录由专家驱动的车辆的运动来获得。
大多数基于采样的路线图构建方法遵循算法2中所示的算法方案，但在实现采样点(X, n)和邻居(X, V)例程时有所不同。函数采样点(X,n)代表了战略选择n个点的配置空间X,而函数的邻居(X,V)代表了战略选择一组相邻顶点为顶点n⊆V X,X算法将尝试连接到的路径段用一个精确的转向功能,steerexact(X,y)。
- Graph Search Strategies: 图形搜索策略:在前一节中，我们讨论了以图形的形式将自由配置空间离散化的技术。为了在这种离散化过程中获得一个实际的最优路径，必须使用一个图形搜索算法。在本节中，我们将回顾与路径规划相关的图形搜索算法。
在图中找到最短路径的最普遍的算法可能是Dijkstra算法。该算法执行了最佳的第一次搜索来构建一个树，它表示从给定的源顶点到图中所有其他顶点的最短路径。当只需要一条路径到单个顶点时，就可以使用启发式来指导搜索过程。最著名的heuristic search algorithm 启发式搜索算法是由Hart, Nilsson和Raphael开发的A* 。如果所提供的启发式函数是可采纳的(即，它从来没有高估成本，A 已经被证明是最优的效率，并且保证返回一个最优解*。对于许多问题，利用加权A可以得到一个有界次优解，该方法的计算工作量较小，它对应的是简单地将启发式乘以一个常数因子ε > 1。可以证明，用这样一个膨胀的试探法A*返回的解路径，保证不低于(1 + ε )的最优路径的所花的时间。
在配置空间的图离散化中寻找路径的算法的一个明显的限制是，在此图上得到的最优路径可能比配置空间中真正的最短路径长得多。任何角度的路径规划算法都被设计成在网格上运行，或者更一般地在表示自由配置空间的单元分解的图上，并试图通过考虑在搜索过程中图形顶点之间的“捷径”来缓解这一缺点。此外，Field D*将线性插值引入到搜索过程中，以产生平滑的路径。
在固定图形离散化过程中搜索的技术的一个缺点是，它们只搜索可由图离散化的原语构造的路径集。因此，这些技术可能无法返回一条可行的路径或返回一个明显的次优路径。
增量可行的运动计划者努力解决这个问题，并提供可行的路径到任何运动规划问题实例，如果存在，给定足够的计算时间。
增量路径规划的一种重要的方法是基于增量的思想，即在车辆的初始配置中建立一棵树，以探索可到达的配置空间。“探索性”行为是通过迭代地从树中选择一个随机的顶点来实现的，并通过应用它的转向函数来扩展选定的顶点。一旦树长到足够大到达到目标区域，就可以通过跟踪目标区域中的顶点与初始配置之间的链接来恢复生成的路径。在算法3中描述了增量树算法的一般算法。
在一定的简化假设条件下，RRT算法被证明具有概率完备性。我们注意到，关于概率完整性的结果并不容易推广到经常使用启发式转向的许多实际实现的RRT版本中。事实上，最近在有关文献中已经表明，使用带有固定时间步的启发式转向的RRT是不可能完成的。
自动驾驶车辆的路径规划方法有三种:变分法、图搜索方法和增量树方法。 自动驾驶系统的实际部署算法来自上述所有类别
例如，即使在美国国防部高级研究计划局(DARPA)城市挑战赛的前四名成功参与者中，运动规划的方法也存在显著差异。挑战的优胜者，CMU的Boss车辆在结构环境中使用了局部轨迹生成的变分技术，以及在4维配置空间(由位置、方向和速度组成)的网格图(包括位置、方位和速度)，以及Anytime D* 以便任何时候在停车场找到无碰撞的路径。据报道，斯坦福大学研究小组开发的第二辆车使用了一种名为“Hybrid A* ”的搜索策略，在搜索过程中，通过递归地应用有限的一组机动动作，将一棵运动原语的树懒地构造出来。搜索是由一个精心设计的启发式和树的稀疏性所引导的，只在给定的配置空间区域内保持一个节点。类似地，由维吉尼亚理工学院的VictorTango团队开发的车辆到达第三阶段，构造了一种可能机动的图形离散化，并使用a *算法对图进行搜索。最后，麻省理工学院开发的车辆采用了一种RRT算法的变体，称为闭环RRT，带有偏置抽样。
最早提出的路径跟踪策略是纯粹追求。这一策略及其变化由于其简单的实现和令人满意的性能，已被证明是车辆控制不可缺少的工具。许多出版物，包括DARPA Grand Challenge的两辆车和DARPA城市挑战赛的三辆车都使用了Pure Pursuit的追踪控制器。
使用后轮位置作为输出来稳定后轮的轨迹。控制器分配为：
该方法被提出并用于斯坦福大学2005年DARPA挑战赛。方法是将前轮位置作为受调节变量，控制使用变量s(t),e(t)和θe(t)与前面的部分,修改,计算e(t)与前轮的位置相对于后轮的位置。利用横向误差的时间导数揭示：
纯追踪控制在没有曲率的时间段内跟踪参考路径。在具有高曲率的区域的路径，纯跟踪控制导致系统偏离参考路径。与此相反，后两个控制器向路径集中，并在高曲率区域跟踪它，形成互补。
基于控制李亚普诺夫函数的控制设计。方法是在固定在汽车上的坐标系中定义跟踪误差。配置误差可以通过参考轨迹和速度(xref, yref, ref, vref, ref)从惯性坐标系的基础上的变化来表示。
(a)当曲率为非零时，纯粹的追求偏离参考。
(b)后轮输出控制器驱动后轮到后轮参考路径。超调是系统二阶响应的结果。
(c)前轮输出的控制器驱动前轮对参考路径的一级反应，并通过机动跟踪路径。
对于较高的车辆速度，应将转向角限制为连续运动。随着的状态变化，从简单的几何考虑设计控制器变得更加困难。在这种情况下，一个好的选择是输出线性化系统。
上述简单的控制律适用于适当的驾驶条件。然而，湿滑的道路或紧急机动可能需要更精确的模型，例如第III-B节所介绍的模型。更复杂模型的附加细节使控制设计复杂化，使控制器难以从配置空间的直觉和几何结构中构造控制器 纯追求:最早提出的路径跟踪策略是纯粹追求。这一策略及其变化由于其简单的实现和令人满意的性能，已被证明是车辆控制不可缺少的工具。许多出版物，包括DARPA Grand Challenge的两辆车和DARPA城市挑战赛的三辆车都使用了纯粹的追踪控制器
以下是在汽车控制器文献中发现的模型预测控制框架的一些变体:
- Unconstrained MPC with Kinematic Models
2）Path Tracking Controllers
- Trajectory Tracking Controllers
许多控制器设计技术是线性的。使线性模型成为可取的系统。然而，在正常驾驶条件下所遇到的广泛的操作点使其很难依赖于一个单一操作点的线性化模型。
横向控制的LPV控制设计的提出，使用LPV模型，并通过预测控制方法实现路径和轨迹的稳定。在自动化水平较低的情况下，提出了集成系统控制的LPV控制技术。在这些设计中，几个子系统结合在单个控制器下，以实现改进的处理性能。
无人驾驶汽车是一种复杂的系统，它已被分解成一个决策问题的层级结构，其中一个问题的解决方案是下一个问题的输入。分解为个人决策问题使我们能够利用各种研究领域的先进方法和技术。然后，任务是将这些方法集成起来，使它们的交互具有语义上的有效性，并且组合的系统在计算上是有效的。一种更有效的运动规划算法可能只能与计算密集型反馈控制器(如模型预测控制)兼容。相反，简单的控制法可能需要更少的计算来执行，但是也不那么健壮，需要使用更详细的模型来进行运动规划。