EM planner 论文阅读

论文题目：Baidu Apollo EM Motion Planner

0 前言

EM和Lattice算法对比

EM planner	Lattice Planner
参数较多（DP/QP，Path/Speed）	参数少且统一化
流程复杂	流程简单
单周期解空间受限	简单场景解空间较大
能适应复杂场景	适合简单场景

1 摘要

基于百度Apollo平台提出的一种实时运动规划系统，该规划系统包括顶层的多车道和其中的单车道自动驾驶：
（1）系统顶层是一种多车道策略，通过并行计算车道级别轨迹来处理车道变更场景
（2）在车道轨迹生成器内部，通过迭代的方法解决基于Frenet坐标的路径规划和速度规划
（3）针对路径和速度规划，提出了动态规划DP与基于样条的二次规划QP相结合的方法，构造了一个可扩展且易于调整的框架，同时处理交通、障碍物决策和平滑度问题
该规划器可扩展至高速公路和城市低速驾驶场景

2 Introduction

（1）高精地图提供了任何人都可以访问的高精地图
（2）感知和定位模块提供了动态环境中必要的信息
（3）运动规划模块考虑生成一条安全平滑的轨迹再交由汽车的控制模块去执行
（4）运动规划以安全第一为原则，遵循各种规则
（5）对于规划的覆盖范围：至少提供8秒或者200米的运动规划轨迹，算法的反应时间必须在100ms（人是300ms反应时间）内做出反应
（6）必须设有安全应急模块，人为操控作为最后一道保障
（7）除了安全以外，乘客的体验感也很重要，舒适度主要取决于轨迹的平滑性

EM planner是Apollo面向L4的实时运动规划算法，该算法通过顶层多车道策略，选择出一条参考线，再根据这条参考线，在Frenet坐标系下，进行车道级的路径和速度规划，规划主要通过DP和基于样条的QP实现。EM planner充分考虑了无人车安全性、舒适性、可扩展性的需求，通过考虑交通规则、障碍物决策、轨迹平滑性等要求，可适应高速公路、低速城区的规划需求。通过Apollo仿真和在环测试，EM Planner算法体现了高度的可靠性，和低耗时性。

2.1 多车道策略

对于L4级别的无人驾驶，变道策略是必须的。常见的办法就是开发一种搜索算法,可以计算所有可能的车道上的代价函数，然后选择代价最低的一条轨迹。
该方法一些弊端：
（1）搜索空间因多车道而增大，导致计算量增大
（2）每个车道的交通规则不同，很难在同一框架下应用相同的算法的代价函数。
（3）应考虑避免突然变道的轨迹稳定性
遵循一致的道路驾驶规则是很重要的，以便其他驾驶员知道自动驾驶汽车的驾驶意图。

通常情况下，多车道策略应涵盖主动和被迫换道的情况。主动车道变换是routing模块因为要到达最终目的地而发出的换道请求。被动换道时因为当前车道被动态环境阻挡时汽车的自主行为。在主动和被动换道中，我们的目标是提供一个安全、平稳、成功率高的车道变换策略。因此提出了一个并行框架来处理主动和被动的环道。对于候选车道，所有障碍物和环境信息都投影到Frenet框架上。然后，将交通规则与车道级别的策略绑定。然后在此框架下，每个候选车道基于规划器都生成一条最佳轨迹。最后，进行多车道轨迹决策确定一个最佳车道。

2.2 路径-速度(Path-Speed)迭代算法

在运动规划中，最优性和效率都是很重要的。因此许多自动驾驶运动规划算法是在Frenet框架下的SLT图，并且借助于参考线降低规划的维度。在Frenet下找到最优的轨迹本质上是三维空间约束优化问题。因此有两种类型方法：直接三维空间优化方法和路径速度解耦法
（1）直接三维空间方法：尝试使用轨迹采样或Lattice（晶格）搜索在SLT中找到最佳轨迹。这种方法受到搜索复杂度的限制，搜索复杂度随着空间和时间搜索分辨率的增加而增加。为了满足时间消耗要求，必须在增加搜索网格大小或采样分辨率上做出折中。因此，所产生的轨迹是次优的。
（2）路径-速度解耦法是分别将路径规划和速度规划分开。路径规划通常考虑静态障碍物，然后，基于生成的路径产生速度曲线。当出现动态障碍物时，路径-速度分解方法可能不是最优的，但是，因为路径规划和速度规划是分离的，因此该方法在路径和速度上都有很大的灵活性。

