1. 速度规划算法总体介绍

    Apollo中对路径规划解耦，分为路径规划与速度规划两部分。并将规划分为决策与优化两个部分。
• 路径规划 —— 静态环境（道路，静止/低速障碍物）
• 速度规划 —— 动态环境（中/高速障碍物）

## 1.1 速度规划的坐标系

速度规划的坐标系

    注意：速度规划的 $s$沿着轨迹的方向，路径规划的 $s$沿着参考线的方向。

1.2 不同场景下的ST图

1.2.1 主车向前匀速行驶

1.2.2 主车先向前匀速行驶，后停车

1.2.3 主车跟随前车行驶

ps:蓝色四边形为障碍车在ST图下的投影。长边的斜率代表车速，短边代表障碍车在主车规划出的路径中占据的长度。

1.2.4 主车跟随前车刹停

1.2.5 障碍车在主车后方跟行

1.3 速度规划算法整体流程

    路径规划的配置文件在lane_follow_config.pb.txt中

// /home/yuan/apollo-edu/modules/planning/conf/scenario/lane_follow_config.pb.txt

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
//路径规划
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
 //速度规划 
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

_DECIDER结尾的为决策部分 _OPTIMIZER结尾的为优化部分。

1.3.1 Task： SPEED_BOUNDS_PRIORI_DECIDER

产生速度可行驶边界

    所形成的区域是非凸的，不能用之前凸优化的方法去做，需要用动态规划的方法去做。

1.3.2 Task：SPEED_HEURISTIC_OPTIMIZER

动态规划规划目标

• 加速度尽可能小
• 离障碍物纵向距离尽可能远
• 满足车辆加减速度要求
• 满足限速要求

产生粗糙速度规划曲线

1.3.3 Task： SPEED_DECIDER

产生速度决策
    根据粗规划出的速度曲线，依据曲线在障碍物的上方还是下方，采取不同的决策。

1.3.4 Task： SPEED_BOUNDS_FINAL_DECIDER

产生速度规划边界
    在障碍物的上方或下方确定可行使区域。

1.3.5 Task： PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER && PIECEWISE_JERK_SPEED_OPTIMIZER

产生平滑速度规划曲线
    根据ST图的可行驶区域，优化出一条平滑的速度曲线。满足一阶导、二阶导平滑（速度加速度平滑）；满足道路限速；满足车辆动力学约束。
PIECEWISE_JERK_SPEED_OPTIMIZER 基于二次规划的速度规划
PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER 基于非线性规划的速度规划
两者二选一即可

1.3.6 Stage: CombinePathAndSpeedProfile

将SL曲线、ST曲线合成为完整轨迹，之后作为Planning的输出。

2. 基于动态规划的速度规划

2.1 动态规划

动态规划——通过把原问题分解为相对简单的子问题，再根据子问题的解来求解出原问题解的方法

状态转移方程
f ( P ) = min ⁡ { f ( R ) + w R → P } f(P) = \min \{ f(R) + {w_{R \to P}}\} f(P)=min{f(R)+wR→P​}

2.2 基于动态规划的速度规划

    基于动态规划的速度规划的流程如下：
1.对路程和时间进行采样
2.搜索出粗略的可行路线
3.选出代价最小的一条

2.2.1 对路程和时间进行采样

    速度规划在ST图进行采样，在$t$的方向上以固定的间隔进行采样，在$s$方向上以先密后疏的方式进行采样（离主车越近，所需规划的精度就需更高；离主车越远，牺牲采样精度，提升采样效率）

// 时间采样的一般参数设置
unit_t: 1.0  //采样时间
dense_dimension_s: 101  // 采样密集区域的点数
dense_unit_s: 0.1   //采样密集区域的间隔
sparse_unit_s: 1.0  //采样系数区域的间隔

2.2.2 设计状态转移方程

2.2.2.1 障碍物cost计算

S_safe_overtake超车的安全距离
S_safe_follow跟车的安全距离
    在设计状态转移方程时，要求不能与障碍物发生碰撞以及和障碍物不发生碰撞。于是可以得到以下方程：

