Apollo EM Planner 论文解读

Apollo EM Planner

参考资料来源：EM Planner原文、Apollo开发者社区、B站老王、csdn和知乎上的笔记摘要、《自动驾驶汽车决策与控制》一书。

注：本文章主要在于EM Planner论文解读，想要全部掌握EM Planner的精髓，需要阅读源代码！！！

1.EM Planner介绍

EM planner是Apollo的经典决策规划算法，擅长处理复杂环境下的决策规划，也是Apollo默认的决策规划算法。该算法在Apollo 1.5加入，虽然目前Apollo目前已经更新至9.0，但依旧存在着EM Planner的思想。对于以后想要从事自动驾驶决策规划方面的同学，可以说EM Planner是必学项目。

2.基础知识（建议不要跳过）

2.1 笛卡尔坐标系与自然坐标系

• 一般情况下，我们使用Cartesian坐标系（笛卡尔坐标系）来描述物体，但对于车辆来说，笛卡尔坐标系并不是最佳选择。因为即使知道了笛卡尔坐标系车辆的位置信息，也难以表达车辆与道路之间的相对位置，导致二者之间的相对关系不明确。

• 如下图所示，在Frenet坐标系下，以参考线为基准，假定沿参考线的方向为纵轴（s）,垂直于参考线方向为横轴（d）。由于横轴、纵轴相互垂直且参考线与车道线平行，容易确定车辆偏离车道中心线的距离以及车辆沿车道线的行驶距离。
  

• 关于笛卡尔坐标系与自然坐标系的转换，直接套用公式很难理解，建议自己推过一遍，理解比较深刻。推荐下面两篇博客以及B站老王的视频进行学习。
  https://blog.csdn.net/u013468614/article/details/108748016
  https://blog.csdn.net/u013468614/article/details/108748016

2.2 动态规划（DP）

直接讲动态规划可能比较枯燥，结合知乎上一博客https://www.zhihu.com/search?type=content&q=动态规划上的例子，让大家快速入门。考虑这样一个问题：如何找到A–>F之间的最短路径，L代表不同节点之间的代价。

问题：如果在道路上根据一定的采样策略进行采样，不同节点之间具有一定的代价，如何找到代价最小的一条路径呢（动态规划）

2.3 二次规划（QP）

• 凸集、凸函数、凸优化：

??凸集的定义：

??凸集的几何意义为：如果集合C中任意两个元素连线上的点也在集合C中，则C为凸集。

??凸函数的定义：

??凸集的几何意义为：表示为函数任意两点连线上的值大于对应自变量处的函数值

??凸优化的定义：研究定义于凸集中的凸函数最小化的问题。它要求目标函数是凸函数，变量所属集合是凸集合的优化问题。凸优化问题具有全局最优解的性质（对于凸优化问题，任何一个局部最优解都是全局最优解）

• 二次规划：
  ??二次规划就是一个很经典的凸优化问题，其有很多的求解手段，其形式如下图
  

• 提出一个问题 :避障空间是一个凸空间嘛？
  ??答：遗憾的是其是非凸空间（以下图为例子，车的前方有一颗树，其可以向左或者右进行避障（紫色部分），但是其轨迹的一半（绿色部分），不满足避障空间的要求（也即不是凸空间））。因此，自动驾驶的运动规划问题并非是一个凸优化问题的。（EM planner的魅力点的其中之一就是如何去处理非凸优化问题，大家看完论文解读之后自会有答案）
  

3.论文解读

注：请大家将该论文解读当成参考资料，同时该文章顺序并非按照论文原文顺序，而是进行一定的揉碎整合，灵魂部分还需要反复读论文和源代码。相信大家在学习之后，会有一种大道至简的想法。

3.1 背景介绍

??Fig.1为百度Apollo开源平台流程框架图，包括感知、定位、预测、规划、控制模块。规划模块包括Routing和Motion Planning，Motion Planning接受的来自各个模块的信息，其输出是一条安全舒适可执行的时空轨迹。
??运动规划安全是首要的，为了保证安全，至少有4点：

    1.任何时候都要遵守交通规则
    2.规划轨迹至少要8s或者200m远，要留足够的空间保持安全
    3.在紧急情况下，系统可以在100毫秒内做出反应（人的反应时间300ms）
    4.一旦上游模块失效，运动规划可以做出紧急措施

??除了安全，驾驶体验也很重要。具体包括：

    1.场景覆盖
    不能只是处理简单场景（停、绕、让、走），而是可以处理复杂环境下问题。
    2.遵守交规
    遵守交规可以最大程度上减少交通事故与紧急情况
    3.舒适性
    舒适在自动驾驶中，一般通过轨迹的平滑度来衡量