EM planner迭代优化路径和速度，在路径规划器中，上一周期的速度曲线被用来估计和迎面来的低速动态障碍物的影响。然后，将生成的路径送到速度规划器以评估最佳速度曲线。对于高速动态障碍物，EMPlanner出于安全考虑，更倾向于变道躲避而不是接近(nudge)。

EM planner 迭代策略可以帮助解决路径-速度分解框架下的动态障碍

2.3 决策和道路规则

在EM Planner中，决策和交通规则是两种不同的约束
交通规则是硬约束，不能被改变，一些障碍物属于软约束，可以协调。对于决策模块来说，一些决策方法考虑的是数值上的最优解，也有同时进行规划和决策。
在百度Apollo EM planner中，先做决策再做规划。决策过程意在于明确道路并缩小轨迹规划的搜索空间来寻找最优的轨迹。许多包含决策的规划器尝试让汽车自我决策生成相应的状态。这些方法进一步被分为手写决策和基于模型的决策。手写方式有局限性，不够灵活。基于模型的方法将汽车状态离散，并使用数据驱动方式来调整模型。

针对L4级别的自动驾驶，决策模块应具有可扩展性和可行性。可扩展是场景表达能力。在多障碍时，很难通过有限的自车状态集准确的描述决策行为。出于可行性考虑，意思是所生成的决策应包括一个可行的区域，自车可以在该区域内进行动态限制。但是，手写决策和基于模型的决策都不会产生无障碍轨迹来验证可行性。

在EM Planner的决策步骤中，以不同的方式描述行为。
（1）通过一个粗略可行的轨迹来描述汽车的移动意图。这条轨迹也可以用来估计与障碍物之间的交互，即便情景变得更加复杂，这种基于可行的轨迹的决策也是可扩展的。
（2）规划器还将基于轨迹生成凸的可行空间，用来平滑样条曲线参数。基于二次规划QP的平滑样条曲线求解器可用于生成遵循该决策的更平滑的路径和速度曲线。这保证了可行且平滑的解决方案。

3 多车道策略的EM Planner框架

Apollo EM Planner的整体架构

在顶层的数据中心，所有来源的信息都被收集和同步。
（1）采集数据后，参考线生成器会生成一系列携带交通规则信息和障碍物信息的参考线。这个过程要基于从Routing模块的导航信息和HD地图。
（2）在运动规划中，先构造Frenet框架，在框架中构建车辆与周围环境的关系。
（3）将构建的关系传递给规划器。规划器模块执行速度和路径规划。
（4）在路径规划过程中，周围环境的信息被投影到Frenet框架中（E-step）。然后基于投影的信息产生一条平滑的路径(M-step)。
（5）速度规划也是如此。速度优化过程中，一旦路径优化器生成了一条平滑的路径，障碍物就会投影到ST图中（E-step），随后，速度优化器将生成平滑的速度曲线（M-step）
（6）结合路径和速度曲线，我们将获得指定车道的平滑轨迹。所有车道级别的最佳轨迹都将发送到参考线轨迹决策器。根据当前的汽车状态，法规和成本，最后将路径和速度结合获得一条平滑的轨迹。最后一步将所有得到的轨迹发送到参考线决策器，根据当前汽车的状态、法规和每条轨迹的代价，轨迹决策器将选择最优的轨迹交给汽车执行

4 车道级别的EM Planner