$i$是$t$方向上的索引，$j$是$s$方向上的索引

    如果在障碍物距离之内，则cost设为无穷；如果在安全距离之外，则cost设为0；如果在安全距离与障碍物之间，则按按之间的距离计算。

2.2.2.2 距离cost计算

    目的是更快的到达目的地
    距离cost计算方式如下

$$C_{spatial}=w_{spatial}(s_{total}-s(j))$$$w_{spatial}$为损失权值
$(s_{total}-s(j))$当前点到目标点的差值。

2.2.2.3 状态转移cost计算

    状态转移cost计算分为三个部分：
$$C_{edge}=C_{speed}+C_{acc}+C_{jerk}$$$C_{speed}$——速度代价
$C_{acc}$——加速度代价
$C_{jerk}$——加加速度代价
节点间速度为：$v=\frac{s(j+k)-s(j)}{\Delta t}$
限速比率：$v_{\det }=\frac{v-v_{limit}}{v_{limit}}$

    $C_{speed}$速度代价的计算如下：
    若速度<0，则是倒车的状况，轨迹不可行，代价值设为无穷大；若速度>0,且高于限速，则会有超速的惩罚；若速度<0,且低于限速，则会有低速的惩罚。在Apollo中，超速的惩罚值（1000）远大于低速的惩罚值（10）。

    加速度的计算如下：
$$a(i+1,j+k)=\frac{\frac{s(k+j)-s(j)}{\Delta t}-\frac{s(j)-s(l)}{\Delta t}}{\Delta t}$$    $C_{acc}$加速度代价的计算如下：    若超过最大加速度或小于最小加速度，则代价值设为无穷大，若在之间，Apollo设计了这样的代价函数进行计算：$$y=x^2+\frac{x^2}{1+e^{x+4}}+\frac{x^2}{1+e^{x+2}}$$    其函数图像如下：    越靠近0，代价值越小；越靠近目标值，代价值越大，满足舒适性与平滑性。
    加加速度的计算方式如下：$$jerk=\frac{s_4-3s_3+3s_2-s_1}{\Delta t^3}$$
    加加速度超过设定边界，设为无穷；若在之间，则按二次方的方式进行计算。加加速度越小越好。

    最后是总的代价：    迭代范围：    在每次迭代时会将总的代价与当前节点的代价进行比较，取最小的一个，进行更新。
    从$s(i,j)$到$s(i+1,j+k)$可以拓展到速度范围内的节点，按代价值的大小进行更新，最后按最后一列代价值最小的点进行求解，再进行回溯，得到ST曲线。

3. 基于二次规划的速度规划

    动态规划得到的轨迹还比较粗糙，需要用优化的方法对轨迹进行进一步的平滑。基于二次规划的速度规划的方法与路径规划基本一致。

1. 确定优化变量
2. 设计目标函数
3. 设计约束

3.1 确定优化变量

    优化变量$x$，$x$有三个部分组成：从$s_0$,$s_1$,$s_2$到$s_{n-1}$，从${\dot{s}}_0$,${\dot{s}}_1$,${\dot{s}}_2$到${\dot{s}}_{n-1}$,从${\ddot{s}}_0$,${\ddot{s}}_1$,${\ddot{s}}_2$到${\ddot{s}}_{n-1}$.ps：三阶导的求解方式为：$\frac{{s^{\prime \prime }}_{i+1}-{s^{\prime \prime }}_i}{\Delta t}$

3.2 设计目标函数

    对于目标函数的设计，我们需要明确以下目标：

• 尽可能贴合决策时制定的st曲线：$\left|s_i-s_{i-ref}\right|\downarrow$
• 确保舒适的体感，尽可能降低加速度/加加速度：$\left|{\ddot{s}}_{i+1}\right|\downarrow$， $\left|{s^{\prime \prime \prime }}_{i\to i+1}\right|\downarrow$
• 尽可能按照巡航速度行驶：$\left|{\dot{s}}_i-v_{ref}\right|\downarrow$
• 在转弯时减速行驶, 曲率越大，速度越小：$\left|p_i{\dot{s}}_i\right|\downarrow$

    最后会得到以下目标函数：

$w_s$——位置的权重
$w_v$——速度的权重
$p_i$——曲率的权重
$w_a$——加速度的权重
$w_j$——加加速度的权重

