自动驾驶入门：规划

目录

概念

基本原理

将地图转化为数据结构图

路径查找算法 A*

A*在现实中的应用

地图级轨迹生成

Frenet 坐标系

速度- 路径解耦规划

路径生成与选择

ST图

速度规划

优化

路径-速度规划的轨迹生成

Lattice规划

ST轨迹的终止状态

SL轨迹的终止状态

Lattice规划的轨迹生成

---

概念

规划也是ppp中最后一个：planning。

路径规划是指通过一定的规则，找到一条通过世界的路径来达到我们想去的地方。规划的第一步是路线导航，侧重于研究如何从地图上的A点前往B点。在路线导航时，将地图数据作为输入，并输出可行驶路径。手机导航系统是路线导航的一个示例。在Apollo中，可以通过路线规划模块处理该任务。一旦我们构建出高水平的路线，我们就会放大至轨迹规划。该轨迹由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。通过轨迹规划，我们可以做出微妙的决策，以避开障碍物，并为乘客创造平稳的乘车体验。在Apollo中，我们通过规划模块处理该任务。路线规划的目标是，找到从地图上的A前往B的最佳路径。轨迹规划的目标是找到避免碰撞和保持舒适度的可执行轨迹。

路径规划使用三个输入，第一个输入为地图，Apollo提供的地图数据包括公路网和实时交通信息。第二个输入为我们当前在地图上的位置。第三个输入为我们的目的地，目的地取决于车辆中的乘客。

基本原理

将地图转化为数据结构图

当人们试图在地图上找到从A到B的路线时，通常会沿着道路追踪路径，以查看是否存在通往目的地的任何路径，这被称为搜索。Apollo也通过搜索来查找路线，但它使用了更智能的搜索算法。在进行智能搜索算法以前，我们需要将地图数据重新格式化为“图形”的数据结构。

该图形由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。例如：在交叉路口，汽车可从节点1移动到节点2、节点3或节点4，反之亦然。

我们可以对一个节点移动到另一个节点所需的成本进行建模。例如在现实生活中，拐过一个交叉路口比直行更费劲，所以从节点1到节点4的成本高于从节点1到节点3的成本。 将地图转换为图形的好处在于，在计算机科学领域中，人们已发现许多用于在图形中查找路径的快速算法（下一节中会详细介绍）。一旦我们在图形中找到了一条好路径，就可以轻松地将图形中的路径重新转换为地图上的路径。

路径查找算法 A*

A* 是经典的路径查找处理算法。

我们把网格 中的每个单元格当作一个节点。并且规定能够从任何一个节点移动到与其相邻的任意节点。这个 特殊网格还包含一些阻挡潜在路径的墙壁（下图中的灰色部分）。

对人类来说，通过查看图形便可以轻易找出最佳路径。但对于计算机而言，这并不明显。计算机必须确认是否存在通往目的地的路径，并竭尽所能地尝试所有可能的路径来从其中选出最优路径。但是，这需要耗费大量的时间，尤其是在地图非常大的时候。从任何给定的节点来看，最多有8个用于下一步的选项，因为存在8个相邻的单元格。对于8个中的每一个选项，下一步又有另外8个选项。如果我们展开所有节点，搜索量将暴增至即使最快的计算机也无法处理的程度。

从初始节点开始，我们需要确定8个相邻节点 中，哪个是最有希望的候选节点。对于每个候选节点，我们考虑两件事情。首先，我们计算从开 始节点到候选节点的成本。然后，我们估计从候选节点前往目的地的成本。计算前往候选节点的 成本很容易，因为它已经与我们相邻。计算从候选节点到目的地的成本很困难。但是，通过简单计算从候选节点到目的地的距离，我们通常可以做出很好的估计。

我们用变量g和h表示每个成本。g值为开始节点前往候选节点的成本，h值为候选节点前往目的地的估计成本或启发式成本。根据我们的具体情况，我们可以自定义成本估算方式。例如，交通堵塞会增加前往目的地的成本，所以交通繁忙的路径具有更高的成本。对于每个候选节点，我们通过添加g值和h值来计算总和，即f值。最佳候选节点是f值最小的节点。每当我们抵达新节点时，我们通过重复此过程来选择下一个候选节点，而且总是选择我们尚未访问过且具有最小f值的节点。这就是A* 算法，它建立了一条稳定前往目的地的路径。

A*在现实中的应用

假设我们到达了一个交叉路口，我们可以沿着公路直走、左转或右转。首先，我们将把这张地图 转换为具有三个候选节点（left,straight,right）的图形。接下来，我们将对选项进行评估。在 实践中，拐过交叉路口很费劲，所以我们为left节点分配了更高的g值（g值表示从起始点到候选 节点的成本）。在查看公路选项之后，我们意识到必须走很长的路，才能离开公路并返回我们的目标 ，所以我们为straight 选 项 分配了更高的h值（h值表从候选节点到目的地的估计成本）。

