旨在对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=\left[\begin{array}{c}y(0)\\ \dot{y(0)}\\ \ddot{y(0)}\\ y(T)\\ \dot{y(T)}\\ \ddot{y(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\\ b_4\\ b_3\\ b_2\\ b_1\\ b_0\end{array}\right]=TXB$$
$X，Y，T$已知，就可以求出$A$和$B$。

衡量舒适性的物理量为跃度$Jerk$
$$Jerk=\frac{da}{dt}$$

$Jerk$的绝对值越小，$a$的变化越平缓，意味着越舒适。

find $f(t)$ 使得${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$最小
subject to :
$$\begin{gathered} S(0)=S_0，S(T)=S_n \\ \dot{S}(0)=v_0，\dot{S}(T)=v_n \\ \ddot{S}(0)=a_0，\ddot{S}(T)=a_n \end{gathered}$$

显然$f(t)$只可能是在$[0,T]$上是有界连续函数，因为无论是无界函数，还是有界间断函数都会使$Jerk$出现无穷大
$f(t)$：五次多项式

化简约束：
$$\begin{gathered} S(T)-S(0)={\int }_0^T\dot{f}dt=C_0 \\ \dot{S}(T)-\dot{S}(0)={\int }_0^T\ddot{f}dt=C_1 \\ \ddot{S}(T)-\ddot{S}(0)={\int }_0^T{f}^{(3)}dt=C_2 \end{gathered}$$

问题描述：
问题变为求${\int }_0^T{f}^{(3)}dt$在约束${\int }_0^T(\dot{f}-\frac{C_0}{T})dt=0,{\int }_0^T(\ddot{f}-\frac{C_1}{T})dt=0,{\int }_0^T(f^{(3)}-\frac{C_2}{T})dt=0$下的极小值

拉格朗日法：
$$\begin{gathered} {\int }_0^T{f}^{(3)}dt+{\lambda }_1{\int }_0^T(\dot{f}-\frac{C_0}{T})dt+{\lambda }_2{\int }_0^T(\ddot{f}-\frac{C_1}{T})dt+{\lambda }_3{\int }_0^T(f^{(3)}-\frac{C_2}{T})dt \\ ={\int }_0^T({\lambda }_1\dot{f}+{\lambda }_2\ddot{f}+{\lambda }_3{f}^{(3)}+{f^{(3)}}^2-{\lambda }_1\frac{C_0}{T}-{\lambda }_2\frac{C_1}{T}-{\lambda }_3\frac{C_2}{T})dt={\int }_0^{T}Ldt \end{gathered}$$

用到广义$Euler-Lagrarge$方程
$$\frac{\partial L}{\partial f}-\frac{d}{dt}(\frac{\partial L}{\partial \dot{f}})+\frac{d^2}{d{t}^2}(\frac{\partial L}{\partial \ddot{f}})-\frac{d^3}{d{t}^3}(\frac{\partial L}{\partial f^{(3)}})=0$$

将$\frac{\partial L}{\partial f}=0,\frac{\partial L}{\partial \dot{f}}={\lambda }_1,\frac{\partial L}{\partial \ddot{f}}={\lambda }_2,\frac{\partial L}{\partial f^{(3)}}={\lambda }_3+2f^{(3)}$代入广义$Euler-Lagrarge$方程得：
$$\frac{d^3}{d{t}^3}({\lambda }_3+2f^{(3)})=0$$

推导过程：
$$\begin{array}{l}{f}^{(6)}(t)=0\\ f^{(5)}(t)=a_0\\ f^{(4)}(t)=a_{0}t+a_1\\ f^{(3)}(t)=\frac{1}{2}{a}_0{t}^2+a_{1}t+a_2\\ f^{(2)}(t)=\frac{1}{6}{a}_0{t}^3+\frac{1}{2}{a}_1{t}^2+a_{2}t+a_3\\ f^{(1)}(t)=\frac{1}{24}{a}_0{t}^4+\frac{1}{6}{a}_1{t}^3+\frac{1}{2}{a}_{2}t+a_{3}t+a_4\\ \\ f(t)=\frac{1}{120}{a}_0{t}^5+\frac{1}{24}{a}_1{t}^4+\frac{1}{6}{a}_2{t}^3+\frac{1}{2}{a}_3{t}^2+a_{4}t+a_5\end{array}$$

2.2 凸优化和非凸优化

① 自动驾驶规划目标：算出一条满足约束的最优轨迹

衡量轨迹质量往往用cost function表示，代价函数越小，意味着越平滑，越舒适，当然也要满足各种约束。

指标：平滑、舒适、尽可能短
约束：轨迹连续、无碰撞、交规、车辆动力学

② 凸优化性质
(a) 代价函数只有一个极小值点且为最小值（凸函数）
(b) 约束空间是一块完整的、不破碎的空间（凸空间）
凸优化：求凸函数在凸空间的最小值问题

③ 自动驾驶避障约束空间是非凸的
(a) 静态和动态避障约束空间都不是凸空间
静态避障：

动态避障：

(b) 求解非凸问题的主要思路是找非凸问题中的凸结构

非凸空间 => 离散化 => 粗解(base) => 迭代 => 最终解

启发式算法：
先随机在约束空间采样一些离散的函数值比大小，取最小的作为迭代初值。

2.3 直角坐标与自然坐标转换

龙格现象：高次多项式拟合可能会出现震荡，慎用高次多项式。

以Cartesian坐标为准

$\overrightarrow{r_n}$：车的位矢
$\vec{v}$：车的速度
$\vec{a}$：车的加速度
$k_h$：车的位矢在车的轨迹上的曲率
$\overrightarrow{{\tau }_h}$：车的位矢在车的轨迹上的切线方向单位向量
$\overrightarrow{n_h}$：车的位矢在车的轨迹上的法线方向单位向量
$\overrightarrow{r_r}$：投影位矢
$\dot{S}$：投影速率
$k_r$：投影位矢在道路几何上的曲率
$\overrightarrow{{\tau }_r}$：投影位矢在道路几何上的切线方向单位向量
$\overrightarrow{n_r}$：投影位矢在道路几何上的法线方向单位向量

已知 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,k_h,{\vec{\tau }}_h,{\vec{n}}_h$
己知 frenet 坐标系起点 $\left(x_0,y_0\right)$
求 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }\dot{s}=\frac{ds}{dt}{l}^{\prime }=\frac{dl}{ds}$ $ds$ 为frenet坐标轴的弧长

主要思路：
（1）找车在Frenet坐标系下的投影点在笛卡尔坐标下的坐标，记为$(x_r,y_r,k_r,{\theta }_r)$
（2）利用向量三角形，以及微积分求$s,\dot{s},\ddot{s},l,l_{}^{\prime },l_{}^{\prime }$
核心公式：${\vec{r}}_r+l{\vec{n}}_r={\vec{r}}_h$

7个辅助公式：
$$\begin{array}{l}{\overrightarrow{\dot{r}}}_h=\vec{v}=|\vec{v}|{\vec{\tau }}_h\\ {\vec{\dot{r}}}_r=\dot{s}{\vec{\tau }}_r\\ {\vec{\tau }}_h=k_h|\vec{v}|{\vec{n}}_h\\ {\vec{n}}_h=-k_h|\vec{v}|{\vec{\tau }}_h\\ {\vec{\dot{\tau }}}_r=k_r\dot{s}{\vec{n}}_r\\ {\overrightarrow{\dot{n}}}_r=-k_r\dot{s}{\vec{\tau }}_r\\ \vec{a}=|\vec{\dot{v}}|{\vec{\tau }}_h+|\vec{v}{|}^2{k}_h{\vec{n}}_h\end{array}$$
向量法详细推导见博客
$s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l_{}^{\prime },l_{}^{\prime \prime }$八个变量，独立的只有6个，因为$\dot{l},\ddot{l}$可以由$l_{}^{\prime },l_{}^{\prime \prime }$推导过来
$$\begin{gathered} \dot{l}=l^{\prime }\ast \dot{s} \\ \begin{array}{rl}\ddot{l}=\frac{dl}{dt}=\frac{d\left(l^{\prime }\dot{s}\right)}{dt}& =\frac{d{l}^{\prime }}{dt}\dot{s}+l^{\prime }\cdot \ddot{s}\\ & =\frac{d{l}^{\prime }}{ds}\cdot \frac{ds}{dt}\cdot \dot{s}+l^{\prime }\ddot{s}\\ & =l^{\prime \prime }{\dot{s}}^2+l^{\prime }\ddot{s}\end{array} \end{gathered}$$
EM planner：$s,\dot{s},\ddot{s},l,l_{}^{\prime },l_{}^{\prime }$
Lattice planner：$s,\dot{s},\ddot{s},l,\dot{l},\ddot{l}$

s的计算方法：
主要思路：
用直线长度代替弧长，当路径点足够密集，误差可以忽略

这里直接给出Frenet和Cartesian互转的公式：

百度Apollo Frenet和Cartesian互转的代码：
modules/common/math/cartesian_frenet_conversion.h

static void cartesian_to_frenet(const double rs, const double rx,
                                  const double ry, const double rtheta,
                                  const double rkappa, const double rdkappa,
                                  const double x, const double y,
                                  const double v, const double a,
                                  const double theta, const double kappa,
                                  std::array<double, 3>* const ptr_s_condition,
                                  std::array<double, 3>* const ptr_d_condition);

static void frenet_to_cartesian(const double rs, const double rx,
                                  const double ry, const double rtheta,
                                  const double rkappa, const double rdkappa,
                                  const std::array<double, 3>& s_condition,
                                  const std::array<double, 3>& d_condition,
                                  double* const ptr_x, double* const ptr_y,
                                  double* const ptr_theta,
                                  double* const ptr_kappa, double* const ptr_v,
                                  double* const ptr_a);                                  

modules/common/math/cartesian_frenet_conversion.cpp

void CartesianFrenetConverter::cartesian_to_frenet(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa, const double x, const double y,
    const double v, const double a, const double theta, const double kappa,
    std::array<double, 3>* const ptr_s_condition,
    std::array<double, 3>* const ptr_d_condition) {
  const double dx = x - rx;
  const double dy = y - ry;

  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);

  const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
  ptr_d_condition->at(0) =
      std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

  const double delta_theta = theta - rtheta;
  const double tan_delta_theta = std::tan(delta_theta);
  const double cos_delta_theta = std::cos(delta_theta);

  const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);
  ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;

  const double kappa_r_d_prime =
      rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);

  ptr_d_condition->at(2) =
      -kappa_r_d_prime * tan_delta_theta +
      one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
          (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);

  ptr_s_condition->at(0) = rs;

  ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;

  const double delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
  ptr_s_condition->at(2) =
      (a * cos_delta_theta -
       ptr_s_condition->at(1) * ptr_s_condition->at(1) *
           (ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /
      one_minus_kappa_r_d;
}

void CartesianFrenetConverter::frenet_to_cartesian(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa,
    const std::array<double, 3>& s_condition,
    const std::array<double, 3>& d_condition, double* const ptr_x,
    double* const ptr_y, double* const ptr_theta, double* const ptr_kappa,
    double* const ptr_v, double* const ptr_a) {
  ACHECK(std::abs(rs - s_condition[0]) < 1.0e-6)
      << "The reference point s and s_condition[0] don't match";

  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);

  *ptr_x = rx - sin_theta_r * d_condition[0];
  *ptr_y = ry + cos_theta_r * d_condition[0];

  const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];

  const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;
  const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);
  const double cos_delta_theta = std::cos(delta_theta);

  *ptr_theta = NormalizeAngle(delta_theta + rtheta);

  const double kappa_r_d_prime =
      rdkappa * d_condition[0] + rkappa * d_condition[1];
  *ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *
                 cos_delta_theta * cos_delta_theta) /
                    (one_minus_kappa_r_d) +
                rkappa) *
               cos_delta_theta / (one_minus_kappa_r_d);

  const double d_dot = d_condition[1] * s_condition[1];
  *ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *
                         s_condition[1] * s_condition[1] +
                     d_dot * d_dot);

  const double delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;

  *ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +
           s_condition[1] * s_condition[1] / cos_delta_theta *
               (d_condition[1] * delta_theta_prime - kappa_r_d_prime);
}

