论文-Baidu Apollo EM Planner
摘要
规划系统包括顶层的多车道和其中的单车道自动驾驶：
- 系统顶层是一种多车道策略，通过并行计算车道级别轨迹来处理车道变更场景；
- 在车道轨迹生成器内部，迭代的解决基于Frenet框架的路径和速度规划；
- 针对路径和速度规划问题，提出了动态规划与基于样条的二次规划相结合的方法，构造了一个可扩展且易于调整的框架，同时处理交通规则、障碍物决策和平滑度问题。
一. 介绍
- 开发了Apollo平台； HD地图提供高精度地图;
- 预测和定位模块被用于定位和感知，更用于预测轨迹
- 运动规划模块考虑所有信息，生成安全而且平滑路径给到控制部分去执行；
- 运动规划安全第一为原则遵循各种规则
- 对于规划的覆盖范围:至少提供8秒或者200米的运动规划轨迹;算法反应时间:必须在100毫米（人是300ms反应时间）内做出反应
- 必须设有安全应急模块； 除了安全乘客体验度也很重要，舒适度主要取决于轨迹的平滑性；
该论文介绍了基于EM迭代算法的规划器：
A 多车道策略
- 对于L4级别的自动驾驶，变道是必须的。一种常见的方法是开发一种搜索算法,可以计算所有可能的车道上的代价函数，然后选择代价最低的一条轨迹。但是该方法存在一些困难。首先，搜索空间大，导致计算量大。其次，每个车道的交通规则不同，很难在同一框架下应用相同的算法的代价函数。此外，避免突然变道的轨迹稳定性也应该被考虑进来.遵循一致的道路驾驶规则是很重要的，以便其他驾驶员知道自动驾驶汽车的驾驶意图。
- 通常情况下，多车道策略应涵盖主动和被迫换道的情况。主动车道变换是routing模块因为要到达最终目的地而发出的换道请求。被动换道时因为当前车道被动态环境阻挡时汽车的自动行为。在主动和被动换道中，我们的目标是提供一个安全、平稳、成功率高的车道变换策略。因此提出了一个并行框架来处理主动和被动的换道。对于候选车道，所有障碍物和环境信息都投影到Frenet框架上。然后，将交通规则与车道级别的策略绑定.然后在此框架下，每个候选车道基于规划器都生成一条最佳轨迹。最后，进行多车道轨迹决策确定一个最佳车道。
B 路径-速度(Path-Speed)迭代算法
- 在运动规划中，最优性和效率都是很重要的。因此，许多自动驾驶运动规划算法在Frenet框架下,都是用时间并且借助于reference来降低规划维数。在Frenet框架下找到最优的轨迹本质上是3维空间约束优化问题。因此有两种类型方法：直接3维空间优化方法和路径速度解耦方法。
- 直接3维空间方法尝试使用轨迹采样或lattice search在SLT中找到最佳轨迹。这些方法受到其搜索复杂度的限制，搜索复杂度随着空间和时间搜索分辨率的增加而增加。为了满足时间消耗要求，必须在增加搜索网格大小或采样分辨率上做出折衷.因此，所产生的轨迹是次优的。
- 相反，路径速度解耦方法分别规划路径和速度.路径规划通常考虑静态障碍。然后，基于生成的路径[6]产生速度曲线。但当出现动态障碍物时，路径速度方法可能不是最优的。但是，由于路径规划和速度规划是分离的，因此此方法在路径和速度规划上都具有很大的灵活性。
- EM规划器迭代的规划路径和速度。在路径规划器中,上一个周期的速度曲线被用估计和迎面来的低速动态障碍物的影响。然后，将生成的路径送到速度规划器以评估最佳速度曲线。对于高速动态障碍物，EM规划器出于安全考虑，更倾向于换道躲避而不是接近。
因此，EM规划器的迭代策略可以帮助解决路径速度解耦框架下的动态障碍。
C 决策和道路规则
- 在EM规划器中，决策和交通规则是两个不同的约束。交通规则是硬约束，不可改变，一些障碍是软约束，可协调。对于决策模块，一些人直接用数值优化进行决策。在EM规划器中，先做决策再给做规划。决策过程意在明确道路意图并缩小轨迹规划的搜索空间。许多包含决策的规划器尝试汽车自己决策。这些方法进一步被分为手写决策和基于模型的决策。手写方式有局限性，不够灵活。基于模型的方法将汽车状态离散，并使用数据驱动的方法来调整模型。
- 针对L4级别的自动驾驶，决策模块应具有可扩展性和可行性。可扩展性是场景表达能力。在多障碍时，很难通过有限的自我汽车状态集准确的描述决策行为。出于可行性考虑，我们的意思是所生成的决策应包括一个可行的区域，自我汽车可以在该区域内进行动态限制。但是，手写方式和基于模型的决策都不会产生无障碍轨迹来验证可行性。
- 在EM规划器的决策步骤中，我们以不同的方式描述行为。首先，通过一个粗略可行的轨迹来描述自我汽车的移动意图。然后，使用该轨迹来测量障碍物。即使情景变得更加复杂，这种可行的基于轨迹的决策也是可扩展的。其次，规划器还将基于轨迹生成凸的可行空间，以平滑样条曲线参数。基于二次规划的平滑样条曲线求解器可用于生成遵循该决策的更平滑的路径和速度曲线。这保证了可行且平滑的解决方案。
本文的其余部分安排如下。在第二部分中，我们介绍了EM规划器内部的多车道框架。在第三部分中，我们将重点放在车道级路径规划上，并逐步讨论EM迭代。在第四节中，我们提供了一个有关迎面到来的规则证明的的例子，以进行简单的演示。第五部分讨论EM计划程序的性能。在第六节中，我们完成了讨论并为EM规划器提供了结论。
二. 带有多车道策略的EM规划器框架
首先介绍EM规划器的结构，然后重点介绍车道规划器的结构。
- 在顶层的数据中心,所有来源的信息都被收集和同步.采集数据后，参考线生成器会生成一些带有交通规则和障碍物信息的参考线。这个过程要基于从Roting模块的导航信息和HD地图。在运动规划中，先构造Frenet框架。在框架中构建车辆与周围环境的关系。再将构建的关系传递给规划器。规划器模块执行速度和路径规划。在路径规划过程中，周围环境的信息被投影到Frenet框架中的E步中。然后基于投影的信息产生一条平滑的路径。速度规划类似;最后将路径和速度结合将获得一条平滑的轨迹。最后一步中将所有得到的轨迹发送到轨迹决策器，汽车根据自身状态选择最优轨迹去执行；
三 车道级EM规划器
-
在本节中，我们将讨论车道级别优化问题。图3示出了车道平面规划内的路径速度EM迭代。迭代过程包括两个E步和两个M步。轨迹信息将在规划周期之间迭代。
-
在第一个E-steps中动态和静态障碍物都被投影到Frenet框架。静态障碍物将基于笛卡尔-Frenet框架变换直接投影。在Apollo框架中，可以预测动态障碍物的移动轨迹。考虑到之前的周期规划轨迹，我们可以在每个时间点估计出动态障碍物和汽车的位置。然后动态障碍物和汽车在某个时间点的重叠可以被映射到Frenet框架中。另外，路径优化过程中动态障碍物的出现最终对导致轻推。因此，出于安全考虑，动态障碍物的SL投影将仅考虑低速交通和即将到来的障碍物。对于高速交通，EM规划器的并行车道变换策略将涵盖该场景。
-
在第二个E-steps中，基于生成的路径在s-t坐标系下，所有障碍物包括高速、低速和即将到来的障碍物都将被估计,生成速度曲线。如果障碍物轨迹和已经规划的路径重叠，将在s-t坐标系下生成一个相应的区域。
-
在两个M-steps中，通过动态规划和二次规划生成路径和速度曲线。尽管我们将障碍物投影到SL和ST坐标系下，但是最优的路径和速度解仍在非凸空间中。因此，我们使用动态规划先获得一个粗略解，同时，这个解可以提供轻推、产量和超车等障碍决策。我们是用粗略解为基于二次规划的样条优化器来确定凸包。然后，优化器可以在凸包内找到解。
A.SL and ST Mapping(E-steps)
- SL投影基于G2（曲率连续）平滑参考线。在笛卡尔空间中，障碍物和车的状态用（x,y,θ）以及曲率和曲率的导数描述。然后将他们映射到Frenet坐标系中，他们代表现在状态，横向和横向导数。由于静态障碍物的位置不会随着时间改变，因此映射很简单。对于动态障碍物，我们借助上一个周期的轨迹来mapped碍物。上一个周期的运动轨迹被投影到Frenet坐标系上，以提取状态方向的速度曲线。给定确定时间就会估计出汽车状态的坐标。估计的汽车状态坐标将会帮助估计动态障碍物的影响。一旦汽车状态坐标同时与障碍物轨迹发生交叉时，SL地图上的阴影区域将被标记。在此，交叉区域被定义为汽车与障碍物的重叠。例如，如图4所示，从预测模块估计的迎面而来的动态障碍物和相应的轨迹被标记为红色。小车是蓝色的。首先将迎面而来的动态障碍物的轨迹随时间离散成若干个轨迹点，然后将这些轨迹点投影到Frenet框架上。一旦我们发现ego车的位置坐标与投射的障碍点有交互作用，重叠区域（图中紫色部分）将在Frenet框架中标记出来。
- ST投影有助于帮助我们评估汽车的速度曲线。路径生成器在Frenet框架中产生一条平滑路径后，如果存在影响，则静态和动态障碍物都被投影到路径上。交互也被定义为边界框重叠。在图5中，一个障碍物在前方2秒40米处切入车辆驾驶路径，用红色标记，汽车后面的一个障碍物用绿色标记。其余区域为速度剖面可行区域。速度优化M步将试图在该区域找到一个可行的光滑解。
B.M-steps:DP-Path
M-steps路径规划器在Frenet框架中规划路径。即找到横坐标的最优函数l=f(s).w.r.t（非凸SL空间中的状态坐标）。因此路径优化器包括两步：基于动态规划的路径决策和基于样条的路径规划。动态规划路径步骤提供了具有可行性车道和障碍物轻推角的大致路径。如图所示，该步骤包括采样器，代价函数，和动态规划搜索。
栅格采样器基于Frenet框架。在图中，首先在车辆之前对多行栅格进行采样。不同行之间的点通过五次多项式平滑连接。行之间的间隔距离取决于速度，道路结构，车道变换等。该框架允许根据场景自定义采样策略。例如，车道变换的采样间隔可能比当前车道行驶更长。此外，出于安全考虑，栅格所有状态距离赢至少为8秒或者200米。
构造完栅格后，通过代价函数的总和来评估每一个图的边缘。我们使用从SL投影得到的信息交通规则和汽车动力学去构建函数。总的边缘成本是函数平滑度，避障和车道成本函数的线性组合。
给定路径的平滑度函数可以通过以下方式表示：
在平滑代价函数中，f’（s）代表车道heading，f’’(s)是与路径的曲率有关，f’’’(s)是与车的曲率函数有关。通过多项式的形式，上述成本也可以通过分析来评估。
障碍物的代价函数是按照一系列固定状态坐标与所有障碍物评估的。障碍物代价函数是基于障碍物与车的边界距离。记为d。代价函数形式如下：
这里Cnudge定义为单调递减函数。出于安全考虑，dc设置保留缓冲区。微调范围dn可根据情况改变。Ccollision是碰撞代价函数，可帮助监测不可行的路径。
**车道代价函数包括两个部分：引导线代价和路上代价。**引导线定义为没有周围障碍物时的理想路径。通常这条线提取为路径的中心线。将引导线函数定义为g(s).
路上代价函数通常被道路边界决定。道路外的路径点将受到很高代价的惩罚；
最终路径成本是所有平滑代价，障碍物代价和车道代价的总和。然后边缘代价用于通过动态规划选择最低成本的候选路径。候选路径还将确定障碍物决策。
C.M-steps:QP-Path
QP路径步骤是对动态规划路径的改进。在动态规划路径中，根据所选的路径生成可行的隧道。然后基于QP步骤将在该隧道中生成一条平滑路径，如图所示：
通过QP求解器求解具有线性约束的目标函数生成最优的QP路径。
QP路径的目标代价函数是平滑度代价和引导线代价的线性组合。该步骤中的引导线是DP路径。引导线给出了绕过障碍物的估计值。在数学上QP路径步骤优化以下函数：
这里g(s)是DP路径结果。f’,f’’,f’’‘分别和方向，曲率，曲率导数有关。目标函数描述了平滑性和绕过障碍物的平衡。
QP路径中的限制条件包含了边界限制条件和动力学可行性。这些限制被应用于f,f’,f’‘在一系列状态坐标。为了提取边界约束，提取各状态点的可行区间。这个可行区间在每一个点被描述为Ll,i<=Lh,i。在EM规划器中，车辆是被看成自行车模型。因此仅仅提供l=f(s)的范围是不够的，因为车的方向也很重要。
在图10中，为了保持边界约束的凸性和线性，我们在汽车的前端和后端分别添加两个半圆。来表示前轮到后轮的中心距离为Lf和车辆宽度为w。然后左前角的横向位置由以下表示：
这里seita是汽车和道路状态方向之间的航向角。可用以下不等式近似进一步线性约束：
类似的线性优化被应用于剩余的三个角度。线性约束足够好是因为seita通常很小。当seita
D.M-Steps DP-Speed
速度规划器在ST里面生成一个速度曲线，该速度曲线表示为相对于时间S(t)的状态函数。与路径规划器相似，在ST图上寻找最优速度曲线也是一个非凸问题，所以采用DP与QP相结合的方法在ST图上寻找一个光滑的速度曲线。在图12中，DP速度规划步骤包括代价函数，ST图网格和DP搜索。生成分段性速度曲线，可行的隧道和障碍物速度决策。如图11所示。在QP速度时采用DP生成的速度曲线作为引导线，利用可行隧道生成凸域。
具体来说，障碍物信息首先在ST图上离散成网格。将（t0,t1…tn)表示为在时间轴上具有间隔dt的等距评估点。分段性速度曲线函数在网格上表示为S=（s0,s1,…sn）。此外，利用有限差分方法对导数进行了逼近。
目标是在ST图中优化一个带有约束的代价函数。确切的说，DP速度优化器的代价被以下表示：
第一项是速度保持代价。表示当没有障碍物或者红绿灯时，车辆应遵守指定的速度。Verf描述参考速度，参考速度由道路速度限制、曲率和其他交通规则确定。g函数被设计为对小或者大于Verf的值具有不同的惩罚。加速度a和加加速度jerk描述了速度曲线的平滑度。最后一个属于Cobs描述了总障碍成本。对车辆到所有的障碍物的距离进行评估，已确定总障碍物成本。
动态规划搜索空间必须在动力学约束范围内。动态约束包括加速度、加加速度jerk和单调性约束。因为我们要求生成的轨迹在道路上不执行倒车操作，只能在停车或者其他指定情况下直行倒车。搜索算法简单明了，并应用了一些基于车辆动态约束的必要剪枝来加速搜索过程。
E.M-steps QP速度优化器
由于分段线性速度曲线不能满足动力学要求，需要用QP来解决这个问题。在图13中，样条速度包括三个部分：代价函数、线性约束条件和样条QP求解器。
代价函数被以下函数描述：
第一项是DP速度参考线Sref和最后需要生成的S之间的距离。加速度和jerk是速度曲线平滑度。因此，目标函数是跟踪引导线和平滑度之间的平衡。
QP优化器是带有线性约束的。QP速度优化器的限制条件包括一下边界限制：
第一个约束是单调性，即不倒车。第二第三和第四 五个约束是来自交通规则和车辆动力学约束的要求。在完成代价函数和约束后，QP求解器将生成一个平滑可行的速度曲线，如图14结合路径曲线，EM规划器将为控制模块生成一个平滑的轨迹。
F.解二次规划问题的笔记
出于安全考虑，我们评估了大概100个不同位置或时间点的路径和速度。约束的数量大于600.对于路径和速度优化，我们发现五项多项式足够好用。样条曲线包括3到5个多项式，大约有30个参数。因此，二次规划问题有相对较小的目标函数，但约束条件较多。因此，主动集QP求解器有助于解决该问题。除了加速二次规划求解外，我们还将上一个循环的计算结果作为热hot start（牛顿法）。QP问题平均可以在3ms内解决，满足了我们对时间消耗需求。
G.关于DP和QP非凸优化的笔记
DP和QP在非凸领域都有局限性。DP和QP的结合将充分利用两点的优点，以达到理想的解决方案。
- DP：如本文前边所述，DP算法依赖于一个采样步骤来生成候选解。由于处理时间的限制，采样网格限制了候选样本的数量。有限网格内的优化会得到一个粗糙的DP解。换言之，DP不一定，而且在几乎所有的情况下，都不会提供最佳解决方案。例如，DP可以从左侧轻推障碍物的路径。但不能选择具有最佳距离的路径。
- QP：相反，QP基于凸域生成解。如果没有DP步骤的帮助，它是不可用的。例如，如果一个障碍物在车辆前面，QP需要一个决策，例如从左侧变道，从右侧变道，跟随或者超车，以生成其约束。随机或基于规则的决策将使QP容易陷入失败或者局部极小值。
- DP+QP：DP+QP算法将两者的局限性降到最低：（1）EM规划器首先使用DP在网格内搜索，以求得粗略的解；（2）DP结果被用于生成凸域并引导QP；（3）QP被用于在凸域内寻找全局最优解。
四 案例学习
如上所述，尽管大多数最先进的规划算法都是基于重决策的，但EM planner是一个基于轻决策的规划器。确实，基于重决策的算法，或者依赖规则的算法，很容易理解和解释。缺点也很明显：它可能被困在角落的情况下(陷入局部最优解的频率与限制条件的复杂性和数量密切关),并不总是最优的。在本节中，我们将通过几个案例来说明基于轻决策的规划算法的优点。这些案例都是在百度很多的重决策规划模块中的日常测试中暴露出来的，并最后由轻决策解决。
图15是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处缓慢推动障碍物，然后主车通过障碍物后加速，这很可能是人类驾驶员在这种情况下的表现。
请注意，创建计划并不一定要始终执行四个步骤。他可以采用更少或者更多的步骤，这取决于场景。一般来说，环境越复杂，可能需要的步骤越多。
五 计算性能
由于将三维状态横向速度问题分解为状态横向速度和状态速度两个二维问题，极大降低了EM规划器的计算复杂度。因而具有很高的规划效率。加入我们有n个障碍物，有M个候选路径和N个候选速度曲线，该算法的复杂度为O(n(M+N))。
六 结论
EM规划器是一个基于轻决策的规划算法。与其他局域重决策的算法相比，Em规划器的优势在于能够在复杂的多障碍场景下执行。当局域重决策的方法试图预先确定如何处理每个障碍物时，困难是显著的：（1）很难理解和预测障碍物如何与主车相互作用，因此他们的跟随运动很难描述，因此很难被任何规则考虑；（2）当多个障碍物阻塞道路时，无法找到满足所有预定决策的轨迹的概率大大降低，从而导致规划失败。
自动驾驶汽车的一个关键问题是安全性和通过性的挑战，严格的规则增加了汽车的安全性，但降低了通过性，反之亦然。以换道为例，如果后面有车辆，只要有简单的规则，就可以很容易地暂停换道过程。这样可以保证安全，但大大降低了通过性。本文所描述的EM-planner，在解决潜在决策与规划不一致的同时，也提高了自主驾驶车辆的通过性。
EM-planner通过将三维状态横向速度问题转化为二维状态横向/状态速度问题，显著降低了计算复杂度。它可以大大缩短处理时间，从而提高整个系统的交互能力。
附录
约束光滑样条与二次规划
样条曲线QP路径和速度优化器使用相同的约束平滑样条曲线框架。在这一节中，我们在二次规划框架和线性化约束下，将光滑样条问题形式化。具有二次凸目标和线性约束的优化问题可以快速、稳定地求解。
在二次规划的条件下，在给定二次目标函数和线性约束条件下，对三阶光滑函数f（x）进行优化。域（A；b）；A=x0；b=xn上的光滑样条函数f（x）表示为分段多项式基的线性组合。表示x0=a；x1；x2；：：；xn=b作为结。然后，f（x）由以下公式给出：
其中fk（x）=pk0+pk1x+：：pkmxm是多项式函数。fk的系数定义为pk=（pk0；pk1；：：；pkm）T。平滑样条曲线参数向量定义为p=（pT0；p T 1；：：：；pTn≤1）T。在数学上，样条曲线解算器试图找到一个函数^f，该函数优化再生核Hilbert空间（RKHS）域上函数的线性组合。域空间描述为
样条QP问题由目标函数，线性化约束和二次规划求解器形成。它描述如下：
L（f（x））<=0。目标函数P（f）是四个函数的线性组合，P i；i=0；1；2；3 on。明确地，
其中g是预先指定的准则函数。第一个表示到引导线g的距离，而其余三个表示f的平滑度。约束函数L（f（x））包括基于问题设置的以下类型：