Apollo无人驾驶课程笔记 第六课-规划

1. 规划简介

在规划中，我们通过结合高精地图、定位和预测来构建车辆轨迹。

规划的第一步是路线导航，侧重于如何从地图上的A点前往B点。在进行路径规划时，将地图数据作为输入，并输出可行使路径。轨迹规划的目标是生成免碰撞和舒适的可执行轨迹，该轨迹由一系列点定义，每一个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。

2. Sebastian 介绍规划

略

3. 路由

路线规划的目标是从地图上的A点前往B点，路线规划有三个输入：地图、起当前在地图上的位置、目的地。

4. 世界到图

在进行路径搜索前，需要将地图转换为“图"这一数据结构。路线规划所用的搜索算法实际上绝大部分都是基于图的搜索算法。我们将地图转换为图，并且引入”开销"(或者说“成本”)的概念，

图论（Graph theory），是组合数学的一个分支，和其他数学分支，如群论、矩阵论、拓扑学有着密切关系。图是图论的主要研究对象。图是由若干给定的顶点及连接两顶点的边所构成的图形，这种图形通常用来描述某些事物之间的某种特定关系。顶点用于代表事物，连接两顶点的边则用于表示两个事物间具有这种关系。

5. 网格世界_a &6. 网格世界_b& 7. 网格世界_c

A*搜索算法（A ^* search algorithm）是一种经典的路径搜索算法，是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。

该算法综合了最良优先搜索（英语：Best-first search）和Dijkstra算法的优点：在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径（基于评估函数）。

在此算法中，如果以g(n)表示从起点到任意顶点n的实际距离，h(n)表示任意顶点n到目标顶点的估算距离（根据所采用的评估函数的不同而变化），那么A*算法的估算函数为：

${\displaystyle f(n)=g(n)+h(n)}$

这个公式遵循以下特性：

• 如果g(n)为0，即只计算任意顶点n到目标的评估函数h(n)，而不计算起点到顶点n的距离，则算法转化为使用贪心策略的最良优先搜索（英语：Best-first
  search），速度最快，但可能得不出最优解；
• 如果h(n)不大于顶点n到目标顶点的实际距离，则一定可以求出最优解，而且h(n)越小，需要计算的节点越多，算法效率越低，常见的评估函数有——欧几里得距离、曼哈顿距离、切比雪夫距离；
• 如果h(n)为0，即只需求出起点到任意顶点n的最短路径g(n)，而不计算任何评估函数h(n)，则转化为单源最短路径问题，即Dijkstra算法，此时需要计算最多的顶点；

有关A*算法，可以参考很多博客，我准备有时间也写一个（flag

8. A*

现在我们来考虑现实中的情况，假设现在到了一个交叉路口，我们可以直走、左转和右转。首先，我们把地图转换为具有三个候选节点的图形。

接下来，我们将对选项进行评估，在实际生活中，拐过交叉口很费劲，所以我们为这个节点分配了更高的g值，g值是从起始点到候选节点的成本。

在查看公路选项之后，我们意识到必须走很长的路才能离开公路，并返回我们的目标。所以我们为该选项分配了更高的h值，h值是从候选节点到目的地的估计成本。

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

9. 从路由到轨迹

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

10. 3D 轨迹

轨迹生成的目标是生成由一系列路径点所定义的轨迹，为每个路径点分配了一个时间戳和速度。让一条曲线与这些路径点拟合生成轨迹的几何表征，由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每个路点都有时间戳。

可以将时间戳与预测模块的输出相结合以确保在无人车计划通过时，轨迹上的每个路径点均未被占用。这些时间戳创建了一个三维轨迹，每个路径点由空间中的两个维度以及时间上的第三个维度来定义。还为每个路径点指定了一个速度，速度用于确保车辆按时到达每个路径点。

11. 评估一条轨迹_a

首先，轨迹应能免于碰撞：

其次，需要让乘客感到舒适，所以轨迹应该平滑：

第三，轨迹必须可行，例如我们无法让一辆高速行驶的汽车立即180°大转弯：

最后，轨迹必须合法，我们需要了解每一个路径点的交通法律，并确保轨迹遵守这些法律法规。

12. 评估一条轨迹_b

在评估轨迹时，可能会有多个不会碰撞、舒适、可行且合法的轨迹，此时我们使用“成本函数”来选择最佳轨迹：成本函数为每个轨迹分配了一个“成本”，选择成本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如偏离道路中心、有可能会碰撞、超出速度限制、轨迹的曲率和加速度让乘客感到不舒服均会使成本增加。轨迹成本将所有这些缺陷聚合为单个数字，能对不同的轨迹进行排名。

车辆甚至可能在不同的环境中使用不同的成本函数，例如高速公路的成本函数可能与停车场的成本函数不同。