3 参考线reference_line

3.0 前言

决策规划流程：
① 全局规划生成一条参考线
② 静态障碍物投影到以参考线为坐标轴的Frenet坐标系上
③ 决策算法对障碍物做决策（往左绕，往右绕，忽略）开辟最优凸空间
④ 规划算法在凸空间搜素出最优的路径
⑤ 后处理，在规划轨迹中选一个点，坐标转成Cartesian，输出给控制跟踪

需要参考线的原因：
（1）参考线是解决方案，能够解决导航路径过长，不平滑的问题
（2）过长的路径不利于坐标转换(找匹配点，计算s的长度)
（3）过长的路径，障碍物投影可能不唯一
（4）路径不平滑

在每个规划周期内，找到车在导航路径上的投影点，以投影点为坐标原点，往后取30m，往前取150m范围内的点，做平滑，平滑后的点的集合称为参考线。

3.1 参考线平滑算法

参考线的三个代价函数：
（1）平滑代价

$|\overrightarrow{P_1{P}_3}|$衡量平滑与不平滑的标准。$|\overrightarrow{P_1{P}_3}|$越小，越平滑。

（2）紧凑型代价

长度尽可能均匀、紧凑

（3）相似代价

$|\overrightarrow{P_1{P}_{1r}}|+|\overrightarrow{P_2{P}_{2r}}|+|\overrightarrow{P_3{P}_{3r}}|$越小，越接近原路径几何。

