Apollo入门课程[6]——规划

规划简介

在规划中，我们通过结合HD Map，定位和预测来构建车辆轨迹，规划的第一步是路线导航，侧重于如何从地图上的A前往B，在进行路线规划时，将地图数据作为输入并输出可行驶路径。在Apollo中通过路径规划模块处理该任务。
轨迹规划的目标是生成免碰撞和舒适的可执行轨迹，该轨迹由一系列点定义，每个点都有一个关联速度，和一个指示何时应抵达那个点的时间戳。

路由

路线规划的目标是找到从地图上的A前往B的最佳路径，路线规划使用了三个输入：

• 第一个输入为地图，Apollo提供的地图数据包括公路网和实时交通信息。
• 第二个输入为我们当前在地图上的位置。
• 第三个输入为我们的目的地

有了这三个输入，路线规划模块就为寻找前往目的地的路径做好了准备

世界到图

当人们试图在地图上找到从A到B的路线时，通常会沿着道路追踪路径以查看是否存在通往目的地的任何路径，这被称为搜索。
在Apollo开始搜索前，它将地图数据重新格式化为“图形”的数据结构，该图形由“节点”和“边缘”组成，节点代表路段，边缘代表这些路段之间的连接。
按照节点移动到另一个节点的成本建模：

地图转换为图形的好处：

• 在图形中查找路径的快速算法已经有很多了
• 在图形中找到最佳路径可以i轻松转换成地图上的路径

网格世界

A是经典路径查找处理算法（详细看机器学习PPT）*
从初始节点，在8个相邻节点中确认最有希望的候选节点，每个候选节点需要考虑两件事：

• 计算从开始节点到候选节点的成本
• 估计从候选节点到目的地的成本

从路由到轨迹

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

轨迹生成的目标是生成由一系列路径点所定义的轨迹，我们为每个路径点分配了一个时间戳和速度，我们让一条曲线与这些路径点拟合，生成轨迹的几何表征，由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每个路点都有时间戳，我们可以将时间戳与预测模块的输出相结合，以确保在我们计划通过时，轨迹上的每个路径点均未被占用。
这些时间戳创建了一个三维轨迹，每个路径点由空间中的两个维度，以及时间上的第三个维度来定义，我们还为每个路径点指定了一个速度，速度用于确保车辆按时到达每个路径点。

评估一条轨迹

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

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

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

• 成本函数为每个轨迹分配了一个“成本”，选择“成本”最低的轨迹
• 轨迹“成本”由各种犯规处罚组成，如偏离道路中心、可能会碰撞、超出速度限制、让人不适等等都会造成“成本”增加。
  
• 可能在不同的环境使用不同的成本函数

Frenet坐标

Frenet坐标描述了汽车相对于道路的位置

• s代表沿道路的距离（纵坐标），d表示与纵向线的位移（横坐标），横轴纵轴永远是垂直的。
  
• 纵坐标表示在道路中的行驶距离，横坐标表示汽车偏离中心线的距离
  

路径-速度解耦规则&路径生成选择

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

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

为了在路径-速度解耦规划中生成候选路径，我们首先将路段分割成单元格，然后对这些单元格中的点进行随机采样，通过从每个单元格中取一个点并将点连接，创建了候选路径。

通过重复此过程我们可以构建多个候选路径，我们使用成本函数对这些路径进行评估，并选择成本最低的路径。

成本函数可能考虑的因素
与车道中心的偏离
与障碍物的距离
速度和曲率的变化
对车辆的压力
其他因素

ST图

选择路径后的下一步，是选择与该路径关联的速度曲线。
“ST图”*（一种工具）*可以帮助我们设计和选择速度曲线。
在ST图中：

• “s”表示车辆的纵向位移“t”表示时间。
• ST图上的曲线是对车辆运动的描述，因为它说明了车辆在不同时间的位置。
• 由于速度是位置变化的速率，所以我们可以通过查看曲线的斜率从ST图上推断速度，斜坡越陡则表示，在更短的时间段内有更大的移动对应更快的速度

速度规划

为构建最佳速度曲线，需要将 ST图离散为多个单元格，单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建，并维持曲线的近似度。
在ST图中，可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。
eg.
预测白车在t0-ti时间段内行驶入车道，该车将在此期间占据位置s0-s1，速度曲线不能与该矩阵相交。

使用优化引擎选择最佳的速度曲线，优化算法通过复杂的数学运算来搜索，受到各种限制的低成本解决方案。
优化：
路径-速度解耦规划在很大程度上取决于离散化，路径选择涉及将道路划分为单元格，速度曲线构建涉及将ST图划分为单元格。离散化可以使问题容易解决，但该解决方案生成的轨迹并不平滑，为此可以使用二次规划方法，将平滑的非线性曲线与这些分段式线性段拟合。

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

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

Lattice规划

Lattice规划的轨迹生成方法：

• 通过使用Frenet坐标可以将环境投射到纵轴和横轴上，我们的目标是生成三维轨迹-纵向维度，横向维度和时间维度。
• 可以将三维问题分解成，两个单独的二维问题，通过分离轨迹的纵向和横向分量来解决。
  • 其中一个二维轨迹是具有时间戳的纵向轨迹，我们称之为ST轨迹。
  • 另一个二维轨迹是，相对于纵向轨迹的横向偏移，我们称之为SL轨迹。

Lattice规划具有两个步骤：

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

ST的终止状态

根据情况，将状态分为3组：

• 巡航：车辆将在完成规划步骤后定速行驶。匀速行驶
  • 对图上的点进行采样，横轴time，纵轴v。
  • 对于图上的点，意味着汽车将进入巡航状态，在时间t以s点的速度巡航
  • 该模式所有最终状态的加速度a均为0

• 跟随：
  • 对位置和时间状态进行采样，并尝试在时间t出现在某辆车的后面。
  • 跟随车辆时，需要与前方车保持安全距离，这是v和a取决于要跟随的车辆
  • 该模式v和a都会进行修正

• 停止：
  • 只需对汽车何时何地停止进行抽样
  • v和a会被修正为0

SL轨迹的终止状态

假设无论车辆怎么进入终止状态，它都应该稳定地与车道中心线对齐。为了实现这样的状态，车的朝向和位置的一阶*（说明不是横向移动）和二阶导数（说明不是横向加速）*都为0。这意味着车正沿着车道直行。

Lattice规划的轨迹生成

需要将ST和SL轨迹转换成笛卡尔坐标系，然后将其相结合构建由二维路径点和一维时间戳组成的三维轨迹。
通过将ST、SL的S值进行匹配来合并轨迹。