自动驾驶之轨迹规划6——Apollo EM Motion Planner

1. 概述

《Baidu Apollo EM Motion Planner》是Apollo官方的出的文章（虽然是官方文章，但其中有表述不是很严谨，也有一些小错误，大家可以审视的研读这篇文章，另外可以找一些该文的参考文献看一看会有很大帮助），网上的讲解很多都是机翻，本文根据自己对《Baidu Apollo EM Motion Planner》的理解整理如下：

Apollo的功能架构如下：这篇文章是专门将Motion planner的，其中：

高精地图模块（HD Map）提供高精度地图信息；
定位模块（Localization）和感知模块（Perception）提供当前车辆周围动态环境；
预测模块（Prediction）提供预测的未来环境信息；
路由模块（routing）基于专用的路由地图routing_map，输出车辆在从出发点到目的地的过程中经过的所有路段（类似百度地图A*）；
运动规划模块（MotionPlanning）接收所有信息生成安全舒适的轨迹发送给车辆控制模块。

备注：Routing类似于现在开车时用到的导航模块，通常考虑的是起点到终点的最优路径（通常是最短路径），Routing考虑的是起点到终点的最短路径，而Planning则是行驶过程中，当前一小段时间如何行驶，需要考虑当前路况，是否有障碍物。Routing模块则不需要考虑这些信息，只需要做一个长期的规划路径即可

论文提出一种适用于高速公路和低速城市场景的多车道-单车道分层架构：

上层针对多车道：用于变道决策（通过比较lane-level trajectory来处理变道场景）
下层针对单车道：基于Frenet frame，通过动态规划&二次规划进行路径和速度规划

备注：对于规划的覆盖范围:至少提供8秒或者200米的运动规划轨迹，算法的反应时间必须在100ms（人是300ms反应时间）内做出反应。

1.1. Multilane Strategy

一般来讲，L4级的自动驾驶应当包含变道，而变道分为主动变道和被动变道。主动变道是routing模块因为要到达最终目的地而发出的换道请求，被动变道是本车道被障碍物挡住被迫避让变道。而Apollo的EM planner提出了自己的解决方案：

对于候选车道（一般是本车道或相邻车道，可理解为多个候选凸域，详见百度技术学院中对Apollo规划中非凸问题的讲解），所有障碍物和环境信息都投影到Frenet frame中；
并将交通规则与候选车道绑定；
每个候选车道基于规划器，都生成一条从当前车道换至本车道的最佳轨迹；
Crosslane trajectory decider模块会根据代价函数和安全选出要换到哪条车道。

1.2. Path-Speed Iterative Algorithm

基于Frenet frame（SLT）方法的轨迹规划一般分为2种：直接在3维空间下优化求解，或将路径和车速解耦求解（轨迹包含两部分：路径和路径上每个点的速度）。

直接在3维空间下优化求解：一般会用trajectory sampling或lattice search，不过计算量较大，如果因为计算量问题对分辨率或采样率妥协，则容易只得出次优解。
将路径和车速解耦求解：路径规划时考虑静态障碍物或低速障碍物，速度规划时考虑动态障碍物（有动态障碍物时，该解耦法可能不是最优解）。

备注：对于高速动态障碍物，考虑到安全问题，Apollo EM planner倾向于变道而不是减速避让。

1.3. Decisions and Traffic Regulations

约束分两类：
硬约束：交通规则
软约束：避让、超车、接近障碍物的决策

Apollo EM planner倾向先做决策在做规划（和其他算法相比，该算法请决策重规划，该特点会在后面阐述），因为决策过程可以明确变道可行性，这样可大大减小搜索空间。

在EM planner的decision阶段有如下几个步骤（1.1中的描述类似，其实可以理解为是一个东西，只是描述侧重不一样），下面的全文都是对这几个步骤的详解：

用一个粗糙但可行的轨迹描述本车行驶意图
基于此轨迹，衡量本车与障碍物之间的距离
规划器根据该轨迹，会生成凸的可行空间（用于平滑轨迹）；
然后二次规划会用于平滑轨迹（路径和速度）。

2. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

本节讲EM planner的多车道 multilane strategy架构（对1.1和1.3的重复描述，但更详细了）：