总结：
$x_1,x_2,x_3$为优化后的路径点。希望cost function越小越好

本质是二次规划问题
$$\begin{array}{l}\frac{1}{2}{X}^{T}HX+f^{T}X=X^T(\frac{H}{2})X+f^{T}X\end{array}$$

令 $H=2\left(w_{\text{smooth }}{A}_1^T{A}_1+w_{\text{length }}{A}_2^T{A}_2+w_{\text{ref }}{A}_3^T{A}_3\right),f^T=w_{\text{ref }}{h}^T$

约束：
$x=(x_1,y_1,...,x_n,y_n)$
$x_{ref}=(x_{1r},y_{1r},...,x_{nr},y_{nr})$
$x$与$x_{ref}$不要太远

曲率约束：非线性约束(一般与车的最大侧向加速度有关)

3.2 Fem smooth平滑

在Apollo中叫 Fem smoother
优势：优化变量少
缺点：无法保证曲率是连续的，添加曲率约束求解麻烦

主要步骤：
（1）找匹配点
（2）采样（前150m，后30m）
（3）Fem smooth 平滑
目标：快，节省计算量（参考线是决策规划的基础和前提，因此必须要快）

怎么快？
（1）减少规划频率，规划算法会100ms执行一次，控制算法每10ms执行一次
（2）充分利用上一个规划周期的结果

