Apollo决策规划 - EM planner

旨在对b站老王所讲的百度Apollo - EM planner算法做浓缩版总结

0 决策规划背景

基于图搜索

优点：
可以得到全局层面最优解，适用于比较低维数的规划问题
缺点：
规划问题维数较高时，面临指数爆炸问题

基于采样

优点：
计算量低，曲率连续，行驶平顺性好
缺点：
曲线通常是四阶或者更高，系数的计算难以使车辆到达确定的运动状态

基于插值的曲率规划

优点：
算法总是可以收敛到满足条件的解，适用于全局规划和局部规划
缺点：
产生的轨迹不连续而且不平稳，路径的最优性受时间影响

基于函数优化

优点：
影响因素考虑多，包含道路限制条件、自车限制，便于处理约束，可以满足不同优化目标，求解速度快
缺点：
最优解一般比较贴近约束，转化为优化问题本身较为困难

自动驾驶总共分成了六个等级：L0 - L5

等级	详述
L0	没有任何自动驾驶功能
L1	有横向和纵向的自动驾驶功能，但是横纵向无法联合作用
L2	横纵向可以联合作用，但是驾驶员必须对一切情况负责
L3	功能和L2基本相同，最大的区别在于责任，对于部分场景，驾驶员不必负责
L4	大部分道路皆可自动驾驶，大部分场景都不需要驾驶员负责
L5	完全自动驾驶

EM Planner是Apollo的经典决策规划算法，擅长处理复杂环境下的决策规划问题。

2 规划需要的背景知识

2.1 五次多项式

1. 位置
   $$\begin{gathered} x(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5 \\ y(t)=b_0+b_{1}t+b_2{t}^2+b_3{t}^3+b_4{t}^4+b_5{t}^5 \end{gathered}$$
2. 速度
   $$\begin{gathered} \dot{x}(t)=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4 \\ \dot{y}(t)=b_1+2b_{2}t+3b_3{t}^2+4b_4{t}^3+5b_5{t}^4 \end{gathered}$$
3. 加速度
   $$\begin{gathered} \ddot{x}(t)=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3 \\ \ddot{y}(t)=2b_2+6b_{3}t+12b_4{t}^2+20b_5{t}^3 \end{gathered}$$

化为矩阵形式。
$$X=\left[\begin{array}{c}x(0)\\ \dot{x(0)}\\ \ddot{x(0)}\\ x(T)\\ \dot{x(T)}\\ \ddot{x(T)}\end{array}\right]=\left[\begin{array}{cccccc}{t}_0^5& t_0^4& t_0^3& t_0^2& t_0& 1\\ 5t_0^4& 4t_0^3& 3t_0^2& 2t_0& 1& 0\\ 20t_0^3& 12t_0^2& 6t_0& 2& 0& 0\\ t_1^5& t_1^4& t_1^3& t_1^2& t_1& 1\\ 5t_1^4& 4t_1^3& 3t_1^2& 2t_1& 1& 0\\ 20t_1^3& 12t_1^2& 6t_1& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{a}_5\\ a_4\\ a_3\\ a_2\\ a_1\\ a_0\end{array}\right]=TXA$$
Y = [ y ( 0 ) y ( 0 ) ˙ y ( 0 ) ¨ y ( T ) y ( T ) ˙ y ( T ) ¨ ] = [ t 0 5 t 0 4 t 0 3 t 0 2 t 0 1 5 t 0 4 4 t 0 3 3 t 0 2 2 t 0 1 0 20 t 0 3 12 t 0 2 6 t 0 2 0 0 t 1 5 t 1 4 t 1 3 t 1 2 t 1 1 5 t 1 4 4 t 1 3 3 t 1 2 2 t 1 1 0 20 t 1 3 12 t 1 2 6 t 1 2 0 0 ] [ a 5 b 4 b 3 b 2 b 1 b 0 ] = T X B Y=\begin{bmatrix}y(0) \\\dot{y(0)} \\\ddot{y(0)} \\y(T) \\\dot{y(T)} \\\ddot{y(T)} \end{bmatrix}=\begin{bmatrix} t_{0}^5& t_{0}^4 & t_{0}^3 & t_{0}^2 & t_{0} &1 \\ 5t_{0}^4 & 4t_{0}^3 & 3t_{0}^2 & 2t_{0} & 1 & 0\\ 20t_{0}^3 & 12t_{0}^2 & 6t_{0} & 2 & 0 & 0\\ t_{1}^5& t_{1}^4 & t_{1}^3 & t_{1}^2 & t_{1}& 1\\ 5t_{1}^4 & 4t_{1}^3 & 3t_{1}^2 & 2t_{1} & 1 & 0\\ 20t_{1}^3 & 12t_{1}^2 & 6t_{1} & 2 & 0 & 0 \end{bmatrix}\begin{bmatrix}a_{5} \\b_{4} \\b_{3} \\b_{2} \\b_{1} \\b_{0} \end{bmatrix}=TXB Y= ​y(0)y(0)˙​y(0)¨​y(T)y(T)˙​y(T)¨​​ ​= ​t05​5t04​20t03​t15​5t14​20t13​​t04​4t03​12t02​t14​4t13​12t12​​t03​3t02​6t0​t13​3t12​6t1​​t02​2t0​2t12​2t