reference line generator会根据交通规则和障碍物，产生一些车道级备选reference line。该过程是基于HD map 和 Routing中的导航信息实现的。（如左右相邻车道？这个reference line没搞明白是什么）；
基于reference line生成对应的Frene Frame；
将障碍物等环境信息投影到path的Frenet frame中；
生成平滑path profile；
将障碍物等环境信息投影到speed的Frenet frame中；
生成平滑speed profile。
reference line decider会在所有line-level的最佳轨迹中，根据车辆状态、交通规则、代价函数选择出最佳轨迹

3. EM PLANNER AT LANE LEVEL

本节将EM planner的单车道 lane level架构：

Path E-step：
1、静态、低速和来向障碍物投影到SL坐标中；
2、动态障碍物用EM planner的变道策略去cover（考虑到安全问题，path optimization时遇到动态障碍物时倾向于避让，因此用变道策略cover该场景）；
3、path的E-step最终输出的是带有静态障碍物、动态障碍物轨迹和预测碰撞区的SL。

Speed E-step：
1、所有障碍物（低速、高速、来向）都投影到基于path profile的ST坐标中；
2、如果障碍物轨迹和path有重合点，则在ST图中该处则会生成碰撞阴影区；
3、speed的E-step最终输出的是带有前后障碍物的ST（其M-step可在空白区域搜索可行车速）。

备注：静态障碍物是直接投影，但动态障碍物是把运动轨迹投影到Frenet frame中。且目前得到的仍是非凸问题。

M-step：使用动态规划和二次规划生成平滑的path和speed
1、E-step后，该问题是一个非凸优化问题；
2、动态规划能够得到一个粗略解，决策应该避让nudge、减速yield还是超车overtake；
3、基于粗略解，可将1个非凸问题分解为多个凸问题。

3.1. SL and ST Mapping (E-step)

SL
1、在SL坐标系中，我们用S、L、侧向速度dL、侧向加速度ddL、侧向加加速度dddL，这5个量描述本车状态；
2、静态障碍物的位置是时不变的，在投影时比较简单；
3、动态障碍物的位置，则需要根据当前时刻本车和障碍物的S方向的速度，预测T时刻后其所处的位置（障碍物的预测位置是prediction module给出的），以此来判断是否有碰撞。
4、path的E-step最终输出的是带有静态障碍物、动态障碍物轨迹和预测碰撞区的SL。

下图中大概3s左右会出现碰撞：

ST
speed的E-step最终输出的是带有前后障碍物的ST（其M-step可在空白区域搜索可行车速）。

3.2. M-Step DP Path

path M-step：其作用是基于Frenet frame生成平滑的path profile（但此时SL空间是非凸的，eg：左右车道避让均可能为局部最优解），分为两大步骤：
1、基于动态规划的path decision：动态规划会提供一个粗略的、带有可行区域和障碍物避让策略（nudge decision）的path profile。其步骤主要包括：lattice sampler、cost function和dynamic programming search；
2、基于样条曲线的path planning：二次规划。

lattice sampler
1、lattice sample的不同行（这个“行”是垂直车道线的，即L方向）之间的点通过五次多项式来平滑连接（应该每段都有一个五次多项式，piecewise就会有多个五次多项式）。行内sample points的间隔距离（L方向）取决于速度，道路结构，变换车道等；
2、具体采样策略可根据场景自定义；
3、但出于安全考虑，lattice sample的S方向的距离最好大于8s或200m。

cost function
总的代价函数是：平滑度、障碍物避让、车道代价3部分线性叠加得来的。

CsmoothC_{smooth}Csmooth中f′(s)f'(s)f′(s)是航向角，f′′(s)f''(s)f′′(s)是曲率，f′′′(s)f'''(s)f′′′(s)是曲率变化率；
CobsC_{obs}Cobs中ddd是障碍物到本车的距离，dcd_cdc考虑安全的缓冲距离，CnudgeC_{nudge}Cnudge应当定义为递减函数，CcollisionC_{collision}Ccollision是碰撞代价；
CguidanceC_{guidance}Cguidance分两部分：: guidance line cost 和 on-road cost。guidance line cost是规划出的path距离guidance path的距离，这个guidance path一般是车道中心线。on-road cost是防止车辆冲出道路边缘的代价。公式中把guidance line function定义为g(s)g(s)g(s)。

