系列介绍
本系列记录Coursera自动驾驶学习课程（链接）的学习笔记。目录如下：
-
自动驾驶综述
-
定位
-
感知
-
路径规划
- 路径规划综述（本文）
- cost function
路径规划介绍
ST图
在选定一条轨迹之后，需要确认这条轨迹上各个点的速度信息。ST曲线可用来描述速度信息。在一个ST曲线中，S表示在DL坐标系下车辆的纵向位置，T表示时间。一条ST曲线可以用来描述车辆的移动。在时刻t的曲率即是该时刻的车速。
速度规划
为了构建最佳速度曲线，需要将ST图离散成多个单元格。单元格间的速度有所变化，但是在每个单元格内，速度保持不变。这个方法可以简化速度曲线的构建，并维持曲线的近似度。在ST图中，可以将障碍物绘制成在特定时间段内阻挡道路某些部分的矩形。例如，预测模块中预测车辆会在t0到t1的时间段内驶入ego所在车道，由于该车在此期间会占据位置s0到s1，因此可以在ST图上绘制一个矩形。同时为了避免碰撞，速度曲线不得与此矩形相交。既然有了一张各种单元格被阻挡的ST图，我们可以使用优化引擎为改图选择最佳的速度曲线。优化引擎会在各种限制的前提下搜索出一条最有曲线（限制包括法规限制，如限速，距离限制，如与障碍物的距离，物理限制，如加速度限制等）。这里可能会有人问，为什么是矩形，而不是平行四边形或者其他，这是因为我们无法知道npc的速度和转向信息，只能提供一个最大值。（可以问下planning实现是不是这样）。
优化
路径-速度解耦优化很大程度上取决于离散化。路径选择涉及到将道路划分成单元格，速度曲线构建设计将ST图划分为单元格。尽管离散化让问题更容易解决，但是解决方案生成的轨迹并不平滑。为了将离散解决方案转换为平滑轨迹，可以使用“二次规划”技术（Quadratic Programming）。 二次规划将平滑的非线性曲线与这些分段式线性段拟合。一旦路径和速度曲线就绪，便可用其构造三维轨迹。
路径-速度解耦规划的轨迹生成
首先在这辆车的周围生成多条候选路线，然后使用成本函数对这些候选路径进行评估，并选择成本最低的路径。
然后我们使用ST图来进行速度规划，我们根据其他车辆随时间变化的位置阻挡了ST图的部分区域。优化引擎可以帮助我们确定改图的最佳速度曲线，该曲线受制于约束和成本函数。
我们可以使用二次规划让路径和速度曲线变得平滑。
最后，我们将路径和速度曲线合并构建轨迹。我们使用该轨迹来安全的绕开其他车辆，并继续前进。这里的轨迹在速度较快时为红色，慢时为蓝色。
Lattice规划
通过使用Frenet坐标系，我们可以将环境投射到纵轴和横轴上，我们的目标是生成三维轨迹-纵向维度、横向维度、时间维度。我们可以将一个三维问题拆解成两个单独的二维问题：这是通过分离轨迹的纵向和横向分量来解决的。其中一个二维轨迹是具有时间戳的纵向轨迹（ST轨迹）；另一个二维轨迹是相对于纵向轨迹的横向偏移（SL轨迹）。Lattice规划具有两个步骤，即分别建立ST和SL轨迹，然后将它们合并。为生成纵向和横向二维轨迹，我们先将车辆初始状态投射到ST坐标系和SL坐标系中，通过对预选模式中的多个候选最终状态进行采样，我们来选择最终车辆状态。对于每个候选最终状态，我们构建一组轨迹，将车辆从初始状态转换为最终状态。我们使用成本函数对这些轨迹进行评估，并选择成本最低的轨迹。
ST轨迹的最终状态
有三种状态：
- Cruise: 巡航意味着乘凉将在完成规划步骤后定速行驶。我们实际上在对图上的点进行采样，在图中，横轴代表时间，纵轴代表速度，对于图中的车辆，将进入巡航状态。在时间t以S点的速度巡航。对于这种模式，所有最终状态的加速度均为0。
- Following：在这种青黄下，我们要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，在跟随车辆时，我们需要与前方的车保持安全距离。这时，速度和加速度将取决于我们要跟随的车辆。这意味着，在这种模式下，速度和加速度都会进行修正。
- Stop：对于这种模式，我们只需要对汽车何时何地停止进行抽样，速度和加速度都会被修正为0。
SL轨迹的最终状态
针对SL规划，会有一个假设：车辆无论进入怎样的终止状态，车辆应该稳定的与车道中心线对齐。这意味着我们只需要在一个小区域内对横向终止位置进行采样。具体的说，我们采样的是道路相邻车道中心线的周围位置。为了确保稳定性，汽车的终止状态应该与车道中心一致。当我们用横向位置和纵向位置作图时，我们想要的候选轨迹应该是以车辆与车道对齐并且直线形式结束。为了达到这种状态，车辆朝向和位置的一阶导数和二阶导数都应该是0。这意味着车辆既不是横向移动的，也不是横向加速的。这意味着车辆正沿着车道直行。
多项式
有了起始状态和最终状态，则可以考虑如何设计曲线拟合。直接用多项式拟合即可。
Lattice规划的轨迹生成
一旦我们同时有了ST轨迹和SL轨迹，我们需要将它们重新转换为笛卡尔坐标系。然后，可以将它们相结合，构建由二维路径点和一维时间戳组成的三维轨迹。ST轨迹随时间变化的纵向位移，SL轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵向坐标S，我们可以通过S值进行匹配来合成轨迹。