一般做法：每个周期做遍历，但是这样速度慢，导航路径可能很长，遍历很费时

改进：
如何充分利用上一个规划周期的结果

以上个周期的match point为起点做遍历，一旦有$l_{i+1}>l_i$，立即退出遍历。$l_i$对应的点作为本周期的匹配点。
判断遍历的方向：

$$\begin{gathered} a)向前遍历：\overrightarrow{\mathrm{d}}\cdot \vec{\tau }>0 \\ b)向后遍历：\overrightarrow{\mathrm{d}}\cdot \vec{\tau }<0 \\ c)本周期的匹配点=上周期的匹配点：|\overrightarrow{\mathrm{d}}\cdot \vec{\tau }|<1e-3 \end{gathered}$$
本方法可以大大加速找匹配点
带来的问题：只适用于上个匹配点结果附近只有一个极小值点

一般情况不会有这种问题，规划周期100ms，即使车速50m/s，车也只走了五米。五米的道路不会像上图一样扭曲的道路几何，在上一个规划周期的匹配点一般只有一个极小值。
改进：
增加increase变量记录l连续增加的次数

在Github算法里
（1）遍历
第一次运行遍历

不是第一次运行

（2）点不够的情况

（3）参考线拼接

如何拼接：


注意：
Fem smoother至少需要3个点

3.3 避障

3.3.1 规划起点

确定规划起点：
错误做法：定位得到的host_x,host_y投影到referenceline，得到SL坐标(0,$l_0$)，以此点为路径规划的起点
正确做法：判断当前定位和上一个周期规划的结果是否误差太大



以（$s_0,l_0$）作为规划的起点
为什么不能从定位的点作为规划起点：考虑到控制不完美，导致帧与帧之间的曲线是跳变的。

判断定位与轨迹的误差不太大的话，用上个轨迹的T+100ms的点作为本周期的规划起点，若误差较大，用车辆运动学进行外推

3.3.2 轨迹拼接

结合上一节，本人觉得理解老王对于规划起点的描述有点模糊，故拜读了下面这篇大佬写的博客，对于轨迹拼接思路一下子就通了。

参考博客：百度Apollo规划算法——轨迹拼接
在apollo的规划算法中，在每一帧规划开始时会调用一个轨迹拼接函数，返回一段拼接轨迹点集，并告诉我们是否重规划以及重规划的原因。

轨迹拼接：在每一帧轨迹规划的开始，我们需要确定规划的起始点$p_0=(x,y,v,a,\theta ,k,t)$，起始点给定了规划的初始状态，然后结合当前帧的环境信息并调用规划算法规划生成当前帧轨迹。

