Coursera自动驾驶课程第20讲：Mission Planning in Driving Environments

在第19讲《Coursera自动驾驶课程第19讲：Mapping for Planning》我们学习了自动驾驶中两种环境建图方法：占用网格图（occupancy grid map） 和 高清地图（high-definition road map）。

在本讲中，我们将详细讨论自动驾驶中的任务规划问题以及如何解决它。正如在第18讲中介绍的，任务规划在整个规划问题中处于最高层，它对于如何将自动驾驶汽车导航到目的地至关重要。本讲中我们将学习三种常用的用于解决任务规划问题的算法：Breadth First Search、Dijkstra 和 A* 搜索。

文章目录

- - 20.1 Creating a Road Network Graph
  - - 20.1.1 Graphs
    - 20.1.2 Breadth First Search
  - 20.2 Dijkstra's Shortest Path Search
  - - 20.2.1 Unweighted/Weighted Graph
    - 20.2.2 Dijkstra's Algorithm
  - 20.3 A-Star Shortest Path Search
  - - 20.3.1 Euclidean Heuristic
    - 20.3.2 A* Algorithm
    - 20.3.3 Extensions to Other Factors

20.1 Creating a Road Network Graph

20.1.1 Graphs

让我们回顾一下自动驾驶任务。其目标是在导航地图中，找到自车从当前位置到给定目的地的最佳路径。在本课程中，我们将根据汽车到达目的地所需的时间或距离来考虑最优性。对于自动驾驶，任务规划被认为是最高一级的规划问题。

在这里插入图片描述

在自动驾驶中，任务规划的空间规划规模往往是公里级的，在任务规划中往往并不关注障碍物或动力学等低级规划约束。相反，任务规划时将关注道路网络的各个方面，例如速度限制和道路长度、交通流量和道路封闭与否。基于地图给我们带来的这些限制，任务规划时需要找到到达我们所需目的地的最佳路径。关于道路网络需要注意的一点是，它是高度结构化的，我们可以在规划过程中利用这一点来简化问题。通过利用该结构，我们可以根据提供给我们的地图有效地找到到达目的地的最佳路径。为此，我们将需要使用一种称为图的数据结构，我们已将其覆盖在此处的道路网络上。

那么什么是图呢？图是由以 $V$ 表示的顶点集合和以 $E$ 表示的边集合而组成的（如下图所示）。在任务规划中， $V$ 中的每个顶点将对应于道路网络上的某个点， $E$ 中的每条边将对应于连接道路网络中任意两个点的路段。从某种意义上来说，图中的一系列连接的边对应于道路网络中从一个点到另一个点的路径。在下图中我们假设了每个路段的长度是相等的，因此该图的边都是未加权的。

但是，在后面的课程中，我们会介绍加权的图。注意，图的边通常是有向的，我们通过在图中的边上使用箭头来显示它们的方向。现在我们有了有向图，我们如何找到到达目的地的最佳路径呢？首先，我们在图中已知我们当前自车位置（表示为 $S$ ）和我们希望的目的地（表示为 $t$ ）的顶点。这两个顶点显示在此处的图上。

一旦我们有了这两个顶点，我们就可以使用高效的图搜索算法来找到到达目的地的最优或最短路径。由于我们的图形公式目前没有加权，一个好的候选算法是广度优先搜索或 BFS。

在这里插入图片描述

20.1.2 Breadth First Search

在BFS 算法中我们常用到三种数据结构：Queue()、Set()、Dict()，分表存储未遍历的顶点、已经遍历过的顶点和用来存储节点之间连接关系的字典。队列是一种先进先出（FIFO）的数据结构，因此第一个入队的顶点也是第一个出队的顶点。字典是一组无序的键值对，对于字典种的每个节点，我们可以很容易找到在此节点之前的顶点。

在这里插入图片描述

为了帮助我们理解 BFS 算法，让我们来看一个具体的例子。