13. Frenet 坐标

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

笛卡尔坐标系的替代解决方案为 Frenet 坐标系，Frenet 坐标系描述了汽车相对于道路的位置，在 Frenet 框架中，s 代表沿道路的距离称为纵坐标，d表示与纵向线的位移称为横坐标。

在道路的每个点上，横轴和纵轴都是垂直的，纵坐标表示在道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

14. 路径 - 速度解耦规划

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

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

15. 路径生成与选择

为了在路径-速度解耦规划中生成候选路径，

• 首先将路段分割成单元格，
• 然后对这些单元格中的点进行随机采样

通过从每个单元格中取一个点并将点连接，创建了候选路径，通过重复此过程可以构建多个候选路径。
使用成本函数对这些路径进行评估并选择成本最低的路径，成本函数可能考虑以下因素：与车道中心的偏离、与障碍物的距离、速度和曲率的变化、对车辆的压力、希望列入的任何其他因素。

16. ST 图

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

17. 速度规划

为构建最佳速度曲线需要将 $ST$ 图离散为多个单元格，单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建并维持曲线的近似度。在 $ST$图中可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形，例如假设预测模块预测车辆，将在 $t0$ 到 $t1$ 的时间段内驶入的车道。由于该车将在此期间占据位置 $s0$ 到 $s1$，因此在 $ST$ 图上绘制了一个矩形，将在时间段 $t0$ 到 $t1$ 期间阻挡位置 $s0$ 到 $s1$。为避免碰撞，速度曲线不得与此矩形相交.

既然有了一张各种单元格被阻挡的 $ST$ 图，便可以使用优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索，受到各种限制的低成本解决方案，可能包括：法律限制如速度限制、距离限制如与障碍物的距离、汽车的物理限制如加速度限制。

18. 优化

路径-速度解耦规划在很大程度上取决于离散化，路径选择涉及将道路划分为单元格.速度曲线构建涉及将 ST 图划分为单元格。

尽管离散化使这些问题更容易解决，但该解决方案生成的轨迹并不平滑。

为了将离散解决方案转换为平滑轨迹，可使用“二次规划”技术。二次规划将平滑的非线性曲线与这些分段式线性段拟合，包括一种由 Apollo 推出的运行方案来生成平滑的轨迹，一旦路径和速度曲线就绪，便可以用其构建三维轨迹。

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

端到端路径-速度解耦规划，假设正在路上行驶，感知系统观察到一辆缓慢行驶的车辆离越来越近。首先在这辆车的周围生成多条候选路线：

接着使用成本函数对这些候选路径进行评估并选择成本最低的路径：

然后使用 ST 图来进行速度规划，根据其他车辆随时间变化的位置阻挡了 ST 图的部分区域。

优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数：

随后使用二次规划让路径和速度曲线变平滑：

最后将路径和速度曲线合并构建轨迹，这里的轨迹在速度较快时为红色，在速度较慢时为蓝色，使用该轨迹来安全地绕开其他车辆并继续我们的旅程：

20. Lattice 规划

我们在这里采用一种称为 Lattice 规划的轨迹生成方法，通过使用 Frenet 坐标可以将环境投射到纵轴和横轴上，目标是生成三维轨迹 - 纵向维度、横向维度、时间维度。

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

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

21. ST 轨迹的终止状态

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

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

最后一种模式是停止，对于这种模式只需对汽车何时何地停止进行抽样，这里速度和加速度会被修正为 0 。

22. SL 轨迹的终止状态

根据这样一个假设来进行SL 规划，即无论车辆进入怎样的终止状态，车辆都应该稳定地与车道中心线对齐。这意味着只需要在一个小区域内，对横向终止位置进行采样。具体来说采样的是道路上相邻车道中心线周围的位置，为了确保稳定性。汽车驶向的终止状态应该与车道中心一致，当用横向位置与纵向位置作图时 ，想要的候选轨迹应该，以车辆与车道对齐并直线行驶而结束。

为了达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为零。这意味着车辆既不是横向移动的(那是一阶导数)，也不是横向加速(那是二阶导数)，这意味着车辆正沿着车道直行。

23. Lattice 规划的轨迹生成

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

24. 项目示例: 路径规划

25. 课程综述

本课主要学习了无人驾驶车如何规划行驶的轨迹，车辆基于高精度地图、感知、预测模块的数据来进行这一规划。

• 首先介绍如何使用 A * 算法规划前往目的地的地图级路线，
• 然后专注于构建逐步引导前往目的地的免碰撞轨迹。
• 学习如何构建这些轨迹，借助路径—速度解耦规划或Lattice规划，通过比较这两种方法，会有更为直观的认知，更好地理解规划以及如何使用成本函数选择轨迹。