3.3 要满足的约束条件

    接下来谈谈约束的设计。
    要满足的约束条件：
• 主车必须在道路边界内，同时不能和障碍物有碰撞$$s_i\in (s_{\min }^i,s_{\max }^i)$$• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：•必须满足基本的物理原理：

•起始点约束：;$s_0=s_{init}$,${\dot{s}}_0=s_{init}$,${\ddot{s}}_0=s_{init}$满足的是起点的约束，即为实际车辆规划起点的状态。

3.4 转化为二次规划问题求解

    代入OSQP求解器进行求解，输出一条平稳、舒适、能安全避开障碍物并且尽快到达目的地的速度分配曲线。

4. 基于非线性规划的速度规划

    为了使得限速更加精细，Apollo提出了一种基于非线性规划的速度规划方法。

4.1 二次规划速度规划算法的问题

    基于二次规划的速度规划中，$p_i$是曲率关于时间$t$的函数，但实际上路径的曲率是与$s$相关的。二次规划在原先动态规划出来的粗糙ST曲线上将关于$s$的曲率惩罚转化为关于$t$的曲率惩罚，如此，当二次规划曲线与动态规划曲线差别不大，规划出来基本一致；若规划差别大，则会差别很大。就如图所示，规划出来的区间差别较大。限速/曲率的函数是关于$s$的函数，而$s$是我们要求的优化量，只能通过动态规划进行转化，如此就会使得二次规划的速度约束不精确。

4.2 确定优化变量

    基于非线性规划的速度规划步骤与之前规划步骤基本一致。
    采样方式：等间隔的时间采样。$s_{lower}$与$s_{upper}$为松弛变量，防止求解失败。

4.3 定义目标函数

    目标函数与二次规划的目标函数差不多，增加了横向加速度的代价值以及松弛变量$w_{soft}{s}_{lower}$与$w_{soft}{s}_{upper}$。
    横向加速度的计算方式：
    曲率是关于$s$的关系式,所以要进行平滑，对于非线性规划的求解器，无论是目标函数还是约束函数，都需要满足二阶可导：$${\kappa }^{\prime }=f^{\prime \prime }(s)$$    曲率的平滑也是用到了二次规划的方法，用曲率的一阶导、二阶导、三阶导作为损失函数.    最后得到一条平滑曲率的曲线。

4.4 定义约束

    接下来是约束条件：

• 规划的速度要一直往前走：$s_i\le s_{i+1}$
• 加加速度不能超过定义的极限值：$jer{k}_{\min }\le \frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{\Delta t}\le jer{k}_{\max }$
• 速度满足路径上的限速：${\dot{s}}_i\le speed\_limit(s_i)$

    限速的函数并非直接可以得到，接下来看看限速函数是怎么来的。
    限速的来源如下图所示：    将所有的限速函数相加，得到下图的限速函数，很明显，该函数既不连续也不可导，所以需要对其进行平滑处理。    对于限速曲线的平滑，Apollo采样分段多项式进行平滑，之后采样二次规划的方式进行求解。限速曲线的目标函数如下：    如此，我们就有了连续且可导的限速曲线。
    再回到约束中,为了避免求解的失败，二次规划中对位置的硬约束，在非线性规划中转为了对位置的软约束。提升求解的精度。    同时还需满足基本的物理学原理

4.5 求解器求解

    最后代入Ipopt中进行非线性规划的求解。
    Ipopt（Interior Point Optimizer）是一个用于大规模非线性优化的开源软件包。它可用于解决如下形式的非线性规划问题：    $g^L$和$g^U$是约束函数的上界和下界，$x^L$和$x^U$是优化变量的上界和下界。

    Ipopt的求解由以下几个函数构成：

1.get_nlp_info()定义问题规模

/** Method to return some info about the nlp */
  bool get_nlp_info(int &n, int &m, int &nnz_jac_g, int &nnz_h_lag,
                    IndexStyleEnum &index_style) override;

• 优化变量数量：n
• 约束函数数量：m
• 雅可比矩阵非0项数量：nnz_jac_g
• 黑塞矩阵非0项数量：nnz_h_lag

