【Baidu Apollo】6.3 EM规划算法

今天这篇文章作为之前6.1 Apollo规划的补充，详细介绍下EM Planner

1. EM Planner 中的EM的含义

最大期望算法 Expectation Maximum

最大期望算法在统计中被用于寻找，依赖于不可观察的隐性变量的概率模型中，参数的最大似然估计。

在统计计算中，最大期望（EM）算法是在概率模型中寻找参数最大似然估计或者最大后验估计的算法，其中概率模型依赖于无法观测的隐性变量。最大期望算法经常用在机器学习和计算机视觉的数据聚类（Data Clustering）领域。最大期望算法经过两个步骤交替进行计算，第一步是计算期望（E），利用对隐藏变量的现有估计值，计算其最大似然估计值；第二步是最大化（M），最大化在E步上求得的最大似然值来计算参数的值。M步上找到的参数估计值被用于下一个E步计算中，这个过程不断交替进行。

EM简单教程

EM是一个在已知部分相关变量的情况下，估计未知变量的迭代技术。EM的算法流程如下：

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

2. 变道超车在EM Planner中是如何考虑的？

对于路径规划而言，变道是重要的组成部分。一般的方法是在所有可能的车道上进行基于cost的最优解搜索。不过这种方法也有以下的缺点：

1. 搜索空间要拓展到不同的车道，算法资源占用相对较多。
2. 不同车道对应了不同的交规，在同样的计算框架下不容易实现。

因此在apollo里面使用了如下的Framework，对不同可能的目标车道分别进行优化寻找最优解，最后的Decider决定最终的路径。

上图中的E-Step对应的基于隐含数据的假设值（当前规划的Trajectory）进行的极大似然估计（Path Planning）。

M-Step对应的是重新给出未知变量的期望估计（Velocity Planning，然后合成完整的Trajectory）。

最后的Trajectory decider会根据不同Trajectory的cost，交规还有车况选择一条最优的trajectory。

3. Path 和 Speed 迭代算法

Path

EM Planner会迭代式的对路径和速度进行优化。首先基于上一步计算得到的trajectory也就是对自身运动的预测与其他运动物体的预测进行碰撞检测，讲可能的碰撞点投射到SL图中。这里在SL图中只考虑低速移动物体或对向行驶的物体，对于高速物体，EM Planner基于安全原因更倾向于变道。

整个路径的规划分为DP的粗糙规划，和QP的平滑规划，QP主要就是对DP的路径进行平滑处理。

这里Cost有三个组成部分：平滑Cost，离障碍物距离Cost，离引导线偏差Cost。

 

Nudge Decision包括nudge, yield和overtake，用来生成convex hull来进行QP的spline优化。

QP的Cost相对简单些，就是负责平滑的路径一阶二阶三阶倒数还有和DP结果与引导线的偏差。

Speed

速度优化和路径优化类似，也是先来DP跟着QP。

Cost多了和目标速度的偏差和与障碍物的距离。

 

 

4.实例

a. 上一个cycle的规划，master vehicle 10m/s直行。

b. 第一次迭代，基于上一个cycle的预测，可能的碰撞区域位于S = 40m 处，映射到SD Graph进行path优化。

c. 基于优化得到的Path进行速度优化，40m处因为距离障碍物较近，会减速到5m/s。

d. 这个时候因为 master vehicle 的 预测更新了，进行第二次迭代，基于新的速度Profile，可能的碰撞（交叉）区域位于S = 30m 处，重新优化Path

e. 基于新优化得到的Path再次进行速度优化，迭代会一直进行知道收敛。