Apollo EM planner

参考：https://arxiv.org/pdf/1807.08048.pdf

最大期望算法经过两个步骤交替进行计算，第一步是计算期望（E），利用对隐藏变量的现有估计值，计算其最大似然估计值；第二步是最大化（M），最大化在E步上求得的最大似然值来计算参数的值。M步上找到的参数估计值被用于下一个E步计算中，这个过程不断交替进行。

多车道 EM 规划框架：

1. reference line generator在采集数据后，会生成一些候选车道级参考线，以及交通规则和障碍物的信息。这个过程基于routing模块提供的HDMap和导航信息。
2. 基于生成的reference line构造一个Frenet frame。重构信息传递给optimizer。
3. lane-level optimizer执行车道优化和速度优化，在路径优化过程中，将周围环境的信息投影到Frenet框架(E-step)上。基于在Frenet框架中投影的信息，生成一条平滑路径(M-step)。
   同样，在速度优化过程中，一旦路径优化器生成一条平滑的路径，障碍物被投影到ST图(E-step)上。然后，速度优化器将生成一个平滑的速度轮廓(M-step)。
4. 结合路径和速度剖面，我们将得到一个平滑的trajectory为指定的车道。最后一步，将所有车道级最佳轨迹发送给参考线轨迹决定器。轨迹决策器根据车辆的当前状态、规则和每条轨迹的成本，确定自我车辆机动的最佳轨迹。

E-Step对应的基于隐含数据的假设值（当前规划的Trajectory）进行的极大似然估计（Path Planning）。

M-Step对应的是重新给出未知变量的期望估计（Velocity Planning，然后合成完整的Trajectory）。

最后的Trajectory decider会根据不同Trajectory的cost，交规还有车况选择一条最优的trajectory。

Path-speed的迭代算法包含两个步骤，两个E-steps，两个M-steps在一个planning的循环；
在第一个E step，障碍物信息被投影到frenet lane frame；这里的投影包含动态障碍物与静态障碍物；静态障碍物的投影直接依据cartesian-frenet的转化frame；在apollo的framework，动态的object的intensions都被描述为运动的轨迹；
出于安全考虑(目前技术处理这类场景的安全性把握不大)，在 SL 投影中，只对低速 (low-speed traffic) 和对向 (oncoming) 动态障碍物使用；高速物体通过变道来解决。

在第二步E step，所有的障碍物，包含高速的，低速的，对面来车的obstacels都会基于生成的path profile来评估时间位置信息。如果障碍物的轨迹与规划的路径相重叠，相应的地点时间frame会产生。

在第二个M step，path与speed profiles通过整合DP与QP产生；即使我们把障碍物投影到SL/ST坐标系上，路径优化与速度解决方案依旧是个non-convex的空间。因此，我们用DP先获得一个rough的解；同时这个解可以提供障碍物的decisions， 例如nudge， yield或者overtake。QP主要就是对DP的路径进行平滑处理；optimizer可以找到凸空间解；

M-step path optimizer： DP-based path decision and spline-based path planning

DP：路径动态规划步骤提供了一个粗略的路径剖面，提供了可行的tunnel和障碍物nudge决策。
添加链接描述
apollo Planning中用二次规划（PIECEWISE_JERK_SPEED_OPTIMIZER）或非线性规划（PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER）进行最后st曲线的优化，然而这种优化方法面对非凸问题就难以求出最优解了，所以在最后规划之前需要用一个粗的速度规划将非凸问题转化为凸问题。apollo中用了动态规划算法作为速度规划中非凸到凸问题的转化。

基于Frenet框架，在自我车辆前面首先采样多行点。用五次多项式边平滑连接不同行之间的点。点之间的间隔距离取决于车速、道路结构、变道等因素。该框架允许根据应用场景定制采样策略。例如，换道可能需要比当前车道行驶更长的采样间隔。此外，出于安全考虑，格点全站距至少为8秒或200米。
从障碍物，交通规则和车辆动力学三方面来计算每个连接边(两个 lattice point 的连接线)的代价。 在计算完通过动态规划搜索来选择代价最低的候选路径。

动态规划将复杂的多阶段决策问题分解为一系列简单的、离散的单阶段决策问题,采用顺序求解方法,通过解一系列小问题达到求解整个问题目的；动态规划的各个决策阶段不但要考虑本阶段的决策目标,还要兼顾整个决策过程的整体目标,从而实现整体最优决策。

这里按我的理解就是，已知每一段连线的cost，从终点开始，先前找点，使得到终点的cost最小（等同于常用的路径最短）。

QP：对DP生成的粗略的路径进行优化(refinement)。
添加链接描述
添加链接描述
S-L域路径规划的QP过程主要进行曲线的平滑，这个曲线平滑的思路，很巧妙的将离散点转换为连续的多项式曲线表达式，速度的非线性优化NLP过程中，就用这个思路把路标点中离散的曲率kappa拟合成了一个关于路程S的连续形式的函数，以便计算向心加速度时使用，速度的上下界也是用这个思路从离散转连续的

在DP path 中，基于选择路线的可行通道 (feasible tunnel) 被确定下来就是一个凸优化问题，spline QP path 是在可行通道中产生一条平滑的路线。
在线性化的约束 (linearized constraint) 中，利用 QP spline solver 来求解优化目标函数 (objective function)，从而生成生成 spline QP path。