??基于此背景，百度提出一种名叫EM PLANNER规划器。该规划器以多车道、路径速度迭代、交通规则和决策相结合的设计为目标，实现了安全性和乘坐体验。

3.2 具体细节

问题描述：在一个局部空间内，找到一条安全舒适可执行的时空轨迹。（注意安全、舒适、可执行、时空这几个关键词）。
该问题本质上是一个SLT三维优化问题。一般有以下两种思路：

• 直接优化：在SLT采样，设计评价函数评估出一条最优的时空路径，但是计算量很可怕，很难在复杂场景下有着出色表现，一般找到的轨迹也是次优的。
• path/speed解耦优化：EM planner采用的就是路径速度解耦方法，将三维问题转换为两个二维子问题，SL图上的路径规划问题和ST图上的速度规划问题，最后二者结合即为时空轨迹。
  

在学习EM Planner算法是需要把握几个关键点（先暂时有一个印象，后续还会总结）：

• 路径速度解耦优化
• DP算法本质上是一个决策算法：其可以开辟一个凸空间，一个粗解，一个决策（向左或向右）
• QP算法本质上是一个路径平滑算法：对DP得到的粗解进行平滑处理
• 路径速度迭代

3.2.1 路径规划

路径规划问题可以总结为在SL图上找一条代价较小的path。可以拆分为下面步骤：

1.由参考线生成Frenet坐标系（path规划的参考线是定位+导航模块生成）
2.将障碍物投影到Frenet坐标系上
3.在SL图上利用DP算法开辟凸空间
4.在SL图上利用QP算法平滑路径

3.2.1.1 DP 过程

请回顾之前提出的问题：如果在道路上根据一定的采样策略进行采样，不同节点之间具有一定的代价，如何找到代价最小的一条路径呢（节点之间的代价可以使用评价函数来表达，节点与节点之间的连线采用五次多项式相连）

路径规划的DP过程框图如下所示，其可分为三步：

1.Lattice Sampling Strategy(设计采样策略，在path中采样)
2.Pointwise Cost Functional(根据交通规则、障碍物、地图、车辆动力学等设计Cost函数)
3.利用DP算法思想，可以开辟一个凸空间，一个粗解，一个决策（向左或向右）

• 采样策略：首先在本车辆前方对多行点进行采样。不同行之间的点通过五次多项式边平滑连接。点行之间的间隔距离取决于速度、道路结构、车道变换等（可根据场景进行定制）

成本函数由平滑项+障碍物项+指导线项组成

平滑项由五次多项式的一阶导（车头朝向）、二阶导、三阶导组成

障碍物项可以理解为一种惩罚函数：距离障碍物大于Dn时认为对安全无影响（故为0）;距离障碍物位于Dc和Dn之间时认为对安全有影响（故第二项是一个递减函数）；距离障碍物小于Dc时认为应该杜绝的（故第三项是一个特别大的数）
指导线即g(s),一般取道路中心线，希望选取的路径f(s)尽量靠近g(s)

• DP结果解读：
  

  如上图所示绿色代表着DP得到的一个粗解，橙色区域代表之前说的凸空间，同时会得到一个决策指令right nudge

3.2.1.2 QP 过程

其实根据上一步DP过程，我们已经得到了一个质量较好的粗解，我们需要做的是对这个解进行一定的平滑处理即可
路径规划的QP过程框图如下所示，其可分为三步：

1.由DP过程得到的Feasible Tunnel(凸空间)+车辆动力学生成线性约束
2.由Smooth Functional + DP path生成目标函数
3.根据二次规划器求解该凸优化问题

• Cost函数：
  

该目标函数前三项依旧与DP过程类似，为平滑项（希望得到的曲线光滑一些）；第四项为与g(s)相似项（这里g(s)指的是DP过程得到的粗解，注意与DP过程当中参考线代价的区别）

• 约束条件:
  1.初始点约束
  2.平滑节点约束
  3.点采样边界约束

注：关于约束条件论文原文描述的也很模糊，具体细节可能只有参考源代码来学习。上述的约束条件是结合论文原文、B站老王视频、《自动驾驶汽车决策与控制一书》整理而成。详细推导过程见本文附录

3.2.2 速度规划

如果大家对前面的路径规划已经有了充分的理解，那么速度规划也可以容易上手，难点可能在与ST图的理解，所以会简单说一下ST图。