假设我们的任务规划器需要通过我们图中现在标记的顶点集找到从点 $s$ 到目的地 $t$ 的最佳路径，第一步是处理原点 $s$ 并将所有相邻顶点添加到我们的队列中并设置他们的前任节点为 $s$ 。相邻顶点的边以蓝色显示。一旦我们将这些相邻点添加到我们的队列中，我们就会将 $s$ 添加到我们的闭集中。
接下来，我们弹出顶点 $a$ 。 $a$ 有两条边分别指向 $d$ 和 $b$ ，但 $b$ 已经在我们的队列中，因此，我们只将 $d$ 添加到队列中并将 $a$ 标记为它的前任节点。我们用红色突出显示了这个重复的路径，以表明我们没有将 $b$ 添加到队列中两次。我们现在已经处理了来自 $a$ 的所有相邻顶点并将其添加到闭集。我们对 $b$ 重复相同的过程，将 $e$ 添加到队列中，其中 $b$ 作为其前任节点，而 $c$ 没有新的相邻顶点（其顶点仍然是 $e$ ）。
接下来，我们从队列中弹出 $d$ ，它只将 $t$ 添加到队列中，其中 $d$ 作为其前任节点（因为 $e$ 已经添加）。当 $e$ 被弹出时，它不会将 $c$ 或 $d$ 添加到队列中，因为这两个顶点都已被处理并存在于闭集中且对应的边是单向的（从 $e$ 到达不了 $b$ 或 $t$ ）。
最后，我们从队列中弹出 $t$ ，这是我们的目标顶点。所以我们现在通过从 $t$ 到 $s$ 的前任链来重构从 $s$ 到 $t$ 的路径。完成此操作后，我们就找到了到达目的地的最佳路径，该路径以绿色突出显示。然后可以使用我们的地图将对应于图中最佳路径的边序列转换为道路网络的边序，将其用于在我们的规划层次结构的后续层中处理更详细的运动规划。

在这里插入图片描述我们应该注意，除了广度优先搜索还有深度优先搜索算法。深度优先搜索使用后进先出堆栈而不是先进先出的队列。此更改意味着评估最近添加的顶点而不是最旧的顶点。结果是搜索在图中快速移动更深，然后最终回溯到更早添加的顶点。

在下一小节，我们将通过向图中的边添加不同的权重来使图更复杂，以更好地反映使用不同路段的不同成本，我们将介绍 Dijkstra 算法。

20.2 Dijkstra’s Shortest Path Search

20.2.1 Unweighted/Weighted Graph

在本小节，我们将修改上一小节中的无权重图来给图中的边加上权重，为我们的任务规划问题提供更合适的表示。然后，我们将讨论这会如何影响我们的算法，以及我们如何在有效规划的同时克服这一挑战。特别地，我们应该能够理解加权图和未加权图之间的区别，以及为什么加权图对任务规划更有用。同时我们应该牢牢掌握 Dijkstra 算法，对加权图非常有用的图搜索算法。

回顾一下，在上一小节，我们介绍了广度优先搜索算法。然而在这个过程中，我们假设所有路段的长度都相等，这是一个过于简单的假设。根据道路长度、交通、速度限制和天气等因素，不同路段的通过成本可能会有很大差异。为简单起见，我们最初仅关注图中的距离。为了反映这一点，我们将为图中的每条边添加权重，对应于相应路段的长度（权重的单位是任意的，只要它们对所有边都是通用的就可以。对于这种情况，假设边的权重是通过该路段所需的公里数）。

下图左边为无权重图，右边为加了权重后的图。和之前一样，我们的目标还是找到从自车当前位置 $s$ 到最终目的地 $t$ 的最优路径。不幸的是，我们的 BFS 算法没有考虑边的权重。所以不能保证在这种情况下找到最优路径。为此，我们需要使用更强大的算法。

在这里插入图片描述

20.2.2 Dijkstra’s Algorithm

Dijkstra 算法，荷兰计算机科学家 Edsger Dijkstra 于 1956 年首次构想出。在 2001 年的一次采访中，他解释说当时他正在和未婚妻一起购物，累了就坐了下来喝杯咖啡。他一直在思考最短路径问题，在 20 分钟内，他在没有纸笔的情况下完成了他的算法。他又花了三年时间写了一篇关于这个主题的论文，但他将这个想法的简单和优雅归功于被迫完全在他的脑海中完成解决方案。

Dijkstra 算法的整体流程与 BFS 非常相似。主要区别在于处理顶点的顺序。下图用蓝色突出显示了与 BFS 算法的区别。关键区别在于，我们将使用最小堆，而不是使用队列。最小堆是一种存储键和值的数据结构，并根据键的值从小到大对键进行排序。在我们的例子中，图中每个边的权重的将对应于从一个顶点到达另一个顶点所需的距离。

在这里插入图片描述

