规划基础知识

1 规划简介

在规划中，我们通过结合高精地图，定位和预测来构建车辆轨迹。

规划的第一步就是路线导航，侧重于如何从地图上的A前往B，在进行路径规划时，将地图数据作为输入，并输出可行驶路径。

在Apollo中，通过路线规划模块处理该任务，一旦构建了高水平的路线，就会放大至轨迹规划。通过轨迹规划，可以做出微妙的决策以避开障碍物，并为乘客创造平稳的乘车体验。

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

2 规划基础知识

1. 路由
   路线规划的目标：找到从地图上A前往B的最佳路径。
   三个输入：
   （1）地图
   Apollo提供的地图数据包括公路网和实时交通信息
   （2）当前在地图上的位置
   （3）目的地
2. 世界到图
   搜索：当人们试图在地图上找到从A到B的路线时，通常会沿着道路追踪路径，以查看是否存在通往目的地的任何路径
   Apollo也通过搜索来查找路线

在Apollo搜索之前，将地图数据重新格式化为"图形"的数据结构，该图形由"节点"和"边缘"组成，节点代表路段，边缘代表这些路段之间的连接

也可以对从一个节点到另一个节点所需的成本进行建模，比如拐过一个交叉路口比直行更费劲，所以1-4的成本比1-3的成本更高。

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

3. 网格世界
   A*是经典的路径查找处理算法
   对人类来说，通过查看图形便可以轻易找出最佳路径，但对于计算机而言，这并不明显，计算机必须检查是否存在通往目的地的任何路径。这会耗费大量的时间，尤其是地图非常大的时候
   
4. Ap4
   
5. 从路由到轨迹
   
   高等级地图路线只是规划的一部分，需要沿这条路线前进的低等级轨迹，意味着要处理不属于地图的物体（车辆、自行车或行人）
   
   这些场景需要更低级别、更高精确度的规划，这一级别的规划称为轨迹生成。
6. 3D轨迹
   轨迹生成的目标：生成由一系列路径点所定义的轨迹，为每个路径点分配了一个时间戳和速度，让一条曲线与这些路径点拟合，生成轨迹的几何表征
   
   由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每一个路点都有时间戳，我们可以将时间戳与预测模块的输出相结合，确保我们计划通过时，轨迹上的每个路径点均未被占用。
   
   这些时间戳创建了一个三维轨迹。
   3D Trajectory:2D Position + Time
   每个路径点由空间中的两个维度以及时间上的第三个维度来定义，还为每个路径点指定了速度，速度用于确保车辆按时到达每个路径点。
7. 评估一条轨迹
   现实中的规划面临多种约束
   （1）首先，轨迹应该能免于碰撞。
   （2）其次，要让乘客感觉到舒适，所以路径点之间的过渡以及速度的任何变化都必须平滑。
   （3）再者，路径点对车辆应实际可行，例如高速行驶的汽车不能立即做180度转弯。不能构建这种不可行机动的轨迹。
   （4）最后，轨迹应合法，需要了解每个路径点的交通法律，并确保轨迹遵守这些法律法规。

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

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

车辆在不同的环境中使用不同的成本函数，例如高速公路的成本函数和停车场的成本函数不同

8. Frenet坐标
   

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

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

纵坐标表示在道路中的行驶距离，横坐标表示汽车偏离中心线的距离

9. 路径-速度解耦规划
   路径-速度解耦规划将轨迹规划分为两步:路径规划和速度规划

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

下一步是确定沿这条路线行进的速度，我们可能希望改变在该路径上的速度，所以我们真正需要选择的是与路径点相关的一系列速度，而不是单个速度。将该序列称作"速度曲线"，我们可以使用优化功能为路径选择受到各种限制的良好速度曲线。

通过将路径和速度曲线相结合，可构建车辆行驶轨迹

10. 路径生成与选择
    为了在路径-速度解耦规划中生成候选路径。

（1）首先将路段分割成单元格

（2）然后对这些单元格中的点随机采样

（3）通过从每个单元格中取一个点并将点连接，便创建了候选路径

