自动驾驶之轨迹规划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模块则不需要考虑这些信息，只需要做一个长期的规划路径即可

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

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

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

1.1. Multilane Strategy

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

1. 对于候选车道（一般是本车道或相邻车道，可理解为多个候选凸域，详见百度技术学院中对Apollo规划中非凸问题的讲解），所有障碍物和环境信息都投影到Frenet frame中；
2. 并将交通规则与候选车道绑定；
3. 每个候选车道基于规划器，都生成一条从当前车道换至本车道的最佳轨迹；
4. 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中的描述类似，其实可以理解为是一个东西，只是描述侧重不一样），下面的全文都是对这几个步骤的详解：

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

2. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

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

1. reference line generator会根据交通规则和障碍物，产生一些车道级备选reference line。该过程是基于HD map 和 Routing中的导航信息实现的。（如左右相邻车道？这个reference line没搞明白是什么）；
2. 基于reference line生成对应的Frene Frame；
3. 将障碍物等环境信息投影到path的Frenet frame中；
4. 生成平滑path profile；
5. 将障碍物等环境信息投影到speed的Frenet frame中；
6. 生成平滑speed profile。
7. 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部分线性叠加得来的。

• C s m o o t h C_{smooth}