车道级别优化，如图所示车道级别规划的路径-速度EM迭代。迭代过程包括两个E-step和两个M-step，轨迹信息将在规划周期之间迭代。
加粗样式
（1）在第一个E-steps中，动态和静态障碍物都被投影到车道Frenet坐标系下。静态障碍物会直接从笛卡尔坐标系转换到Frenet坐标系，而动态障碍物的信息则以其运动轨迹来描述。在Apollo框架中，可以预测动态障碍物的移动轨迹。通过上一帧的预测信息，和自车的运动信息，可以估算自车和动态障碍物在每个时间点的交互情况，轨迹重叠的部分会被映射到Frenet坐标系中。另外，动态障碍物的出现会最终导致自车做出避让的决策。因此，出于安全考虑，SL投影只考虑低速和来向障碍物，而对于高速的动态障碍物，EM Planner的平行变道策略会考虑这种情景。
（2）在第二个E-steps中，基于生成的路径在ST坐标系下，所有障碍物（包括高速、低速和即将到来的障碍物）都将被估计，生成速度曲线。如果障碍物轨迹和已经规划的路径重叠，那么将在ST坐标系下对应区域将会再重新生成。
（3）在两个M-steps中，通过动态规划和二次规划生成路径和速度曲线。尽管我们将障碍物投影到SL和ST坐标系下，但是最优的路径和速度解仍在非凸空间中。因此，先使用动态规划获得一个粗略解，同时，这个解可以提供避让、减速、超车等障碍物决策。通过这个粗略的解，可以构建一个凸包，然后使用基于二次规划的样条优化器来求解。

---

（1） E-step SL和ST投影

SL投影

SL投影是基于类似于G2平滑参考线（曲率导数连续）。在笛卡尔空间中，将障碍物和车的状态以及曲率和曲率的导数转换到Frenet坐标系中。由于静态障碍物的位置不会随着时间改变，因此转换很简单。对于转换动态障碍物，我们需要借助上一个周期的轨迹来实现。把上一个周期的运动轨迹被投影到Frenet坐标系上，以提取状态方向的速度曲线。给定确定的时间就会估计出汽车状态的坐标。估计的汽车状态坐标将会帮助估计动态障碍物的影响。一旦汽车状态坐标同时与障碍物轨迹发生交叉时，SL地图上的阴影区域将被标记。在此，交叉区域被定义为汽车与障碍物的重叠区域。

如图所示，从预测模块估计的迎面而来的动态障碍物和相应的轨迹被标记为红色，小车标记为蓝色。首先将迎面而来的动态障碍物的轨迹随时间离散成若干个轨迹点，然后将这些轨迹点投影到Frenet坐标上。一旦我们发现自车的位置坐标与投射的障碍点有交互作用，重叠区域（图中紫色部分）将在Frenet坐标中标记出来。

ST投影

ST投影可以帮助我们评估自车的速度曲线。当路径优化器在Frenet坐标中生成光滑的路径曲线后，与自车有交互的静态障碍物及动态障碍物的轨迹都将被投影在路径上。同理，这种交互也定义为包围盒的重叠。

如图，红色区域表示在2s处距离自车40m远切入规划路径的动态障碍物ST信息，绿色表示在自车后的动态障碍物ST信息，M-step将会在剩下的区域找到可行光滑的最优解。

（2） M-Step DP Path

M-step求解Frenet坐标系下的最优路径规划，实际上在一个非凸的区间（从左和从右避让是两个局部最优情景）就是找到一个最优的$l=f(s)$方程。主要包括两步：基于动态规划的路径决策和基于样条的路径规划。
基于动态规划的路径步骤提供一条粗略的路径信息，可以带来可行通道和避障决策。如下图所示，包括Lattice采样，代价函数，动态规划搜索

（1）Lattice采样
Lattice采样器基于Frenet坐标系，如下图，首先在车辆之前对多行Lattice采样。不同行之间的点通过五次多项式来平滑连接。行之间的间隔取决于速度，道路结构，变换车道等。该框架允许根据场景自定义采样策略。

