自动驾驶— Planning - Udacity Self-Driving Car Engineer

第六讲规划

6.1规划简介

在规划中，我们结合了高清地图、定位和预测来构建车辆的轨迹。规划的第一步是路线导航，重点是如何在地图上从A到B。路由以地图数据作为输入，并输出可导航路径。手机上的导航系统就是路线规划的一个例子。在Apollo中，路由模块处理这项任务。
一旦我们构建了一条高水平的路线，我们就会进行轨迹规划。轨迹规划是我们如何做出微妙的决定，以避开障碍物，为乘客创造一个平稳的旅程。在阿波罗号上，计划模块负责处理这项任务。轨迹规划的目标是生成一个无碰撞且舒适的轨迹来执行。这个轨迹是由一系列点定义的。每个点都有一个相关的速度和一个时间戳，指示我们应该何时到达该点。
我们将在两种不同的背景下讨论规划。一个是在非常具有象征意义的城市层面，你将计划要转弯多少才能到达目标目的地，另一个是运动轨迹层面，你可以决定汽车在执行某个动作时应该转弯。这些轨迹不仅仅是时间曲线，这些轨迹也不仅仅是空间曲线。它们也是时间曲线，因为你可能会选择走得很快或很慢，在计划中，你必须考虑你能走多快，以确保你有一个可行的计划。因此，让我们深入了解计划的迷人世界。

6.2路线Routing

路线的目标是在地图上找到从A到B的最佳路线。路由需要三个输入，首先是地图。Apollo提供的地图数据包括道路网络和实时交通信息。第二，我们目前在地图上的位置。第三，我们的目的地。目的地通常来自车辆内的乘客。有了这三个输入，路由模块就可以找到到达目的地的路线了。

6.3图形世界 World to Graph

当人类试图在地图上找到从a到B的路线时，他们通常会沿着道路追踪路径，看看这些路径中是否有通往目的地的路径。这叫做搜索。Apollo也使用搜索来查找路线，但它使用了更智能的搜索算法。在阿波罗开始搜索之前，它会将地图重新格式化为一个称为图的数据结构。图形由节点和边组成。节点表示路段，边表示这些路段之间的连接。例如，在十字路口，我们的车可以从节点一移动到节点二、节点三或节点四，反之亦然。

我们还可以对从一个节点移动到另一个节点所需的成本进行建模。例如，在现实生活中，转弯通过十字路口比直行需要付出更多的努力。因此，从节点一转到节点四的成本高于从节点一直接转到节点三的成本。将地图转换为图的优势在于，计算机科学已经发现了许多在图中查找路径的快速算法。一旦我们在图中找到了一条好的路径，就很容易将图中的路径转换回地图上的路线。

6.4网格世界Grid World

A_star（A*）是一种处理路径查找的经典算法。让我们演示A_star是如何使用网格世界工作的。我们假设这个世界上的每个细胞都是一个节点，我们可以从任何一个节点移动到它的任何相邻节点。这个特殊的世界包括一些阻挡潜在路径的墙。

对于人类来说，只需看图表就很容易找到最佳路径。对于计算机来说，这就不那么明显了。计算机必须检查是否有通向目标的路径。计算机可以穷尽所有可能的路径来找到最短的路径。然而，这需要花费大量时间，尤其是当地图变大时。从任何给定的节点，我们下一步最多有8个选项，因为我们有8个相邻的单元。对于这8个选项中的每一个，我们还有8个选项用于下一步。如果我们扩展所有节点，搜索将爆炸式增长，即使是最快的计算机也无法处理。

从初始节点开始，我们需要决定8个相邻节点中的哪一个是最有希望的候选节点。对于每个候选节点，我们考虑两件事。首先，我们计算从起始节点到候选节点所需的成本。其次，我们估计从候选节点到目标的旅行成本。计算到达候选节点的成本很容易，因为它已经与我们相邻。计算从候选节点到目标的成本很难。但是，通常我们可以生成一个很好的估计，通常只需测量从候选节点到目标的距离。我们使用变量g和h来表示这些成本中的每一个。g值是从起始节点行进到候选节点的成本，h值是从候选节点行进到目标的估计成本或启发式成本。

根据我们的情况，我们可以定制我们估计成本的方式。例如，交通堵塞增加了前往目的地的成本，因此交通繁忙的道路应该包含更大的成本。对于每个候选节点，我们将g和h值相加以计算一个和，称为f值。最佳候选节点是f值最小的节点。每次到达一个新节点时，我们都会重复这个过程来选择下一个候选节点，始终选择尚未访问的f值最低的节点。这就是A*算法。它建立了一条通往目标的道路。