方案1
根据ADC（Autonomous Driving Car）实际位置状态信息，通过车辆运动学推导100ms后的位置状态信息，作为规划起始点状态，在Apollo中，这一方案叫做重规划Replan。
方案2：考虑的是连续帧之间轨迹的平滑性和连续性
根据当前帧时间戳以及ADC实际位置信息，在上一帧轨迹中寻找匹配点，将上一帧轨迹中匹配点往后100ms所对应的轨迹点作为当前帧的规划起始状态点，待当前帧规划生成轨迹后，再和上一帧轨迹中所选择的规划起始点前一段距离的轨迹点进行拼接，形成一条完整的车辆运动轨迹(思路一下就清晰了，牛逼！)，发送给下游控制模块。

理论上，如果控制跟踪和定位完美，不会出现任何误差，则完全可以使用方案1选择规划起始点，但是，由于定位误差和控制误差/滞后的原因，会导致每一规划时刻ADC位置大概率的偏离上一帧所规划的轨迹点，如果此时仍采用方案1，会导致当前帧所规划轨迹和上一帧规划轨迹的不平滑和不连续，从而导致控制的抖动，也可能会引起ADC行驶轨迹发散，跟踪误差越来越大，偏离设定参考线。而方案2可以保证ADC附近一段范围内前后两帧轨迹的平滑性，从而不断引导ADC沿着期望轨迹行驶。

3.3.3 结合Apollo代码理解轨迹拼接的细节

选择方案1的情况：

1. 上一帧不存在轨迹（一般是起步或上一帧规划失败的情况）
2. 没有使能方案2进行轨迹拼接
3. 没有处在自动驾驶模式
4. 上一帧存在轨迹但是没有轨迹点
5. 当前时间相对于上一帧的相对时间戳小于上一帧起点相对时间戳
6. 时间匹配点超出上一帧轨迹点的终点实际范围
7. 匹配点处不存在匹配的path_point
8. ADC实际位置和匹配点横纵向偏差超过给定阈值

方案1生成轨迹拼接点主要有两种情况：
① 起步阶段：由于起步阶段属于规划算法执行的第一帧，且速度、加速度值很小，可以直接使用当前状态点作为规划的起始状态点
② 非起步阶段：则根据当前ADC状态点结合运动学模型外推T时间之后的状态点作为本帧规划起始点

$$\left[\begin{array}{l}{x}_{t+1}\\ y_{t+1}\\ {\psi }_{t+1}\end{array}\right]=\left[\begin{array}{l}{x}_t\\ y_t\\ {\psi }_t\end{array}\right]+\left[\begin{array}{c}\cos (\psi )\\ \sin (\psi )\\ \frac{\tan \left({\delta }_f\right)}{l}\end{array}\right]\cdot v$$

方案二（以上情况均不满足时）：
该方案同时考虑了时间匹配和位置匹配
时间匹配：根据当前时间戳和上一帧轨迹起点的时间戳对比，计算当前时间ADC应该到达的时间匹配点（图中t_pos）及该匹配点在上一帧轨迹中对应的索引值t_index，若t_pos不存在路径点，则选择方案1。
位置匹配：根据ADC当前位置点在上一帧轨迹中寻找最近的匹配点（图中p_pos）及该匹配点在上一帧轨迹中对应的索引值p_index，并比较实际位置点和匹配点之间的横纵向位置偏差，若任一偏差超过给定阈值，则选择方案1。

若以上判断均通过，根据 （当前时刻的绝对时间 - 上一时刻轨迹的初始时间 + planning_cycle_time） 得到forward_rel_time，选取时间匹配索引和位置匹配索引中的较小索引作为匹配点索引值matched_index，在上一帧的轨迹上截取出matched_index往前n个点开始，至forward_rel_time的一段轨迹，作为stitching_trajectory。最后，遍历这段轨迹，对其relative_time和s进行赋值。最终，使用stitching_trajectory的最后一个点，即图中的$p_0$，作为当前帧轨迹规划的起始点。

选择当前时间+planning_cycle_time的时间对应的上一帧轨迹的位置点作为本帧规划起点的原因：当本帧规划需要时间planning_cycle_time，将生成轨迹送到规划模块时对应的实际时间理论上就是当前时间+planning_cycle_time。

TrajectoryStitcher::ComputeStitchingTrajectory

