Coursera自动驾驶课程第18讲：The Planning Problem

在第17讲《Coursera自动驾驶课程第17讲：An Autonomous Vehicle State Estimator》我们学习了如何使用多传感器融合进行自车定位，以及传感器的内外参标定和时间同步，我们还讨论了在实际应用中常遇到的问题。

从本讲开始我们将学习一个新的模块，也是本课程最后一个模块：运动规划。运动规划问题是指在遵守道路规则的情况下以安全舒适的方式将车辆导航到目的地的任务。正如在第一门课程所讨论的，该任务可以分解为分层的优化问题，每一层都有不同的约束和目标。

在本模块结束时，我们应该了解在执行自动驾驶运动规划时需要解决的子问题类型。在本讲中，我们将介绍自动驾驶中的任务，一些最普遍的道路驾驶场景，以及处理每种场景所需的行为，以及一些自动驾驶运动规划中具有挑战的重要案例。

文章目录

- - 18.1 Driving Missions, Scenarios, and Behaviour
  - - 18.1.1 Autonomous Driving Mission
    - 18.1.2 Road Structure Scenarios
    - 18.1.3 Obstacle Scenarios
    - 18.1.4 Behaviors & Challenges & Hierarchical Planning
  - 18.2 Motion Planning Constraints
  - - 18.2.1 Bicycle Model
    - 18.2.2 Curvature
    - 18.2.3 Vehicle Dynamics
    - 18.2.4 Obstacles
  - 18.3 Objective Functions for Autonomous Driving
  - - 18.3.1 Efficiency
    - 18.3.2 Reference Tracking
    - 18.3.3 Smmothness
  - 18.4 Hierarchical Motion Planning
  - - 18.4.1 Mission Planner
    - 18.4.2 Behavioural Planner
    - 18.4.3 Local Planner

18.1 Driving Missions, Scenarios, and Behaviour

18.1.1 Autonomous Driving Mission

首先，让我们讨论自动驾驶的任务。在最高层面上，自动驾驶的任务是使车辆从 A 点到达 B 点。从导航的角度来看，我们需要从地图上的一个点或当前位置导航到与我们最终目的地相对应的另一个点。在这样做时，它应该考虑不同街道的连接及其相关的交通。为了在路网中找到在行驶时间或距离方面最有效的路径（如常用的导航软件规划的路线），自动驾驶任务规划将许多重要的低层级的变量从问题中抽象出来，以简化任务规划过程。然而，这些低层级的变量，如道路结构、障碍物和道路上的其它代理，对于自动驾驶运动规划问题至关重要。这些低层级的变量定义了不同的驾驶场景，我们已经在本课程的第 1个模块中讨论了一些驾驶场景。

在这里插入图片描述

18.1.2 Road Structure Scenarios

首先，我们来介绍一些与道路结构相关的常见场景。道路结构是指与驾驶员相关的车道边界和监管要素。

第一种场景是驾驶时最简单的场景（如左上图所示），通常又被称为车道保持。在这种情况下，汽车沿着当前车道的中心线行驶。在这种情况下，我们的目标是尽量减少与路径中心线的偏差，并达到设置的速度，这通常是确保有效到达目的地的道路速度限制（如定速巡航功能）。
第二种场景是汽车必须执行变道操作。我们需要在横向和纵向加速度的约束下优化变道轨迹。道路的速度限制，以及执行变道操作的时间限制，所有这些参数都会影响变道轨迹的形状，可能会导致汽车缓慢的被动变道，或者更激进的不太舒适的变道。
第三种场景是汽车需要左转或右转。当汽车经过交叉路口时，通常需要这样做。与变道一样，转弯的形状因情况而异。此外，自动驾驶汽车可以采取的行动的可行性受到周围环境状态的影响。例如，即使路口畅通，自动驾驶汽车也无法在红灯时左转。
作为最后一个例子，这里有一个 U 形转弯。 U 形转弯可以很高效地改变汽车的行驶方向，但又高度依赖于周围环境的状态，因为根据有些国家或地区的法律，在十字路口执行 U 形转弯并不是合法的。此时，道路结构并不是道路场景中的唯一因素。

在这里插入图片描述

18.1.3 Obstacle Scenarios

现在我们来关注有障碍物的场景。静态和动态障碍物将极大地改变场景的结构以及确定场景所需行为的难度。在自动驾驶中，动态障碍物被定义为自车规划工作空间中的移动代理。相反，静态障碍物是不移动的障碍物，如停放的汽车、中间带和弯道。