6.5 A*

现在让我们考虑一下现实世界中的地图。假设我们把车停在一个十字路口。我们可以直接走高速公路，向右走或向左走。首先，我们将此映射更改为具有三个候选节点的图。接下来，我们评估我们的选择。在实践中，转弯通过十字路口需要付出很大的努力。因此，我们给这个节点分配一个更高的g值。g值是从起点到候选节点的成本。

考虑到高速公路的选择，我们意识到我们必须走很长的路才能离开高速公路，然后返回才能达到我们的目标。因此，我们为这个选项指定一个更高的h值。h值是从候选节点到目标的估计成本。接下来，我们将g值和h值相加，以计算每个节点的f值。我们看到，最小的f值实际上是右边的候选节点。因此，这成为我们的下一步行动。

6.6 从路线到轨迹From Routing to Trajectories

高级地图路线只是规划过程的一部分。我们仍然需要建立低水平的轨迹才能沿着这条路线前进。这意味着要处理不在地图上的其他车辆、自行车或行人等物体。例如，我们可能需要与一辆试图在我们面前掉头的汽车互动，或者我们可能想在高速公路上经过一辆慢车。这些场景需要更低级别、更高精度的规划。我们将这种水平的规划称为轨迹生成。

6.7三维轨迹3D trajectory

轨迹生成的目标是生成由一系列路点定义的轨迹。我们为每个航路点指定时间戳和速度。我们可以将曲线拟合到这些路点，以创建轨迹的几何表示。由于移动的障碍物可能会暂时阻塞部分道路，因此轨迹中的每个航路点都有时间戳。我们可以将时间戳与预测模块的输出相结合，以验证轨迹上的每个航路点在我们计划移动时都是空的。这些时间戳创建了一个三维轨迹，每个航路点由两个空间维度和第三个时间维度定义。我们还为每个航路点指定一个速度。速度可确保车辆按计划到达每个航路点。

6.8评估轨迹Evaluating a Trajectory

限制条件
现实世界的规划面临多重约束。首先，轨迹应该是无碰撞的，这意味着它必须是无障碍的。其次，我们希望乘客感到舒适，因此航路点和速度变化之间的过渡应该是平稳的。第三，轨道在物理上对车辆来说应该是可行的。例如，一辆高速行驶的汽车不可能瞬间转动180度。我们无法建立一个包含这种不可能的机动的轨迹。最后，轨迹应该是合法的。我们需要了解每个航路点的交通规律，并确保轨迹遵守这些规律。

成本函数：
在道路上的任何两点之间，都可能有多条无碰撞、舒适、可行且合法的轨迹。我们如何选择最佳轨迹？答案是使用成本函数。成本函数为每个轨迹分配一个成本。我们选择成本最低的轨迹。轨迹的成本由各种惩罚组成。例如，偏离道路中心或潜在碰撞或超过速度限制，或者如果轨道的曲率或加速度对乘客来说不舒服，可能会产生成本。轨迹的成本将所有这些缺陷聚合为一个数字，这使我们能够对不同的轨迹进行排序。车辆甚至可能针对不同的环境使用不同的成本函数。例如，高速公路的成本函数可能与停车场的成本函数不同。

6.9Frenet坐标

传统上，我们使用笛卡尔坐标来描述物体的位置，但笛卡尔坐标对车辆来说并不是最优的。考虑到汽车的X、Y位置，如果我们不知道道路在哪里，就很难知道汽车行驶了多远，或者是否偏离了车道中心。

替代笛卡尔坐标系的是Frenet坐标系。Frenet坐标描述了汽车相对于道路的位置。在Frenet坐标系中，S表示沿道路的距离，也称为纵向坐标。D表示从纵向线的位移，也称为横向坐标。在道路上的每一点上，横轴和纵轴都是垂直的。纵向坐标告诉我们沿着道路行驶了多远，横向坐标告诉我们汽车偏离中心线的距离。

6.10路径速度解耦规划Path-Velocity Decoupled Planning

路径-速度解耦规划将轨迹规划分为两个步骤：路径规划和速度规划。
首先，在路径规划步骤中，我们生成候选曲线，这些曲线是车辆可以行驶的路径。我们使用成本函数来评估每条路径，该函数包含了平滑度、安全性、与车道中心的偏差以及我们想要考虑的任何其他因素。我们按成本对路径进行排序，并选择成本最低的路径。
下一步是确定沿着这条路径行驶的速度。我们可能想在路径上改变速度，所以我们真正需要选择的是与路线点相关的一系列速度，而不是单一的速度。我们可以将这个序列称为速度剖面。我们可以使用优化为路径选择一个良好的速度剖面，但要受到各种约束。该路径和速度轮廓组合起来构成了车辆行驶的轨迹。