/* Planning from current vehicle state if:
1. the auto-driving mode is off
(or) 2. we don't have the trajectory from last planning cycle
(or) 3. the position deviation from actual and target is too high
*/
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
const VehicleState& vehicle_state, const double current_timestamp,
const double planning_cycle_time, const size_t preserved_points_num,
const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
std::string* replan_reason) {
    //a.是否使能轨迹拼接
    if (!FLAGS_enable_trajectory_stitcher) {
        *replan_reason = "stitch is disabled by gflag.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //b.上一帧是否生成轨迹
    if (!prev_trajectory) {
        *replan_reason = "replan for no previous trajectory.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //c.是否处于自动驾驶模式
    if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        *replan_reason = "replan for manual mode.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //d.上一帧是否存在轨迹点 
    size_t prev_trajectory_size = prev_trajectory->NumOfPoints();
    if (prev_trajectory_size == 0) {
        *replan_reason = "replan for empty previous trajectory.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    const double veh_rel_time = current_timestamp - prev_trajectory->header_time();
    size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

    //e.判断当前时间相对于上一帧的相对时间戳是否小于上一帧起点相对时间戳
    if (time_matched_index == 0 &&
        veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
        *replan_reason =
            "replan for current time smaller than the previous trajectory's first "
            "time.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }
    
    //f.判断时间匹配点是否超出上一帧轨迹点范围
    if (time_matched_index + 1 >= prev_trajectory_size) {
        *replan_reason =
            "replan for current time beyond the previous trajectory's last time";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    auto time_matched_point = prev_trajectory->TrajectoryPointAt(
    static_cast<uint32_t>(time_matched_index));

    //g.判断时间匹配点处是否存在path_point
    if (!time_matched_point.has_path_point()) {
        *replan_reason = "replan for previous trajectory missed path point";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
    {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);

    //计算实际位置点和匹配点的横纵向偏差
    auto frenet_sd = ComputePositionProjection(
    vehicle_state.x(), vehicle_state.y(),
    prev_trajectory->TrajectoryPointAt(
    static_cast<uint32_t>(position_matched_index)));

    //h.判断横纵向偏差
    if (replan_by_offset) {
        auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
        auto lat_diff = frenet_sd.second;
        //h.1:横向偏差不满足条件
        if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) {
            const std::string msg = absl::StrCat(
            "the distance between matched point and actual position is too "
            "large. Replan is triggered. lat_diff = ",
            lat_diff);
            *replan_reason = msg;
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
    	}
        //h.2:纵向偏差不满足条件
        if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) {
            const std::string msg = absl::StrCat(
            "the distance between matched point and actual position is too "
            "large. Replan is triggered. lon_diff = ",
            lon_diff);
            *replan_reason = msg;
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
    	}
    } else {
        ADEBUG << "replan according to certain amount of lat and lon offset is "
        "disabled";
    }

    //计算当前时刻后T时刻的时间，并计算其在上一帧轨迹中对应的索引值
    double forward_rel_time = veh_rel_time + planning_cycle_time;
    size_t forward_time_index =
    prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

    ADEBUG << "Position matched index:\t" << position_matched_index;
    ADEBUG << "Time matched index:\t" << time_matched_index;

    //选择时间匹配索引和位置匹配索引中的较小索引作为匹配点索引
    auto matched_index = std::min(time_matched_index, position_matched_index);

    //构建拼接轨迹，<匹配索引点前n个点，当前时刻后的T时刻所对应的匹配点>
    std::vector<TrajectoryPoint> stitching_trajectory(
    prev_trajectory->begin() +
    std::max(0, static_cast<int>(matched_index - preserved_points_num)),
    prev_trajectory->begin() + forward_time_index + 1);

    const double zero_s = stitching_trajectory.back().path_point().s();
    for (auto& tp : stitching_trajectory) {
        if (!tp.has_path_point()) {
            *replan_reason = "replan for previous trajectory missed path point";
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
        }
        //适配时间和s值
        tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
        current_timestamp);
        tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
    }
    return stitching_trajectory;
}

3.4 避障

避障问题是非凸问题，存在多个极小值点，要怎样找到最终解？找粗解

3.4.1 避障流程

避障流程：
① 在离散空间上求最优路径，此最优路径开辟一个凸空间，在此凸空间上做二次规划，离散空间的最优路径称为粗解，粗解开辟凸空间，在这个凸空间上开辟最优解。
② 在离散空间上找到粗解：DP;在凸空间上优化最终解：QP

连接两点的曲线：五次多项式
$l=f(s)=a_0+a_{1}s+a_2{s}^2+...+a_5{s}^5$

3.4.2 代价函数

代价函数：评价路径的优劣

① 平滑代价$f()$
越像直线越平滑，两点之间直线最短，期望$\int \sqrt{\left(1+f^{\prime }(x{)}^2\right)}$最小，等价于$\int \sqrt{\left(f^{\prime }(x{)}^2\right)}$最小，即$\sum f^{\prime }{\left(s_i\right)}^2$最小
② 障碍物距离代价$g()$

③ 参考线距离代价$l()$

代价函数

图的最短问题：
① 每个路径都有对应的cost，问题变成了找图最优问题：A*，Dijstra
②EM planner用动态规划解决了这类问题

控制接口：
规划发出的轨迹信息包含$x,y,\theta ,k,v,a,t$，时间为绝对时间。控制根据当前控制周期的时间搜索轨迹

3.5 自动驾驶之轻决策与重决策

3.5.1 二次规划

求解使二次型$\frac{1}{2}{x}^{T}Hx+f^{T}x$值最小的x，并且满足约束$Ax\le ub$

无需在意二次规划的约束形式（$lb\le ub，a\le b，Ax\le b$）

二次规划要求解空间是凸的，而动态规划（决策）开辟了凸空间。

3.5.2 轻决策与重决策

决策算法：轻决策（基于代价函数）和重决策（基于人给定的规则）
重决策

优点：
（1）计算量小
（2）在感知不强的情况下仍然能做决策（融合了人的智慧）
缺点：
（1）场景多无法覆盖
（2）人给出的决策所开辟的凸空间未必满足约束

if-else条件语句，不利于最优控制问题的求解。

轻决策

轻决策：无先验规则，空间离散化，设计cost function,动态规划算法求解离散空间的最优路径，该最优路径开辟凸空间。

优点：
（1）无人为规则，可以处理复杂场景
（2）有粗解，通过设计代价函数，可以使粗解“满足”硬约束（碰撞、最小曲率），这样使二次规划求解成功的机率大大增加（因为粗解在凸空间内部，所以该凸空间的“扭曲”程度至少有粗解兜底）大大缓解了基于人为规则的决策所造成的凸空间扭曲情况
缺点：
（1）复杂场景计算量很大
（2）依赖预测
（3）对感知，定位要求高
（4）代价函数设计 / 最优解未必符合人的驾驶习惯

L2+ 高速L3 => 重决策
L4 => 轻决策
轻决策和重决策的融合（博弈论）

3.5.3 二次规划算法

$l_{min1},l_{min2},...,l_{max1}.l_{max2},...构成凸空间$
二次规划的求解空间就在此凸空间中搜索

Apollo 3.5/5.0新增，f(s)尽可能在凸空间的中央
分段加加速度优化法




设有$l_1,l_1^{{}^{\prime }},l_1^{{}^{\prime \prime }},...,l_n,l_n^{{}^{\prime }},l_n^{{}^{\prime \prime }}$需要优化
则分段加加速度约束为

记为$A_{eq}x=b_{eq}$等式约束

这样做并不准确


$d_1$：汽车质心到车头距离
$d_2$：汽车质心到车尾距离
$w$：汽车宽度








记为$Ax\le b$

求解出$l_1,l_1^{{}^{\prime }},l_1^{{}^{\prime \prime }},...,l_n,l_n^{{}^{\prime }},l_n^{{}^{\prime \prime }}$与$s_1,s_2,...,s_n$结合即得到二次规划下的最优路径，再转化为Cartesian坐标，路径规划完成。

3.5.4 二次规划崩溃

靠近障碍物时，二次规划崩溃


为什么在无避障时不会崩溃？

为什么规划的路径会贴着边界？

因为二次规划的性质决定，带约束的二次规划最小值只会在极小值点或边界点取得

改善二次规划崩溃的方法：
① 规划起点不管了，障碍物放大一点做安全缓冲

② 改极小值点的位置

$centr{e}_{}line=\frac{1}{2}(l_{min}+l_{max})$
优点：几何稳定
缺点：$w_{centre}$调小不起明显作用，调大了路径不平滑。

优点：dp曲线平滑相对好做
缺点：$d{p}_{path}$几何不稳定

dp_path每一帧之间的几何形状都会有巨大变化，若二次规划以dp_path为centre line,qp_path也会震荡。相反，$centr{e}_{}line=\frac{1}{2}(l_{min}+l_{max})$就很稳定

3.5.5 车控制不稳，抖动剧烈

根源：车规划的路径会天然往边界靠拢的趋势

车在边界上当方唐镜，左右横跳。
解决办法：
加不允许反复横跳的约束

新的问题：约束越多，求解越慢
（1）搜索空间增大，需要对更多的可能性进行搜索和验证
（2）约束之间的冲突，需要进行额外的处理来解决这些冲突
（3）优化目标的复杂性，约束条件的增加可能会导致优化目标的复杂性增加

解决：削二次规划的规模，由60 => 20，二次规划求解完毕后再进行插值增密。

3.5.6 决策“朝令夕改”

一会决策向右绕行，一会决策向左绕行

Apollo方法：打标签
这一帧：只对无标签的obs做动态规则决策，若障碍物已经有标签，直接使用标签的决策

3.5.7 速度规划对路径规划的影响

Path只处理静态障碍物，有问题
这种情况无论速度规划怎么规划，都会碰撞（除非倒车）

已知上一帧的速度规划为10m/s匀速，上一帧的path为直线

路径规划要拿上一帧的轨迹去判断与动态障碍物的交互

EM planner是SL planning与ST planning反复迭代的过程

上一帧的SL，ST => 当前SL => 当前ST => 下一帧SL

4 SL、ST迭代优化

4.1 ST图、预测

已有cartesian的path，作速度规划

ST图画法

4.2 SL、ST迭代求解

当前帧规划端


当前帧的规划结果是下一帧的上一次的规划结果


SL、ST迭代过程
上一帧的trajectory + 当前帧的预测 => 当前帧SL的虚拟障碍物 => 当前帧SL（几何受影响）=> 当前帧ST => 改变当前帧ST => 当前帧轨迹trajectory改变

下一帧拿当前帧的trajectory + 下一帧的预测 => 下一帧SL => 下一帧ST

EM planner核心思想
① 将SLT三维规划问题降维，分解成两个二维的SL、ST规划问题大大降低了规划难度
② 上一帧的trajectory优化当前帧的SL、SL优化ST，不断迭代

4.3 SL与ST迭代

规划的基本工具：ST图
ST图依赖预测(预测、博弈、感知最难)
预测模型：恒定 $\vec{v}$ 或者恒定 $\vec{a}$ 模型（简化问题）

SL、ST迭代问题

Apollo 1.5 EM planner 引入SL,ST解耦规划，凸空间，先决策后规划。在一些复杂工况以及强算力的这个场景下，SL和ST分开优化求解是工业界最广泛最流行的方法。
Apollo 2.5 加入了lattice planner
Apollo 3.5 大幅修改EM planner，变成了public road planner(取消了SL,ST迭代机制，SL只管静态障碍物，ST只管动态障碍物,加入scenario模块)
Apollo 5.0 再次大幅修改public road planner，SL的dp被取消
Apollo 6.0~7.0 改动不大

取消了SL,ST迭代机制：

减轻ST对SL的干扰
因为车的纵向变化能力高于横向，导致动态障碍物车的速度变化剧烈程度远高于路径，预测难以测准，预测不准导致ST图剧烈变化=>速度规划结果剧烈变化
若ST影响SL，会导致SL规划容易不稳定，朝令夕改

车百公里加速：4-5秒 汽油车：5-7秒
百公里刹停距离：32-45m不等
证明汽车在一个非常短的时间、距离内，纵向的速度可以发生剧烈的变化。变化能力远高于横向的。

SL只管静态，ST只管动态，是一种兼顾灵活性和稳定性的一种做法

有没有一种方法既能保证路径稳定，又能保证速度稳定，还能应对这个剧烈环境（动态障碍物速度）的剧烈变化，做出相应的完美策略？一般没有。

这种方法SL还是会影响ST，ST不影响SL，因为SL会生成一个路径，路径的几何形状会影响到动态障碍物的交互。所以SL影响ST，ST不影响SL。

有两种场景ST需要影响SL：
① 上一帧的host trajectory与obs trajectory在纵向s有交互的区域内，obs所走过的空间仍然是host不能去的。


② 纵向有相交，相向而行


下一帧

特殊情况：

同向而行，动态障碍物很快的方向向前行驶，主车更快，Apollo认为这种情况过于危险，通过变道解决。

---

百度Apollo EM planner决策规划算法到这里，理论结束。不正之处望读者指教