为了巩固我们的理解，让我们逐步将 Dijkstra 算法应用于我们的新加权图。

与 BFS 一样，要处理的第一个顶点是 $s$ ，然后将 $a 、 b 、 c$ 添加到最小堆中。因此到达 $a 、 b 、 c$ 的成本分别为 5、7 和 2。由于我们使用的是最小堆而不是队列，因此最小堆中的顶点顺序现在是 $c 、 a 、 b$ （成本从低到高排序），然后将 $s$ 添加到闭集。
接下来，我们从堆中弹出 $c$ 。因为它是迄今为止成本最低的顶点。 $c$ 只连接到顶点 $e$ 。因此，我们将其添加到最小堆中，成本为 2+8 为 10，同时将 $c$ 作为其前任节点。我们的新堆排序是 $a 、 b 、 e$ ，然后我们将 $c$ 添加到闭集。
下一个从堆中弹出的顶点是 $a$ ，它连接到 $d$ 和 $b$ 。 $d$ 尚未被访问，因此我们将其添加到堆中，成本为 5+2 为 7。然而 $b$ 已经被访问，它目前的累计成本为 7。边 $a b$ 的权重为 1，因此经过 $a$ 的新成本是 5+1 等于 6。由于这低于 $b$ 的当前成本（当前为 7 ），我们将堆中 $b$ 的成本更新为 6，并将其前任节点从 $s$ 更改为 $a$ 。为了表明 $b$ 的成本已更新，我们将新边标记为紫色，然后将 $a$ 添加到闭集。
现在最小堆中的顶点顺序现在是 $b 、 d 、 e$ 。我们现在从堆中弹出 $b$ ，它只连接到 $e$ 。到目前为止到达 $b$ 的成本是 6，因此我们到达 $e$ 的成本是 9 。而 $e$ 已经存储在最小堆中，成本为 10。因此，我们需要将 $e$ 的成本更新为 9，并将其前任节点从 $c$ 更改为 $b$ 。最后，我们将顶点 $b$ 添加到闭集，我们在最小堆中的新顶点排序是 $d 、 e$ 。
接下来，我们从堆中弹出 $d$ 。 $e$ 已经在堆中，成本为 9，从 $d$ 到 $e$ 的新路径的成本为 14。由于这高于 $e$ 当前的成本，我们可以忽略这条新路径。为了表明我们忽略了它，我们将 $e$ 的新边标记为红色而不是紫色。然后，我们将 $t$ 添加到成本为 7 的堆中，加 1 后为 8，将 $d$ 设置为其前任节点。最后，我们将 $d$ 添加到闭集。
我们的新堆排序是 $t$ ，然后是 $e$ 。我们弹出的最后一个顶点是 $t$ ，这是我们的目标顶点。这样就完成了规划过程，我们现在通过将 $t$ 的前任节点链接在一起形成了一条最佳路径，一直到原点，如图所示。

在这里插入图片描述
接下来，让我们看看这个任务规划问题在真实地图上的样子。在这里，我们有一张加利福尼亚州伯克利的地图。图中的顶点对应于交叉路口，边对应于我们前面讨论的路段。两个红点，对应于我们计划的起点和终点。我们必须记住，某些道路是单向道路。因此，该图是有向的。运行 Dijkstra 算法后，两个节点之间的最短路径由红色路径给出。Dijkstra 的算法非常有效，这使得它可以很好地扩展到现实世界的问题，例如此处显示的问题。

在这里插入图片描述

20.3 A-Star Shortest Path Search

20.3.1 Euclidean Heuristic

在小节中，我们将在 Dijkstra 算法的基础上引入一种新的更快的方法，可以将其用于我们的任务规划问题。为此，我们将在搜索中利用启发式算法，这将帮助我们改进搜索过程以提高效率。我们应该了解启发式在图搜索中的作用，并确定哪些启发式对我们的任务规划问题有效，哪些无效。我们还应该能够通过使用 A* 搜索算法在我们的图搜索问题中利用启发式算法，并了解如何将 A* 搜索应用于我们目前讨论的其它形式的任务规划问题。

回顾一下（下图所示），在上一小节我们介绍了 Dijkstra 算法来帮助我们解决加权图下的任务规划问题，这比我们之前的未加权图实例更贴近实际，因为它让我们考虑了不同道路上的变化距离。然而，Dijkstra 算法要求我们搜索图中存在的几乎所有边，即使其中只有少数对构建最优路径有用。这对于我们的小范围示例图来说不是问题。当我们将问题扩展到更现实的比例时（例如完整的城市道路网络），它会引起一些问题。