6.11路径生成和选择Path Generation and Selection

为了生成路径速度解耦规划的候选路径，我们首先将道路分割成单元。然后我们从每个细胞中随机抽取点。我们通过从每个单元格中提取一个点并将这些点连接在一起来创建候选路径。通过重复这个过程，我们可以构建多个候选路径。我们使用成本函数来评估这些路径，并选择成本最小的路径。

我们的成本函数可以考虑：
偏离车道中心。
与障碍物的距离。
速度和曲率的变化。
车辆上的应力。
或者我们想包括的任何其他因素。

6.12 ST图

选择路径后的下一步是选择与该路径关联的速度分布。一种称为ST图的工具可以帮助我们设计和选择速度剖面。在ST图中，S表示车辆的纵向位移，T表示时间。ST图上的曲线是对车辆运动的描述，因为它说明了车辆在时间上的位置。速度是位置变化的速率，所以我们可以通过观察曲线的斜率从ST图中推断速度。更陡的斜率表示在更短的时间内有更多的运动，这对应于更高的速度。

6.13速度profile

为了建立最佳的速度profile，我们可以将ST图离散为多个单元。细胞之间的速度变化，但在每个细胞内，速度保持不变。这种方法简化了速度profile的构造，同时保持了曲线的近似值。在ST图上，障碍物可以画成矩形，在特定的时间段内挡住道路的某些部分。例如，假设预测模块预测一辆汽车将在时间T0到T1驶入我们的车道。由于该汽车将在该时间段内占据位置S0至S1，我们在ST图上画一个矩形，在时间段T0至T1期间遮挡位置S0到S1。为了避免碰撞，我们的速度profile不能与这个矩形相交。

现在我们有了一个ST图，其中各种单元格被封锁，我们可以使用优化引擎为该图选择最佳速度分布。优化引擎使用复杂的数学来搜索受各种约束的低成本解决方案。这些限制可能包括法律限制，如速度限制，距离限制，如与障碍物的距离，或对汽车的物理限制，如加速度限制。

6.14优化Optimization

路径速度解耦规划在很大程度上依赖于离散化。路径选择包括将道路划分为多个单元，速度剖面构建包括将ST图划分为多个子单元。离散化使这些问题更容易解决，但解决方案并不顺利。

为了将离散解转换为平滑的轨迹，我们可以使用一种称为二次规划的技术。二次规划将平滑的非线性曲线拟合到这些分段线性段。二次规划背后的数学是复杂的，但就我们的目的而言，细节并不是真正必要的。我们可以简单地使用几个不同的优化包中的一个，包括Apollo的实现，以生成平滑的结果。一旦我们的路径和速度剖面都准备好了，我们就可以使用它们来构建三维轨迹。

6.15Trajectory Generation for Path Velocity Planning 路径速度规划的轨迹生成

让我们从头到尾回顾一下路径速度解耦规划。想象一下，我们在路上行驶，我们的感知系统观察到我们正在缩小一辆缓慢行驶的车辆的差距。首先，我们围绕这辆车生成多条候选路径。我们使用成本函数来评估这些候选路径，并选择成本最低的路径。然后，我们使用ST图进行速度规划。我们根据其他车辆随时间推移的位置来屏蔽ST图的部分。优化引擎帮助我们根据成本函数中的约束为该图确定最佳速度分布。我们可以应用二次规划来平滑路径和速度分布。最后，我们可以将路径和速度剖面绑定在一起，以构建轨迹。这里，轨迹在速度较高的地方是红色的，在速度较低的地方是蓝色的。我们利用这条轨迹安全地超越另一辆车，继续我们的旅程。

6.16 Lattice Planning 格点规划

让我们考虑一种称为网格规划的轨迹生成方法。使用Frenet坐标，我们可以将环境投影到纵轴和横轴上。我们的目标是生成三个维度的轨迹：纵向维度、横向维度和时间维度。我们可以通过分离轨迹的纵向和横向分量，将三维问题分解为两个单独的二维问题。一个二维轨迹将是具有时间戳的纵向轨迹。我们称之为ST轨迹。另一个二维轨迹将是相对于纵向轨迹的横向偏移。我们称之为SL轨迹。

