【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER

TASK系列解析文章

1. [Apollo study notes] - LANE_CHANGE_DECIDER of planning module TASK
2. [Apollo study notes] - PATH_REUSE_DECIDER of planning module TASK
3. [Apollo study notes] - PATH_BORROW_DECIDER of planning module TASK
4. [Apollo study notes] - — PATH_BOUNDS_DECIDER of planning module TASK
5. [Apollo study notes] — PIECEWISE_JERK_PATH_OPTIMIZER of planning module TASK
6. [Apollo study notes] — PATH_ASSESSMENT_DECIDER of planning module TASK
7. [Apollo study notes] — PATH_DECIDER of planning module TASK
8. [Apollo study notes]——RULE_BASED_STOP_DECIDER of planning module TASK
9. [Apollo study notes]——SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER of planning module TASK
10. [Apollo study notes]——SPEED_HEURISTIC_OPTIMIZER of planning module
TASK 11. [Apollo study notes]——Planning Module TASK SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

前言

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK，本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限，文章可能有纰漏的地方，还请批评斧正。

在modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中，我们可以看到LaneFollow所需要执行的所有task。

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

本文将继续介绍LaneFollow的第13个TASK——PIECEWISE_JERK_SPEED_OPTIMIZER

PIECEWISE_JERK_SPEED_OPTIMIZER功能简介

产生平滑速度规划曲线

根据ST图的可行驶区域，优化出一条平滑的速度曲线。满足一阶导、二阶导平滑（速度加速度平滑）；满足道路限速；满足车辆动力学约束。

PIECEWISE_JERK_SPEED_OPTIMIZER相关配置

modules/planning/conf/planning_config.pb.txt

default_task_config: {
    
    
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  piecewise_jerk_speed_optimizer_config {
    
    
    acc_weight: 1.0
    jerk_weight: 3.0
    kappa_penalty_weight: 2000.0
    ref_s_weight: 10.0
    ref_v_weight: 10.0
  }
}

PIECEWISE_JERK_SPEED_OPTIMIZER流程

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

原理可以参考这篇论文：
《Optimal Trajectory Generation for Autonomous Vehicles UnderCentripetal Acceleration Constraints for In-lane Driving Scenarios》

QP问题的标准类型定义：

$$\begin{gathered} minimize\frac{1}{2}\cdot x^T\cdot H\cdot x+f^T\cdot x \\ s.t.LB\le x\le UB \\ {A}_{eq}x=b_{eq} \\ Ax\le b \end{gathered}$$

优化变量

速度规划的坐标系

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

优化变量$x$，$x$有三个部分组成：从$s_0$,$s_1$,$s_2$to $s_{n-1}$, from ${\dot{s}}_0$,${\dot{s}}_1$,${\dot{s}}_2$to ${\dot{s}}_{n-1}$,from ${\ddot{s}}_0$, ${\ddot{s}}_1$, ${\ddot{s}}_2$to ${\ddot{s}}_{n-1}$.
x = { s 0 , s 1 , … , s n − 1 , s 0 ˙ , s 1 ˙ , … , s n − 1 ˙ , s 0 ¨ , s 1 ¨ , … , s n − 1 ¨ } x=\{s_0,s_1,\ldots,s_{n-1},\dot{s_0},\dot{s_1},\ldots,\dot{s_{n-1}},\ddot{s_0},\ddot{s_1},\ldots,\ddot{s_{n-1}}\} x={ s0​,s1​,…,sn−1​,s0​˙​,s1​˙​,…,sn−1​˙​,s0​¨​,s1​¨​,…,sn−1​¨​}

ps：三阶导的求解方式为：$s^{{}^{\prime \prime \prime }}=\frac{{s^{\prime \prime }}_{i+1}-{s^{\prime \prime }}_i}{\Delta t}$

设计目标函数

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

• 尽可能贴合决策时制定的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|{\dot{s}}_i^2{\kappa }_{s_i}\right|\downarrow$
• The terminal penalty term makes terminal $s,v,a$ tends to be expected

