城市道路自动驾驶车辆运动规划和控制技术综述(2)

本文为翻译《A survey of motion planning and control techniques for self-driving Urban vehicles》部分内容.

IV.运动规划

　　运动规划层负责计算从车辆的当前构型到决策层级的行为层提供的目标构型的安全，舒适且动态可行的轨迹。根据环境，目标构型可能不同。例如，目标位置可以是当前车道的中心点，即行进方向前方的数米，下一个交叉点处的停止线的中心，或下一个期望的停车位。运动规划组件接受关于车辆周围的静态和动态障碍物的信息，并产生无碰撞轨迹，其满足对车辆运动的动力学和运动学约束。通常，运动规划器也最小化给定的目标函数。除了旅行时间之外，目标函数还可以是危险运动或导致乘客不适的运动的惩罚函数。在典型设置中，运动规划器的输出然后被传递到本地反馈控制层。反过来，反馈控制器产生输入信号以调节车辆以遵循该给定的运动计划。
　　车辆的运动规划可以采用路径或轨迹的形式。在路径规划坐标系内，可行路径表示为函数 $σ (α) : [0, 1] \to X$ ，其中 $X$ 是车辆的构型空间。请注意，此类解决方案未规定应如何跟踪此路径，并且可以选择路径的速度规划方式或将此任务委派给决策层次结构的较低层。在轨迹规划框架内，控制执行时间需要明确考虑。这种考虑必须在车辆动力学和动态障碍物进的允许范围内。在这种情况下，可行轨迹表示为时间参数化函数 $π (t) [0, T] \to X$ ，其中 $T$ 是规划周期。与路径不同，轨迹规定了车辆构型如何随时间演变。
　　在以下两节中，我们提供了路径规划和轨迹规划问题的正式问题定义，并回顾了两种规划的主要复杂性和算法结果。

A. 路径规划