格点规划是一个分两步进行的过程，首先独立开发ST和SL轨迹，然后将其组合。为了生成二维纵向和横向轨迹，我们首先将初始车辆状态投影到ST坐标系和SL坐标系上。我们通过从预先选择的模式中采样多个候选结束状态来选择车辆的结束状态。对于每个候选的结束状态，我们建立一组轨迹，将车辆从初始状态过渡到结束状态。我们使用成本函数来评估这些轨迹，并选择成本最低的函数轨迹。

6.17 ST轨迹的结束状态Ending States for ST Trajectories

根据情况，我们可以从巡航、跟随和停止三组中采样结束状态。
巡航意味着我们的汽车将在计划步骤后以恒定的速度结束。记住，我们在图上采样点，横轴表示时间，纵轴表示速度。对于图上的一点，这意味着汽车将在时间t以s的速度进入巡航状态。对于这种模式，所有结束状态的加速度都为零。
下一个需要考虑的模式如下。在这种情况下，我们对位置和时间进行采样，这意味着我们会试图在时间t出现在某辆车的后面。要跟随那辆车，我们需要留出一段安全距离，因此速度和加速度取决于领先车辆。这意味着在这种模式下，速度和加速度都是固定的。
最后的状态是停止。对于这种模式，我们只需要采样车辆何时何地停止。在这个场景中，我们将速度和加速度都固定为零。

6.18 SL轨迹的结束状态Ending States for SL Trajectories

我们将基于这样一个假设来执行SL规划，即无论汽车进入何种结束状态，它都应该稳定地与车道的中心线对齐。这意味着我们只需要在一个小区域内对横向结束位置进行采样。具体来说，我们对道路上相邻车道中心线周围的位置进行采样。为了确保稳定性，车辆行驶的终点状态应与车道中心对齐。当我们根据纵向位置绘制横向位置时，我们想要的候选轨迹应该以车辆与车道对齐并笔直行驶结束。为了实现这种结束状态，航向和位置的一阶导数和二阶导数都应该为零。这意味着车辆既没有横向移动，这是一阶导数，也没有横向加速，这是二阶导数。车辆正沿着车道直线行驶。

6.19多项式Polynomial

拟合多项式和求值
我们如何连接初始状态和结束状态？
我们将用多项式来拟合这两个条件。这里，初始条件和结束条件都是在s坐标上具有位置、速度和加速度的元组。注意，速度是位置的一阶导数，加速度是位置的二阶导数。通常，我们将相对于时间的导数表示为变量上方的一个点。
现在我们插入初始条件和结束条件，我们有6个方程。这里我们假设结束条件发生在时间t=T。

通过这些方程，我们可以求解a、b、c、d、e和f。这表示一条平滑连接两个条件的曲线。

6.20格点规划的轨迹生成Trajectory Generation for Lattice Planning

一旦我们有了ST和SL轨迹，我们就需要将它们转换回笛卡尔坐标系。然后我们可以将它们组合起来，构建一个由二维路径点和一维时间戳组成的三维轨迹。ST轨迹是随时间的纵向位移，SL轨迹是纵向轨迹上每个点的横向偏移。由于两条轨迹都有一个纵向坐标S，我们可以通过在它们的S值上匹配来组合轨迹。

6.21 SDCND的路径规划项目Path Planning Project from SDCND

规划是制造自动驾驶汽车最困难、最具挑战性的部分之一。自动驾驶汽车工程师纳米学位项目的规划项目为学生提供了一个高速公路模拟器，并要求他们建立一个在交通中导航的规划师。这是一个有趣、现实、非常困难的项目，因为你必须预测未来路上的其他车辆会在哪里，你应该如何应对它们，并最终为车辆的执行建立一个轨迹。纳米级程序中的规划模块涵盖了一种基于有限状态机的新算法。在这种方法中，规划者首先选择一种行为，比如变道、加速或减速。然后规划人员建立一个执行该行为的轨迹。

6.22概述

在本课中，我们将探讨自动驾驶汽车如何规划穿越世界的轨迹。车辆基于高清地图、感知和预测模块的数据制定此计划。首先，我们介绍如何使用a星规划到达目的地的地图级路线。然后，我们专注于建立无碰撞的轨迹，引导我们一步一步地到达目的地。我们学习了如何使用过去的速度解耦规划或网格规划来构建这些轨迹。比较这两种方法将培养你的规划直觉以及如何使用成本函数选择轨迹。规划是开发自动驾驶汽车最困难的部分之一。

6.23 Referance

1. Translate From 《Udacity自动驾驶基础》第6讲 路径规划
2. A*算法实施Github链接：Robotics-Path-Planning-03-Hybrid-A-Star
3. 无人驾驶学习笔记–路径规划（一）【Path Finding – Hybrid A* 】