最后会得到以下目标函数：
$$\begin{array}{rl}myf& =\sum _{i=0}^{n-1}{w}_s(s_i-s_{i-ref}{)}^2+w_v({\dot{s}}_i-v_{ref}{)}^2+w_i{\dot{s}}_i^2{\kappa }_{s_i}+w_a{\ddot{s}}_i^2+\sum _{i=0}^{n-2}{w}_j{s^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\\ & +w_{end-s}(s_{n-1}-s_{end}{)}^2+w_{end-ds}(\dot{s_{n-1}}-\dot{s_{end}}{)}^2+w_{end-dds}(\ddot{s_{n-1}}-\ddot{s_{end}}{)}^2\end{array}$$

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

为了统一格式，方便与代码对照，在此对目标函数进行变换：
$$\begin{array}{rl}minf& =\sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{i-ref}{)}^2+w_{ds-ref}({\dot{s}}_i-{\dot{s}}_{ref}{)}^2+p_i{\dot{s}}_i^2+w_{dds}{\ddot{s}}_i^2+\sum _{i=0}^{n-2}{w}_{ddds}{s^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\\ & +w_{end-s}(s_{n-1}-s_{end}{)}^2+w_{end-ds}(\dot{s_{n-1}}-\dot{s_{end}}{)}^2+w_{end-dds}(\ddot{s_{n-1}}-\ddot{s_{end}}{)}^2\end{array}$$

注：$p_i$Represents penalty and is the curvature κ \kappaThe weight of$\kappa$$w_i$product of .

接着，对目标函数按阶次整理可得：
$$\begin{array}{rl}myf& =\sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{i-ref}{)}^2+w_{end-s}(s_{n-1}-s_{end}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{ds-ref}({\dot{s}}_i-{\dot{s}}_{ref}{)}^2+p_i{\dot{s}}_i^2+w_{end-ds}(\dot{s_{n-1}}-\dot{s_{end}}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{dds}{\ddot{s}}_i^2+w_{end-dds}(\ddot{s_{n-1}}-\ddot{s_{end}}{)}^2\\ & +\sum _{i=0}^{n-2}{w}_{ddds}{s^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\end{array}$$

用$s^{{}^{\prime \prime \prime }}=\frac{{s^{\prime \prime }}_{i+1}-{s^{\prime \prime }}_i}{\Delta t}$代入目标函数，
$$\begin{array}{rl}\sum _{i=0}^{n-2}(s_i^{\prime \prime \prime }{)}^2& ={\left(\frac{s_1^{\prime \prime }-s_0^{\prime \prime }}{\Delta t}\right)}^2+{\left(\frac{s_2^{\prime \prime }-s_1^{\prime \prime }}{\Delta t}\right)}^2+\cdots +{\left(\frac{s_{n-2}^{\prime \prime }-s_{n-3}^{\prime \prime }}{\Delta t}\right)}^2+{\left(\frac{s_{n-1}^{\prime \prime }-s_{n-2}^{\prime \prime }}{\Delta t}\right)}^2\\ & =\frac{{\left(s_0^{\prime \prime }\right)}^2}{\Delta t^2}+2\cdot \sum _{i=1}^{n-2}\frac{{\left(s_i^{\prime \prime }\right)}^2}{\Delta t^2}+\frac{{\left(s_{n-1}^{\prime \prime }\right)}^2}{\Delta t^2}-2\cdot \sum _{i=0}^{n-2}\frac{s_i^{\prime \prime }\cdot s_{i+1}^{\prime \prime }}{\Delta t^2}\end{array}$$

$$\begin{array}{rl}myf& =\sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{i-ref}{)}^2+w_{end-s}(s_{n-1}-s_{end}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{ds-ref}({\dot{s}}_i-{\dot{s}}_{ref}{)}^2+p_i{\dot{s}}_i^2+w_{end-ds}(\dot{s_{n-1}}-\dot{s_{end}}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{dds}{\ddot{s}}_i^2+w_{end-dds}(\ddot{s_{n-1}}-\ddot{s_{end}}{)}^2\\ & +w_{ddds}[\frac{{\left(s_0^{\prime \prime }\right)}^2}{\Delta t^2}+2\cdot \sum _{i=1}^{n-2}\frac{{\left(s_i^{\prime \prime }\right)}^2}{\Delta t^2}+\frac{{\left(s_{n-1}^{\prime \prime }\right)}^2}{\Delta t^2}-2\cdot \sum _{i=0}^{n-2}\frac{s_i^{\prime \prime }\cdot s_{i+1}^{\prime \prime }}{\Delta t^2}]\end{array}$$

约束条件

接下来谈谈约束的设计。
要满足的约束条件：
• 主车必须在道路边界内，同时不能和障碍物有碰撞$$s_i\in (s_{\min }^i,s_{\max }^i)$$ •According to the current state, the lateral speed/acceleration/jerk of the host vehicle has specific kinematic constraints:
$$s_i^{\prime }\in \left(s_{min}^i{}^{\prime }(t),s_{max}^i{}^{\prime }(t)\right)\text{,}{s}_i^{\prime \prime }\in \left(s_{min}^i{}^{\prime \prime }(t),s_{max}^i{}^{\prime \prime }(t)\right)\text{,}{s}_i^{\prime \prime \prime }\in \left(s_{min}^i{}^{\prime \prime \prime }(t),s_{max}^i{}^{\prime \prime \prime }\left(t\right)\right)$$
•Continuity constraints:
$$\begin{array}{rl}{s}_{i+1}^{\prime \prime }& =s_i^{\prime \prime }+{\int }_0^{\Delta t}{s}_{i\to i+1}^{\prime \prime \prime }dt=s_i^{\prime \prime }+s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t\\ s_{i+1}^{\prime }& =s_i^{\prime }+{\int }_0^{\Delta t}{s}^{\prime \prime }(t)dt=s_i^{\prime }+s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^2\\ & =s_i^{\prime }+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i+1}^{\prime \prime }\ast \Delta t\\ s_{i+1}& =s_i+{\int }_0^{\Delta t}{s}^{\prime }(t)dt\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^3\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{3}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i+1}^{\prime \prime }\ast \Delta t^2\end{array}$$

• Start point constraint : $s_0=s_{init}$,${\dot{s}}_0={\dot{s}}_{init}$, ${\ddot{s}}_0={\ddot{s}}_{init}$What is satisfied is the starting point constraint, which is the state of the actual vehicle planning starting point.

可以看到和路径规划部分的约束条件基本一致，因此在约束矩阵$A$部分，路径规划和速度规划矩阵一致，不用再次编写。

---

相关矩阵

回到代码中，PiecewiseJerkSpeedOptimizer的主流程依旧在Process中，我们暂且不关注其他细节，先关注与二次规划主体部分。

以下代码创建了一个类为PiecewiseJerkSpeedProblem的对象piecewise_jerk_problem，PiecewiseJerkSpeedProblem继承自PiecewiseJerkProblem。其中参数意义如下：
num_of_knots：表示采样点的数量。
delta_t：表示每个采样点之间的时间间隔。
init_s：表示起点位置。

  // 分段加加速度速度优化算法
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t,
                                                   init_s);

接着看下一段代码：
通过调用Optimize，进行二次规划问题的解决

  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
    
    
    const std::string msg = "Piecewise jerk speed optimizer failed!";
    AERROR << msg;
    speed_data->clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

Optimize这部分代码和路径规划部分一致，具体可参考【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER

速度规划部分约束矩阵$A$,$upperbound$,$lowerbound$均和路径规划部分一致：
$$lb=\left[\begin{array}{c}{s}_{lb}[0]\\ ⋮\\ s_{lb}[n-1]\\ {s^{\prime }}_{lb}\left[0\right]\\ ⋮\\ {s^{\prime }}_{lb}[n-1]\\ {s^{\prime \prime }}_{lb}\left[0\right]\\ ⋮\\ {s^{\prime \prime }}_{lb}[n-1]\\ {s^{\prime \prime \prime }}_{lb}\left[0\right]\cdot \Delta t\\ ⋮\\ {s^{\prime \prime \prime }}_{lb}[n-2]\cdot \Delta t\\ 0\\ ⋮\\ 0\\ s_{init}\\ {s^{\prime }}_{init}\\ {s^{\prime \prime }}_{init}\end{array}\right],ub=\left[\begin{array}{c}{s}_{ub}\left[0\right]\\ ⋮\\ s_{ub}[n-1]\\ {s^{\prime }}_{ub}\left[0\right]\\ ⋮\\ {s^{\prime }}_{ub}[n-1]\\ {s^{\prime \prime }}_{ub}\left[0\right]\\ ⋮\\ {s^{\prime \prime }}_{ub}[n-1]\\ {s^{\prime \prime \prime }}_{ub}\left[0\right]\cdot \Delta t\\ ⋮\\ {s^{\prime \prime \prime }}_{ub}[n-2]\cdot \Delta t\\ 0\\ ⋮\\ 0\\ s_{init}\\ {s^{\prime }}_{init}\\ {s^{\prime \prime }}_{init}\end{array}\right]$$

$$A=\left[\begin{array}{ccc}\begin{array}{cccc}1& 0& \cdots & 0\\ 0& \ddots & & ⋮\\ ⋮& & \ddots & 0\\ 0& \cdots & 0& 1\end{array}& & \\ & \begin{array}{cccc}1& 0& \cdots & 0\\ 0& \ddots & & ⋮\\ ⋮& & \ddots & 0\\ 0& \cdots & 0& 1\end{array}& \\ & & \begin{array}{cccc}1& 0& \cdots & 0\\ 0& \ddots & & ⋮\\ ⋮& & \ddots & 0\\ 0& \cdots & 0& 1\end{array}\\ & & \begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}\\ & \begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}& \begin{array}{cccc}-\frac{\Delta t}{2}& -\frac{\Delta t}{2}& & \\ & \ddots & \ddots & \\ & & -\frac{\Delta t}{2}& -\frac{\Delta t}{2}\end{array}\\ \begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}& \begin{array}{cccc}-\Delta t& & & \\ & \ddots & & \\ & & -\Delta t& \end{array}& \begin{array}{cccc}-\frac{\Delta t^2}{3}& -\frac{\Delta t^2}{6}& & \\ & \ddots & \ddots & \\ & & -\frac{\Delta t^2}{3}& -\frac{\Delta t^2}{6}\end{array}\\ \begin{array}{cccc}1& & & \\ & & & \\ & & & \end{array}& \begin{array}{cccc}& & & \\ 1& & & \\ & & & \end{array}& \begin{array}{cccc}& & & \\ & & & \\ 1& & & \end{array}\end{array}\right]$$