在这里插入图片描述

为了提高实际效率，我们可以通过使用 A* 算法而不是 Dijkstra 算法根据搜索启发式算法来找到我们的目的地。什么是启发式搜索？在这种情况下，启发式搜索是对从图中的任何给定顶点到达目标顶点的剩余成本的估计。当然，我们使用的任何启发式方法都不准确，因为这需要知道我们搜索问题的答案。相反，我们可以根据问题实例的结构来开发一个快速计算的合理估计。

在我们的例子中，图中的顶点对应于空间中的点，边对应于路段，其权重对应于这些路段的长度。因此，对于给定的顶点 $v$ 和目标 $t$ ，任何两个顶点之间长度的有用估计是它们之间的直线或欧几里德距离，公式为：
$h(v)=∥t−v∥(20.1)h(v)=\|t-v\| \tag{20.1}$

因此，对于我们在搜索中遇到的任何顶点，我们对目标顶点剩余成本的估计将只是该顶点与目标之间的欧几里得距离。请注意，这种估计始终低估了到达目标的真实距离，因为任何两点之间的最短距离是直线。这是 A* 搜索的一个重要前提，满足此前提的启发式称为可容许启发式。作为一个示例计算，假设我们有一个起始顶点 $a$ 和一个目标顶点 $c$ 。顶点 $a$ 对应坐标为 $(0.0, 0.0)$ ，顶点 $b$ 对应坐标为 $(2.0, 0.0)$ 。在这种情况下，顶点 $c$ 对应坐标为 $(2.0, 2.0)$ 。因此， $a$ 和 $c$ 之间的欧几里得距离为 2.828，这是我们对目标成本的启发式估计。

请注意，这里任何两个相邻顶点之间的边成本不等于这些顶点之间的距离。这是因为路段不是直线路径，通常路段长度会受到道路形状的影响。由于图的简化，我们可以看到从 $a$ 到 $c$ 的路径的实际成本是 4.6，通过将 $a b$ 和 $b c$ 边成本相加。正如预期的那样，我们的启发式方法低估了真实成本。

在这里插入图片描述

20.3.2 A* Algorithm

在这里，我们有 A* 算法的伪代码。它与 Dijkstra 的算法基本相同，但有一些关键差异，我们用蓝色突出显示。让我们更仔细地看一下具体的变化。回想一下，在 Dijkstra 的算法中，我们将待遍历的顶点连同它们从原点累积的成本一起推送到最小堆上。然后最小堆按相关的累积成本对顶点进行排序。 Dijkstra 算法与 A* 的区别在于：我们不使用累积成本，而是使用累积成本加上 $h (v)$ ，启发式估计目标顶点的剩余成本作为我们推入最小堆的值。然后，最小堆基本上按照目标的估计总成本对待遍历顶点进行排序。

从这个意义上说，根据我们的启发式搜索，A* 将搜索偏向可能是最佳路径的一部分的顶点。由于我们存储的是基于启发式的总成本和最小堆，我们还需要跟踪每个顶点的真实成本，并将其存储在成本结构中。需要注意的一件有趣的事情是，如果我们将所有顶点的启发式设为零，这仍然是一个可接受的启发式，那么我们最终会得到 Dijkstra 算法。与之前的 Dijkstra 算法一样，我们将原点添加到最小堆中，然后将每个顶点从堆中弹出并将所有相邻顶点添加到堆中，直到我们处理目标顶点。

在这里插入图片描述

让我们将 A* 算法应用于我们之前介绍的例子，这里我们添加了每个顶点的位置坐标（这里没有严格按坐标位置和比例绘制）。