最后，我们通过将g值和h值相加来计算每个节点的f值。我们看到最低f值实际对应右边的候选节点。所以这是我们接下来要前往的节点。

地图级轨迹生成

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

轨迹生成的目标是生成一系列路径点所定义的轨迹。我们为每个路径点分配了一个时间戳和速度。由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每个路径点都有时间戳。我们可以将时间戳与预测模块的输出相结合，以确保我们计划通过时，轨迹上的每个路径点均未被占用。这些 时 间 戳 和 空 间 上 的 两 个 维 度 （ 2D position ） 共 同 创 建 了 一 个 三 维 轨 迹 （ 3D Trajector y）。我们还为每个路径点指定了一个速度，用于确保车辆按时到达每个路径点。

现实世界中的规划面临多种约束。首先轨迹应能免于碰撞，这意味着必须没有障碍物。其次，要让乘客感到舒适，所以路径点之间的过渡以及速度的任何变化都必须平滑。再者，路径点对车辆应实际可行，例如高速行驶的汽车不能立即做180度转弯。我们不能构建包含这种不可行机动的轨迹。最后，轨迹应合法。我们需要了解每个路径点的交通法律，并确保轨迹遵守这些法律法规。

在道路的任何两点，可能会有多个不会发生碰撞、行驶舒适、可行且合法的轨迹。我们如何选择最佳轨迹呢？答案是使用“成本函数”。成本函数为每个轨迹分配了一个“成本”，我们选择成本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如：偏离道路中心，有可能产生碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。

轨迹成本将所有这些缺陷聚合为单个数字，这使我们能对不同的轨迹按数字大小进行排名。车辆甚至可能在不同的环境中使用不同的成本函数。例如，高速路的成本函数可能与停车场的不同。

Frenet 坐标系

我们通常使用笛卡尔坐标系描述物体的位置，但笛卡尔坐标系对车辆来说并不是最佳选择。即使给出了车辆位置(x,y)，如果我们不知道道路在哪，就很难知道车辆行驶了多远也很难知道车辆是否偏离了道路中心。

笛卡尔坐标系的替代解决方案为Frenet坐标系。Frenet坐标系描述了汽车相对于道路的位置。在Frenet框架中，s代表沿道路的距离，也被称为纵坐标。d表示与纵向线的位移，也被称为横坐标。在道路的每个点上，横轴和纵轴都是垂直的。纵坐标表示道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

速度- 路径解耦规划

路径-速度解耦规划将轨迹规划分为两步：路径规划、速度规划。首先在路径规划步骤中生成候选曲线，这是车辆可行驶的路径。使用成本函数对每条路径进行评估，该函数包含平滑度、安全性、与车道中心的偏离以及开发者想要考虑的其他任何因素。然后按成本对路径进行排名并选择成本最低的路径。

下一步是确定沿这条路线行进的速度。我们可能希望改变在该路径上的速度，所以真正需要选择的是与路径点相关的一系列速度，而不是单个速度。我们将该序列称作“速度曲线”。我们可以使用优化功能为路径选择受到各种限制的良好速度曲线。通过将路径和速度曲线相结合可构建车辆行驶轨迹。

路径生成与选择

为了在路径-速度解耦规划中生成候选路径，首先将路段分割成单元格。然后对这些单元格中的点进行随机采样。通过从每个单元格中取一个点并将点连接，我们创建了候选路径。通过重复此过程可以构建多个候选路径。使用成本函数对这些路径进行评估并选择成本最低的路径，成本函数可能考虑以下因素：与车道中心的偏离、与障碍物的距离、速度和曲率的变化、对车辆的压力、或希望列入的任何其他因素。

ST图

选择路径后的下⼀步是选择与该路径关联的速度曲线，⼀个被称为“ST 图”的⼯具可以帮助设计和选择 速度曲线。在ST图中，“s”表⽰⻋辆的纵向位移、“t”表⽰时间。ST 图上的曲线是对⻋辆运动的描述，因 为它说明了⻋辆在不同时间的位置。由于速度是位置变化的速率，所以可以通过查看曲线的斜率从 ST图上推断速度。斜坡越陡则表⽰在更短的时间段内有更⼤的移动，对应更快的速度。

速度规划

为构建最佳速度曲线需要将 ST 图离散为多个单元格。单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建并维持曲线的近似度。在 ST 图中可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。例如，假设预测模块预测车辆将在 t0到 t1 的时间段内驶入的车道。由于该车将在此期间占据位置 s0 到 s1，因此在 ST 图上绘制了一个矩形，它将在时间段 t0 到 t1 期间阻挡位置 s0 到 s1。为避免碰撞，速度曲线不得与此矩形相交。既然有了一张各种单元格被阻挡的 ST 图，便可以使用优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案。这些限制可能包括：法律限制，如速度限制；距离限制，如与障碍物的距离；汽车的物理限制，如加速度限制。