不同之处在于二次项系数矩阵$H$与一次项系数向量$q$

二次项系数矩阵$H$

$$H=2\cdot \left[\begin{array}{ccc}\begin{array}{cccc}{w}_{s-ref}& 0& \cdots & 0\\ 0& \ddots & & ⋮\\ ⋮& & w_{s-ref}& 0\\ 0& \cdots & 0& w_{s-ref}+w_{end-s}\end{array}& & \\ & \begin{array}{cccc}{w}_{ds-ref}+p_0& 0& \cdots & 0\\ 0& \ddots & & ⋮\\ ⋮& & w_{ds-ref}+p_{n-2}& 0\\ 0& \cdots & 0& w_{ds-ref}+p_{n-1}+w_{end-s}\end{array}& \\ & & \begin{array}{ccccc}{w}_{dds}+\frac{w_{ddds}}{\Delta t^2}& 0& \cdots & \cdots & 0\\ -2\cdot \frac{w_{ddds}}{\Delta t^2}& w_{dds}+2\cdot \frac{w_{ddds}}{\Delta t^2}& & & ⋮\\ 0& -2\cdot \frac{w_{ddds}}{\Delta t^2}& \ddots & & ⋮\\ ⋮& & \ddots & w_{dds}+2\cdot \frac{w_{ddds}}{\Delta t^2}& 0\\ 0& \cdots & 0& -2\cdot \frac{w_{ddds}}{\Delta t^2}& w_{dds}+\frac{w_{ddds}}{\Delta t^2}+w_{end-dds}\end{array}\end{array}\right]$$