与之前一样，我们添加到最小堆的第一个顶点是原点 $s$ 。累积成本为零， $s$ 和 $t$ 之间的欧几里得距离为 4.472，因此最终成本为4.472。
接下来，我们处理第一个节点 $s$ 并将相邻的顶点 $a 、 b$ 和 $c$ 添加到最小堆中。请记住在将每个顶点添加到最小堆时，我们需要将累积成本添加到目标的启发式成本中。因此，对于顶点 $a$ ，我们的成本是 $5 + 3 = 8$ 。对于顶点 $b$ ，我们的成本是 $7 + 2 = 9$ ，对于顶点 $c$ ，我们的成本是 $2+13=5.6062+\sqrt13=5.606$ 。因此最小堆中顶点的顺序现在是 $c a b$ 。我们还将 $s$ 作为顶点 $a 、 b$ 和 $c$ 的前任节点，然后将 $s$ 添加到闭集。
下一个要处理的顶点是 $c$ ，它只连接到顶点 $e$ 。 $e$ 的成本为 11.414。所以我们现在有一个顶点顺序 $a b e$ 。然后我们将 $e$ 的前任节点指定为 $c$ 并将 $c$ 添加到闭集。
接下来，我们从最小堆中弹出 $a$ ，我们看到它连接到顶点 $b$ 和 $d$ 。 $b$ 的成本为 8，这低于 $b$ 的原始估计成本。所以我们在最小堆中更新它的成本，并将其前身从 $s$ 更改为 $a$ 。为了显示我们已经更新了它的成本，我们用紫色突出显示了边。我们还将顶点 $d$ 添加到最小堆中，成本也为 8（这里课程PPT写为7）。因此，新的堆排序为 $d b e$ 。然后我们将 $a$ 指定为 $d$ 的前任节点，并将 $a$ 添加到闭集。
以 $d$ 顶点继续，它连接到顶点 $e$ 和 $t$ 。 $e$ 的估计成本为 15.414。由于这高于 $e$ 在最小堆中的当前成本，我们忽略了这条通往 $e$ 的新路径，因此我们将这条边标记为红色。 $t$ 的估计成本为 8。由于它是目标节点，因此它对目标的启发式成本为零。完成此操作后，新的最小堆排序为 $t b e$ 。我们将 $t$ 的前身设置为 $d$ 并将 $a$ 添加到闭集。最后，我们处理作为目标顶点的顶点 $t$ （这里课程中没有继续介绍以 $b$ 顶点继续的情况）。从 $s$ 到 $t$ 的最终最短路径显示在此图上，并且由于 A* 方法，我们能够避免同时处理 $b$ 和 $d$ （这句话有一点问题，在上一步中得到 $b$ 和 $d$ 的估计成本是相等的，这里其实需要再一步处理的，不过课程中PPT写错了，导致没有进一步处理）。

在这里插入图片描述

20.3.3 Extensions to Other Factors

不幸的是，上面这个示例中图非常小，无法演示该算法的工作原理。然而，当我们将问题扩展到更大的图时，我们将看到 A* 算法中使用的启发式算法将导致它探索的顶点比 Dijkstra 算法少得多。下图中可以明显看到，A* 算法比 Dijkstra 算法探索的顶点要少得多。其中绿色顶点为起点，红色顶点为目标顶点，黄色顶点为规划的路径。

在这里插入图片描述

现在，在我们的示例中，我们已经简化了任务规划问题，例如地图中的边权重或沿路段的距离。但是，如果我们将交通、速度限制或天气等其他因素包括在我们的任务规划问题中，那么沿路径的道路距离将过于简单，无法捕捉到问题的范围。为了解决这个问题，我们可以将估计的穿过路段的时间作为我们的权重，这将所有提到的因素都考虑在内。然而，这使得我们的欧几里得距离度量毫无用处，因为它不再能捕捉到目标的真实成本。为了解决这个问题，我们可以使用到达目标点的时间的估计值，即欧几里得距离除以所有路段允许的最大速度。汽车不应超过限速。因此，即使在理想的交通和天气条件下，以及通往目标的直线路径，这也是汽车可以行驶到目标的绝对最短时间。这意味着它始终是目标真实成本的下限，因此它是一种可接受的启发式方法。

例如，这里我们将最大速度设置为 $100 k m / h$ ，边的权重对应于穿过路径所需的时间。在计算我们的启发值 $h (a) = 101.8 s$ 后，我们可以看到它远低于从 $a$ 到 $c$ 的真实时间，即 165.6 秒。这是因为一般来说，这种启发式方法的下限很差，因为这些路段的遍历时间通常比计算出的最小值要长得多。与严格专注于最小化距离的问题实例相比，较差的下界会降低我们的启发式算法引导我们搜索更复杂问题目标的能力。可以使用更高级的方法来预先计算附加值并考虑修改后的启发式定义，从而有效地搜索具有基于时间估计的大型网络。