Apollo自动驾驶入门课（6）——规划

规划需要结合高精度地图、定位、预测的知识，从路线导航和轨迹生成两方面构建车辆轨迹。

1.规划

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

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

2. A*搜索算法

将地图转为图

Apollo也使用搜索算法来查找路线。在进行智能搜索算法以前，我们需要将地图数据重新格式化为“图”的数据结构。

图由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。对一个节点移动到另一个节点所需的成本进行建模，这些成本又称为“代价”。

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

A*算法

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

3.轨迹生成

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

3D轨迹

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

评估一条轨迹

现实世界中的规划面临多种约束：

• 首先，轨迹应能免于碰撞，这意味着必须没有障碍物。
• 其次，要让乘客感到舒适，所以路径点之间的过渡以及速度的任何变化都必须平滑。
• 再者，路径点对车辆应实际可行，不能构建包含不可行机动的轨迹。
• 最后，轨迹应合法。这就需要了解每个路径点的交通法律，并确保轨迹遵守这些法律法规。

在道路的任何两点，可能会有多个不会发生碰撞、行驶舒适、可行且合法的轨迹，使用“成本函数”可以选择最佳轨迹。

成本函数为每个轨迹分配了一个“成本”，我们选择成本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如：偏离道路中心，有可能产生碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。

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

4. Frenet坐标

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

5. 路径-速度解耦规划

路径-速度解耦规划

路径-速度解耦规划将轨迹规划分为两步：路径规划、速度规划。

• 路径规划生成候选曲线，是车辆可行驶的路径。使用成本函数对每条路径进行评估，该函数包含平滑度、安全性、与车道中心的偏离以及开发者想要考虑的其他任何因素。然后按成本对路径进行排名并选择成本最低的路径。
• 速度规划，需要选择的是与路径点相关的一系列速度，而不是单个速度，将该序列称作“速度曲线”。使用优化功能为路径选择受到各种限制的良好速度曲线。
• 通过将路径和速度曲线相结合可构建车辆行驶轨迹。

路径生成与选择

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

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 图的部分区域。优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数。使用二次规划让路径和速度曲线变平滑。最后，将路径和速度曲线合并构建轨迹。这里的轨迹在速度较快时为红色，在速度较慢时为蓝色。使用该轨迹来安全地绕开其他车辆并继续前进。

6.Lattice规划

Lattice规划

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

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

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

ST轨迹的终止状态

将状态分成 3 组：巡航 、跟随、停止：

• 巡航Cruising 意味着车辆将在完成规划步骤后定速行驶，实际上在对图上的点进行采样，在图中横轴代表时间，纵轴代表速度。对于该图上的点，这意味着汽车将进入巡航状态，在时间 t 以 s 点的速度巡航，对于这种模式，所有最终状态的加速度均为零。
• 跟随Following 在这种情况下要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，在跟随车辆时，需要与前方的车保持安全距离，这时速度和加速度将取决于要跟随的车辆，这意味着在这种模式下，速度和加速度都会进行修正。
• 停止Stopping 只需对汽车何时何地停止进行抽样，这里速度和加速度会被修正为 0 。

SL轨迹的终止状态

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

Lattice规划的轨迹生成

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