优化

路径-速度解耦规划在很大程度上取决于离散化。路径选择涉及将道路划分为单元格，速度曲线构建涉及将 ST 图划分为单元格。尽管离散化使这些问题更容易解决，但该解决方案生成的轨迹并不平滑。

为 了 将 离 散 解 决 方 案 转 换 为 平 滑 轨 迹 ， 可 使 用 “ 二 次 规 划 ” 技 术 （ Quadratic Programming)。二次规划将平滑的非线性曲线与这些分段式线性段拟合。尽管二次规划背后 的数学运算很复杂，但对于我们的目的而言，细节并不是必需的。我们只需简单使用几种不同的 优化包中的一种，包括一种由 Apollo 推出的运行方案来生成平滑的轨迹，一旦路径和速度曲线就绪，便可以用其构建三维轨迹。

路径-速度规划的轨迹生成

假设我们正在路上行驶，感知系统观察到一辆缓慢行驶的车辆 离我们越来越近。首先，在这辆车的周围生成多条候选路线，使用成本函数对这些候选路径进行评 估并选择成本最低的路径。然后使用 ST 图来进行速度规划，根据其他车辆随时间变化的位置阻挡 了 ST 图的部分区域。优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数。 我们可以使用二次规划让路径和速度曲线变平滑。最后，将路径和速度曲线合并构建轨迹。这里的 轨迹在速度较快时为红色，在速度较慢时为蓝色。我们使用该轨迹来安全地绕开其他车辆并继续我们的旅程。

Lattice规划

通过使用 Frenet 坐标可以将环境投射到纵轴和横轴上，目标是生成三维轨迹：纵向维度、横向维度、时间维度。

可以将三维问题分解成两个单独的二维问题，这是通过分离轨迹的纵向和横向分量来解决的。其中一个二维轨迹是具有时间戳的纵向轨迹称之为 ST 轨迹，另一个二维轨迹是相对于纵向轨迹的横向偏移称之为 SL 轨迹。

Lattice 规划具有两个步骤即先分别建立 ST 和 SL 轨迹，然后将它们合并为生成纵向和横向二 维轨迹。先将初始车辆状态投射到 ST 坐标系和 SL 坐标系中，通过对预选模式中的多个候选最 终状态进行采样。来选择最终车辆状态。对于每个候选最终状态构建了一组轨迹将车辆从其初始状态转换为最终状态，使用成本函数对这些轨迹进行评估并选择成本最低的轨迹。

ST轨迹的终止状态

根据情况可以将状态分成 3 组：巡航 、跟随、停止。巡航意味着车辆将在完成规划步骤后定速行驶，实际上在对图上的点进行采样，在图中横轴代表时间，纵轴代表速度。对于该图上的点，这意味着汽车将进入巡航状态，在时间 t 以 s 点的速度巡航，对于这种模式，所有最终状态的加速度均为零。

下⼀个要考虑的模式为跟随⻋辆，在这种情况下要对位置和时间状态进⾏采样，并尝试在时间t出现在某辆⻋后⾯，在跟随⻋辆时，需要与前⽅的⻋保持安全距离，这时速度和加速度将取决于要跟随的⻋辆，这意味着在这种模式下，速度和加速度都会进⾏修正。

最后⼀种模式是停⽌，对于这种模式只需对汽⻋何时何地停⽌进⾏抽样，这⾥速度和加速度会被修正为0。

SL轨迹的终止状态

根据这样⼀个假设来进⾏SL 规划，即⽆论⻋辆进⼊怎样的终⽌状态，⻋辆都应该稳定地与⻋道中⼼线对⻬。这意味着只需要在⼀个⼩区域内，对横向终⽌位置进⾏采样。具体来说采样的是道路上相邻⻋道中线周围的位置。为了确保稳定性，汽⻋驶向的终⽌状态应该与⻋道中⼼⼀致。当⽤横向位置与纵向位置作图时 ，想要的候选轨迹应该以⻋辆与⻋道对⻬并直线⾏驶⽽结束。为了达到这种终⽌状态，⻋的朝向和位置的⼀阶和⼆阶导数都应该为零。这意味着⻋辆既不是横向移动的，那是⼀阶导数；也不是横向加速，那是⼆阶导数。这意味着⻋辆正沿着⻋道直⾏。

Lattice规划的轨迹生成

⼀旦同时拥有了 ST 和 SL 轨迹，就需要将它们重新转换为笛卡尔坐标系。然后可以将它们相结合构建由⼆维路径点和⼀维时间戳组成的三维轨迹。ST 轨迹是随时间变化的纵向位移，SL 轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标 S，所以可以通过将其 S 值进⾏匹配来合并轨迹。