　　路径规划问题是在车辆的构型空间 $X$ （或更一般地，机器人）中找到路径 $σ (α) : [0, 1] \to X$ ，该路径从初始构型开始并到达目标区域，同时满足给定的全局和局部约束。根据是否考虑解决方案路径的质量，使用可行和最优的术语来描述该路径。可行路径规划是指在不关注解决方案质量的情况下确定满足某些给定问题约束的路径的问题; 而最优路径规划指的是找到一条路径，该路径优化了受给定约束条件限制的某些质量标准。
　　最佳路径规划问题可以正式陈述如下。设 $X$ 为车辆的构型空间， $\sum (X)$ 表示 $[0, 1] \to X$ 的所有连续函数的集合。车辆起始构型为 $x_{i n i t} \in X$ .路径要在目标区域 $X_{g o a l} \subseteq X$ 终止。所有车辆可达构型成为*构型空间，记为 $X_{f r e e}$ 。典型的*构型为不会与障碍物发生碰撞的构型，但是*构型集也可以表示路径上的其他完整约束。路径上的微分约束用属于 $D (x, x^{'}, x^{″}, . . .)$ 表示，可用于加强车辆路径的某种程度的平滑度，例如路径曲率和/或曲率的界限。例如，在 $X \subset R^{2}$ 情况下，微分约束可以使用Frenet-Serret公式强制执行路径的最大曲率，如下所示：
　　

D (x, x^{'}, x^{″}, . . .) \Leftrightarrow \frac{‖ x^{'} \times x^{″} ‖}{‖ x^{'} ‖^{3}}

　　进一步地，设

J (σ) : \sum (X) \to R

为代价泛函。然后，路径规划问题的最佳版本通常可以如下陈述。
　　问题 IV.1 (最优路径规划)给定5-元组

(X_{f r e e}, x_{i n i t}, X_{g o a l}, D, J)

,找到

σ^{*} =

\underset{α \in \sum (X)}{\arg min} J (σ) subj. to σ (0) = x_{i n i t} and σ (1) \in X_{g o a l} σ (α) \in X_{f r e e} \forall α \in [0, 1] D (σ (α), σ^{'} (α), σ^{″} (α), . . . .) \forall α \in [0, 1]

　　在过去的几十年中，人们对可行和最优路径规划的问题进行了广泛的研究。很容易理解这个问题的复杂性，并且已经开发了许多实用的算法。
　　复杂性：大量文献致力于研究运动规划问题的复杂性。以下是关于这些问题的计算复杂性的一些主要结果的简要调查。
　　在问题IV.1中公开的找到受完整和差分约束影响的最佳路径的问题已知是PSPACE-hard。这意味着它至少与解决任何NP完全问题一样困难，因此，假设P ≠ NP，则没有能够解决问题的所有实例的有效（多项式时间）算法。此后，研究注意力一直致力于研究近似方法或一般运动规划问题子集的方法。
　　初步研究主要集中在多边形/多面体环境中的完整车辆模型的可行（即，非最佳）路径规划。也就是说，假设障碍物是多边形/多面体，并且在得到的路径上没有微分约束。 1970年，Reif [64]发现，在2-D和3-D环境中，可以在多项式时间内找到完整车辆的无障碍路径，其足迹可以描述为单个多面体.Canny [65] 已经表明，使用多项式表示的*空间中可行路径规划的问题在PSPACE中，这使得可行路径规划的决策版本没有差分约束作为PSPACE完全问题。
　　对于最优规划，目标是找到最短的无障碍路径。众所周知，在多项式时间内可以找到具有多边形障碍的二维环境中完整车辆的最短路径[66]，[67]。更准确地说，它可以在时间

O (n^{2})

中计算，其中

n

是多边形障碍物的顶点数[68]。这可以通过构建和搜索所谓的可视图来解决[69]。相比之下，Lazard，Reif和Wang [70]确定在多边形障碍物（即，类似汽车的机器人的路径）中在二维平面中找到最短曲率有界路径的问题是NP难的，其中表明没有已知的多项式时间算法来找到多边形障碍物中类似汽车的机器人的最短路径。一个相关的结果是，在多边形环境中存在曲率约束路径可以在EXPTIME [71]中确定。
　　可以有效计算解决方案的特殊情况是无障碍环境中的最短曲率有界路径。 Dubins [57]已经表明，在给定的两点

p_{1}, p_{2}

和规定的切线

θ_{1}, θ_{2}

之间具有由

k

限定的曲率的最短路径是由至多三个段组成的曲线，每个段是圆弧段或直线。 Reeds和Shepp [58]扩展了可以向前和向后移动的汽车的方法。由Agraval等[72]引起的另一个值得注意的案例是

O (n_{2} \log n)

算法，用于在凸多边形内找到具有有界曲率的最短路径。类似地，Boissonnat和Lazard [73]提出了一种多项式时间算法，用于在障碍物具有有界曲率边界的环境中找到精确的曲率有界路径。
　　由于对于自动驾驶感兴趣的大多数问题，具有实际计算复杂性的精确算法是不可用的，人们不得不采用更通用的数值求解方法。这些方法通常找不到精确的解决方案，但试图找到一个满意的解决方案或一系列可行的解决方案，这些解决方案会聚到最优解决方案。这些方法的效用和性能通常通过它们适用的问题类别以及它们收敛到最佳解决方案的保证来量化。路径规划的数值方法可大致分为三大类：
　　变分方法将路径表示为由有限维向量参数化的函数，并且通过使用非线性连续优化技术对向量参数进行优化来寻求最优路径。这些方法对于快速收敛到局部最优解决方案具有吸引力; 然而，除非提供适当的初始猜测，否则它们通常缺乏找到全局最优解的能力。有关变分方法的详细讨论，请参阅第IV-C节。
　　图搜索方法将车辆的构型空间离散化为图形，其中顶点表示车辆构型的有限集合，并且边缘表示顶点之间的过渡。通过在这样的图中搜索最小成本路径来找到期望的路径。图搜索方法不容易陷入局部最小值，但是，它们仅限于在有限的一组路径上进行优化，即可以从图中的元运动构造的路径。有关图搜索方法的详细讨论，请参阅第IV-D节。
　　增量搜索方法对构型空间进行采样，并逐步构建可达性图（通常是树），该图可维护一组离散的可达构型以及它们之间的可行转换。一旦图形足够大以使至少一个节点在目标区域中，则通过跟踪从起始构型导致该节点的边缘来获得期望的路径。与更基本的图搜索方法相比，基于采样的方法逐渐增加图的大小，直到在图中找到满意的解。有关增量搜索方法的详细讨论，请参阅第IV-E节。
　　显然，通过组合它们可以利用这些方法中的每一种的优点。例如，可以使用粗略图搜索来获得变分方法的初始猜测。表1给出了选择路径规划方法的关键属性的比较。在本节的其余部分，我们将详细讨论路径规划算法及其属性。

B. 轨迹规划

　　动态环境中或动态约束下的运动规划问题可以更适当地在轨迹规划框架中制定，其中问题的解决方案是轨迹，即时间参数化函数 $π (t) : [0, T] \to X$ ，其规定车辆的配置的演变及时。
　　令 $\prod (X, T)$ 表示所以连续函数 $[0, T] \to X$ 的集合，车辆起始构型为 $x_{i n i t} \in X$ .路径结束点为 $x_{g o a l} \in X$ 。在 $t \in [0, T]$ 时刻所有可行构型构成的集合定义为 $X_{f r e e} (t)$ 用于构成完整约束，比如用于路径上避免与静止、及动态障碍物发生碰撞。路径上的微分约束用属于 $D (x, x^{'}, x^{″}, . . .)$ 表示，可用于增强轨迹上的动态约束。进一步，令 $J (π) : \prod (X, T) \to R$ 为损失函数。基于该假设，轨迹规划问题的最佳版本可以一般化地表述为：
　　问题 IV.2(最优轨迹规划)给定6-元组 $(X_{f r e e}, x_{i n i t}, X_{g o a l}, D, J ， T)$ ,找到 $π^{*} =$
　　

\underset{π \in Π (X ， T)}{\arg min} J (π) subj. to π (0) = x_{i n i t} and π (T) \in X_{g o a l} π (t) \in X_{f r e e} \forall t \in [0, T] D (π (t), π^{'} (t), π^{″} (t), . . . .) \forall t \in [0, T]

　　复杂性：由于动态环境中的轨迹规划是静态环境中路径规划的一般化，因此问题仍然为PSPACE-hard。此外，动态环境中的轨迹规划已被证明比路径规划更难，因为在动态环境中考虑类比问题时，在静态环境中易处理的问题的某些变体变得难以处理。特别是，回想一下，在多项式时间内可以有效地找到静态二维多边形环境中点机器人的最短路径，并将其与Canny和Reif [76]的结果进行对比，确定找到速度有界无碰撞轨迹在移动的多边形障碍物中的完整点机器人是NP难的。类似地，虽然在三维多面体环境中具有固定*度的机器人的路径规划易于处理，但Reif和Sharir [85]确定了在平移和旋转三维多面体障碍物中具有2个*度的机器人的轨迹规划是PSPACE-hard。
　　易控精确算法不适用于自动驾驶中发生的非平凡轨迹规划问题，使得数值方法成为该任务的流行选择。可以直接在时域中使用一些变分方法或通过将轨迹规划问题转换为具有附加时间维度的配置空间中的路径规划来数值地解决轨迹规划问题。
　　一般按照下列方法从轨迹规划问题

(X_{f r e e}^{T}, x_{i n i t}^{T}, X_{i n i t}^{T}, D^{T}, J^{T}, T)

转换为路径规划问题

(X_{f r e e}^{T}, x_{i n i t}^{P}, X_{i n i t}^{P}, D^{P}, J^{P})

。路径规划发生的配置空间定义为

X^{P} := X^{T} \times [0, T]

。对于任意的

y \in X^{P}

，用

t (y) \in [0, T]

表示点

y

时间部分，

c (y) \in X^{T}

表示点

y

构型部分。一条路径

σ (α) : [0, 1] \in X^{P}

可以被转换为轨迹

π (t) : [0, T] \in X^{T}

，如果路径的起始和终点被定义为
　　

t (σ (0)) = 0 t (σ (1)) = T

并且路径为单调递增的，该条件可以用微分约束进行限制：

t (σ^{'} (α)) > 0 \forall α \in [0, 1] .

　　此外，*构型空间，初始构型，目标区域和微分约束被映射到它们的路径规划对应物，如下所示：
　　

X_{f r e e}^{P} = (x, t) : x \in X_{f r e e}^{T} (t) \land t \in [0, T] x_{i n i t}^{P} = (x_{i n i t}^{T}, 0) X_{g o a l}^{P} = {(x, T) : x \in X_{g o a l}^{T}} D^{P} (y, y^{'}, y^{″}, . . .) = D^{T} (c (y), \frac{c (y^{'})}{t (y^{'})}, \frac{c (y^{″})}{t (y^{″})}, . . .)

　　然后使用可以处理差分约束并转换回轨迹形式的路径规划算法找到这种路径规划问题的解决方案。

C. 变分方法

　　我们将首先在非线性连续优化的框架内解决轨迹规划问题。在这种情况下，问题通常被称为轨迹优化。在本小节中，我们将采用轨迹规划公式，并理解这样做不会影响一般性，因为路径规划可以表示为单位时间间隔内的轨迹优化。为了利用现有的非线性优化方法，有必要将轨迹的无限维函数空间投影到有限维向量空间。此外，大多数非线性编程技术要求将问题IV.2中提出的轨迹优化问题转换为以下形式
　　　　

\underset{π \in Π (X ， T)}{\arg min} J (π) subj. to π (0) = x_{i n i t} and π (T) \in X_{g o a l} f (π (t), π^{'} (t), . . .) = 0 \forall t \in [0, T] g (π (t), π^{'} (t), . . .) \leq 0 \forall t \in [0, T]

其中完整和微分约束被表示为一个等式和不等式约束的系统。
　　在一些应用中，使用惩罚或屏障函数将约束优化问题放宽到不受约束的优化问题。在这两种情况下，约束都被增强的成本函数所取代。使用惩罚方法，成本函数采用形式
　　

\tilde{J} = J (π) + \frac{1}{ε} \int_{0}^{T} [{‖ f (π, π^{'}, . . .) ‖}^{2} + ‖ max (0, g (π, π^{'}, . . .)) ‖] d t

　　类似地，可以使用障碍函数代替不等式约束。在这种情况下，增强成本函数采用的形式
　　

\tilde{J} = J (π) + ε \int_{0}^{T} h (π (t)) d t

　　其中，障碍函数满足

g (p i) < 0 \Rightarrow h (π) < \infty ， g (p i) \geq 0 \Rightarrow h (π) = \infty

，并且

lim_{g (π) \to 0} h (π) = \infty

。
　　两个增强成本函数背后的直觉是，通过使

ϵ

小，成本最小值将接近原始成本函数的最小值。障碍函数的一个优点是局部最小值仍然可行，但必须用一个可行解初始化以具有优先的增加成本。另一方面，惩罚方法可以用任何轨迹初始化并优化到局部最小值。然而，局部最小值可能违反问题约束。在[60]中提出了使用障碍函数的变分公式。坐标的变化用于将车辆在道路上保留的约束转换为线性约束。对数障碍与类似牛顿的方法一起使用，与内点法相似。该方法有效地计算了一段车道上的详细车辆模型的最小时间轨迹。
　　接下来讨论变分方法的两个子类：直接方法和间接方法。
　　直接方法：直接变分方法背后的一般原则是将近似解限制为

\prod (X, T)

的有限维子空间。为了这个目的，一般归纳为：
　　

π (t) \approx \tilde{π} (t) = \sum_{i = 1}^{N} π_{i} ϕ_{i} (t)

　　其中

π_{i}

为

R

的系数，

ϕ_{i} (t)

为选定子控件的基础函数。已经证明许多数值近似方案可用于将轨迹优化问题表示为非线性程序。我们在这里提到两种最常见的方案：具有配点法和伪谱方法的数值积分器。
　　1)基于小波的数值积分器：with collocation，要求近似轨迹满足一组离散点

{t_{j}}_{j = 1}^{M}

中的约束。该要求导致两个离散约束系统：近似于系统动力学的非线性方程组系统
　　

f (\tilde{π} (t_{j}), {\tilde{π}}^{'} (t_{j})) = 0 \forall j = 1, . . ., M

　　和一种非线性不等式系统，它近似于轨迹上的状态约束
　　

g (\tilde{π} (t_{j}), {\tilde{π}}^{'} (t_{j})) \leq 0 \forall j = 1, . . ., M

　　数值积分技术用于近似collocation点之间的轨迹。例如，分段线性
　　

ϕ_{i} (t) = {\begin{aligned} (t - t_{i - 1}) / (t_{i} - t_{i - 1}) if t \in [t_{i - 1}, t_{i}] \\ (t_{i + 1} - t) / (t_{i + 1} - t_{i}) if t \in [t_{i}, t_{i + 1}] \\ 0 其他 \end{aligned}

与collocation一起产生欧拉积分方法。高阶多项式结果为Runge-Kutta族积分方法。使用collocation和欧拉方法或Runge-Kutta方法之一编写非线性程序比其他方法更直接，使其成为流行选择。在[14]中提出了一种成功使用欧拉方法进行轨迹数值逼近的实验系统。
　　与Euler方法相比，Adams近似在[87]中进行了研究，以优化详细车辆模型的轨迹，并显示出提高了数值精度和收敛速度。
　　２）伪谱方法：数值积分技术利用时间间隔的离散化和collocation点之间的插值函数。伪谱近似方案通过另外用基础表示内插函数来建立在该技术上。在collocation点之间插值的典型基函数是勒让德或切比雪夫多项式的有限子集。当使用自适应方法选择配置点和基函数时，这些方法通常具有比基本配置方法更高的收敛速度，尤其如此[88]。
　　间接方法：Pontryagin的最小原则[89]是最优控制的一个著名结果，它为问题IV.2提供了解决方案的最优性条件。顾名思义，间接方法通过找到满足这些最优性条件的解决方案来解决问题。这些最优性条件被描述为控制状态和一组共态的常微分方程（ODE）的增强系统。然而，这种ODE系统导致两点边界值问题，并且难以在数值上解决。一种技术是改变问题的*初始条件并将系统向前集成以寻找导致所需终端状态的初始条件。这种方法被称为射击方法，这种方法的一个版本已经应用于停车机动计划。与拍摄方法的情况一样，间接方法的优点是将优化问题的维数降低到状态空间的维度。
　　变分方法的主题非常广泛，因此，以上仅是对选择方法的简要描述。参见[91]，[92]。

D. 图搜索方法

　　虽然在许多情况下都很有用，但变分方法的适用性受到它们仅收敛到局部最小值的限制。在本节中，我们将讨论尝试通过在离散化版本的路径空间中执行全局搜索来缓解问题的方法类。这些所谓的图搜索方法将车辆的构型空间 $X$ 离散化并以图形的形式表示，然后在这样的图上搜索最小成本路径。
　　在这种方法中，构型空间被表示为图 $G = (V, E)$ ，其中被称为顶点的 $V \subset X$ 为选定配置的离散集， $E = {(o_{i}, d_{i}, σ_{i})}$ 为边的集合，其中 $o_{i} \in V$ 边的起点， $d_{i}$ 表示边的终点， $σ_{i}$ 表示连接 $o_{i} 和 d_{i}$ 的路径段。假设路径段 $σ_{i}$ 连接两个顶点： $σ_{i} (0) = o_{i} 和 σ_{i} (1) = d_{i}$ 。此外，假设初始配置 $x_{i n i t}$ 是图的顶点。边以这样的方式构造，使得与它们相关联的路径段完全位于 $X_{f r e e}$ 中并满足差分约束。结果，通过连接与通过图的路径边相关联的路径段，可以将图上的任何路径转换为车辆的可行路径。
　　存在许多用于构建离散化车辆的*构型空间的图的策略。在下面的小节中，我们将讨论三种常见的策略：手工制作的车道图，从几何表示得出的图和通过控制或构型采样构建的图。
　　1）车道图：当路径规划问题涉及在结构化道路网络上行驶时，足够的图形离散化可以包括表示车辆应该在每个车道内遵循的路径的边和穿越交叉点的路径。
　　道路车道图通常部分地通过较高级别的街道网络地图和部分人工编辑的算法生成。这种图的一个例子如图IV.1所示：
　　城市道路自动驾驶车辆运动规划和控制技术综述(2)
　　虽然大多数时候自动驾驶车辆遵循道路车道图中编码的路径就足够了，但有时它必须能够在设计道路网络图时未考虑的障碍物周围或在图未覆盖的环境中导航。考虑例如阻挡车辆计划穿过的车道的故障车辆 - 在这种情况下，必须使用更一般的运动规划方法来找到围绕检测到的障碍物的无碰撞路径。
　　一般路径规划方法可以基于它们如何表示环境中的障碍而大致分为两类。所谓的几何或组合方法与障碍物的几何表示一起工作，其中实际上障碍物最常使用多边形或多面体来描述。另一方面，所谓的基于抽样的方法从内部表示障碍的方式中抽象出来，并且仅假设访问确定任何给定路径段是否与任何障碍物碰撞的函数。
　　2）几何方法：在本节中，我们将重点介绍使用障碍物几何表示的路径规划方法。我们将首先关注路径规划而不受差别约束，因为对于这种表述，存在有效的精确路径规划算法。虽然不能强制执行微分约束限制了传统驾驶汽车的路径规划，因为无法考虑最小转弯半径的约束，这些方法可用于获得曲率约束路径长度的下限和上限和路径规划可以在现场转向更具外来的汽车结构。
　　在路径规划中，术语路线图用于描述 $X_{f r e e}$ 的图形离散化，它很好地描述了*构型空间的连通性，并且具有 $X_{f r e e}$ 中的任何点可以从路线图的某些顶点轻易到达的属性。当可以使用线性或半代数模型几何地描述集合 $X_{f r e e}$ 时，可以在算法上构造用于 $X_{f r e e}$ 的不同类型的路线图，并且随后用于获得完整的路径规划算法。最值得注意的是，对于 $X_{f r e e} \subset R^{2}$ 和构型空间的多边形模型，存在几种用于构建这种路线图的有效算法，例如垂直单元分解，广义Voronoi图和可视图。对于由一般半代数模型描述的更高维度构型空间，可以使用称为圆柱代数分解的技术在构型空间中构建路线图，从而为非常一般的路径规划问题提供完整的算法。这个类中最快的是Canny 开发的算法，它在构型空间的维度上具有（单个）指数时间复杂度。然而，结果大部分是理论性质的，迄今没有任何已知的实施方式。
　　由于其与类似汽车的车辆的路径规划的相关性，对于具有最大曲率约束的路径规划问题也存在许多结果。 Backer和Kirkpatrick提供了一种算法，用于构造具有有界曲率的路径，该路径是域的特征数量的多项式，输入的精度以及连接指定构型的最简单的无障碍Dubins路径上的段数。由于在多边形障碍物中找到具有有界曲率的最短路径的问题是NP难的，因此没有精确的多项式求解算法也就不足为奇了。 Jacobs和Canny首先提出了在多边形障碍物中寻找最短曲率有界路径的近似算法，后来由Wang和Agarwal以时间复杂度 $O (\frac{n^{2}}{ϵ^{4}} l o g n)$ 进行了改进，其中 $n$ %是障碍物的顶点数， $ϵ$ 是近似因子。对于所谓的中等障碍物的特殊情况，其特征是具有由 $k$ 限定的曲率的平滑边界，Boissonnat和Lazard已经开发了用于找到具有至多 $k$ 的边界的曲率的精确多项式算法。
　　3) 基于采样的方法：在自动驾驶中， $X_{f r e e}$ 的几何模型通常不能直接获得，并且从原始传感器数据构造成本太高。此外，对结果路径的要求通常比简单的最大曲率约束复杂得多。这可以解释基于采样的技术的普及，其不强制执行*构型集和动态约束的特定表示。基于抽样的方法不是推理几何表示，而是使用转向和碰撞检测程序探索*构型空间的可达性：
　　转向函数 $s t e e r (x, y)$ 返回从构型 $x$ 开始朝向构型 $y$ （但不一定到达 $y$ ）的路径段，确保满足约束，即，所得到的运动对于考虑的车辆模型是可行的。实现转向功能的确切方式取决于使用它的环境。在文中讨论的一些典型方法为：
　　1）随机转向：该函数返回一个路径，该路径是通过车辆的正向模型从状态 $x$ 应用随机控制输入得到的，用于固定或可变时间步长。
　　2)启发式转向：该函数返回一个路径，该路径来自应用启发式构造的控制，以引导系统从 $x$ 向 $y$ 引导。这包括从预先设计的离散集（库）操纵中选择操纵。
　　3)精准转向：该函数返回一条可行路径，指导系统从 $x$ 到 $y$ 。这样的路径对应于2点边界值问题的解。对于一些系统和成本函数，可以解析地获得这样的路径，例如，用于完整系统的直线，用于向前移动的单轮脚踏车的Dubins曲线，或用于双向单轮车的Reeds-Shepp曲线。对于差分平坦系统也存在解析解，而对于更复杂的模型，可以通过求解双点边界值问题来获得精确的转向。
　　4)最优精准转向：该函数返回相对于给定成本函数的最佳精确转向路径。实际上，假设成本函数是路径的弧长，直线，Dubins曲线和前一点的Reeds-Shepp曲线是最优解。
　　如果路径段 $σ$ 完全位于 $X_{f r e e}$ 中，碰撞检测函数 $c o l - f r e e (σ)$ 返回真。其用于确保作为结果的路径不与任何障碍物发生碰撞。
　　获得转向和碰撞检查功能后，主要挑战变成如何构建一个能够很好地接近 $X_{f r e e}$ 连接的离散化，而无需访问其几何的显式模型。我们现在将从文献中回顾基于抽样的离散化策略。
　　一种直接的方法是选择一组运动基元（固定机动）并通过从车辆的初始配置 $x_{i n i t}$ xinit开始递归地应用它们来生成搜索图，例如，使用算法1中的方法。对于没有差分约束的路径规划，运动基元可以只是一组具有不同方向和长度的直线。对于类似汽车的车辆，这种运动基元可以通过一组弧来表示汽车将遵循不同的转向值的路径。可以使用各种技术来为无人驾驶车辆生成运动原语。一种简单的方法是采样多个控制输入并使用车辆模型及时模拟前方以获得可行的运动。为了具有连续的曲率路径，有时也使用回旋曲线段。还可以通过记录由专家驾驶员驾驶的车辆的运动来获得运动基元。
　　城市道路自动驾驶车辆运动规划和控制技术综述(2)
　　观察到运动基元的递归应用可以生成树图，其中在最坏的情况下没有两个边缘导致相同的配置。然而，存在一组运动基元，称为晶格生成，其产生类似于晶格的规则图形。有关说明，请参见图IV.2a。晶格生成基元的优点在于搜索图的顶点均匀地覆盖构型空间，而树通常可以在根顶点周围具有高密度的顶点。 Pivtoraiko等。在[107]中使用术语“状态晶格”来描述这样的图形，并指出通过首先在原点周围产生规则间隔的配置然后将原点连接到这样的系统，可以获得用于手中系统的一组晶格生成运动基元。通过路径进行配置，该路径表示两种配置之间的两点边界值问题的解决方案。
　　城市道路自动驾驶车辆运动规划和控制技术综述(2) 　　

图IV.2 格子和非格子图，都有5000个边。（a）递归施加90°左圆弧，90°右圆弧和直线得到的图形。（b）递归应用89左圆弧，89右圆弧和直线得到的图。这些基元的递归应用确实形成树而不是格子，其中许多分支在原点附近循环。因此，右图所覆盖的区域较小。

　　通过生成覆盖（*）构型空间的离散样本集并通过使用精确转向程序获得的可行路径段来连接它们，可以实现类似于从初始构型递归应用网格生成运动基元的效果。
　　大多数基于采样的路线图构建方法遵循算法2中所示的算法方案，但在 $n e i g h b o r s (x, V)$ 和 $s a m p l e - p o i n t s (X, n)$ 的实现方面不同。函数 $s a m p l e - p o i n t s (X, n)$ 表示从构型空间 $X$ 中选择点的策略，而函数 $n e i g h b o r s (x, V)$ 表示为顶点 $x$ 选择一组相邻顶点 $N \subseteq V$ 的策略，该算法将尝试使用精确的转向函数 $s t e e r_{e x a c t} (x, y)$ 通过路径段连接到x。
　　城市道路自动驾驶车辆运动规划和控制技术综述(2)
　　两个最常见的实现函数 $s a m p l e - p o i n t s (X, n)$ 是1)返回以规则网格排列的n个点，和2）从 $X$ 中返回n个随机采样点。虽然随机抽样具有普遍适用且易于实现的优点，但所谓的Sukharev网格已被证明可实现单位超立方体中的最佳 $L_{\infty}$ -dispersion，即它们最小化最大空球的半径，内部没有采样点。为了深入讨论基于采样的路径规划环境中随机和确定性采样的相对优点，我们将读者引用到[108]。实现函数 $n e i g h b o r s (x, V)$ 的两种最常用的策略是1)将 $k$ -最近邻居的集合取为 $x$ 或2)位于以 $x$ 为中心以 $r$ 为半径的球中的点集。
　　特别地，在 $d$ 维网格中确定性地排列的样本，其中邻域被视为2-D中的4或8个最近邻点或更高维度中的类似模式，表示*构型空间的直接确定性离散化。这部分是因为它们自然地来自广泛使用的机器人构型空间的*和占据区域的位图表示[109]。
　　Kavraki等[110]提倡在概率路线图（Probabilistic Roadmaps, PRM）框架内使用随机抽样，以便在高维配置空间中构建路线图，因为与网格不同，它们可以自然地以任何时间运行。类似版本的PRM [79]遵循算法2中的方案，随机采样并在具有固定半径 $r$ 的球内选择邻居。由于PRM的一般公式，它们已被用于各种系统的路径规划，包括具有差分约束的系统。然而，该算法的理论分析主要集中在没有差分约束的系统的算法性能上，即当使用直线连接两种构型时。在这样的假设下，[80]中的PRM是概率完备的并且渐近最优。也就是说，结果图包含可行解（如果存在）的概率收敛到图的大小增加的概率，并且图中最短路径的成本收敛到最优成本。 Karaman和Frazzoli [80]提出了一种称为PRM *的自适应PRM，它只是将具有对数收缩半径的球中的相邻顶点连接起来，随着样本数量的增加，以保持渐近最优性和计算效率。
　　在同一篇论文中，作者提出了快速探索随机图（RRG*），这是一种增量离散策略，可以在保持渐近最优性的同时终止。最近，快速行进树（FMT*）[111]已被提出作为PRM*的渐近最佳替代方案。该算法通过对一组采样顶点执行惰性动态编程递归，将离散化和搜索结合到一个过程中，该采样顶点随后可用于快速确定从初始构型到目标区域的路径。
　　最近，理论分析也扩展到差分约束系统。 Schmerling等人[81]提出了PRM*和FMT*的差分版本，并证明了无漂移控制器动力系统算法的渐近最优性，该系统包括非旋转轮式车辆的模型。
　　4) 图搜索策略：在上一节中，我们讨论了以图形形式对*构型空间进行离散化的技术。为了在这种离散化中获得实际的最佳路径，必须使用图搜索算法之一。在本节中，我们将回顾与路径规划相关的图搜索算法。
　　用于在图中找到最短路径的最广泛认可的算法可能是Dijkstra算法[32]。该算法执行最佳的第一次搜索以构建表示从给定源顶点到图中所有其他顶点的最短路径的树。当仅需要到单个顶点的路径时，可以使用启发式来指导搜索过程。最突出的启发式搜索算法是由Hart，Nilsson和Raphael [112]开发的A*。如果提供的启发函数是可接受的（即，它永远不会过高估计要花费的成本），则已经证明A*是最佳有效的并且保证返回最优解。对于许多问题，使用加权A* [113]可以用较少的计算量获得有界的次优解，这对应于简单地将启发式乘以常数因子 $ϵ$ > 1.可以证明，具有这种夸大的启发式算法的A *返回的解决方案路径保证不会比最佳路径的成本高（1 + $ϵ$ ）倍。
　　通常，每当使用传感数据更新世界模型时，重复地寻找从车辆的当前构型到目标区域的最短路径。由于每次更新通常只影响图表的一小部分，因此每次从头开始运行搜索可能会很浪费。实时重新计算搜索算法系列，如D* [114]，Focussed D* [115]和D* Lite [116]，设计用于在每次基础图变化时有效地重新计算最短路径，同时利用来自先前搜索工作的信息。
　　Anytime搜索算法都试图快速提供第一次优路径并且以更多的计算时间不断改进解决方案。 Anytime A* [117]使用加权启发式来找到第一个解决方案并通过继续搜索来实现Anytime行为，第一个路径的成本作为上限，可接受的启发式作为下限，而随时修复A*（ ARA*）[118]使用膨胀的启发式执行一系列搜索，减轻权重并重新使用先前迭代的信息。另一方面，Anytime Dynamic A*（ADA*）[119]将D* Lite和ARA*背后的思想结合起来生成随时搜索算法，用于动态环境中的实时重新计划。
　　在构型空间的图离散化上搜索路径的算法的明显限制是在这样的图上得到的最佳路径可能明显长于构型空间中的真实最短路径。任意角度路径规划算法(Any-angle path palnning algorithms)[120] - [122]被设计为在网格上操作，或者更一般地在表示*构型空间的单元分解的图上，并且试图通过考虑顶点之间的“快捷方式”来减轻这个缺点。此外，Field D*将线性插值引入搜索过程以产生平滑路径[123]。

E.增量搜索技术

　　搜索固定图离散化的技术的缺点是它们仅搜索可以从图离散化中的图元构造的路径集。因此，这些技术可能无法返回可行路径或返回明显不理想的路径。
　　增量可行运动规划器努力解决该问题，并且如果存在且有足够的计算时间，则为任何运动规划问题实例提供可行路径。通常，这些方法递增地构建构型空间越来越精细的离散化，同时尝试确定在每个步骤的离散化中是否存在从初始构型到目标区域的路径。如果实例是“简单的”，the solution is between provided quickly，但通常计算时间可能是无限的。同样，增量最优运动规划方法除了快速找到可行路径之外，尝试提供一系列增加的解决方案质量收敛到最佳路径。
　　在文献中使用术语概率完备来描述找到解决方案的算法（如果存在的话），其中概率接近于计算时间增加的解。请注意，如果解决方案不存在，则概率完备算法可能不会终止。类似地，渐近最优的术语用于以概率1收敛到最优解的算法。
　　在有限条件下获得完备和最优的简单策略是在构型空间的固定离散化上解决一系列路径规划问题，每次都具有更高的离散化分辨率。这种方法的一个缺点是，各个分辨率级别的路径规划过程是独立的，没有任何信息重用。此外，在启动新图搜索之前，离散化的分辨率应该增加多快，即添加单个新配置，配置数量加倍，或者离散数量加倍是不是很明显每个构型空间维度的值。为了克服这些问题，增量运动规划方法交织构型空间的增量离散化，并在一个集成过程中搜索路径。
　　用于增量路径规划的一类重要方法基于以下方式：逐步增长根据车辆的初始构型向外生长的树以探索可到达的构型空间。通过从树中迭代地选择随机顶点并通过从其应用转向函数来扩展所选择的顶点来实现“探索”行为。一旦树长到足以到达目标区域，通过追踪从目标区域中的顶点向后到初始构型的链接来恢复所得到的路径。算法3中描述了基于增量树的算法的一般算法方案。
　　城市道路自动驾驶车辆运动规划和控制技术综述(2)
　　最早的基于树的增量规划器之一是由Hsu等人提出的扩展空间树（EST）规划器[124]。该算法从V中随机选择一个顶点 $x_{s e l e c t e d}$ 进行扩展，其概率与其邻域中的顶点数成反比，从而促进向未探测区域的增长。在扩展期间，算法在围绕 $x_{s e l e c t e d}$ 的固定半径的邻域内采样新的顶点 $y$ ，并且使用相同的技术来偏置采样过程以从相对较少探索的区域中选择顶点。然后它返回 $x_{s e l e c t e d}$ 和 $y$ 之间的直线路径。在[125]中引入了动态环境中具有动力学约束的规划思想的概括，其中该算法的能力在不同的非完整机器人系统上得到证明，并且作者使用算法的理想化版本来确定找到可行路径的失败概率取决于状态空间的扩展性质，并随样本数量呈指数衰减。
　　La Valle已经提出快速探索随机树（RRT）[101]作为寻找高维非完整系统的可行轨迹的有效方法。通过从*构型空间获取随机样本 $x_{r n d}$ 并沿随机样本的方向扩展树来实现快速探索。在RRT中，顶点选择函数 $s e l e c t (V)$ 根据两个构型之间的给定距离度量返回随机样本 $x_{r n d}$ 的最近邻。扩展函数 $e x t e n d ()$ 然后通过对固定时间步长应用控件来生成配置空间中的路径，该控制最小化到 $x_{r n d}$ 的距离。在某些简化假设下（随机转向用于扩展），RRT算法已被证明是概率完备的[83]。我们注意到概率完备性的结果并不容易推广到经常使用启发式转向的许多实际实现的RRT版本。实际上，最近在[126]中已经表明，使用具有固定时间步长的启发式转向的RRT在概率上并不是完备的。
　　此外，Karaman和Frazzoli [127]证明RRT收敛于概率为1的次优解，并设计了RRT算法的渐近最优自适应，称为RRT*。如算法4所示，每次迭代时的RRT *都会考虑位于新添加的顶点 $x_{n e w}$ 附近的一组顶点，并a) 将 $x_{n e w}$ 连接到邻域中的顶点，最小化从 $x_{i n i t}$ 到 $x_{n e w}$ 的路径成本，并b) 将邻域中可以降低从 $x_{i n i t}$ 到该顶点路径成本的任何顶点重新连接为 $x_{n e w}$ 。该算法的一个重要特征是邻域区域定义为以 $x_{n e w}$ 为中心的球，半径是树大小的函数: $r = γ \sqrt[d]{\frac{(\log n)}{n}}$ ，其中 $n$ 是树中顶点的数量， $d$ 是构型空间的维数， $γ$ 是一个依赖于实例的常量。结果表明，对于这样的函数，球中预期的顶点数是树的大小的对数，这对于确保算法几乎肯定会收敛到最优路径，同时保持与次优RRT相同的渐近复杂度。
　　城市道路自动驾驶车辆运动规划和控制技术综述(2)
　　在[80]中描述了在差分约束下RRT *的渐近最优性的充分条件，并且证明了对于Dubins车辆和双积分器系统是可满足的。在后来的工作中，作者进一步表明，在小型本地可达系统的背景下，该算法可以进行调整，不仅可以保持渐近最优性，还可以保持计算效率[82]。其他相关工作主要集中在通过局部线性化系统动力学[128]或通过导出具有线性动力学的系统的闭合解[129]来推导非完整系统的距离和导向函数。另一方面，RRTX是一种扩展RRT的算法，以允许在障碍区域改变时进行实时增量重新计划，例如，面对来自传感器的新数据[130]。
　　基于采样的算法领域的新发展包括在不能获得精确的转向过程的情况下实现渐近最优的算法。特别是，Li [84]最近提出了用于渐近（近似）最优路径规划的稳定稀疏树（SST）方法，该方法基于构建通过动态的前向模型传播的随机采样控制树。系统使得修剪局部次优分支以确保树保持稀疏。

F.实际部署

　　已经针对自动驾驶车辆讨论了三类路径规划方法：变分方法，图搜索方法和基于树的增量方法。自驱动系统上的实际现场部署算法来自上述所有类别。例如，即使在DARPA城市挑战赛的前四个成功参与者中，用于运动规划的方法也存在显着差异。作为挑战的赢家，CMU的Boss车辆使用变分技术在结构化环境中生成局部轨迹，在四维配置空间（由位置，方向和速度组成）中使用格子图以及Anytime D*来发现无碰撞停车场的路径[131]。据报道，斯坦福大学研究小组开发的亚军车辆使用了一种搜索策略混合A*，在搜索过程中，懒惰地通过递归地应用一组有限的机动来构造运动图元树。搜索由精心设计的启发式指导，并且通过仅将单个节点保持在配置空间的给定区域内来确保树的稀疏性[132]。同样，由弗吉尼亚理工大学的VictorTango团队开发的第三辆车开始进行可能机动的图形离散化，并使用A *算法搜索图表[133]。最后，麻省理工学院开发的车辆采用了RRT算法的一种变体，称为闭环RRT，具有偏置采样[134]。