【论文笔记】Baidu Apollo EM Motion Planner

Abstract

EMplanner通过一个层级结构的方式去考虑多车道和单车道的情况：
(1)顶层策略通过比较不同车道级轨迹来处理变道场景；
(2)在Frenet坐标下不断迭代求解路径优化和速度优化问题来生成轨迹；
(3)为了使框架易于扩展和调整，可以同时处理交通规则、障碍物决策和平滑曲线，他们提出了一种融合DP和QP采样的优化算法。

I. INTRODUCTION

Motion Planning安全性考虑因素：

1. 交通规则。
2. 范围。目标轨迹需要至少8秒的视野和200米的轨迹长度。
3. 工作周期。小于100ms。
4. 紧急情况。

乘员体验也是是十分重要的，考虑以下几点：

1. 场景。不仅仅是简单的超车、让行的场景，更要能适应多车道、复杂、动态的场景。
2. 交通规则。
3. 舒适度。轨迹平滑。
   
   Apollo的Pipeline

EM思想的最早起源：《Maximum likelihood from incomplete data via the EM algorithm》

EM算法，全称期望最大化算法（Expectation-Maximization Algorithm），是一种迭代算法，用于含有缺失数据的概率模型参数估计。

EM算法分为两步，即“期望步骤”（E步）和“最大化步骤”（M步）。在E步中，求出在当前参数下缺失数据的概率分布，即求出缺失数据的期望。在M步中，利用E步中求出的缺失数据的期望，最大化似然函数（或极大化后验概率）来估计模型参数。这样，利用EM算法可以通过迭代更新参数估计值，逐渐逼近真实数据分布的参数值。

EM算法广泛应用于数据挖掘、图像处理、自然语言处理等领域中的模型参数学习，比如高斯混合模型、隐马尔可夫模型等

A. Multilane Strategy

采用对不同车道进行cost计算的方式可能存在的难点：

1. 多车道带来的计算复杂度。
2. 不容易结合交通规则。
3. 需要考虑不同Planning Cycle之间的变化对轨迹产生的影响。

多车道的策略需要考虑主动换道(routing规划时产生)和被动换道的场景(受障碍物、环境变化影响)。

对于每一条候选车道，将障碍物以及环境信息投影到Frenet坐标系中；每一条车道加以相应的交规约束，并对每一条车道生成一条轨迹；利用cost function和规则决定选择哪一条车道。

B. Path-Speed Iterative Algorithm

在Frenet框架中寻找最优轨迹本质是一个三维约束的优化问题。主要有两种解决方式：

1. 综合ST问题，直接求解：通常是采用采样+搜索的算法进行，得到一条符合kinodynamic的轨迹。计算复杂度高。若为了减小计算量，可能会采用减小resolutions的方式，但这样一来会导致生成的轨迹非最优。
2. ST解耦。路径优化考虑静态障碍物，速度优化考虑动态障碍物。求解效率高，但有可能存在求解失败的问题。

【论文笔记】Perception, Planning, Control, and Coordination for Autonomous Vehicles这篇论文也涉及相关问题的讨论。

路径优化利用上一帧的speed profile来估计低速障碍物。对于高速障碍物，EM planner更倾向于采取变道而不是nudge操作。

C. Decisions and Traffic Regulations

不同于以往的一些方法直接利用数值优化的方法同时求解决策和轨迹规划，EM planner采取了先决策后轨迹规划的方式，去减少求解最优轨迹时的计算量。

决策方法可分为基于规则（手动操作）和基于模型的方法：

1. 基于规则：容易手动调整，但是泛用性差。
2. 基于模型：通常将车辆状态离散化为有限驾驶状态，再使用数据驱动的方式去调整。

多智能体的交互在预测和决策等方面很重要，这就需要POMDP、RL等方法了。

EM的决策流程：

• 首先，通过一条粗糙可通行的轨迹来表示自车的意图。同时也利用这个轨迹来测量障碍物之间的交互。
• 接着，基于这个轨迹，生成凸空间。基于凸优化的问题，生成遵循决策的平滑路径和速度曲线。

II. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

Data Center：用于收集和同步所有的信息。
Reference Line Generator:提供车道级的参考线，参考线包括交规、障碍物等信息。

基于Frenet坐标系估计自车和周围环境的相对关系，并将信息传递到车道级的优化器之中。

Optimizer：

• path optimization：
  • (E-step):将周围环境信息投影到Frenet坐标系（SL）。
  • (M-step):生成光滑的轨迹。
• speed optimization:
  • (E-step): 将障碍物投影到ST图。
  • (M-step):生成光滑的速度曲线。