2.get_bounds_info()定义约束边界约束

  /** Method to return the bounds for my problem */
  bool get_bounds_info(int n, double *x_l, double *x_u, int m, double *g_l,
                       double *g_u) override;

• 自变量的下边界：x_l
• 自变量的上边界： x_u
• 约束函数下边界：g_l
• 约束函数的上边界：g_u

3.get_starting_point()定义初值

  /** Method to return the starting point for the algorithm */
  bool get_starting_point(int n, bool init_x, double *x, bool init_z,
                          double *z_L, double *z_U, int m, bool init_lambda,
                          double *lambda) override;

• 定义优化变量的初始值x
    对于速度规划问题，如何计算初始解？
    Apollo同样用分段多项式二次规划的求解方式，得到符号约束的速度平滑曲线，作为非线性规划的初值。
4.eval_f()求解目标函数

  /** Method to return the objective value */
  bool eval_f(int n, const double *x, bool new_x, double &obj_value) override;

• 变量值：x
• 目标函数值：obj_val

5.eval_grad_f()求解梯度

  /** Method to return the gradient of the objective */
  bool eval_grad_f(int n, const double *x, bool new_x, double *grad_f) override;

• 变量值：x
• 梯度值：grad_f

梯度的定义：
目标函数：偏导数：

6.eval_g()求解约束函数

  /** Method to return the constraint residuals */
  bool eval_g(int n, const double *x, bool new_x, int m, double *g) override;

• 变量值：x
• 约束函数值：g

7.eval_jac_g()求解约束雅可比矩阵

  /** Method to return:
   *   1) The structure of the jacobian (if "values" is nullptr)
   *   2) The values of the jacobian (if "values" is not nullptr)
   */
  bool eval_jac_g(int n, const double *x, bool new_x, int m, int nele_jac,
                  int *iRow, int *jCol, double *values) override;

• 变量值：x
• 雅可比矩阵非0元素数量：nele_jac
• 雅可比矩阵值：values

雅可比矩阵：    求解器通过稀疏矩阵来保存值。

稀疏矩阵的例子

    求解雅可比矩阵需要对约束函数进行求偏导：

微分关系等式约束：

8.eval_h()求解黑塞矩阵

  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is
   * nullptr) 2) The values of the hessian of the lagrangian (if "values" is not
   * nullptr)
   */
  bool eval_h(int n, const double *x, bool new_x, double obj_factor, int m,
              const double *lambda, bool new_lambda, int nele_hess, int *iRow,
              int *jCol, double *values) override;

• 变量值：·x·
• 拉格朗日乘数：·lambda·
• 黑塞矩阵值：·values·
• 目标函数因数：·obj_factor·

黑塞矩阵：拉格朗日函数Ipopt的拉格朗日黑塞矩阵：
目标函数的二阶偏导数：

约束函数的二阶偏导数：

9. finalize_solution()

  /** @name Solution Methods */
  /** This method is called when the algorithm is complete so the TNLP can
   * store/write the solution */
  void finalize_solution(Ipopt::SolverReturn status, int n, const double *x,
                         const double *z_L, const double *z_U, int m,
                         const double *g, const double *lambda,
                         double obj_value, const Ipopt::IpoptData *ip_data,
                         Ipopt::IpoptCalculatedQuantities *ip_cq) override;

目标函数取得最小值时的优化量：x
目标函数最小值：obj_value

5. 速度规划算法实践

云实验地址——Apollo规划之速度规划仿真调试
1.启动DreamView

bash scripts/bootstrap.sh

5.1 加速超车，减速让行场景速度规划实践

5.2 通过减速带场景速度规划实践

5.3 弯道场景速度规划实践

%matplotlib notebook

run /apollo/modules/planning/tools/plot_st_nlp.py -f planning.INFO -t 23:29:03

default_task_config: {
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  piecewise_jerk_nonlinear_speed_optimizer_config {
    acc_weight: 2.0
    jerk_weight: 3.0
    lat_acc_weight: 10.0
    s_potential_weight: 0.05
    ref_v_weight: 5.0
    ref_s_weight: 100.0
    soft_s_bound_weight: 1e6
    use_warm_start: true
  }
}