Apollo课程学习小记录5——规划
学习前言
今天的学习内容是规划。
什么是规划
一、地图路线规划——高等级
路线规划的目标是找到从地图上的A前往B的最佳路径，路线规划使用了三个输入：地图、当前在地图上的位置、目的地。
在Apollo开始使用搜索算法查找路线之前，先将地图数据重新格式化为“图形”的数据结构，该图形由 “节点”（路段）和“边缘”（路段之间的连接） 组成。因为在计算机科学领域，人们已发现许多用于在图形中查找路径的快速算法，可以轻松地将图形上的路径重新转换为地图上的路径。
二、轨迹生成——低等级
处理不属于地图上的物体（如其他车辆、行人和自行车等），这些场景需要更低等级、更高精度的规划。轨迹规划的目标是生成免碰撞和舒适的可执行轨迹，该轨迹由一系列点定义。我们为每个路径点分配了一个时间戳和速度。
- 时间戳。将时间戳与预测模块的输出相结合，以确保我们计划通过时，轨迹上的每个路径点均未被占用。这些时间戳创建了一个3D轨迹。
- 速度。确保车辆按时到达每个路径点。
A*算法——路线规划
如下如所示，我们将网格中的每个单元格视为一个节点，网格中也包含一些阻挡潜在路径的墙壁（灰色部分）。从初始节点开始，我们需要确定8个相邻节点中最有希望的候选节点。根据具体情况可自定义成本估算方式。
- g值：从开始节点前往候选节点的成本。
- h值：从候选节点前往目的地的估计成本（启发式成本）。
- f 值：g值和h值的计算和，最佳候选节点是f 值最小的节点。
每当抵达新节点时，将重复此过程来选择下一个候选节点（尚未访问过且具有最小f值的节点）。
轨迹的评估
- 轨迹应能免于碰撞，必须无障碍物。
- 要让乘客感到舒适，路径点之间的过渡以及速度的任何变化都必须平滑。
- 路径点对车辆实际可行，不能构建包含不可行机动的轨迹。
- 轨迹应合法，我们需要了解每个路径点的交通规律，并确保轨迹遵守这些法律法规。
车辆在不同的环境中使用不同的成本函数选择最佳轨迹。
Frenet坐标
我们常用的笛卡尔坐标系对车辆来说并不是最佳选择，所以选用可描述汽车相对于道路的位置的Frenet坐标。
- 纵坐标s：代表沿道路的距离。
- 横坐标d：表示汽车偏离中心线的距离。
ST图
ST图上的曲线是对车辆运动的描述，说明了车辆在不同时间的位置。
- s：表示车辆的纵向位移。
- t：表示时间。
- 速度：曲线的斜率。
路径-速度解耦规划（轨迹生成）
- 路径规划。我们首先将路段分割成单元格，然后对这些单元格中的点进行随机采样，通过从每个单元格中取一个点并将点连接，通过重复此过程，我们就创建了多个候选路径。然后使用自定义的成本函数对每条路径进行评估，选择成本最低的路径。
- 速度规划。我们首先将ST图离散成多个单元格，单元格之间的速度有所变化，但每个单元格内速度保持不变。将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形（黄色部分），为避免碰撞，速度曲线不能与此矩形相交。接着，我们便可以使用优化引擎来搜索受到各种限制的低成本解决方案，选择最佳的速度曲线。
- 优化——二次规划。路径-速度解耦规划很大程度取决于离散化，尽管离散化使这些问题更容易解决，但生成的轨迹并不平滑，我们使用二次规划将平滑的非线性曲线与这些分段式线性段拟合，可将离散解决方案转换为平滑轨迹。最后我们将路径和速度曲线合并构建轨迹。
Lattice规划（轨迹生成）
通过使用Frenet坐标，我们可以将环境投射到纵轴和横轴上，将三维问题分解成两个单独的二维问题。
- ST轨迹：具有时间戳的纵向轨迹。
- SL轨迹：相对于纵向轨迹的横向偏移。
Lattice规划首先分别建立ST和SL轨迹，然后通过s值进行匹配来合并轨迹，构建由二维路径点和一维时间戳组成的三维轨迹，最后对预选模式中的多个候选最终状态进行采样，使用成本函数对这些轨迹进行评估，并选择成本最低的轨迹。
最终状态分类
一、ST图最终状态
- 巡航。车辆将在完成规划步骤后定速行驶，在时间 t 点以s点的速度巡航，最终状态的加速度为0。
- 跟随。在时间 t 时出现在某辆车后面，在跟随车辆时，需要与前方的车保持安全距离，速度和加速度将取决于我们要跟随的车辆。
- 停止。加速度和速度会被修正为0。
二、SL图最终状态
为了确保稳定性，汽车驶向的终止状态应该与车道中心一致，ST图轨迹应该车辆与车道线对齐并直线行驶而结束，如下图所示。为了达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为0，这意味着车辆既不是横向移动的，也不是横向加速的。