会基于代价函数，用动态规划搜索最合适的path，这就涉及到了决策decision，即应对障碍物的策略：nudge、overtake还是yield。

3.3. M-Step Spline QP Path

path M-step中的DP已经可以求解出一个粗略的feasible tunnel，而QP要做的就是平滑这个DP输出来的guidance line。具体效果类似下图：

目标函数：

path的QP本质是优化一个带有线性约束的目标函数，其代价函数如下。可以看出，其前3项是尽量平滑（f′(s)f'(s)f′(s)是航向角，f′′(s)f''(s)f′′(s)是曲率，f′′′(s)f'''(s)f′′′(s)是曲率变化率。曲率其实是考虑了车辆的动力学可行性）；第4项是尽量和DP的path保持一致（不要偏离太远，以免发生碰撞）。

约束：
DP的约束有两个：boundary constraints 和 dynamic feasibility。

boundary constraints：边界约束也可以理解为碰撞约束，可以在车头车尾加两个半圆作为车辆边界以保证约束的线性和凸性。
dynamic feasibility：DP给出了由很多points（每个row上一个point）组成guidance line，QP可以此为基础调整每个point在L方向的位置以实现平滑path。但调整是有可行范围的，该范围可量化为QP的约束（lmin和lmax）。另外曲率f′′(s)f''(s)f′′(s)和曲率变化率f′′′(s)f'''(s)f′′′(s)也可以用dynamic feasibility去约束衡量（如下图）。

3.4. M-Step DP Speed Optimizer

speed的M-step会基于ST生成speed profile。这也是个非凸问题，同样可以使用动态规划+二次规划生成speed profile。

动态规划包含cost function、ST graph、dynamic programming search。输出物为：分段线性speed profile（是QP的guidance line）、feasible tunnel（a convex region）、speed decision（yield or overtake）。

DP Speed Optimizer的代价函数，其中：

第1项是车速保持代价，该项想表征的是：在没有障碍物和交通灯约束时车子的期望车速，Vref一般由道路车速限制、曲率和其他交通规则所决定；
第2项和第3项是加速度aaa和加加速度jerkjerkjerk的平顺性代价；
第4项是障碍物距离代价。

这里的约束主要是车辆动力学约束，如加速度aaa和加加速度jerkjerkjerk的约束，另外还有个比较重要的是车辆直行不倒车的约束（下面的Speed QP会着重解决该问题）。

3.5. M-Step QP Speed Optimizer

Speed的M-step中的QP主要是为了平滑速度（具体效果类似下图），包含三部分：cost function、linearized constraint 和 spline QP solver。

其代价函数如下：

第1项是QP得到的S距离DP给出的guidance line之间的距离；
第2、3项是平滑度衡量。

speed M-step的QP的线性约束如下，其中：

第1项是单调性：保证车辆只能前行不能倒车；
第2项是ST曲线在某一时刻的上下界，算是表征碰撞的约束；
第3项是车速上限，一般从交通规则或车辆动力学得出；
第4项是加速度限制，一般由车辆动力学性能、舒适性决定；
第5项是加加速度限制，一般也由车辆动力学性能、舒适性决定；

3.6. Notes on Solving Quadratic Programming Problems

出于安全的考虑，Apollo评估了大概100个不同位置或时间点的路径和速度。约束的数量超过600。对于选择所需的路径和速度优化器，我们发现五项多项式是比较合适的。样条曲线包括3到5个多项式，大约有30个参数。

3.7. Notes on Non-convex Optimization With DP and QP

DP：如前文所述，DP算法通过采样在每个凸域内生成局部最优解，然后通过代价函数求得整个非凸空间内的近似全局最优解（该句待定）。由于计算时间的限制，采样密度会受到限制，而有限网格内的优化会得到一个粗糙的DP解。换句话说，DP不一定能所有的场景下都取得最优解。例如，DP求出从左侧避开障碍物的路径，但不能选择具有最佳距离的路径。
QP：相反，QP是基于DP的粗略解进行曲线平滑。如果没有DP，单纯的QP是无法实现决策规划的。例如，如果一个障碍物在车辆前面，QP需要一个决策，例如从左侧变道，从右侧变道，跟随或者超车，来生成其约束。随机或基于规则的决策容易使QP陷入失败或者局部极小值。
DP+QP：DP+QP算法将两者的局限性降到最低：（1）EM规划器首先用DP在网格内搜索，以求得粗略的解；（2）DP的结果被用于生成凸域并引导QP；（3）QP被用于在凸域内寻找全局最优解。

总的来讲，DP对非凸问题做决策（nudge from right or left or follow or overtake）。QP以DP决策为基础，在凸域内平滑曲线。

4. CASE STUDY

如上所述，虽然大多数高级的规划算法都是基于重决策的，但EM planner是一个基于轻决策的规划器。确实，基于重决策的算法，或者依赖规则的算法，很容易理解和解释。但是缺点也很明显：它可能被困在角落的情况下(陷入局部最优解的频率与限制条件的复杂性和数量密切关),并不总是最优的。在本节中，我们将通过几个案例来说明基于轻决策的规划算法的优点。这些案例都是在百度很多的重决策规划模块中的日常测试中暴露出来的，并最后由轻决策解决。

上图是EM规划器如何在规划周期和迭代周期之间迭代以获得最佳轨迹的实际例子。在这个案例研究中，我们演示了当障碍物进入我们的路径时如何生成轨迹。假设车辆的速度为10m/s，并且有一个动态障碍物以10m/s的速度朝我们移动，EM规划器将按照一下步骤迭代生成路径和速度曲线。

原始规划（图15a）。在历史规划速度曲线中，即动态障碍物进入前，车辆以10m/s的恒定速度直线前进；
路径曲线的迭代（b）。在这一步中，速度曲线以10m/s的速度从原始曲线巡航。根据该速度推算，车子和动态障碍物将在s=40m处相撞，因此，避免该障碍物的最佳方法是在s=40m处从右侧躲避。
速度曲线迭代（c）。从步骤1开始，根据从右侧避开的路径曲线，原始根据其与障碍物的距离来调整期望速度。因此，如乘客所期望的一样，车辆速度将减至5m/s以较低速度通过障碍物。
路径曲线迭代2（d）。在新的速度曲线下，车辆不会在s=40m处通过障碍物，而是在s=30m处通过。因此应躲避障碍物的路径更新为一个新的路径，以使躲避的距离最大化为s=30。
速度曲线迭代2（e）。在新的路径曲线下，在s=30m处执行而不再需要在s=40m处进行减速。新的速度曲线显示，车辆可以在s=40处加速，在s=30处仍然能平稳通过。

因此，基于这四个步骤生成的最终轨迹整体过程是:在s=30处缓慢躲避障碍物，然后车辆通过障碍物再加速，这才是人类驾驶员在这种情况下可能的操作。
请注意，规划并不一定要始终执行四个步骤。它根据不同的场景可以采用更少或者更多的步骤。一般来说，环境越复杂，可能需要的步骤就越多。

5. COMPUTATIONAL PERFORMANCE

由于将三维状态横向速度问题分解为状态横向速度和状态速度两个二维问题，极大降低了EM Planner的计算复杂度。因而具有很高的规划效率。假设我们有nnn个障碍物、MMM条候选路径和NNN条候选速度曲线，则该算法的计算复杂度为O(n(M+N))O(n(M+N))O(n(M+N))。

6. CONCLUSION

EM Planner是一个基于轻决策的规划算法。与其他基于重决策的算法相比，Em规划器的优势在于能够在复杂的多障碍场景下执行。当基于重决策的方法试图预先确定如何处理每个障碍物时，困难是显而易见的：（1）很难理解和预测障碍物如何与主车相互作用，因此它们的跟随运动难以描述，很难用约束和规则来表征；（2）当多个障碍物阻塞道路时，无法找到满足所有预定决策的轨迹概率大大提高，从而导致规划的失败。

自动驾驶汽车的一个关键问题是对安全性和通过性的挑战，严格的规则增加了汽车的安全性，但降低了通过性，反之亦然。以换道为例，如果后面有车辆，只要有简单的规则，就可以很容易地暂停换道过程。这样可以保证安全，但大大降低了通过性。本文所描述的EM-planner，在解决潜在决策与规划不一致的同时，也提高了自主驾驶车辆的通过性。

EM计划器通过将3维的（SLT）问题转换为两个两维问题（SL和ST），大大降低了计算复杂度。它可以大大减少处理时间，从而提高了整个系统的交互能力。