void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector<c_float>* P_data,
                                                std::vector<c_int>* P_indices,
                                                std::vector<c_int>* P_indptr) {
    
    
  const int n = static_cast<int>(num_of_knots_);
  const int kNumParam = 3 * n;
  const int kNumValue = 4 * n - 1;
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(kNumParam);
  int value_index = 0;

  // x(i)^2 * w_x_ref
  for (int i = 0; i < n - 1; ++i) {
    
    
    columns[i].emplace_back(
        i, weight_x_ref_ / (scale_factor_[0] * scale_factor_[0]));
    ++value_index;
  }
  // x(n-1)^2 * (w_x_ref + w_end_x)
  columns[n - 1].emplace_back(n - 1, (weight_x_ref_ + weight_end_state_[0]) /
                                         (scale_factor_[0] * scale_factor_[0]));
  ++value_index;

  // x(i)'^2 * (w_dx_ref + penalty_dx)
  for (int i = 0; i < n - 1; ++i) {
    
    
    columns[n + i].emplace_back(n + i,
                                (weight_dx_ref_ + penalty_dx_[i]) /
                                    (scale_factor_[1] * scale_factor_[1]));
    ++value_index;
  }
  // x(n-1)'^2 * (w_dx_ref + penalty_dx + w_end_dx)
  columns[2 * n - 1].emplace_back(
      2 * n - 1, (weight_dx_ref_ + penalty_dx_[n - 1] + weight_end_state_[1]) /
                     (scale_factor_[1] * scale_factor_[1]));
  ++value_index;

  auto delta_s_square = delta_s_ * delta_s_;
  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
  columns[2 * n].emplace_back(2 * n,
                              (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                  (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  for (int i = 1; i < n - 1; ++i) {
    
    
    columns[2 * n + i].emplace_back(
        2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }

  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  for (int i = 0; i < n - 1; ++i) {
    
    
    columns[2 * n + i].emplace_back(2 * n + i + 1,
                                    -2.0 * weight_dddx_ / delta_s_square /
                                        (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }

  CHECK_EQ(value_index, kNumValue);

  int ind_p = 0;
  for (int i = 0; i < kNumParam; ++i) {
    
    
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns[i]) {
    
    
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}

一次项系数向量$q$

$$q=\left[\begin{array}{c}-2\cdot w_{s-ref}\cdot s_{0-ref}\\ ⋮\\ -2\cdot w_{s-ref}\cdot s_{(n-2)-ref}\\ -2\cdot w_{s-ref}\cdot s_{(n-1)-ref}-2\cdot w_{end-s}\cdot s_{end}\\ -2\cdot w_{ds-ref}\cdot {\dot{s}}_{ref}\\ ⋮\\ -2\cdot w_{ds-ref}\cdot {\dot{s}}_{ref}\\ -2\cdot w_{end-ds}\cdot {\ddot{s}}_{end}-2\cdot w_{ds-ref}\cdot {\dot{s}}_{ref}\\ 0\\ ⋮\\ 0\\ -2\cdot w_{end-dds}\cdot {\ddot{s}}_{end}\end{array}\right]$$

void PiecewiseJerkSpeedProblem::CalculateOffset(std::vector<c_float>* q) {
    
    
  CHECK_NOTNULL(q);
  const int n = static_cast<int>(num_of_knots_);
  const int kNumParam = 3 * n;
  q->resize(kNumParam);
  for (int i = 0; i < n; ++i) {
    
    
    if (has_x_ref_) {
    
    
      q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0];
    }
    if (has_dx_ref_) {
    
    
      q->at(n + i) += -2.0 * weight_dx_ref_ * dx_ref_ / scale_factor_[1];
    }
  }

  if (has_end_state_ref_) {
    
    
    q->at(n - 1) +=
        -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];
    q->at(2 * n - 1) +=
        -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];
    q->at(3 * n - 1) +=
        -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];
  }
}

Set OSQP solution parameters

OSQPSettings* PiecewiseJerkSpeedProblem::SolverDefaultSettings() {
    
    
  // Define Solver default settings
  OSQPSettings* settings =
      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->eps_abs = 1e-4;
  settings->eps_rel = 1e-4;
  settings->eps_prim_inf = 1e-5;
  settings->eps_dual_inf = 1e-5;
  settings->polish = true;
  settings->verbose = FLAGS_enable_osqp_debug;
  settings->scaled_termination = true;

  return settings;
}

---

Process

Process is the main process of the speed optimization algorithm based on quadratic programming, which mainly includes the following steps:

1. Determine whether the end point has been reached, and if so, return directly.
2. Check if the path is empty
3. Set related parameters
4. UpdateSTBoundary
5. Update velocity bounds and reference s
6. Speed ​​optimization
7. output

enter:

• const PathData& path_data,
• const SpeedData& speed_data,
• const common::TrajectoryPoint& init_point.

Output:
Get the optimal speed, information includes $opt\_s,opt\_ds,opt\_dds$ . In the Process function, the final result is saved in speed_data.

Set related parameters

  StGraphData& st_graph_data = *reference_line_info_->mutable_st_graph_data();

  const auto& veh_param =
      common::VehicleConfigHelper::GetConfig().vehicle_param();
  // 起始点(s,v,a)
  std::array<double, 3> init_s = {
    
    0.0, st_graph_data.init_point().v(),
                                  st_graph_data.init_point().a()};
  double delta_t = 0.1;
  double total_length = st_graph_data.path_length();
  double total_time = st_graph_data.total_time_by_conf();
  int num_of_knots = static_cast<int>(total_time / delta_t) + 1;
  // 分段加加速度速度优化算法
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t,
                                                   init_s);
  // 设置相关参数
  const auto& config = config_.piecewise_jerk_speed_optimizer_config();
  piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
  piecewise_jerk_problem.set_weight_dddx(config.jerk_weight());

  piecewise_jerk_problem.set_x_bounds(0.0, total_length);
  piecewise_jerk_problem.set_dx_bounds(
      0.0, std::fmax(FLAGS_planning_upper_speed_limit,
                     st_graph_data.init_point().v()));
  piecewise_jerk_problem.set_ddx_bounds(veh_param.max_deceleration(),
                                        veh_param.max_acceleration());
  piecewise_jerk_problem.set_dddx_bound(FLAGS_longitudinal_jerk_lower_bound,
                                        FLAGS_longitudinal_jerk_upper_bound);

  piecewise_jerk_problem.set_dx_ref(config.ref_v_weight(),
                                    reference_line_info_->GetCruiseSpeed());

This part needs attention:
you can see that in terms of speed planning, $The\; constraint\; of\; s$ is$0\le s\le pathlength$ , ST boundary has not been considered at this time;$s$The constraint of$\dot{}$$0\le \dot{s}\le v_{max}$，$v_{max}$Select FLAGS_planning_upper_speed_limitthe largest one with the starting point speed; $s$The constraint of$\ddot{}$$a_{dec-max}\le \ddot{s}\le a_{acc-max}$, that is, within the range of maximum deceleration and maximum acceleration; $s^{{}^{}}$The constraint of${}^{{}^{\prime \prime \prime }}$$s_{lb}^{{}^{\prime \prime \prime }}\le s^{{}^{\prime \prime \prime }}\le s_{ub}^{{}^{\prime \prime \prime }}$, jerk jerkThe constraints of$j$$er$$k$FLAGS_longitudinal_jerk_lower_bound are determined byFLAGS_longitudinal_jerk_upper_boundtwo parameters;${\dot{s}}_{ref}$The reference speed depends on the cruising speed of the current path.

UpdateSTBoundary

Traverse each point and update STBoundary according to different obstacle types.

  // Update STBoundary
  std::vector<std::pair<double, double>> s_bounds;
  for (int i = 0; i < num_of_knots; ++i) {
    
    
    double curr_t = i * delta_t;
    double s_lower_bound = 0.0;
    double s_upper_bound = total_length;
    for (const STBoundary* boundary : st_graph_data.st_boundaries()) {
    
    
      double s_lower = 0.0;
      double s_upper = 0.0;
      if (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
    
    
        continue;
      }
      switch (boundary->boundary_type()) {
    
    
        case STBoundary::BoundaryType::STOP:
        case STBoundary::BoundaryType::YIELD:
          s_upper_bound = std::fmin(s_upper_bound, s_upper);
          break;
        case STBoundary::BoundaryType::FOLLOW:
          // TODO(Hongyi): unify follow buffer on decision side
          s_upper_bound = std::fmin(s_upper_bound, s_upper - 8.0);
          break;
        case STBoundary::BoundaryType::OVERTAKE:
          s_lower_bound = std::fmax(s_lower_bound, s_lower);
          break;
        default:
          break;
      }
    }
    if (s_lower_bound > s_upper_bound) {
    
    
      const std::string msg =
          "s_lower_bound larger than s_upper_bound on STGraph";
      AERROR << msg;
      speed_data->clear();
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
    s_bounds.emplace_back(s_lower_bound, s_upper_bound);
  }
  piecewise_jerk_problem.set_x_bounds(std::move(s_bounds));

This function is involved GetUnblockSRange, and its purpose is to obtain the range of s in the unblocked segment.

bool STBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
                                  double* s_lower) const {
    
    
  CHECK_NOTNULL(s_upper);
  CHECK_NOTNULL(s_lower);
  // FLAGS_speed_lon_decision_horizon: Longitudinal horizon for speed decision making (meter) 200m;
  *s_upper = FLAGS_speed_lon_decision_horizon;
  *s_lower = 0.0;
  // 若不在boundary的范围内，说明未被阻塞
  if (curr_time < min_t_ || curr_time > max_t_) {
    
    
    return true;
  }

  size_t left = 0;
  size_t right = 0;
  // Given time t, find a segment denoted by left and right idx, that contains the time t. 
  // - If t is less than all or larger than all, return false.
  if (!GetIndexRange(lower_points_, curr_time, &left, &right)) {
    
    
    AERROR << "Fail to get index range.";
    return false;
  }

  if (curr_time > upper_points_[right].t()) {
    
    
    return true;
  }
  // 求出curr_time在segment中所占比例
  const double r =
      (left == right
           ? 0.0
           : (curr_time - upper_points_[left].t()) /
                 (upper_points_[right].t() - upper_points_[left].t()));
  // 线性插值
  double upper_cross_s =
      upper_points_[left].s() +
      r * (upper_points_[right].s() - upper_points_[left].s());
  double lower_cross_s =
      lower_points_[left].s() +
      r * (lower_points_[right].s() - lower_points_[left].s());
  // 根据障碍物类型，更新s_upper或s_lower
  if (boundary_type_ == BoundaryType::STOP ||
      boundary_type_ == BoundaryType::YIELD ||
      boundary_type_ == BoundaryType::FOLLOW) {
    
    
    *s_upper = lower_cross_s;
  } else if (boundary_type_ == BoundaryType::OVERTAKE) {
    
    
    *s_lower = std::fmax(*s_lower, upper_cross_s);
  } else {
    
    
    ADEBUG << "boundary_type is not supported. boundary_type: "
           << static_cast<int>(boundary_type_);
    return false;
  }
  return true;
}

Update velocity bounds and reference s

  // Update SpeedBoundary and ref_s
  std::vector<double> x_ref;
  std::vector<double> penalty_dx;
  std::vector<std::pair<double, double>> s_dot_bounds;
  const SpeedLimit& speed_limit = st_graph_data.speed_limit();
  for (int i = 0; i < num_of_knots; ++i) {
    
    
    double curr_t = i * delta_t;
    // get path_s
    SpeedPoint sp;
    reference_speed_data.EvaluateByTime(curr_t, &sp);
    const double path_s = sp.s();
    x_ref.emplace_back(path_s);
    // get curvature
    PathPoint path_point = path_data.GetPathPointWithPathS(path_s);
    penalty_dx.push_back(std::fabs(path_point.kappa()) *
                         config.kappa_penalty_weight());
    // get v_upper_bound
    const double v_lower_bound = 0.0;
    double v_upper_bound = FLAGS_planning_upper_speed_limit;
    v_upper_bound = speed_limit.GetSpeedLimitByS(path_s);
    s_dot_bounds.emplace_back(v_lower_bound, std::fmax(v_upper_bound, 0.0));
  }
  piecewise_jerk_problem.set_x_ref(config.ref_s_weight(), std::move(x_ref));
  piecewise_jerk_problem.set_penalty_dx(penalty_dx);
  piecewise_jerk_problem.set_dx_bounds(std::move(s_dot_bounds));

Speed ​​optimization

  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
    
    
    const std::string msg = "Piecewise jerk speed optimizer failed!";
    AERROR << msg;
    speed_data->clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

output

  // Extract output
  const std::vector<double>& s = piecewise_jerk_problem.opt_x();
  const std::vector<double>& ds = piecewise_jerk_problem.opt_dx();
  const std::vector<double>& dds = piecewise_jerk_problem.opt_ddx();
  for (int i = 0; i < num_of_knots; ++i) {
    
    
    ADEBUG << "For t[" << i * delta_t << "], s = " << s[i] << ", v = " << ds[i]
           << ", a = " << dds[i];
  }
  speed_data->clear();
  speed_data->AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);
  for (int i = 1; i < num_of_knots; ++i) {
    
    
    // Avoid the very last points when already stopped
    if (ds[i] <= 0.0) {
    
    
      break;
    }
    speed_data->AppendSpeedPoint(s[i], delta_t * i, ds[i], dds[i],
                                 (dds[i] - dds[i - 1]) / delta_t);
  }
  SpeedProfileGenerator::FillEnoughSpeedPoints(speed_data);
  RecordDebugInfo(*speed_data, st_graph_data.mutable_st_graph_debug());
  return Status::OK();