静态障碍物限制了自动驾驶汽车在规划通往目的地的路径时可以占据的位置。如果汽车行驶时的空间与障碍物相重叠，将会发生碰撞。另一方面，动态障碍物对汽车的速度分布和驾驶行为有更大的影响。驾驶中的一个常见示例是，当我们进行车道保持操作时，汽车前方还有其它车。显然，这辆车将对我们的决策产生影响。假设这辆车的行驶速度比我们的设定速度慢 10 km/h。如果我们保持当前的设定速度，我们最终会撞到前面的车辆。理想情况下，我们希望在自车和前车之间保持一个时间间隔，该时间间隔被定义为在保持当前速度的同时到达前车当前位置的时间量。所以现在我们有两个相互矛盾的需求。我们希望尽可能接近我们的设定速度，同时在我们的汽车和前车之间保持时间间隔以确保安全。

动态障碍物会影响转弯和变道场景。根据它们的位置和速度，自车可能只有特定的时间窗口来执行这些操作。这些时间窗口将是估计值，因为它们将基于环境中所有其他代理的预测。如下面的一个例子（右上图所示），当我们在左转之前等待交叉路口畅通无阻。自动驾驶汽车决定执行的行为将在很大程度上取决于迎面而来的交通行为。如果附近有迎面而来的车辆快速向我们驶来，那么唯一安全的行为就是向它们让行。但是，如果自动驾驶汽车与迎面而来的车辆之间的距离更长，则自动驾驶汽车将有机会转弯。

现在，并非所有动态障碍物都是汽车，因为还有其他类型的障碍物，例如骑自行车的人、卡车和行人。他们每个人都会有自己的行为，在路上也有自己的规则要遵循。可以看到，即使我们只定义了驾驶任务中最有可能的核心组件，但显然还有很多丰富多样的潜在场景需要处理。

在这里插入图片描述

18.1.4 Behaviors & Challenges & Hierarchical Planning

尽管存在上述的各种复杂场景，但这些场景中的大多数执行动作都可以被看作是多种行为的组合，常见的行为有速度跟踪、减速停车、保持停车、让车和紧急停车。让我们深入了解这些行为的真正含义。

