自动驾驶面经总结（三）

EM planner

本文主要是对英文论文《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函数的构造和求解。

Baidu Apollo代码解析之轨迹规划中的轨迹评估代价函数

apollo中包含了两种轨迹规划方法：Lattice和EM Planner，其中Lattice主要是采样和剪枝的思想，EM主要是优化的思想。二者的目标都是求取代价最小的路径
lattice planner只要设计了六个代价函数，考虑了到达目标，平滑，避免碰撞，向心加速度，横向偏移，舒适性
em planner将轨迹规划分为两部分优化：path即d-s（轨迹的横向偏移量和纵向行驶距离），speed即s-t（轨迹的纵向行驶距离与时间的关系）；优化分为两个步骤：DP动态规划求大致解，更像是给出nudge或者overtake这样的决策，把优化问题变为凸优化问题，为后续的QP二次规划提供参考。QP则求精细平滑的轨迹。