⾃动驾驶（⼋⼗三）---------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算法是含有隐变量的概率模型参数估计的最⼤似然估计法，第⼀步是计算期望（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被⽤于在凸域内寻全局最优解。