速度跟踪是一种很常见的驾驶行为。我们有一个参考速度，在车道上向前行驶时希望汽车保持该速度。
减速停车顾名思义，如果前面出现停车标志，我们需要平稳地减速停车，以保持舒适性。
如果开车时遇到红灯，那么我们需要停下来，直到交通灯变绿才能继续行驶。
一些交通规则会要求我们进行让车；当自动驾驶汽车检测到某些问题，需要立即停并靠边停车时，汽车会执行紧急停车`。

这些行为还可以通过导航行为来增强，例如执行车道变换和转弯（如下图所示）。
在这里插入图片描述
通过将上面所有这些行为结合起来，我们现在可以涵盖最基本的驾驶场景。但要注意的是，这一系列行为并不是详尽无遗的，而且有很多方法可以增加行为复杂性，以处理更多有趣的场景。就驾驶场景而言，我们实际上才刚刚开始触及表面。

有许多不寻常的情况，使自动驾驶任务充满挑战。例如，假设有一个乱穿马路的行人，我们会让汽车违反道路规则，这使得汽车的行为从运动规划的角度来看是不可预测的。另一个例子是，当摩托车手越过车道线时，这可能是合法的，也可能不是合法的，这取决于当地的交通规则。

在这里插入图片描述
不难看出，解决运动规划问题是一项复杂的任务。在没有任何形式的选择性抽象和简化的情况下求解最佳运动规划，将是非常棘手的。为了解决这个问题，我们将任务分解为分层的优化问题。通过这样做，我们可以将每个优化问题的输入和输出调整到正确的抽象层次，这将使我们能够实时执行运动规划。在这个分层结构中，层次越高意味着待优化问题处于更高的抽象层次。

在这个层次结构的最上层是任务规划，它专注于解决我们之前讨论过的在地图级别导航到目的地的自动驾驶任务。
接下来，我们有行为规划问题。这决定了自动驾驶汽车应该根据其当前的驾驶场景采取哪些行为。
基于该操作，我们然后使用局部规划器计算到达所需目标状态的无碰撞路径和速度分布。在这里，我们把这个过程解耦为路径规划和速度特性生成，以提高性能。
最后，我们计算出的运动规划将提供给控制器以跟踪我们在第一门课程中设计的运动规划。

这些优化问题中的每一个都有不同的目标和约束需要被解决。我们将在后面进一步介绍。

在这里插入图片描述
简单总结一下本小节，我们将自动驾驶运动规划任务确定为从自车当前位置导航到所需目的地的问题。我们还研究了一些常见的公路驾驶场景，以及道路结构和障碍物如何决定场景性质。然后，我们讨论了一些有用的驾驶行为来导航我们确定的场景。最后，我们描述了运动规划优化的分层结构。

18.2 Motion Planning Constraints

18.2.1 Bicycle Model

在本小节，我们将讨论运动规划中涉及的一些最重要的约束。这些约束通常对于保持车辆稳定性和舒适性以及在给定驾驶场景中保持所有代理（主要指汽车）的安全至关重要。具体来说：

我们将了解车辆运动学和车辆动力学模型如何约束我们对车辆的运动规划。
我们还将了解静态和动态障碍物如何影响我们的车辆运动规划。
最后，我们将看到规则要素（一般指交通规则）如何影响我们在运动规划过程中可用的行为。

第一个要介绍的与运动规划约束是车辆运动学。正如我们在课程第一个模块中所讨论的，在自动驾驶的运动规划中，自车的运动学通常被简化为自行车模型。选择此模型的一个原因是自行车具有与汽车相似的一系列可接受的转向角度。对于固定速度 $V$ ，车轮转向角度范围应对应于车辆可以遵循的曲率 $K\mathcal{K}$ 范围。这意味着，在使用自行车模型执行运动规划时，在通过 $Kmax\mathcal{K}_{max}$ 表示的给定路径时可以达到最大曲率。自动驾驶汽车也是如此，这意味着我们规划的任何路径的曲率都需要注意这个最大曲率。

在这里插入图片描述

不幸的是，这是一个非完整约束。直观地说，这意味着约束不仅仅取决于汽车的状态，还取决于汽车如何达到其当前状态。非完整约束减少了汽车在其工作空间中任何给定点可以采取的方向数量。通常，非完整约束使规划问题变得更加复杂。

这里我们将运动学模型约束用公式表达则为：
$∣κ∣≤κmax⁡(18.1)|\kappa| \leq \kappa_{\max } \tag{18.1}$

18.2.2 Curvature

现在我们可能想知道曲率如何影响我们运动规划中给定路径的形状。为了直观地了解曲率是什么，这里我们有一条任意曲线（下图所示）。对于曲线上的每个点，我们可以根据该点在空间中曲线的瞬时变化率将一个圆拟合到该点。这类似于我们最开始用来推导自行车模型的瞬时旋转中心的想法。根据绘图的形状，有些点似乎比其他点更弯曲（圆的半径更小）。所以，我们的直觉应该是沿曲线的不同点会有不同的曲率。

在这里插入图片描述
现在我们来看一个例子，我们有拟合曲线上两个点的圆。正如我们所看到的，其中一个相当大的半径 $r_1$ ，曲率的直观解释是它是圆的半径的倒数，公式为：
$κ=1r(18.2)\kappa=\frac{1}{r} \tag{18.2}$

由于这个半径相当大，所以这个点的曲率相对较小，从图中可以看出，该点的弯曲更平缓。相反，如果我们看一下这条曲线上的第二个点，我们可以看到它的半径 $r_2$ 比前一个点要小。这对应于一个更大曲率的点，从图中可以看出，这个点比前一个点具有更大的弯曲度。这种圆拟合方法可用作几何解释，同时这里我们给出曲率的更精确的数学公式：
$κ=x′y′′−y′x′′(x′2+y′2)3/2(18.3)\kappa=\frac{x^{\prime} y^{\prime \prime}-y^{\prime} x^{\prime \prime}}{\left(x^{\prime 2}+y^{\prime 2}\right)^{3 / 2}} \tag{18.3}$

18.2.3 Vehicle Dynamics

下一个将要讨论的约束是车辆动力学。动力学约束的重点是使汽车保持稳定的安全状态。第一个动力学约束是由汽车的摩擦椭圆施加的。如果还记得的话，我们在课程第一个模块中讨论了自行车模型中的轮胎打滑和摩擦椭圆。摩擦椭圆表示汽车轮胎和路面之间可以产生的最大摩擦力。如果汽车发动机的作用力超过轮胎的摩擦力，轮胎就会在路面上打滑。车辆的转向功能依赖于轮胎抓地能力。为了保持车辆控制稳定性，汽车受力必须保持在摩擦椭圆内。归根结底，这可以归结为横向和纵向加速度限制。

一般来说，在非紧急情况下，我们经常关注加速度的舒适矩形。横向和纵向加速度各自位于舒适矩形范围内，表示为沿轴向的纵向加速度 $a_{long}$ 和横向加速度 $a_{lat}$ 轴。如下图所示，蓝色的舒适矩形正好位于摩擦椭圆内。这导致对运动规划的可行加速度的约束比摩擦椭圆所需的更严格。

在这里插入图片描述
从横向加速度约束，以及路径的曲率，我们现在可以间接约束汽车的速度。汽车的速度和横向加速以及瞬时转弯半径之间关系为：
$alat=v2r,alat≤alatmax⁡(18.4)a_{l a t}=\frac{v^{2}}{r}, \quad a_{l a t} \leq a_{l a t_{\max }} \tag{18.4}$

同时，瞬时曲率是汽车转弯半径的倒数，即 $κ=1r\kappa=\frac{1}{r}$ 。如果我们联立上面两个方程，我们可以看到速度的平方受最大横向加速度 $a_{lat_{max}}$ 的约束以及路径曲率 $κ\kappa$ 的影响，它在路径上的每个点都在变化。因此很明显，当我们为自动驾驶汽车生成速度曲线时，我们必须考虑路径的曲率以及车辆的最大横向加速度。速度、最大横向加速度、曲率三者之间约束关系为：
$v2≤alatmax⁡κ(18.5)v^{2} \leq \frac{a_{l a t_{\max }}}{\kappa} \tag{18.5}$

18.2.4 Obstacles

静态障碍物会限制我们的路径规划过程。由于诸如停放的汽车或施工塔等静态障碍物具有不随时间变化的固定位置，因此它们通常通过遮挡自车工作空间的某些块来建模。我们将会在下一讲中更详细地介绍这个模型，我们到时将在其中讨论占用网格图。本质上，静态障碍物限制了汽车沿其路径可以占据的位置。有许多方法可以执行静态碰撞检查：

一种方法是在车辆沿给定路径行驶时根据车辆形成的条带来检查，当自车穿过路径时，该条带是由自车的车身占据的所有位置的联合。如果条带与静态障碍物重叠，则该路径是不可行的。
另一个方法是用一组圆圈来近似汽车的车身，并在汽车沿着其路径移动时计算圆圈的位置。如果静态障碍物位于某一圆圈内，则该路径被认为是不可行的。这些圆圈的位置联合通常比车辆的车身大，从而确保了保守的近似。我们将在后面的课程中更详细地讨论这个问题。

在这里插入图片描述

另一方面，动态障碍物为运动规划问题提供了一些最具挑战性的约束。汽车、自行车和行人等不同类别的动态障碍物都会有不同的行为和运动模型。基于这些代理的行为来约束我们的运动规划通常会涉及预测模块，这会受到很大不确定性的影响。但是，如果我们采取保守的方法并将自己限制在所有代理的所有可能行为上，我们的运动规划问题很快就会变得过度约束并且无法得到有意义地解决。

下面举个简单的例子，在一个十字交叉路口。如果两辆车正进入彼此正交的交叉路口，那么就有可能发生碰撞。检查是否会发生碰撞的一种方法是跟踪由自车行驶方向和从自车位置到其他代理位置的向量形成的角度。如果这个角度随着时间的推移保持不变，那么自我车辆将与其他代理发生碰撞（这里可以看一下课程视频动画），我们的运动规划器将需要减速以防止这种情况发生。

因此，动态障碍物迫使我们根据障碍物在我们的驾驶场景中的行驶方向来改变自车的行为。我们在上一小节中讨论的另一个例子是当前车出现在自车前面时。这辆领先的车辆对自车的纵向速度设置了上限，因为如果我们在保持在同一车道上的同时超过它们的速度，我们最终会撞车。因此，动态障碍物将限制我们做出机动决策的行为规划过程以及影响我们的速度特性规划的局部规划过程。我们将在后面的章节中更详细地讨论这一点。

在这里插入图片描述
我们将讨论的最后一个约束包括道路规则以及自车工作空间中存在的交通规则。虽然道路规则为规划问题提供了一些限制，但它们也帮助我们对环境中的其他代理行为做出明智的决定。例如，迎面而来的车辆很可能停留在其车道上，而不是试图与我们的自车正面相撞。当试图预测其他代理会做什么时，这有助于减少搜索空间。道路规则施加的最常见的限制之一是车道限制。简单地说，车道限制是为了防止我们的运动规划离开自车当前车道，除非这样做是合法的。车道约束也通知自车在哪里可以安全地进行转弯操作。我们还需要遵守其它的道路软规则，例如保持自车和我们车道上的领先车辆之间的时间间隔。时间间隔是自车在以自车的当前速度行驶时到达领先车辆当前位置所需的时间量。保持相当大的时间间隔有助于确保运动规划期间的安全，因为它可以让自车对工作空间中的事件有足够的反应时间。

工作空间中的交通规则也会影响我们的驾驶行为。自车显然需要遵守红绿灯和停车标志以确保安全，但还需要了解不同区域的限速和其他交通规则。一般来说，交通规则将对我们在执行运动规划时可以使用哪些行为产生很大影响，我们将在后面章节中进一步详细讨论。

在这里插入图片描述
现在我们已经讨论了许多自动驾驶运动规划约束，让我们总结一下到目前为止我们学到的知识：

我们首先回顾了我们的自行车模型，并研究了模型的运动学和动力学，以及路径曲率如何约束我们的运动规划问题。
我们研究了静态障碍物如何限制我们的汽车可以安全占据的位置，这限制了我们可行的运动规划工作空间，以及动态障碍物如何影响自车可用的行为及其速度分布。
最后，我们讨论了规则要素的作用以及它们如何影响我们的驾驶行为。

在下一小节中我们将深入探讨用于解决运动规划问题的一些优化目标。

18.3 Objective Functions for Autonomous Driving

18.3.1 Efficiency

在本小节中，我们将探讨自动驾驶运动规划问题中最常用的一些目标函数。在这种情况下，我们的运动规划问题的目标函数为我们提供了一种对当前运动规划进行评分的方法，并允许我们优化运动规划，使其具有理想的特性。

在进行路径规划时，最简单、最直观的目标是我们希望尽可能高效地到达目的地。在路径规划中，这通常转化为最小化我们规划的路径弧长。弧长是什么？直观地说，路径的弧长是汽车在经过路径时行驶的总距离。在许多情况下，路径将由弧长参数化表示。所以这可以表述为与弧长成比例的惩罚项。在其他情况下，我们必须通过数值积分来计算路径的弧长。弧长积分公式为：
$sf=∫xixf1+(dydx)2dx(18.6)s_{f}=\int_{x_{i}}^{x_{f}} \sqrt{1+\left(\frac{d y}{d x}\right)^{2}} d x \tag{18.6}$

其中， $s_f$ 是积分后的弧长， $x_i$ 是路径起始 $x$ 坐标， $x_f$ 是路径结束 $x$ 坐标。

在速度优化时，到达目的地的时间通常也是我们希望最小化的目标。直观表达，这相当于将总距离除以在每个点沿曲线行进的速度。到达所需时间为：
$Tf=∫0s1v(s)ds(18.7)T_{f}=\int_{0}^{s} \frac{1}{v(s)} d s \tag{18.7}$

通过最小化这个积分，我们可以最小化到达目的地的所需时间，同时遵循给定的规划路径。

18.3.2 Reference Tracking

下面让我们看一下相同起点和终点之间的两条可能路径。第一条路径显然比第二条路径具有更长的弧长，因此在到达目的地的任务中效率较低。我们可以看到，即使它们具有相同的初始状态和最终状态，第二条路径也会导致弧长目标函数的值较低。现在，在自动驾驶的路径规划中，我们可能会得到一条我们想要遵循的参考路径。

在这里插入图片描述
在许多情况下，为了确保我们尽可能地遵循参考路径，我们会引入差分项积分。本质上，它会惩罚与输入参考路径 $x_ref$ 的偏差，因为我们遍历的是计算路径：
$∫0sf∥x(s)−xref(s)∥ds(18.8)\int_{0}^{s_{f}}\left\|x(s)-x_{r e f}(s)\right\| d s \tag{18.8}$

类似地，在执行速度优化时，我们经常希望最小化整个路径的速度与某个参考速度 $v_{ref}$ 之间的偏差：
$∫0sf∥v(s)−vref(s)∥ds(18.9)\int_{0}^{s_{f}}\left\|v(s)-v_{r e f}(s)\right\| d s \tag{18.9}$

虽然上述可以称为绝对差值惩罚，但通常情况下，我们希望对超出参考速度的惩罚比对低于参考速度的惩罚更严厉。因此，我们可以将所谓的 Hinge loss 项添加到目标中。本质上，该术语仅在速度曲线超过参考速度时才有效，即当我们的速度与参考速度之间的差异为正时。
$∫0sf(v(s)−vref(s))+ds(18.10)\int_{0}^{s_{f}}\left(v(s)-v_{r e f}(s)\right)_{+} d s \tag{18.10}$

这使我们能够更严厉地惩罚超过速度约束的行为，同时仍然允许我们鼓励自动驾驶汽车达到其所需的参考速度。

18.3.3 Smmothness

我们要探讨的下一个目标是行驶平顺性。回顾上一小节，为了保持稳定性和舒适性，我们限制了最大加速度幅值。在优化我们速度曲线的目标函数的舒适度时，我们将注意力转向最小化沿轨迹的抖动，通常使用Jerk来表示，为加速度相对于时间的变化率，或位置的三阶导数（好像不能直接显示三阶导数~）：
$∫0sf∥x¨(s)˙∥2ds(18.11)\int_{0}^{s_{f}} \|\dot {\ddot{x}(s)}\|^{2} d s \tag{18.11}$

汽车轨迹上的颠簸极大地影响了用户在车内的舒适度。因此，在规划我们的速度曲线时，我们希望尽可能小地保持 jerk 的累积绝对值，这里用积分表示。

让我们看一下两个不同速度曲线的示例。左侧是线性曲线，在轮廓的开始和结束处有最大加加速度。这显示在我们开始和结束时的大尖峰，对于乘客来说这可能会非常不舒服。如果我们将其与右侧所示的二次曲线进行比较，我们可以看到没有颠簸尖峰，这将为乘客带来更加平稳的乘坐体验。由于这种舒适度的差异，重要的是选择尽可能减少乘客感觉的颠簸的配置文件。

在这里插入图片描述
在规划路径时，我们还必须要注意路径的结构也会影响速度曲线的舒适度。回想一下上一节，高曲率的路径将沿路径的最大速度限制为较低值，以便使横向加速度保持在车辆的摩擦椭圆内。为了确保我们避开路径上的高曲率点，我们需要对大的绝对曲率值制定一些惩罚，而通常用来表示这种惩罚的目标函数称为路径的弯曲能量。本质上，它是沿路径积分的曲率平方，其中 $κ\kappa$ 表示曲率。
$∫0sfκ(s)∥2ds(18.12)\int_{0}^{s_f} \kappa(s) \|^{2} d s \tag{18.12}$

此目标沿路径更均匀地分布曲率，防止沿路径的任何人达到过高的总曲率值。例如下图，在这条路径中，我们规划了一条路径，在到达目标状态之前有一个非常陡峭的弯道左转。根据我们的弯曲能量目标，这将受到高度惩罚，因为沿路径存在峰值曲率点。如果我们要为这条路径规划一个速度参考，我们需要在到达弯道时大大减速。否则，我们将牺牲乘客的舒适度，甚至可能牺牲车辆的稳定性。如果我们改为查看右侧的路径，我们会发现一条左转路径具有更广泛分布的较低整体曲率。这将导致更平滑更舒适的术语，并允许我们保持合理的速度，同时在优化我们的速度曲线时仍保持在舒适矩形内。

在这里插入图片描述
现在我们已经了解了运动规划的各种目标函数，让我们花一些时间来总结一下我们在这一节中讨论的内容：

我们首先讨论了与效率相关的目标函数，还讨论了它们如何减少路径长度和传输时间。
我们讨论了如何轨迹跟踪参考路径和速度参考特性，以及如何通过惩罚高路径曲率和加加速度来提高给定轨迹的舒适度。

到目前为止，我们对优化路径和轨迹时需要考虑的某些类型的目标函数有一个大致的了解。在设计一个总体目标函数时，这些术语中的每一个都需要组合成一个函数，通常是一个加权和。我们选择的特定权重组合是一项工程决策，需要根据特定应用微调我们的选择。

18.4 Hierarchical Motion Planning

18.4.1 Mission Planner

到目前为止，在本模块中，我们已经从整体上介绍了规划问题，引入了用于解决复杂规划问题的层次结构，并且讨论了使规划变得如此困难的约束和目标。

在本小节中，我们将更详细地讨论将整体规划问题分解为更小的子问题的层次结构。让我们首先回顾一下我们的规划层次结构。我们将分层规划器的每一层概括为定义其自己的优化问题，每一层都具有相对于相关目标函数和约束的特定抽象程度。

任务规划器是最高级别的优化问题，我们专注于从车辆当前位置到给定目的地的地图级导航。
行为规划将关注当前的驾驶场景，并根据工作空间中的其他代理和道路规则来决定执行哪种操作。
局部规划器将专注于以解耦的方式生成运动学上可行的、无碰撞的路径以及舒适的速度剖面。
最后将其输出到我们在课程一中讨论过的车辆控制器。

通过将规划问题分解为一个个子问题，我们限制了运动规划过程中解决每个模块所需的知识和计算，这允许更有效的计算。然而，这需要权衡，因为当我们分解问题时，每个子问题都会丢失一些域信息。与同时考虑规划过程的所有方面的规划相比，我们最终得到了一个次优的运动规划。对于自动驾驶，实时运行我们的运动规划器的能力至关重要，因此这种权衡几乎总是值得的。

在这里插入图片描述

现在，我们来详细了解每一步。如前所述，分层运动规划器中的第一个也是最高层是任务规划器。任务规划器专注于从自车当前位置到所需目的地的地图级导航。由于任务规划的规模通常以公里为单位，因此我们需要高度抽象化使问题变得易于处理。从这个意义上说，我们经常忽略运动规划问题的各个方面，例如障碍物和规则要素，而是关注问题的宏观方面，例如交通和道路连接情况。通过这样做，我们可以将问题简化到可以在大的空间范围内规划所需驾驶任务的程度。

下图是一个示例，我们在加利福尼亚州伯克利的地图上两点之间进行规划，同时考虑到地图上的各种十字路口和道路连接，以及每条道路的方向性。我们计算道路网中到目的地的最短路径。这可以通过多种方式完成。如果我们关注道路网中道路的空间长度，我们可以从道路网中构建一个图，然后使用 Dijkstra 算法或 A* 搜索 来找到最短路径。相反，如果我们将到达目的地的时间作为我们最小化的目标，那么问题会变得更加复杂，但仍然可以使用相同的算法来解决。我们将在第20讲中更详细地讨论这些算法。

在这里插入图片描述

18.4.2 Behavioural Planner

任务规划器下面是行为规划器。什么是行为规划？行为规划是运动规划的一部分，专注于遵循道路规则所需的高级决策，并识别在给定的驾驶场景中哪些操作是安全的。我们在第1小节中一起讨论了其中的许多场景。例如，回想一下我们讨论的场景之一是在十字路口转弯。举例来说，自车进入路口左转，此时为绿灯。

行为规划器应向路径规划器和速度生成器明确说明，只有在自车位置与交叉路口迎面而来的车辆和行人之间有足够的时间间隔时才能安全行进。按照我们在第1小节讨论的操作分类，行为规划器应输出一个让车操作，这将确保它只有在被允许这样做时才会继续。当然，这里的分类法绝不是通用的，在决定自动驾驶汽车应该表现出什么样的操作类型和行为范围时，有很多可能性。
在这里插入图片描述
现在我们可能想知道行为规划器应该如何获取有关自动驾驶汽车周围环境的所有信息并计算它应该以有效的方式执行什么操作？这是一个积极研究的领域，目前没有确定的最佳方法来实现这一目标。一般来说，似乎存在三种不同的流行架构来解决行为规划问题。

第一种是使用有限状态机的方法。有限状态机的关键概念有：自车所需机动的状态，以及自车状态应如何根据输入而做的转换。输入可能是：车辆位置、交通信号灯转换或我们当前驾驶场景中感兴趣的任何其他元素。例如，可以想象一个处理停止标志的状态机具有诸如减速停止、停止和跟踪速度之类的状态。当汽车遇到停车标志时，它会首先进入减速停止状态，然后行为规划器会发出信号表明该减速了。一旦它在停止位置停止，行为规划器将在设定的时间内保持停止状态。一旦超过设定时间并且交叉路口畅通无阻，行为规划器将进入跟踪速度状态，这意味着继续驾驶是安全的。

后面我们会更详细地介绍此示例。需要注意的是：在决定在有限状态机的每个步骤中采取哪种转换时，我们完全关注当前状态以及有限状态机的输入。本质上，这意味着有限状态机是无记忆的。过去的状态不会影响当前的转换或输出。
在这里插入图片描述
下一种常见的行为规划器类型是基于规则的系统。本质上，这类系统由规则按层次结构组成，其中层次代表每个规则的相对重要性。每个规则可以对应于道路规则，例如在红灯处停车，或者可以对应于最佳驾驶实践，例如在自车和前车之间保持两秒的间隔。

自车及其周围环境的状态将以逻辑的形式输入到基于规则的系统中。然后将针对每个规则评估这些逻辑，并根据规则优先级，将对应于最高优先级规则的机动输出到运动规划器。例如，假设我们有以下两条规则：

第一条规则检查前方是否有绿灯，并决定汽车继续行驶。
第二条规则检查车道上是否有行人，并决定执行紧急停车操作。

在我们的系统中，为前面的行人停车比通过绿灯行驶具有更高的优先级。现在假设我们正在开车，并在即将到来的十字路口观察到绿灯以及在十字路口前有行人路过。在这种情况下，两个规则都会触发，但由于紧急停止具有更高的优先级，自车需要执行紧急停车操作。可以想象，确保基于规则的系统在逻辑上是一致的既重要又具有挑战性。否则，他们可能会表现出不稳定的行为。

在这里插入图片描述
最后一组行为规划方法是基于机器学习的方法。这种方法的一个有趣的例子是使用强化学习。强化学习是确定最大化某个奖励函数 $R$ 的最优决策策略的过程。强化学习的过程需要代理在由模拟器给出的环境中执行动作。然后根据它与环境的交互来奖励这个代理。然后允许它通过连续的交互收敛到最优策略。不过在本课程中我们不会展开介绍这一方法。

在这里插入图片描述

18.4.3 Local Planner

分层规划器中的最后一个模块是局部规划器。如前所述，局部规划器的目标是为自车生成运动学上可行且无碰撞的路径以及舒适且安全的速度曲线。我们将局部规划问题分解为两个子问题：路径规划和速度曲线生成。

开发路径规划算法的关键在于减少优化的搜索空间。路径规划器主要分为三类：sampling-based planners、variational planners 和 lattice planners。sampling-based planners 随机地对汽车的控制输入进行均匀采样，以便为汽车生成潜在的穿越路径。

最具代表性的基于采样的算法之一是 RRT。RRT 通过在随机采样的位置生成点并从树中最近的点规划到该点的路径来构建路径树的分支。如果路径没有与任何静态障碍物发生碰撞，则将该路径添加到树中。这棵树快速探索具有许多潜在路径的工作空间，当到达目标区域时，将返回终止于该区域的路径。基于采样的算法通常非常快，但其代价是在短时间内运行时可能会产生质量较差的不稳定路径。
在这里插入图片描述
Variational planners 依靠成本函数来优化轨迹函数，该函数根据障碍物和机器人动力学的一些成本函数将时间点映射到工作空间中的位置。其通常是轨迹规划器，这意味着它们将路径规划和速度规划结合为一个步骤。因此他们不必将路径规划和速度规划问题解耦。

如下图所示，假设我们从空间路径的给定离散采样开始。起初，它也与所示的障碍物发生碰撞。Variational planners 将迭代地移动离散点，使它们相对于机器人的动力学是平滑的，并保持它们无碰撞。到最后，相对于机器人的动力学，轨迹将既无碰撞又更平滑。然而，这类方法通常更慢、更复杂，并且它们对可行路径的收敛对初始条件很敏感。

在这里插入图片描述

最后一组路径规划器是 lattice planner，此规划器通过限制自车在工作空间中的任何点可以采取的行动来限制搜索空间。这组动作称为晶格规划器的控制集。当与工作空间的离散化配对时，该控制集隐含地定义了一个图形。然后可以使用诸如 Dijkstra 或 A* 之类的图搜索算法来搜索该图，这会导致路径的快速计算。障碍物可以将跨越它们的边设置为无限成本。因此，图搜索也允许我们执行碰撞检查。虽然此规划器通常非常快，但路径的质量对所选控制集很敏感。
在这里插入图片描述
局部规划的第二部分是速度曲线生成。速度曲线生成通常设置为一个有约束的优化问题。通常，我们会结合上一小节中描述的许多速度曲线目标，例如最小化加加速度或最小化与所需参考的偏差的目标。我们还在第二小节中讨论了一些重要的限制。其中之一是舒适加速度的矩形。一旦目标和约束被形式化，它就变成了有效解决问题的问题。一种方法是计算优化域和目标的凸近似值，这有助于确保我们的优化器不会陷入局部最小值。