最后合并路径和速度部分，并送入reference line trajectory decider基于车辆状态、规则、cost选出最优路径。

III. EM PLANNER AT LANE LEVEL

两步E-step和两步M-step；轨迹随着规划周期进行迭代。

在第一个E-step中，障碍物被投射到车道Frenet坐标系，包括静态障碍物和动态障碍物。在Apollo框架中，动态障碍物的意图是用障碍物的移动轨迹来描述的.处于安全的考虑，路径优化部分只考虑低速或相向的障碍物，高速情况利用变道进行解决。

在第二个E-step中，所有障碍物，包括高速、低速和迎面而来的障碍物都会投影到ST图中进行评估。

在M-step中，通过DP+QP的方式生成path profiles和speed profiles。利用DP给出一个粗糙的决策，将非凸的空间转化为凸空间。QP进行优化。

A. SL and ST Mapping (E-step)

SL部分

SL投影基于参考线二阶导连续。
状态向量：

• 在笛卡尔坐标系中：$(x,y,\theta ,\kappa ,\dot{\kappa })$；
• 在Frenet坐标系中：$(s,l,dl,ddl,dddl)$

对于动态障碍物，利用上一帧自车的轨迹。给定一个时间，可以估计出相应的位置，再利用这个位置与障碍物的轨迹进行交互决策。

下面是一个例子。

蓝色：自车；
红色：迎面而来的障碍车以及其轨迹；
紫色：交互区域。

---

ST部分
路径优化之后，可能产生交互作用的静态障碍物和动态障碍物都被投影到路径上。ps：注意apollo在之后方案与此处的不同。

一个障碍物在$t=2s$,$s=40m$处切入当前车道，其在ST图中的投影为红色。后方车辆在ST图中的投影为绿色。剩余的区域为可行区域。

B. M-Step DP Path

DP的结构：包括lattice采样、代价函数以及DP search。

动态规划用于先给出一个粗略的路径。下面是一个示例。

• 在自车的前方撒点。不同行之间的点通过五次多项式进行连接。采样间隔依赖于速度、道路结构以及场景(比如说换道场景会增大采样间隔)。
• 采样结束后，利用SL投影、交规以及车辆动力学去构造代价函数。
• 最后DP找到最低cost的路径，同时给出相应的决策。

DP-path总的代价函数：
$$C_{total}(f(s))=C_{smooth}(f)+C_{obs}(f)+C_{guidance}(f)$$

路径平滑程度的代价：
$$\begin{array}{rl}{C}_{smooth}(f)& \begin{array}{rl}& =w_1\int (f^{\prime }(s){)}^{2}ds+w_2\int (f^{\prime \prime }(s){)}^{2}ds\end{array}\\ & +w_3\int (f^{\prime \prime \prime }(s){)}^{2}ds.\end{array}$$
$f^{\prime }(s)$代表车道和自车航向角之差；
$f^{\prime \prime }(s)$代表路径的曲率，$\kappa =\frac{d\theta }{ds}$;
$f^{\prime \prime \prime }(s)$代表路径曲率的二阶导。

