Apollo规划模块：Em Planner算法原理解析（论文研读）

先来介绍下整体的Pipeline。Apollo开源自动驾驶平台中，高清地图模块提供了每个在线模块都可以访问的高清地图。感知和定位模块提供了必要的动态环境信息，可以在预测模块中进一步用于预测未来的环境状态。运动规划模块考虑所有信息，以生成安全平滑的轨迹，并将其输入车辆控制模块。 EM planner是基于百度Apollo（开源）的实时运动规划系统。旨在解决工业4级运动规划问题。该规划器以多车道、路径速度迭代、交通规则和决策组合设计的安全性和乘坐体验为目标。

系统分层覆盖了多车道和单车道自动驾驶；（1）系统的顶层是一种多车道策略，通过比较并行计算的车道级轨迹来处理车道变换场景。 （2）在车道级轨迹生成器中，它基于Frenet框架迭代求解路径和速度优化问题.（3）对于路径和速度优化，将动态规划DP和基于样条曲线QP的二次规划相结合，构建了一个可扩展且易于调整的框架，处理交通规则、障碍决策和平滑度。 Apollo 上的 EM planner 对 Path-Speed 进行迭代优化。首先生成一条 Optimal Path, 在最优路径的基础上生成 Optimal Speed Profile 。在下一个迭代周期，在优化后的 Speed 的基础上，进一步优化 Path，依次类推, 分四步走，两步 E step 和 M step 。这种算法的缺点是不一定能收敛到全局最优解。 ps: 自动驾驶目的是为了减少交通事故和提高交通效率。更安全、更高效。

通常的变道策略：划分栅格，找出所有可能的路线并设计一个cost function来进行衡量，挑选出cost值最小的那条。SLT三维优化方法. 不足： a、搜索空间太大，非常耗算力。 b、每条lane上的交规可能是不同的，无法做到分别考虑。 c、利用cost function的方式，可能会导致轨迹规划的轨迹稳定性不够，如果cost function设计的不好，前后帧可能会产生跳变。

并行框架处理被动换道（路由模块为了达到目的地）和非被动换道（动态障碍物阻挡），对于候选车道，所有障碍物和环境信息都投影在基于车道的Frenet框架上。车道水平下的策略受交通规则约束。每个候选车道将根据车道级优化器生成一个最佳可能轨迹。决策器将根据成本函数和安全规则来决定选择哪条车道。目的：安全、平稳且成功率高

分别对路径和速度进行优化。路径优化通常考虑静态障碍。根据生成的路径创建速度曲线。当动态障碍的出现，路径速度方法可能不是最优的。然而由于路径和速度是解耦的，在路径和速度优化方面实现了更大的灵活性。

EM规划器迭代优化路径和速度。在路径优化器中，上一个周期的速度曲线用于估计与迎面而来的低速动态障碍的交互作用。生成的路径被发送到速度优化器，以评估最优的速度曲线。 对于高速动态障碍物，规划器更偏向采取换车道的策略，而不是轻推。EM规划器的迭代策略可以在路径-速度解耦框架下帮助解决动态障碍物。

交通规则是硬约束，Em规划器在提供平滑轨迹之前进行决策。为了明确道路意图，减少寻找最优轨迹的搜索空间。

在EM规划器的决策步骤中，首先用一个粗略可行的轨迹描述自我小车的运动意图。利用该轨迹测量障碍物之间的相互作用。即使场景变得更加复杂，这种基于轨迹的可行决策也是可扩展的。

其次，规划器根据轨迹生成凸可行空间来平滑样条参数。基于二次规划的平滑样条求解器可用于生成更平滑的路径和速度曲线，遵循决策，保证了一个可行和顺利的解决方案。

EM是一个在已知部分相关变量的情况下，估计未知变量的迭代技术。 初始化分布参数；

重复直到收敛的步骤如下： E步骤：根据隐含数据的假设值，给出当前的参数的极大似然估计； M步骤：重新给出未知变量的期望估计，应用于缺失值。

所有的信息源都在数据中心模块中收集和同步。

数据采集后，参考线生成器将生成一些候选车道级参考线，以及交通规则和障碍物的信息，基于来自路由模块的高清地图和导航信息。

在车道水平运动规划中，首先构建一个基于指定参考线的Frenet框架。在参考线和交通规则构成的Frenet框架内，评价自我车与周围环境的关系。重新构造的信息传递给车道级优化器。

车道级优化模块执行路径优化和速度优化。在路径优化过程中，将周围环境的信息投影到Frenet框架(E-step)上。基于Frenet框架中投影的信息，生成一条平滑路径(M-step)。在速度优化过程中，一旦路径优化器生成了一条平滑的路径，障碍物就被投影到S-T图(E-step)上。然后，速度优化器将生成一个平滑的速度轮廓(M-step)。结合路径和速度剖面，我们将得到指定车道的平滑轨迹。

最后,将所有车道级最佳轨迹发送给参考线轨迹决策器。轨迹决策器根据当前车辆状态、规则和每条轨迹的成本，为自我车辆机动确定最佳轨迹。

车道级优化问题。在一个规划周期内的两个e步和两个m步。轨迹信息将在规划周期之间迭代。

第一个E-steps：动态和静态障碍物都被投影到Frenet框架。静态障碍物将基于笛卡尔-Frenet框架变换直接投影。在Apollo框架中，用障碍物运动轨迹来描述动态障碍物的意图。考虑到之前的周期规划轨迹，我们可以在每个时间点估计出动态障碍物和汽车的位置。

