本文主要是对英文论文【Baidu Apollo EM Motion Planner】（By Haoyang Fan, Fan Zhu）的学习总结。
自动驾驶的技术，从软件层面来说，分成基于嵌入式系统的软件平台部分，以及算法部分。前者提供类似OS（如Windows、MacOs，或手机上的IOS、Android）一样的运行支持和资源分配管理功能，后者更接近于提供具体功能的APP。
自动驾驶的算法一般主要分成感知、融合、规划、控制，其中规划处于承上启下的位置，作用是根据感知结果和环境信息，产生车辆的移动轨迹，从而让控制算法按照这个轨迹控制车辆的移动。
这篇文章说的就是规划算法之中一个常用的算法：EM Planner。它针对的是L4级别的自动驾驶场景。
EM planner同时覆盖多车道和单车道的自动驾驶，基于EM-type iterative algorithm。
一、从总体来看
整体思路：
- 并行计算每条车道的轨迹，基于Frenet frame来迭代计算最优的path和speed。
- 比较每条车道的轨迹，判断换道策略。
- 计算最优的path和speed：结合动态规划和基于样本的二次规划求解。
主要考虑的自动驾驶安全角度 ：
- 交通法规
- 覆盖范围：8秒或200米的轨迹
- 工作周期：100ms内
- 紧急安全：当前方输入模块出错，输出紧急保险策略
阿波罗顶层软件架构：
考虑的驾驶体验及优化方向：
- 场景覆盖（基本行驶、换道、拥堵等）
- 交通法规
- 舒适度（轨迹的smoothness）
多车道策略
对于L4自动驾驶，必须考虑换道策略。要决定车辆在不同车道中，走哪一条车道的哪个轨迹，常规做法是在所有车道中，基于cost来搜索最优轨迹，但这样的弊端在于：
- 搜索空间太大成本高；
- 交规在不同车道是不同的；
- 应避免突然换道。
EM中的换道策略分成主动换道和被动换道，主动换道由routing模块触发，默认车道被阻挡则触发被动换道。
所有障碍物和环境信息都会投射到每个备选车道的Frenet frames，每个车道再各自符合交规，每个车道都会根据cost生成一条最优轨迹，再选出全局最优轨迹。
求解最有轨迹的算法： Path-Speed Iterative Algorithm
在F坐标里面找最优轨迹是一个3D约束最优化问题，一般有两种方式：
1、直接求解：
轨迹采样或栅格（lattice）搜索。受限于随着空间时间而增加的搜索复杂度，因此得出的轨迹是suboptimal次优的。path主要考虑静态障碍，然后在生成的路径上计算速度。对于动态障碍可能找不到最优解。
2、path-speed去耦求解：
首先，由最新的speed信息评估未来的、低速的动态障碍，从而得出基于这个速度的最优path。然后生成的这个path又返回给speed计算针对新的path的最优的speed。
注意，这里的speed不会再马上传给path，所以迭代不是在同一个计算周期里进行，而是在下一个周期再进行迭代的。这里最好想一想，理解一下。
对于高速障碍，EM采用换道而不是闪躲来处理，因此对于高速障碍，EM也能处理。至于为什么要换道而不是闪躲，我猜测是因为闪躲对于动态障碍物的预测精度要求更高，而去耦求解的EM的预测前提是「预测都是基于当前周期的speed和path来进行的」，并不会考虑基于这个预测作出的行动会影响到障碍物的轨迹，这种交互对于高速动态障碍物来说，足够产生无法忽略的预测偏差，导致不能进行安全闪躲。
决策和交规
EM中，为了平滑轨迹，在规划开始之前就进行决策。
常见决策有两种模式：
Hand-tuning decision：可以调，但受限于规模。还有可能出现超出其定义的决策规则。
Model-based decision：将车辆状态分解成有限的驾驶状态，用大量数据来训练模型。
EM的决策同时采用了以上两种模式。但说到模型应该更多在于预测模块对障碍物轨迹的预测，EM的决策目标还是提供一个大概的可选空间，决策的内容主要是换道、重用、针对每个障碍物判断是否闪躲超车等。
更抽象来说，EM中的决策，首先将ego car的移动意图描述成一个粗略但灵活的轨迹，在这条轨迹上评估与障碍的交互，从而保证scalable。然后，planner还会生成一个凸空间，用于在轨迹上平滑样点参数。最后，一个二次规划求解器依据decision生成平滑的path和speed。
二、多车道策略的软件架构
（EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY ）
首先，所有数据汇总到data center，生成车道级的参考线（带交规和障碍物，基于高精地图和导航信息）。对每个车道，首先基于参考线构建F坐标系，随后所有评估均基于此。
随后的决策和优化，均基于场景管理器和任务管理器进行。在一个场景下，按顺序执行定义好的tasks，其中就包含各种path和speed的决策和优化tasks。
具体来看，经过决策后将可选轨迹空间限定缩小范围，重构数据再传给Optimizer（优化器），进行path和speed优化。
在path优化时，周围信息投影到F坐标，优化的目标是在决策限定的空间内生成一条平滑的path，可以想象成闪躲障碍物往前走的游戏，玩家需要选择一条路线通过这条狭窄的通道，舒适又快捷地通过。
在speed优化时，障碍物投影到ST图（station-time graph），同样是为了生成一段平滑的speed profile（也就是尽量减少加速刹车）。
然后，组合path和speed，获取该车道的平滑轨迹。
最后，所有车道的最佳轨迹汇总，轨迹决策器基于当前车辆状态、交规、每条轨迹的成本，选择最优轨迹。
三、车道级的EM Planner
现在分析单个车道的规划过程。
首先，迭代是发生在不同的cycle之间。计算新轨迹是基于上一个周期计算的轨迹结果来开始的。
- SL投影：
障碍物投影到F坐标。
直接通过Cartesian-Frenet 转换（笛卡尔坐标到F坐标）。
在Apollo中通过障碍物移动轨迹描述，结合上一个cycle计算得到的轨迹，可以评估动态障碍物和ego car之间的在不同时间点的位置关系。其中两者重叠的部分会被标记在F坐标中。
（由于动态障碍物往往会导致闪避nudge，所以SL投影里只会考虑低速和靠近中的障碍物）
2. ST投影：
根据生成的path，将所有障碍物（包含高速的）投影到station-time frame，这个ST图是基于参考线做的，但无论是障碍物的轨迹，还是自车的轨迹，都是基于每条path来存储及对比是否重叠的以及何时重叠的。
如果障碍物轨迹与规划轨迹重叠，便标记出这个区域。
3. 两个M-Steps：
通过动态规划获取一个rough solution（从此获取障碍物策略：闪躲、让道、超车等），然后基于此决定二次规划样点优化器的凸空间主体（a convex hull for the quadratic-programming-based spline optimizer）。
（虽然所有计算都基于凸空间——内部所有点的连接线都在空间内——但实际上最优轨迹在非凸空间里。）
4. 以上模块再详细来分析一下：
A. SL and ST Mapping (E-step)
SL投影基于G2(continuous curvature derivative) 平滑参考线。
在笛卡尔坐标系，障碍物和自车状态通过定位和朝向(x,y,θ) ，车道曲度及曲度变化率(κ, dκ) 来描述。
在F坐标下，这些信息转换成(s, l, dl, ddl, dddl) ，即station、lateral及它的导数。
静态障碍物与时间无关，因此坐标转换能直接进行。
动态障碍物的坐标转换需要上一次计算出的自车轨迹。首先自车轨迹投射到F坐标，获取the station direction speed profile（提供自车在某个时间点的station坐标，从而评估与动态障碍物的相对关系）。自车station坐标与障碍物轨迹重叠部分就可以计算出来，如下图紫色部分：
ST投影能帮助评估自车的speed profile。
当path投影到F坐标后，若障碍物轨迹会跟自车产生交互，就会在图上显示。
上图中，一个障碍物在2秒后驶入自车道40m前；空白部分就是speed profile的可选空间。
B. M-Step DP Path
查找横向坐标 l = f(s) 的最优解（在SL空间中基于station坐标计算），包含两步：
- 动态规划
- spline-based 规划
（但实际上Apollo 6.0之中，path的DP过程被简化成获取粗略可选空间的path boundary，并不需要复杂的cost函数优化求解过程。）
包含cost计算、lattice采样、动态搜索。其中lattice采样需要在自车前获取数row的点，不同row的点平滑连接。row里撒点的间距由速度、道路结构、换道等决定，可以自定义。距离覆盖8秒或200米。
Lattice建立后，每个图边缘通过cost函数评估——基于SL投影、交规、车动态信息。总edge cost函数是「平滑性、避障、车道成本」的线性组合。
平滑cost函数中，f ′ (s)代表车道与自车的偏差角， f ′′ (s)是车道曲率。w则代表不同项的权重。
障碍物cost函数，与障碍物距离d大于设定的安全值dn时为0，小于碰撞距离dc则是无穷大。
参考线cost，代表与参考线的距离成本，其中g(s)是参考线函数。
C. M-Step Spline QP Path
QP的目标，是在DP产生的path可选空间中，通过QP spline solver对由线性化约束的目标函数进行最优化，在其内生成一条平滑的path。
QP的目标函数是平滑cost和guidance line cost的线性组合。后者用到的比较项是DP产生的可选空间里的大量path，用于评估障碍物的闪避距离。然后通过优化找最优解的方式，找出其中cost最低，也就是最优的path。
由于车辆的朝向会影响车辆边界与周围的位置关系，也就是需要确定车辆四个角与station点的关系，而不只是l=f(s)。
这样就需要知道车头朝向，其他三个角类似。
边界的cost函数都可以进行线性化处理，随后就可以用二次规划快速求解。
D. M-Step DP Speed Optimizer
DP在ST图里生成speed profile。包含cost函数，ST图栅格，动态搜索。
具体来说，首先障碍物信息分散到ST的栅格中，栅格根据间隔dt平均划分，速度的具体参数大概可以认为：
目标函数是令cost最低：
Vref 是参考速度，一般是车道的限速。第一项代表维持速度的cost，其后两项代表平滑性，最后是障碍物cost——自车与所有障碍物的距离。
E. M-Step QP Speed Optimizer
其中必须满足的边界约束：
平滑拟合的速度曲线：
F. Notes on Solving Quadratic Programming Problems
基于安全考虑，Apollo EM评估的点约100个，约束条件大于600个。
QP中计算结果包含3到5个多项式，约30个参数。
因此二次规划的目标函数较小而约束较多，求解较容易。同时为了加速计算，采用上周期的结果作为计算起点（hot start），平均3ms就能得到结果（看算力）。
G. Notes on Non-convex Optimization With DP and QP
DP和QP在非凸空间都有各自的不足。
DP：依赖于采样栅格，有限的栅格数量只能给出一个粗略的DP解。即很可能不是最优解。譬如DP能得出向左闪躲的路径，但闪躲的距离不一定是最好的。
QP：相反，QP解依赖于凸空间，因此不能脱离DP使用。譬如，没有DP的decision，QP无法决定该怎么走。
DP+QP：将各自的不足最小化。先找一个粗略的解，再在这个凸空间里找最优。
四、 CASE STUDY
根据初始速度计算交汇点，然后得出path，同时根据约束计算speed，speed更改后交汇点发生变化于是重新计算path，根据新的path重新计算交汇之后的speed（交汇之前的若也变化，那这个迭代就需要不断进行了）。
五、算力
COMPUTATIONAL PERFORMANCE
本来的三维问题（station-lateral-speed）分解成两个二维问题（station-lateral和station-speed），因此计算复杂度大大降低。对n个障碍物，M条备选path和N条备选speed，计算复杂度是O( n(M+N) ) 。
六、结论
EM planner是一个轻决策算法，与其他基于大量规则的算法不同，无需提前预估障碍物之间的相互影响，也不会因为规则变多导致找不到合适轨迹而规划失败。
安全性和通过性互相矛盾，安全规则增加后，可选的路径就会减少，而且很容易导致走到一半的换道突然被中断，而EM Planner则更可能保持连贯性，且得到可用的轨迹。
EM的计算复杂度较低。
EM已在北京和CA实地测试过，截止20180516，跑了3380小时、68000公里。并在仿真环境跑了10w小时和100w公里。
以上是论文的结论。
EM Planner整个框架能处理L4的自动驾驶场景，而且采用场景管理和任务管理，在去耦的角度看处理的也比较好，将三维问题分解成两个二维问题，是这个算法最大的特点了。整个算法中，最核心的部分在于要理解Cost函数的构造和求解。之后我会写关于这方面的学习备忘。
