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会根据不同Traject