Apollo路径规划学习

原文链接: Apollo自动驾驶入门课程第⑦讲 — 规划（上）.
原文链接: Apollo自动驾驶入门课程第⑦讲 — 规划（下）.

规划简介

路线规划

找到从地图上的A前往B的最佳路径

轨迹规划

找到避免碰撞和保持舒适度的可执行轨迹

将地图转为图形

在进行智能搜索算法以前，我们需要将地图数据重新格式化为“图形”的数据结构。
该图形由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。例如：在交叉路口，汽车可从节点1移动到节点2、节点3或节点4，反之亦然。
我们可以对一个节点移动到另一个节点所需的成本进行建模。例如在现实生活中，拐过一个交叉路口比直行更费劲，所以从节点1到节点4的成本高于从节点1到节点3的成本。

路径查找算法：A*

使用g和h的和表示总损失f即对于一点其损失为f(n) = g(n) + h(n),其中g(n)是从起始点到该点的确定性损失（exact cost），h(n)表示从该点到终点的启发式评估损失（heuristic estimated cost）。
根据我们的具体情况，我们可以自定义成本估算方式。例如，交通堵塞会增加前往目的地的成本，所以交通繁忙的路径具有更高的成本。对于每个候选节点，我们通过添加g值和h值来计算总和，即f值。最佳候选节点是f值最小的节点。每当我们抵达新节点时，我们通过重复此过程来选择下一个候选节点，而且总是选择我们尚未访问过且具有最小f值的节点。这就是A* 算法，它建立了一条稳定前往目的地的路径。

轨迹生成

高等级地图路线只是规划过程中的一部分，我们仍需要构建沿这条路线前进的低等级轨迹。这意味着要处理一些不属于地图的物体：如其他车辆、自行车或行人。例如，我们可能需要与试图在我们前面掉头的汽车互动，或者我们可能希望超过一辆在公路上行驶的慢车。这些场景需要更低级别、更高精确度的规划。我们将这一级别的规划称为轨迹生成。
轨迹生成的目标是一些列由路径点所定义的轨迹，在每一个轨迹点上添加时间戳和速度，通过时间戳与预测模块的输出相结合，可以保证轨迹上每一个点均未被占用，这些时间戳和空间上的两个维度(2D Position)共同构建了一个三维轨迹(3D Trajectory)，同时在时间戳上也添加了到达该点的速度。
在道路的任何两点，可能会有多个不会发生碰撞、行驶舒适、可行且合法的轨迹。我们如何选择最佳轨迹呢？答案是使用“成本函数”。成本函数为每个轨迹分配了一个“成本”，我们选择成本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如：偏离道路中心，有可能产生碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。

Frenet坐标系

使用Frenet坐标系，可以描述汽车相对于道路的位置，s表示沿道路的距离，即纵坐标，d或L表示与纵向线（一般是车道中心线）的位移，即横坐标

路径-速度解耦规划

路径规划

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

速度规划

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

路径生成与选择

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

ST图

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

速度规划

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

优化

路径-速度解耦规划在很大程度上取决于离散化。路径选择涉及将道路划分为单元格，速度曲线构建涉及将 ST 图划分为单元格。尽管离散化使这些问题更容易解决，但该解决方案生成的轨迹并不平滑。
为了将离散解决方案转换为平滑轨迹，可使用“二次规划”技术（Quadratic Programming)。二次规划将平滑的非线性曲线与这些分段式线性段拟合。

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

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

Lattice规划

Sample Candidate Trajectories

上面已经介绍了Frenet坐标系，为了生成一条轨迹，我们可以通过计算得到汽车在Frenet坐标系下的在零时刻的起始状态，即当前状态，之后便是在Frenet坐标系下采样一个在t1时刻的末状态，之后通过多项式拟合分别形成横向和纵向的多项式轨迹。

即拟合得到ST图和LS图，有了横向和纵向轨迹之后，进行二维合成，给定一个T*时刻，可以计算出在该时刻的纵向偏移量和横向偏移量，从而还原一个二维平面的轨迹点，通过给定T0~T1的采样点来得到一系列的二维平面点

采样横向轨迹

预先设计三个末状态的横向偏移量，-0.5，0，0.5以及四个到达这些横向偏移量的纵向位移，分别为10，20，30，40，80，遍历所有情况，通过多项式拟合即可得到一系列的横向轨迹
拟合多项式：
L(S₀) = L₀, L^/(S₀)=L^/₀, L^//(S₀)=L^//₀
L(S₁) = L₁, L^/(S₁)=0, L^//(S₁)=0

采样纵向轨迹

对于纵向轨迹需要考虑巡航、跟车或超车三种状态
根据速度限制v_u（S^/），分成6份v_t（0，1/6v_u ,…,v_u）,以及时间1,2,3,4,5,6,7,8来拟合s(t)
拟合多项式：
s(t₀) = s₀, s^/(t₀) = s^/₀, s^//(t₀) = s^//₀
s^/(t₁) = s^/₁, s^//(t₁) = 0

也可以根据终点的s₁，来拟合多项式，即停车状态：
s(t₀) = s₀, s^/(t₀) = s^/₀, s^//(t₀) = s^//₀
s(t₁) = s₁, s^/(t₁) = 0, s^//(t₁) = 0

由跟车/超车来拟合多项式
之前已经介绍了ST图，下图中，蓝色障碍车从车道右侧切入，从T_in时刻开始出现一块斜向上的阴影区域。这块阴影区域的高度就是蓝色障碍车的车身长，上边界表示车头，下边界表示车尾，斜率表示车速，障碍车从T_in时刻进入车道，然后在T_out时刻离开车道。那么这个场景对应的S-T图为：

在采样的时候，可以分别在ST图中障碍物对应的阴影区域的上方和下方分别采样，上方的末状态对应超车的，下方的末状态对应跟车,若有多个障碍物则对这些障碍物分别采样超车和跟车所对应的状态。

则对应的拟合多项式为：
s(t₀) = s₀, s^/(t₀) = s^/₀, s^//(t₀) = s^//₀
s(t₁) = s₁, s^/(t₁) = v_obstacle, s^//(t₁) = 0

将所有的轨迹组合起来，取其并集，得到总的纵向轨迹，再将纵向轨迹和横向轨迹两两配对二维合成，便完成轨迹的采样。

Assign Costs

在到达目的地、符合交规、避免碰撞、平稳舒适四点上，设计六个Cost，从cost越高越不满足要求，即以下6点：

1.目标任务完成损失（如果为停车指令，到达目标点的速度越小损失越小，反之越大）
2.横向偏移损失（保持车辆沿车道中心线行驶）
3.碰撞损失（碰撞风险越高损失越大）
4.纵向加加速度损失（纵向加加速度越大，推背感越重，刹车越急损失越大）
5.横向加速度损失（保持车辆平稳环道）
6.向心加速度损失（转弯掉头能够减速行驶）

Select lowest-cost trajectory<====>Constraint & Collision Check

Output