比如，车道变换的采样间隔可能比当前车道行驶更长。此外，出于安全考虑，路径总长可以达到200m或者覆盖8s的行驶长度
（2）代价函数
构造完Lattice，通过代价函数的总和来评估每一条路径。使用SL投影得到的信息结合交通规则、汽车动力学模型去构建函数。总的代价是平滑度、避障和车道代价函数的线性组合。
每段 Lattice (晶格) 路径的代价通过光滑程度、障碍物避让、车道代价来评价：

光滑程度smooth用以下方程来衡量

一阶导表示朝向偏差，二阶导表示曲率，三阶导表示曲率导数
障碍物的代价由以下方程给出，方程中的d由自车bounding box到障碍物bounding box的距离表示。

$C_{nudge}$定义为单调递减函数，处于安全考虑，$d_c$设置保留缓冲区，可根据情况微调$d_n$范围，$C_{collision}$是碰撞代价函数。
车道代价函数包括两个部分：参考线代价和车道代价。参考线定义为周围没有障碍物时的理想路径，通常这条线提取的是道路中心线。车道代价函数通常被道路边界决定，道路外的路径点将受到很高代价的惩罚：

最终总的路径代价函数包含平滑度、障碍和车道代价。然后边缘代价会用于通过动态规划去选择最低成本的候选路径，候选路径也将确定障碍物决策。

（3） 样条QP路径（M-Step）

样条QP路径步骤是对动态规划路径的改进，在动态规划路径中，会根据所选的路径生成可行的走廊。然后基于样条QP在该走廊生成一条平滑路径，如下图所示：

通过QP求解器求解具有线性约束的目标函数生成最优的QP路径，如下图所示：

QP路径的目标代价函数是平滑度代价和引导线代价的线性组合。该步骤中的引导线是动态规划挑选出来的路径。引导线提供了避开障碍物的估计值。在数学上QP路径步骤优化以下函数：

$g(s)$是动态规划的结果。$f^{{}^{\prime }}(s),f^{{}^{\prime \prime }}(s),f^{{}^{\prime }{‘}^{\prime }}(s)$分别表示朝向、曲率和曲率的导数。该目标函数描述了避开障碍物和平滑度之间的平衡

QP路径中的限制条件包含了边界约束条件和动力学可行性。这些约束都会施加在每个s ss处。为了提取边界约束，提取各状态点的可行区间。在EM Planner中，车辆被看成自行车模型。因此仅仅提供$l=f(s)$的范围是不够的，因为车的方向也很重要
如下图，为了保持边界约束的凸包性和线性，在汽车的前后两端分别添加两个半圆，其中前轮到后轮的中心距离为$l_f$和车辆宽度为$w$

车的左前角的横向位置表示：

$\theta$是汽车和道路状态方向之间的航向角，可用下面的不等式进一步近似的线性约束：

类似地，线性优化剩下的三个角度。线性约束足够好是因为$\theta$通常很小($\theta <\pi /12$)。

$f^{\prime \prime }(s)$和$f^{\prime {\prime }^{\prime }}(s)$的区间限制可表示为动力学约束，因为曲率与曲率导数有关，除了边界约束外，生成的路径还可以匹配汽车的横向初始位置和导数$(f(s_0),f^{\prime }(s_0),f^{\prime \prime }(s_0))$。由于所有约束相对于样条曲线参数都是线性的，因此可以使用二次规划求解器来快速解决问题。

（4） DP速度优化器（M-Step）

M-Step速度规划器在ST图里面生成一个速度曲线，该速度曲线表示为相对于时间$S(t)$的状态函数。与路径规划器相似，在ST图上寻找最优速度曲线也是一个非凸问题，所以采用DP与QP相结合的方法在ST图上寻找一个光滑的速度曲线。
如下图所示，在QP速度时采用DP生成的速度曲线作为引导线，利用可行走廊生成凸域。

如下图，DP速度规划步骤包括代价函数，离散ST图网格和DP搜索。生成分段线性速度曲线、可行走廊、障碍物速度决策。

具体步骤，障碍物信息首先在ST图上离散成网格。将$(t_0,t_1,...,t_n)$表示为在时间轴上具有间隔$dt$的等距评估点。分段线性速度曲线在网格上表示为$S=(s_0,s_1,...,s_n)$。此外，利用有限差分法对导数进行逼近。

