自动驾驶（八十三）---------Apollo之规划模块

     规划是自动驾驶的重心，所有模块都是围绕规划而存在的，之前的模块以及介绍了很多这方面的内容，首先一起回顾一下：

       1. 初探：内容包含：带约束的多项式拟合算法、贝赛尔曲线、三次样条插值公式。

       2. 再论：内容包含：A*算法、行为规划介绍、有限分层状态机。

       3. 三顾：内容包含：主要介绍apollo的ppt

       4. 四解：内容包含：基于优化的三次样条

       5. 五阶：内容包含：LatticePlan的轨迹规划

      以上种种都是没有系统的设计轨迹规划，这次利用apollo的做法，系统阐述一下自动驾驶的轨迹规划。

1. RTK规划器

      之前介绍apollo的定位有三种模式：RTK定位、多传感器融合定位、高精地图视觉融合定位，很明显在RTK和高精地图的加持下，基本等同于寻迹模式，所以在规划器中RTK肯定是最简单的一个。

      首先需要有轨迹文件，一般保存每一时刻的：坐标、角度、车速、加速度、曲率、角速度、油门、方向盘转角等信息，同时需要得到当前车辆状态和目的地，按照之前的轨迹线开到目的地。

      1. 计算当前位置和目标轨迹线的最近距离，如果大于阈值，从当前位置生成一条引导线到目标轨迹上；如果小于阈值，直接用目标轨迹最为引导线。

      2. 设置规划的点的个数，我们不需要所有的轨迹去计算控制信号，取本车前面的一段点来计算。

      3. 判断取的点数是否满足要求，不满足继续在原来的轨迹中加点，继续计算控制信号。

2. EM规划器

      EM规划器面临的问题是，有多个车道，有障碍物，如和规划出一条合理的轨迹给控制，这中间没有RTK的轨迹参考，可能有视觉定位车道信息，也有高精地图作为参考，但是没有高精定位，这也是我们自动驾驶最常遇到的场景。

      这里简单介绍一下EM算法：EM算法是含有隐变量的概率模型参数估计的最大似然估计法，第一步是计算期望（E），也就是将隐藏变量象能够观测到的一样包含在内，从而计算最大似然的期望值；另外一步是最大化（M），也就是最大化在 E 步上找到的最大似然的期望值从而计算参数的最大似然估计。M 步上找到的参数然后用于另外一个 E 步计算，这个过程不断交替进行。

      1. 在第一个E-steps中，把动态和静态障碍物都投影到Frenet坐标系中，其中动态障碍物以其运动轨迹来描述（预测模块提供），所以动态障碍物和汽车在某个时间点的重叠可以被映射到Frenet框架中。如图迎面而来的汽车相应的轨迹为红色，本车为为蓝色。重叠区域（图中紫色部分）为预计碰撞区域：

      2. 在第二个E-steps中，把所有障碍物的速度曲线都描述到对应的s-t坐标系下，ST投影可以帮助我们评估自车的速度曲线。如图：红色表示在2s处、距离自车40m远的地方有动态障碍物切入本车规划路径中，绿色则在本车车后有动态障碍物6s后进入本车当前的位置。

      3. M-step：尽管将障碍物投影到SL和ST框架上，但最优路径和速度解仍然存在于非凸空间中。因此使用动态规划来获得一个大概的解。

M-steps路径规划器在Frenet框架中规划路径。即找到横坐标的最优函数l=f(s).w.r.t（非凸SL空间中的状态坐标）。因此路径优化器包括两步：基于动态规划的路径决策和基于样条的路径规划。动态规划路径步骤提供了具有可行性车道和障碍物轻推角的大致路径。如图所示，该步骤包括采样器，代价函数，和动态规划搜索。

QP路径步骤是对动态规划路径的改进。在动态规划路径中，根据所选的路径生成可行的隧道。然后基于QP步骤将在该隧道中生成一条平滑路径，如图所示：

 

 通过QP求解器求解具有线性约束的目标函数生成最优的QP路径。
QP路径的目标代价函数是平滑度代价和引导线代价的线性组合。该步骤中的引导线是DP路径。引导线给出了绕过障碍物的估计值。在数学上QP路径步骤优化以下函数：

 速度规划器在ST里面生成一个速度曲线，该速度曲线表示为相对于时间S(t)的状态函数。与路径规划器相似，在ST图上寻找最优速度曲线也是一个非凸问题，所以采用DP与QP相结合的方法在ST图上寻找一个光滑的速度曲线。在图12中，DP速度规划步骤包括代价函数，ST图网格和DP搜索。生成分段性速度曲线，可行的隧道和障碍物速度决策。如图11所示。在QP速度时采用DP生成的速度曲线作为引导线，利用可行隧道生成凸域。

 

      由于分段线性速度曲线不能满足动力学要求，需要用QP来解决这个问题。在图13中，样条速度包括三个部分：代价函数、线性约束条件和样条QP求解器。

 代价函数被以下函数描述：

      第一项是DP速度参考线Sref和最后需要生成的S之间的距离。加速度和jerk是速度曲线平滑度。因此，目标函数是跟踪引导线和平滑度之间的平衡。

QP优化器是带有线性约束的。QP速度优化器的限制条件包括一下边界限制：

第一个约束是单调性，即不倒车。第二第三和第四 五个约束是来自交通规则和车辆动力学约束的要求。在完成代价函数和约束后，QP求解器将生成一个平滑可行的速度曲线，如图14结合路径曲线，EM规划器将为控制模块生成一个平滑的轨迹。

 

 F.解二次规划问题的笔记
出于安全考虑，我们评估了大概100个不同位置或时间点的路径和速度。约束的数量大于600.对于路径和速度优化，我们发现五项多项式足够好用。样条曲线包括3到5个多项式，大约有30个参数。因此，二次规划问题有相对较小的目标函数，但约束条件较多。因此，主动集QP求解器有助于解决该问题。除了加速二次规划求解外，我们还将上一个循环的计算结果作为热hot start（牛顿法）。QP问题平均可以在3ms内解决，满足了我们对时间消耗需求。

G.关于DP和QP非凸优化的笔记
DP和QP在非凸领域都有局限性。DP和QP的结合将充分利用两点的优点，以达到理想的解决方案。

DP：如本文前边所述，DP算法依赖于一个采样步骤来生成候选解。由于处理时间的限制，采样网格限制了候选样本的数量。有限网格内的优化会得到一个粗糙的DP解。换言之，DP不一定，而且在几乎所有的情况下，都不会提供最佳解决方案。例如，DP可以从左侧轻推障碍物的路径。但不能选择具有最佳距离的路径。
QP：相反，QP基于凸域生成解。如果没有DP步骤的帮助，它是不可用的。例如，如果一个障碍物在车辆前面，QP需要一个决策，例如从左侧变道，从右侧变道，跟随或者超车，以生成其约束。随机或基于规则的决策将使QP容易陷入失败或者局部极小值。
DP+QP：DP+QP算法将两者的局限性降到最低：（1）EM规划器首先使用DP在网格内搜索，以求得粗略的解；（2）DP结果被用于生成凸域并引导QP；（3）QP被用于在凸域内寻找全局最优解。