百度无人驾驶入门课程，记录六

——————————————————

百度无人驾驶入门课程，记录六

无人驾驶工程师，一起塑造无人驾驶未来，从这里开始！

课程记录

第六课：规划

了解 Apollo应用于无人驾驶车路径规划的几种不同方式。

1、规划简介

在规划中，通过结合高精度地图定位和预测来构建车辆轨迹。规划的第一步是路线导航，侧重于如何从地图上的 A 前往 B，在进行路线规划时将地图数据作为输入并输出可行驶路径。

轨迹规划的目标是生成免碰撞和舒适的可执行轨迹，该轨迹由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。

2、介绍规划

将在两种不同的背景下讨论 “规划”，一个是你要去的地方 也就是一个象征性的城市，另一个是运动轨迹水平，用来决定在执行特定操作时汽车应该采取哪种转向。这些轨迹不只是时间曲线也不只是空间曲线，它们也是时间曲线 因为你可能会选择速度非常快或非常慢，在规划中必须考虑你可以有多快才能确保一个可行的计划。

3 、路由

路线规划的目标是找到从地图上的 A 前往 B 的最佳路径，路线规划使用了三个输入：第一个输入为地图，Apollo提供的地图数据包括公路网和实时交通信息，第二个输入为当前在地图上的位置，第三个输入为目的地，目的地通常取决于车辆中的乘客。有了这三个输入路线规划模块就为寻找前往目的地的路径做好了准备。

4、世界地图

当试图在地图上找到从 A 到 B 的路线时，通常会沿着道路追踪路径以查看是否存在通往目的地的任何路径，称为搜索。在 Apollo 开始搜索之前将地图数据重新格式化为“图形”的数据结构，该图形由“节点”和“边缘”组成，节点代表路段，边缘代表这些路段之间的连接，例如在交叉路口，汽车可从节点 1 移动到节点 2。

也可以对从一个节点移动到另一个节点所需的成本进行建模，例如在现实生活中拐过一个交叉路口比直行更费劲所以从节点 1 到节点 4 的成本高于从节点 1 到节点 3 的成本。

将地图转换为图形的好处在于在计算机科学领域中，人们已发现许多用于在图形中查找路径的快速算法，一旦我们在图形中找到了一条好路径就可以轻松地将图形中的路径重新转换为地图上的路径。

5、网络世界

网格世界 1

A*是经典的路径查找处理算法，A*如何通过网格工作，将该网格中的每个单元格当做一个节点，能够从任何一个节点移动到其任意相邻节点,这个特殊网格包含一些阻挡潜在路径的墙壁。

网格世界2

对人类来说通过查看图形便可以轻易找出最佳路径，但对于计算机而言，这并不明显、计算机必须检查是否存在通往目的地的任何路径，计算机可能竭尽所能地尝试所有可能的路径以找到最短的路径，但是这耗费大量时间，尤其是在地图非常大的时候。

从任何给定的节点来看，最多有8个用于下一步的选项，因为存在 8个相邻的单元格，对于这8个选项中的每一个，在下一步有另外8个选项。

迈出最好的一步

从初始节点开始，需要确定8个相邻节点中，哪个是最有希望的候选节点。对于每个候选节点，考虑两件事情，首先计算从开始节点到候选节点的成本，然后估计从候选节点前往目的地的成本

计算前往候选节点的成本很容易，因为它已经与我们相邻，计算从候选节点到目的地的成本很困难，但是通过简单计算，从候选节点到目的地的距离，通常可以做出很好的估计。使用变量 g 和 h 来表示每个成本，g 值为从开始节点前往候选节点的成本，h 值为从候选节点前往目的地的估计成本或启发式成本。

根据具体情况，可以自定义成本估算方式例如交通堵塞会增加前往目的地的成本，所以交通繁忙的路径具有更高的成本。对于每个候选节点，通过添加 g 值和 h 值来计算总和即 f 值，最佳候选节点是 f 值最小的节点。

每当抵达新节点时，通过重复此过程来选择下一个候选节点，而且总是选择尚未访问过且具有最小f值的节点，这就是 A * 算法，建立了一条稳定前往目的地的路径。

6、A*

现在考虑一下真实世界中的地图，假设到达了一个交叉路口，可以沿着公路直走、右转、左转。首先将把这张地图转换为具有三个候选节点的图形，接下来将对选项进行评估，在实践中拐过交叉路口很费劲，所以为这个节点分配了更高的 g 值，g值是从起始点到候选节点的成本。在查看公路选项之后，意识到必须走很长的路才能离开公路并返回我们的目标，所以为该选项分配了更高的 h 值，h 值是从候选节点到目的地的估计成本。接下来 通过将 g 值和 h 值相加来计算每个节点的 f 值，最低 f 值是实际对应右边的候选节点。

7、从路由到轨迹

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

8、3D轨迹

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

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

9、评估一条轨迹

约束

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

损失函数

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

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

10、坐标

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

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

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

11、路径-速度解耦规划

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

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

12、路径生成与选择

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

13、ST图

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

14、速度规划

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

15、优化

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

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

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

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

17、Lattice规划

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

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

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

18、ST轨迹的终止状态

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

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

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

19、SL轨迹的终止状态

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

20、Polynomial

21、Lattice规划的轨迹生成

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

22、小结

探讨无人驾驶车如何规划行驶的轨迹，车辆基于高精度地图、感知、预测模块的数据来进行这一规划。首先介绍如何使用 A * 算法规划前往目的地的地图级路线，然后专注于构建逐步引导前往目的地的免碰撞轨迹。学习如何构建这些轨迹，借助路径—速度解耦规划或Lattice 规划，通过比较这两种方法，会有更为直观的认知，更好地理解规划以及如何使用成本函数选择轨迹。