路径规划问题可以总结为在ST图上找一条代价较小的路线。可以拆分为下面步骤：

1.由参考线生成Frenet坐标系（speed规划的参考线是path规划轨迹生成，与path参考线不一致！！！）
2.将障碍物投影到Frenet坐标系上
3.在ST图上利用DP算法开辟凸空间
4.在ST图上利用QP算法平滑速度路径

3.2.2.0 ST 图

所以我们的任务就是在ST图上找一条不碰撞障碍物的速度曲线

3.2.2.1 DP 过程

路径规划的DP过程框图如下所示，其可分为三步：

1.Discretized ST Grid(与path不同，speed需要栅格化)
2.Pointwise Cost Functional(根据交通规则、障碍物、地图、车辆动力学等设计Cost函数)
3.利用DP算法思想，可以开辟一个凸空间，一个粗解，一个决策（超车或者让行）

• Cost函数：

??第一项Vref指的推荐速度，二三项指的加速度和jerk,最后一项指的距离障碍物成本，与path类似。不同的是在栅格网络中寻找最短路径，本质上还是一个图搜索问题，因此还是可以用动态规划的思想去解决。

• 结果解读：
  

  如上图所示绿色代表着QP得到的一个粗解，橙色区域代表之前说的凸空间，同时会得到一个决策指令

注：论文原图看的不清晰，可以看一下这张图理解

3.2.2.2 QP 过程

其实根据上一步DP过程，我们已经得到了一个质量较好的粗解，我们需要做的是对这个解进行一定的平滑处理即可
速度规划的QP过程框图如下所示，其可分为三步：

1.由DP过程得到的Feasible Tunnel(凸空间)+车辆动力学+交通规则生成线性约束
2.由Smooth Functional + DP speed生成目标函数
3.根据二次规划器求解该凸优化问题

• Cost函数：
  

第一项中Sref指的是DP过程得到的粗解，第二项，第三项指的是加速度和jerk

• 约束条件:
  1.单调性（不允许倒车现象）
  2.初始状态约束（包括初始位置、初始速度、初始加速度信息；原论文图中没有，与path类似）
  3.边界约束（包括S，速度，加速度，jerk限制）
  

3.2.3 路径速度迭代

截至目前，我们已经学习了EM Planner的一些关键部分（路径速度解耦、DP决策开辟凸空间、QP平滑轨迹），这一节介绍最后的关键部分路径速度规划迭代过程

3.2.3.1 虚拟障碍物生成

对于静态障碍物的投影比较简单，关键在于动态障碍物的处理。对于动态障碍物的处理可以通过设置虚拟障碍物来处理。

例如，如图所示，从预测模块估计的迎面而来的动态障碍物和相应的轨迹被标记为红色。汽车用蓝色标出。首先将迎面而来的动态障碍物的轨迹随时间离散成几个轨迹点，然后将这些点投影到Frenet坐标系中。一旦我们发现汽车的S坐标与投影的障碍点有交互作用，重叠区域(在图中以紫色显示，可以理解为虚拟障碍物)将在Frenet坐标系中标记

3.2.3.2 路径速度迭代过程

EM Planner迭代过程框图如上图所示，可以归结为以下步骤：

1.上一个周期轨迹与预测模块生成虚拟障碍物，投影到SL图
2.在SL图进行路径规划
3.以SL图找到的路径作为参考线再次投影障碍物
4.在ST图上进行速度规划
5.ST图+SL图生成时空轨迹
6.返回第1步

大家也可以参考B站老王的笔记

3.2.4 并行的多车道策略

刚刚前面讲述的是LANE级别的规划，当涉及到多LANE级别的规划时，EM planner采用并行处理方案最后加以比较（如图中的Lane1 Lane2）

3.3 结论与思考

1.关于EM planner需要把握以下几个关键词（路径速度解耦、DP决策开辟凸空间、QP平滑、路径速度迭代）

• Motion planning问题本质上是一个SLT三维优化问题，可以通过SL和ST解耦优化
• DP算法叫做动态规划，其实本质上是一个基于动态规划思想的一种决策算法，其可以开辟一个凸空间，为后续的路径平滑奠定基础
• QP算法大家可能需要关注一下约束条件和求解器的调用
• 路径速度迭代理解过程，关键在于源代码的细节实现

2.如果想完全掌握EM planner算法需要从源代码学习，这里面有很多的细节部分（参考线生成，坐标系的转换、控制模块生成的轨迹和规划的轨迹不一致时，如何轨迹拼接、优化参数很多，如何快速求解等）

3.4 附录

3.点采样边界约束