（4）通过重复此过程，构建了多个候选路径，使用成本函数对这些路径进行评估，并选择成本最低的路径

成本函数考虑的因素：
（1）与车道中心的偏离
（2）与障碍物的距离
（3）速度和曲率的变化
（4）对车辆的压力等

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

12. 速度规划
    为了构建最佳速度曲线，需要将ST图离散为多个单元格
    

单元格之间的速度有所变化，但在每个单元格内，速度保持不变。该方法可以简化速度曲线的构建并维持曲线的近似度，在ST图中，可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。
例如，假设预测模块预测车辆将在t0到t1的时间段内驶入我们的车道，由于该车将在此期间占据位置s0到s1，因此在ST图上绘制了一个矩形。它将在时间段t0到t1期间阻挡位置s0到s1,为避免碰撞，速度曲线不得与此矩形相交

既然有了一张各种单元格被阻挡的ST图，便可以使用优化引擎为该图选择最佳的速度曲线

优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案。
限制有：
（1）法律限制，如速度限制
（2）距离限制，如与障碍物的距离
（3）汽车的物理限制，如加速度限制

13. 优化
    路径-速度解耦规划在很大程度上取决于离散化。
    

路径选择涉及将道路划分为单元格。速度曲线构建涉及将ST图划分为单元格。尽管离散化使这些问题更容易解决，但是生成的轨迹并不平滑。

为了将离散解决方案转化为平滑轨迹，使用"二次规划"技术，二次规划将平滑的非线性曲线与这些分段式线性段相拟合

一旦路径和速度曲线就绪，便可以用其构建三维轨迹

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

感知系统观察到一辆缓慢行驶的车辆离得越来越近

首先在这辆车的周围生成多条候选路径，使用成本函数对这些候选路径进行评估，并选择成本最低的路径

然后使用ST图来进行速度规划，根据其他车辆随时间变化的位置阻挡了ST图的部分区域，优化引擎可以确定该图的最佳速度曲线，该曲线受制于约束和成本函数


使用二次规划让路径和速度曲线变得更加平滑。

最后，将路径和速度曲线合并构建轨迹。蓝色曲线慢，红色曲线快。

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

可以将一个三维问题分为两个单独的二维问题，通过分离轨迹的纵向和横向分量来解决


其中，一个二维轨迹是具有时间戳的纵向轨迹，称为ST轨迹。另一个二维轨迹是相对于纵向轨迹的横向偏移，称为SL轨迹。

Lattice规划具有两个步骤：分别建立ST和SL轨迹，然后将他们合并

为生成纵向和横向二维轨迹

（1）先将初始车辆状态投射到ST坐标系和SL坐标系中
（2）通过对预选模式中的多个候选最终状态进行采样来选择最终车辆状态

（3）对于每个候选最终状态，构建了一组轨迹，将车辆从初始状态转换为最终状态。使用成本函数对这些轨迹进行评估并选择成本最低的轨迹

ST轨迹的最终状态
根据情况，可以将状态分为三组：巡航、跟随、停止

巡航：

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

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

只需对汽车何时何地停止进行抽样，这里，速度和加速度会被修正为0。

SL轨迹的最终状态

进行SL规划做出一个假设：
无论车辆进入怎样的终止状态，车辆都应该稳定地与车道中心线对齐。
意味着只需要在一个小区域内，对横向终止位置进行采样。采样的就是道路上相邻车道中心线周围的位置

为了确保稳定性，汽车驶向的终止状态应该与车道中心一致，当用横向位置与纵向位置作图时，以车辆与车道对齐并直线行驶而结束。

达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为0，一阶导数意味着车辆既不是横向移动，二阶导数意味着不是横向加速。意味着车辆正沿车道直行。

Lattice规划的轨迹生成：
一旦同时拥有了ST和SL轨迹，需要将他们重新转化为笛卡尔坐标系，然后将他们相结合，构建由二维路径点和一维时间戳组成的三维轨迹

ST轨迹是随时间变化的纵向位移，SL轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标S，所以可以通过将其S值进行匹配来合并轨迹