障碍物的代价：
$$C_{obs}(d)=\{\begin{array}{llc}0,& d>d_n& \\ C_{nudge}(d-d_c),& d_c\le d\le d_n& \\ C_{collision}& d<d_c& \end{array}$$
遍历路径上的$s$序列$s_0,s_1,...,s_n$，比较和障碍物的距离$d$。$C_{nudge}$是一个单调递减函数。$d_c$是一个基于安全考虑的buffer。$d_n$是一个阈值，超过它，cost为0，阈值可随场景变更；$C_{collision}$是碰撞的cost，通常会设一个很大的值。

车道的代价由两部分组成：
一部分是参考线的代价，使得轨迹靠近参考线；另一部分是道路的代价，保证轨迹在道路边界之内（若不在，设置一个很高的惩罚）。
$$C_{guidance}(f)=\int (f(s)-g(s){)}^{2}ds$$
注意：这里的参考线指的是道路中心线。

C. M-Step Spline QP Path

QP在DP的基础上平滑轨迹。

QP的流程，包含目标函数和线性化的约束。

QP的目标函数:
$$\begin{array}{rl}{C}_s(f)& \begin{array}{rl}& =w_1\int (f^{\prime }(s){)}^{2}ds+w_2\int (f^{\prime \prime }(s){)}^{2}ds\end{array}\\ & \begin{array}{rl}& +w_3\int (f^{\prime \prime \prime }(s){)}^2+w_4\int (f(s)-g(s){)}^{2}ds.\end{array}\end{array}$$

注意这里的$g(s)$是DP规划出来的路径。因为DP规划出来的路径包含了对nudge障碍物的考虑，因此QP的目标函数则是对nudge以及路径平滑程度两者之间的权衡。

QP路径的约束考虑了边界约束以及动态可行性。边界约束包括$f(s),f^{\prime }(s),f^{\prime \prime }(s)$。

为了保证约束的凸性和线性，在自车bounding box前面和后面分别添加了一个半圆。
$$l_{\text{left front corner}}=f(s)+sin(\theta )l_f+w/2$$
$l_f$后轴到车前部的距离；
$w$半车宽；
$\theta$车辆航向角和道路方位角之差。
$$\begin{array}{rl}\begin{array}{r}f(s)+sin(\theta )l_f+w/2\end{array}& \begin{array}{rl}& \le f(s)+f^{\prime }(s)l_r+w/2\end{array}\\ & \le l_{\text{left corner bound}}\end{array}$$
线性化近似之后得到下式。其余三个点也一样。

当然还有起点约束。

这部分还可参考二次规划（QP）样条路径优化

D. M-Step DP Speed Optimizer

还有剪枝的操作，可参考【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER

速度DP结果：分段线性化的speed profile（在QP中作为参考线）、可行区域(在QP中用以生成凸空间)以及对障碍物的决策。

代价函数：
$$\begin{array}{rl}{C}_{total}(S)& \begin{array}{r}=w_1{\int }_{t_0}^{t_n}g(S^{\prime }-V_{ref})dt\end{array}\\ & \begin{array}{rl}& +w_2{\int }_{t_0}^{t_n}(S^{\prime \prime }{)}^{2}dt+w_3{\int }_{t_0}^{t_n}(S^{\prime \prime \prime }{)}^{2}dt\end{array}\\ & +w_4{C}_{obs}(S)\end{array}$$

考虑因素：

• 速度接近于参考速度$V_{ref}$。$V_{ref}$取决于限速、交规、曲率。$g$用以适应于不同场景调整相应的惩罚。
• 平滑程度。$S^{\prime \prime },S^{\prime \prime \prime }$
• 障碍物。

DP搜索空间考虑的约束有：加速度、加加速度以及单调递增(不考虑向后行车)。

E. M-Step QP Speed Optimizer

QP的结构

$$\begin{array}{rl}{C}_{total}(S)& \begin{array}{rl}& =w_1{\int }_{t_0}^{t_n}(S-S_{ref}{)}^{2}dt+w_2{\int }_{t_0}^{t_n}(S^{\prime \prime }{)}^{2}dt\end{array}\\ & +w_3{\int }_{t_0}^{t_n}(S^{\prime \prime \prime }{)}^{2}dt.\end{array}$$
$S_{ref}$为DP规划出的speed profile。

约束：
$$\begin{array}{c}\begin{array}{r}S(t_i)\le S(t_{i+1}),i=0,1,2,...,n-1,\end{array}\\ \begin{array}{r}{S}_{l,t_i}\le S(t_i)\le S_{u,t_i},\end{array}\\ S^{\prime }(t_i)\le V_{upper},\\ \begin{array}{r}-De{c}_{max}\le S^{\prime \prime }(t_i)\le Ac{c}_{max}\end{array}\\ \begin{array}{r}-J_{max}\le S^{\prime \prime \prime }(t_i)\le J_{max}\end{array}\end{array}$$

F. Notes on Solving Quadratic Programming Problems

• 采用五次多项式进行QP求解。参考：二次规划ST速度优化、二次规划（QP）样条路径优化
• 利用上一次规划的结果作为热启动以提高求解效率。

G. Notes on Non-convex Optimization With DP and QP

• DP采样的节点数受到采样间隔的影响，同时DP在复杂场景的计算量会很大，占据大量计算时间。一般的，DP只是提供一个粗解，并不会给出最优解。
• QP需要依赖于DP获得凸空间，否则可能会陷入局部最小值或规划失败。(QP并非一定需要DP作为前置步骤，若能转换到凸问题进行求解也是可以的)

CASE STUDY

EM planner是一个基于轻决策的规划器，相比传统的重决策或者完全rule-based的方案，更具有泛化能力。

一个两轮迭代的例子。

(a) 驻车沿着历史轨迹行驶。
(b) 两车相遇点在s=40处，因此在s=40处采用nudge操作通过可以有效避开障碍物。
© 主车会在s=40处降低速度通过。
(d) 更新nudge距离到s=30。
(e) 在s=30处nudge通过，之后在s=40处加速。

Else

附录里提到了reproducing kernel Hilbert space (RKHS)的概念，有些不大理解，后续再看。