然后动态障碍物和汽车在某个时间点的重叠可以被映射到Frenet框架中。路径优化过程中动态障碍物的出现最终对导致轻推。出于安全考虑，动态障碍物的SL投影将仅考虑低速交通和即将到来的障碍物。对于高速交通，EM规划器的并行车道变换策略将涵盖该场景。

第二个E-steps：基于生成的路径在s-t坐标系下，所有障碍物包括高速、低速和即将到来的障碍物都将被估计，生成速度曲线。如果障碍物轨迹和已经规划的路径重叠，将在s-t坐标系下生成一个相应的区域。

两个M-steps：通过动态规划和二次规划生成路径和速度曲线。尽管我们将障碍物投影到SL和ST坐标系下，但是最优的路径和速度解仍在非凸空间中。使用动态规划先获得一个粗略解，这个解可以提供轻推、让行和超车等障碍决策。用粗略解为基于二次规划的样条优化器来确定凸包。然后，优化器可以在凸包内找到解。

(1) SL投影 笛卡尔空间中，用位置和航向(x，y，θ)以及ego汽车的曲率和曲率导数(κ，dκ)来描述障碍物和ego汽车状态。映射到Frenet框架坐标(s，l，dl，ddl，dddl )，这些坐标表示站位、横向和横向导数。

静态障碍物的位置不会随着时间改变，直接投影即可。对于动态障碍物，借助上一个周期的轨迹来投影碍物。上一个周期的运动轨迹被投影到Frenet坐标系上，以提取S方向的速度曲线。给定确定时间就会估计出汽车状态的坐标。估计的汽车状态坐标将会帮助估计动态障碍物的影响。

一旦汽车状态坐标同时与障碍物轨迹发生重叠时，SL地图上的阴影区域将被标记。交叉区域被定义为汽车与障碍物的重叠。

下图从预测模块估计的迎面而来的动态障碍物和相应的轨迹被标记为红色。小车是蓝色的。首先将迎面而来的动态障碍物的轨迹随时间离散成若干个轨迹点，然后将这些轨迹点投影到Frenet框架上。一旦发现ego车的位置坐标与投射的障碍点有交互作用，重叠区域（图中紫色部分）将在Frenet框架中标记出来。 (1) ST投影 ST投影有助于帮助评估汽车的速度曲线。路径生成器在Frenet框架中产生一条平滑路径后。如果存在交互，则静态和动态障碍物都被投影到路径上。一个交互也被定义为bounding boxes overlapping。

如图，红色障碍物在前方2秒40米处切入车辆驾驶路径，汽车后面的绿色障碍物。其余区域为速度剖面可行区域。速度优化M步将试图在该区域找到一个可行的光滑解。

M-Step DP Path（E-Step） M-step路径优化器求解Frenet坐标系下的最优路径规划，实际上在一个非凸的区间（从左和从右避让是两个局部最优情景）求最优的l = f(s)方程。包括两步：基于动态规划的路径决策和基于样条的路径规划。

基于动态规划的路径步骤提供一条粗略的路径信息，其可以带来可行通道和避障决策，如图所示，具体包括Lattice采样、代价函数、动态规划搜索。 Lattice采样器基于Frenet框架。如图所示，首先在ego车辆之前对多行点进行采样。不同行之间的点通过五次多项式边平滑连接。行之间的间隔距离取决于速度、道路结构、车道变换等。该框架允许根据应用场景定制采样策略。例如，换道可能需要比当前车道驾驶更长的采样间隔。出于安全考虑，S方向距离将至少覆盖8秒或200米。 接着，通过代价函数的总和来评估每条路径。使用从SL投影得到的信息结合交通规则、汽车动力学模型去构建函数。总的代价是平滑度，避障和车道代价函数的线性组合。 平滑度通过以下方程来衡量，一阶导表示航向偏差，二阶导表示曲率，三阶导表示曲率导数： 障碍物代价由以下方程给出，d表示自车bounding box到障碍物bounding box的距离。Cnudge定义为单调递减函数。出于安全考虑，dc设置保留缓冲区。可根据情况微调dn范围。Ccollision是碰撞代价函数，可帮助检测不可行的路径。

简单来说：障碍物太远不考虑，一定范围内离缓冲区越近cost越大，小于缓冲区cost非常大。 车道代价如下，考虑是否在道路上行驶和与参考线之间的差异。包括两个部分：引导线代价和车道代价。引导线定义为周围没有障碍物时的理想路径，通常这条线提取的是道路中心线。车道代价函数通常被道路边界决定，道路外的路径点将受到很高代价的惩罚。 最终总的路径代价函数包含平滑度、障碍和车道代价。通过动态规划去选择最低成本的候选路径，候选路径也将确定障碍物决策。

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

通过QP求解器求解具有线性约束的目标函数生成最优的QP路径，框架如下： 代价函数是平滑度代价和引导线代价的线性组合。该引导线是动态规划挑选出来的路径。引导线提供了避开障碍物的估计值。函数如下： g(s)是动态规划的结果。f(s),f"(s), f"’(s)分别为航行、曲率和曲率的导数。该目标函数描述了避开障碍物和平滑度之间的平衡。

QP路径中的限制条件包含了边界约束条件和动力学可行性。这些限制被应用于f(s),f"(s), f"’(s)在一系列状态坐标s0，s1，…，sn。为了提取边界约束，提取各状态点的可行区间。这个可行区间在每一个点被描述为Llow,i