目标是在ST图中优化一个带有约束的代价函数。准确说，DP速度优化器的代价：

第一项是速度保持代价，表示当没有障碍物或者没有红绿灯时，车辆应按照指定的速度行驶。$V_{ref}$描述的是参考速度，参考速度由道路速度限制、曲率和其他交通规则来确定。g函数被用来对小于或者大于$V_{ref}$值施加不同的惩罚。最后一个$C_{obs}$描述了总障碍物代价，对车辆到所有的障碍物距离进行评估，确定总障碍物代价。

动态规划搜索空间必须在动力学约束范围内，包括加速度、加加速度$jerk$和单调性约束。因为我们要求生成的轨迹在道路上不执行倒车的操作，只能在泊车或者其他指定情况下执行倒车。搜索算法是向前的，基于车辆动态约束的一些必要修剪也被应用以加速该过程。

（5） QP速度优化器（M-Step）

由于分段线性速度曲线不能满足动力学要求，需要用QP来解决这个问题。如下图，样条QP速度优化器包括三个部分：代价函数、线性约束条件和样条QP求解器

代价函数这样表示：

第一项是DP速度参考线$S_{ref}$和最后需要生成$S$之间的距离。加速度和$jerk$是速度曲线平滑度。因此，目标函数是为了平衡引导线和平滑度。

QP速度优化器的限制条件包括了边界限制、初始速度和加速度匹配的约束：

第一个约束是单调性，第二、第三和第四、第五个约束是来自交通规则和车辆动力学约束的要求。在完成代价函数和约束后，QP求解器将生成一条平滑可行的速度曲线，如下图所示结合路径曲线，EM规划器将给控制模块生成一条平滑的轨迹。

（6）求解二次规划问题笔记

出于安全的考虑，我们评估了大概100个不同位置或时间点的路径和速度。约束的数量超过600。对于选择所需的路径和速度优化器，发现五项多项式是比较合适的。样条曲线包括3到5个多项式，大约有30个参数。因此，二次规划问题有相对较小的目标函数，但约束条件较多。因此，主动设置QP求解器有助于解决该类问题。除了加速二次规划求解外，我们还将上一个循环的计算结果作为hot start（热启动）。QP问题平均可以在3ms内解决，满足了我们对时间消耗的需求。

（7）求解DP和QP非凸问题笔记

DP和QP在非凸领域都有各自的局限性。DP和QP的结合将充分利用两者的优点，以达到最理想的解决方案。

1. DP：算法依赖于一个采样步骤来生成候选解。由于处理时间的限制，采样网格限制了候选样本的数量。有限网格内的优化会得到一个粗糙的DP解。换句话说，DP不一定能在所有的场景下都取得最优解。例如，DP求出从左侧避开障碍物的路径，但不能选择具有最佳距离的路径。
2. QP：相反，QP是基于凸域生成解。如果没有DP步骤的帮助，它是不可用的。例如，如果一个障碍物在车辆前面，QP需要一个决策，例如从左变道，从右变道，跟随或者超车，来生成约束。随机或者基于规则的决策很容易使QP陷入失败或者局部极小值。
3. DP+QP：DP+QP算法将两者的局限性降到最低：（1）EM规划器首先用DP在网格内搜索，以求得粗略的解（2）DP的结果被用于生成凸域并引导QP（3）QP被用于在凸域内寻找全局最优解。

5 Case Study

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

如图所示，EM规划器如何在规划周期和迭代周期之间迭代以获得最佳轨迹。

当障碍物进入到规划的路径如何生成轨迹。假设车辆的速度为10m/s，并且有一个动态障碍物以10m/s的速度朝我们移动，EM规划器将按照以下步骤迭代生成路径和速度曲线。

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

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

6 计算性能

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

7 结论

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

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

EM planner通过将三维规划问题转换为两个二维规划问题，大大降低了计算复杂度。大大减少了处理时间，从而提升整个系统的交互能力。