Apollo 7.0 PiecewiseJerkSpeedOptimizer（分段加加速度优化器）代码解读

Apollo 7.0 PiecewiseJerkSpeedOptimizer（分段加加速度优化器）代码解读

贺志国
2022-7-30

PiecewiseJerkSpeedOptimizer （对应PIECEWISE_JERK_SPEED_OPTIMIZER任务）使用QP来求解速度曲线。Apollo 3.0中的QpSplineStSpeedOptimizer使用多段五次多项式连接的样条曲线（每2s使用一个五次多项式拟合，8s时长的轨迹共有8/2=4段五次多项式。实际求解过程中，为了降低计算量，实际求解中仅使用了3段多项式来求解[0, 6]s区间内的速度曲线。这些五次多项式隐含的控制点和节点向量满足样条曲线的迭代表达式就形成了五次多项式样条曲线）来拟合速度，使用二次规划来优化五次多项式的系数，并使用qpOASES库来求解，之后再使用求解出的多项式系数来确定自变量为时间t，因变量为s的五次多项式样条曲线，再通过样条曲线来计算出不同时刻的位移s，最后借助运动学公式计算出指定位移s处的速度v和加速度a。Apollo 7.0不使用样条曲线而是使用位移s关于时间t的三阶偏导（称为jerk）为分段常量作为优化项来建模二次规划问题，并使用OSQP库来求解，并且求解的结果直接就是多个采样点处的s, v, a（采样时间间隔为0.1 s），采样点以外的s, v, a未给出，直接由控制模块自行插值获取。

一、数学模型

1.1 凸二次规划介绍

1.1.1 凸二次规划的一般表达式

$$min\frac{1}{2}{x}^{T}Px+q^{T}xs.t.l\le Ax\le u$$
其中，$P$称为内核矩阵，要求是半正定矩阵。正定矩阵在复数域上是共轭对称矩阵（Hermite矩阵），在实数域上是对称矩阵。因为我们在实数域内求解，$P$也是一个对称矩阵。$q$称为偏移向量，$A$称为仿射矩阵。

凸二次规划的求解算法主要有如下几种：

• 有效集法：Active Set Method
• 内点法：Interior Point Method
• 一阶方法：First Order Method
• 交替方向乘子法：Alternating Direction Method of Multipliers（ADMM）

PiecewiseJerkSpeedOptimizer使用的求解库OSQP利用交替方向乘子法（ADMM）。Apollo 3.0中QpSplineStSpeedOptimizer使用的求解库qpOASES则利用有效集法。从网上给出的测评报告看，OSQP库的求解效率比qpOASES库要高很多。

1.1.2 凸二次规划的示例

为了形象地说明凸二次规划的求解过程，下面给出一个简单的示例：
$$min\frac{1}{2}{x}^T\left[\begin{array}{cc}4& 1\\ 1& 2\end{array}\right]x+{\left[\begin{array}{c}1\\ 1\end{array}\right]}^{T}x$$
s.t. （subject to）：
$$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]\le \left[\begin{array}{cc}1& 1\\ 1& 0\\ 0& 1\end{array}\right]x\le \left[\begin{array}{c}1\\ 0.7\\ 0.7\end{array}\right]$$
本示例中，内核矩阵$P=\left[\begin{array}{cc}4& 1\\ 1& 2\end{array}\right]$的元素全部为实数，因此它一定是对称矩阵。对于对称矩阵，只需要存储上三角矩阵即可，这也是OSQP存储稀疏矩阵的常见做法，当然也是该库提高计算效率的有效措施。

具体而言，OSQP库使用csc_matrix(indices, indptr, data, csc = Compressed Sparse Column)的方式来存储一个矩阵。csc_matrix存储格式定义如下：

• indices is array of row indices
• data is array of corresponding nonzero values
• indptr points to column starts in indices and data

下面仍然以内核矩阵$P=\left[\begin{array}{cc}4& 1\\ 1& 2\end{array}\right]$为例进行说明。首先，将内核矩阵简化为上三角矩阵：
$$P=\left[\begin{array}{cc}4& 1\\ 0& 2\end{array}\right]$$
$P$的非零元素个数为3，记为：P_nnz = 3 (nnz: number of non-zero elements)

$P$中非零元素为：4, 1, 2，记为：P_x = {4, 1, 2}；

$P$中非零元素行索引为：0, 0, 1，这是按照先第一列，再第二列。。。，最后第N列的顺序标记。第一列的非零元素是4，行索引为0；第二列的非零元素为1, 2，行索引分别为：0, 1，于是记为：P_i = {0, 0, 1}；

$P$中第一列非零元素是1个，第二列非零元素是2个，记为：P_p = {0, 1, 3}。这个记法有些反人类，也很难形象理解，容我再详细解释一番。P_p元素个数为矩阵列数加1，P_p的第一个元素为0，P_p中前后两个元素的差值表示矩阵P相应列的非零元素个数。例如：P_p = {0, 1, 3}表示：1 - 0 = 1 代表矩阵P第一列元素非零个数为１个，3 - 1 = 2 代表矩阵P第二列元素非零个数为２个。助记方法：P_i (i代表indices), P_p(p代表indptr)。

根据上述介绍，下面给出该示例的求解代码：

#include "osqp.h"
int main(int argc, char **argv) {
    // P矩阵是对称矩阵，只存储上三角部分,下三角部分视为零值
    c_float P_x[3] = {4.0, 1.0, 2.0, }; 
    // P矩阵非零元素个数为3
    // nnz = number of non-zero elements.
    c_int P_nnz = 3; 
    // P矩阵非零元素行索引，按照先第一列，再第二列。。。
    // 的顺序排列。下例表示非零元素的行序号为0、0、1
    c_int P_i[3] = {0, 0, 1, }; 
    // 1-0=1表示第0列非零元素个数
    // 3-1=2表示第1列非零元素个数
    c_int P_p[3] = {0, 1, 3, }; 
    c_float q[2] = {1.0, 1.0, };
    // A矩阵非零元素
    c_float A_x[4] = {1.0, 1.0, 1.0, 1.0, };
    // A矩阵非零元素个数为4
    c_int A_nnz = 4;
    // A矩阵非零元素的行序号为0、1、0、2(按列排序）
    c_int A_i[4] = {0, 1, 0, 2, };
    // 2-0=2表示第0列2个元素非零
    // 4-2=2表示第1列2个元素非零
    c_int A_p[3] = {0, 2, 4, };
    c_float l[3] = {1.0, 0.0, 0.0, };
    c_float u[3] = {1.0, 0.7, 0.7, };
    c_int n = 2;
    c_int m = 3;

    // Exitflag
    c_int exitflag = 0;
    // Workspace structures
    OSQPWorkspace *work;
    OSQPSettings  *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
    OSQPData      *data     = (OSQPData *)c_malloc(sizeof(OSQPData));

    // Populate data
    if (data) {
        data->n = n;
        data->m = m;
        data->P = csc_matrix(data->n, data->n, P_nnz, P_x, P_i, P_p);
        data->q = q;
        data->A = csc_matrix(data->m, data->n, A_nnz, A_x, A_i, A_p);
        data->l = l;
        data->u = u;
    }

    // Define solver settings as default
    if (settings) {
        osqp_set_default_settings(settings);
        settings->alpha = 1.0; // Change alpha parameter
    }

    // Setup workspace
    exitflag = osqp_setup(&work, data, settings);

    // Solve Problem
    osqp_solve(work);

    // Cleanup
    if (data) {
        if (data->A) c_free(data->A);
        if (data->P) c_free(data->P);
        c_free(data);
    }
    if (settings) c_free(settings);

    return exitflag;
}

1.2 速度规划待优化的状态变量

1.3 速度规划目标函数

我们希望速度曲线尽可能贴近启发式搜索给出的位移和限速，同时希望加速度平滑，加加速度为常量，得出目标函数如下：

c o s t = m i n { c o s t 1 + c o s t 2 + c o s t 3 } cost = min \left\{ cost1 + cost2 + cost3 \right\} cost=min{cost1+cost2+cost3}

其中：

$$cost1=\sum _{i=0}^{n-2}\left[w_s{s}_i^2+(w_v+p_i){\dot{s}}_i^2-\frac{2w_{jerk}}{\Delta t^2}{\ddot{s}}_i{\ddot{s}}_{i+1}\right]+\sum _{i=1}^{n-2}(w_a+\frac{2w_{jerk}}{\Delta t^2}){\ddot{s}}_i^2$$
$$cost2=(w_s+w_{s_{end}})s_{n-1}^2+(w_v+p_{n-1}+w_{v_{end}}){\dot{s}}_{n-1}^2+(w_a+\frac{w_{jerk}}{\Delta t^2}){\ddot{s}}_0^2+(w_a+\frac{w_{jerk}}{\Delta t^2}+w_{a_{end}}){\ddot{s}}_{n-1}^2$$
$$cost3=-2\sum _{i=0}^{n-1}\left[w_s{s}_{re{f}_i}{s}_i+(w_v+p_i)v_{re{f}_i}{\dot{s}}_i\right]$$
注意：对于终点处的$s_{n-1}$额外增加了惩罚系数$w_{s_{end}}$，对于终点处的${\dot{s}}_{n-1}$额外增加了惩罚系数$w_{v_{end}}$，对于终点处的${\ddot{s}}_{n-1}$额外增加了惩罚系数$w_{a_{end}}$。

预备知识：

1. 矩阵与列向量相乘，等于矩阵中的每一列的线性组合，组合系数为列向量中的每个元素：

$$\left[\begin{array}{ccc}a& b& c\\ d& e& f\end{array}\right]\left[\begin{array}{c}m\\ n\\ o\end{array}\right]=m\left[\begin{array}{c}a\\ d\end{array}\right]+n\left[\begin{array}{c}b\\ e\end{array}\right]+o\left[\begin{array}{c}c\\ f\end{array}\right]$$
2. 行向量与矩阵相乘，等于矩阵中的每一行的线性组合，组合系数为行向量中的每个元素：
$$\left[\begin{array}{cc}m& n\end{array}\right]\left[\begin{array}{ccc}a& b& c\\ d& e& f\end{array}\right]=m\left[\begin{array}{ccc}a& b& c\end{array}\right]+n\left[\begin{array}{ccc}d& e& f\end{array}\right]$$
基于上述预备知识，将上述表达式写成矩阵形式：
$$min\frac{1}{2}{\left[\begin{array}{c}{s}_0\\ s_1\\ ⋮\\ s_{n-1}\\ {\dot{s}}_0\\ {\dot{s}}_1\\ ⋮\\ {\dot{s}}_{n-1}\\ {\ddot{s}}_0\\ {\ddot{s}}_1\\ ⋮\\ {\ddot{s}}_{n-1}\end{array}\right]}^T\underset{\mathbf{P}}{\underbrace{\left[\begin{array}{cccc}2A_{n\ast n}& 0& 0& \\ 0& 2B_{n\ast n}& 0& \\ 0& 0& 2C_{n\ast n}& \end{array}\right]}}\left[\begin{array}{c}{s}_0\\ s_1\\ ⋮\\ s_{n-1}\\ {\dot{s}}_0\\ {\dot{s}}_1\\ ⋮\\ {\dot{s}}_{n-1}\\ {\ddot{s}}_0\\ {\ddot{s}}_1\\ ⋮\\ {\ddot{s}}_{n-1}\end{array}\right]+\underset{\mathbf{q}}{\underbrace{{\left[\begin{array}{c}-2w_s{s}_{re{f}_0}\\ -2w_s{s}_{re{f}_1}\\ ⋮\\ -2w_s{s}_{re{f}_{n-1}}\\ -2(w_v+p_0)v_{re{f}_0}\\ -2(w_v+p_1)v_{re{f}_1}\\ ⋮\\ -2(w_v+p_{n-1})v_{re{f}_{n-1}}\end{array}\right]}^T}}\left[\begin{array}{c}{s}_0\\ s_1\\ ⋮\\ s_{n-1}\\ {\dot{s}}_0\\ {\dot{s}}_1\\ ⋮\\ {\dot{s}}_{n-1}\end{array}\right]$$

接下来我们写出矩阵$A_{n\ast n}$、$B_{n\ast n}$和$C_{n\ast n}$的具体表达式（空余部分元素全部为0）：
$$A_{n\ast n}=\left[\begin{array}{ccccc}{w}_s& & & & \\ & w_s& & & \\ & & \ddots & & \\ & & & w_s& \\ & & & & w_s+w_{s_{end}}\end{array}\right]$$

$$B_{n\ast n}=\left[\begin{array}{ccccc}{w}_v+p_0& & & & \\ & w_v+p_1& & & \\ & & \ddots & & \\ & & & w_v+p_{n-2}& \\ & & & & w_v+p_{n-1}+w_{v_{end}}\end{array}\right]$$

---

$$C_{n\ast n}=\left[\begin{array}{cccccc}{w}_a+\frac{w_{jerk}}{\Delta t^2}& -\frac{2w_{jerk}}{\Delta t^2}& & & & \\ & w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{2w_{jerk}}{\Delta t^2}& & & \\ & & \ddots & \ddots & & \\ & & & w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{2w_{jerk}}{\Delta t^2}& \\ & & & & w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{2w_{jerk}}{\Delta t^2}\\ & & & & & w_a+\frac{w_{jerk}}{\Delta t^2}+w_{a_{end}}\end{array}\right]$$

注意：上述矩阵$C_{n\ast n}$不是对称的，按照OSQP要求转换为对称矩阵：
$$C_{n\ast n}=\left[\begin{array}{ccccc}{w}_a+\frac{w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}& & & \\ -\frac{w_{jerk}}{\Delta t^2}& w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}& & \\ & \ddots & \ddots & \ddots & \\ & & -\frac{w_{jerk}}{\Delta t^2}& w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}\\ & & & -\frac{w_{jerk}}{\Delta t^2}& w_a+\frac{w_{jerk}}{\Delta t^2}+w_{a_{end}}\end{array}\right]$$
根据OSQP对于内核矩阵的存储要求，保留上三角矩阵，将下三角矩阵的非零元素全部抹去，结果为（Apollo转换对称矩阵的做法是错误的）：
$$C_{n\ast n}=\left[\begin{array}{ccccc}{w}_a+\frac{w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}& & & \\ & w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}& & \\ & \ddots & \ddots & \ddots & \\ & & & w_a+\frac{2w_{jerk}}{\Delta t^2}& -\frac{w_{jerk}}{\Delta t^2}\\ & & & & w_a+\frac{w_{jerk}}{\Delta t^2}+w_{a_{end}}\end{array}\right]$$
内核矩阵$P$中的系数就是PiecewiseJerkSpeedProblem(在文件modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc中)的CalculateKernel函数中的系数，偏移向量q中的系数就是PiecewiseJerkSpeedProblem的CalculateOffset函数中的系数（注意Apollo对于偏移向量的实现忽略了曲率惩罚系数$p_i$，我认为这是不正确的）。

1.4 速度规划约束条件

$$s_{lb}[i]\le s_i\le s_{ub}[i]$$
$${\dot{s}}_{lb}[i]\le {\dot{s}}_i\le {\dot{s}}_{ub}[i]$$
$${\ddot{s}}_{lb}[i]\le {\ddot{s}}_i\le {\ddot{s}}_{lb}[i]$$
$$jer{k}_{lb}\Delta t\le {\ddot{s}}_{i+1}-{\ddot{s}}_i\le jer{k}_{ub}\Delta t$$

其中，位移下、上限$s_{lb}[i]$和$s_{ub}[i]$通过动态规划算法在ST图上搜索得到，速度下、上限${\dot{s}}_{lb}[i]$和${\dot{s}}_{lb}[i]$通过限速决策得到，加速度下、上限${\ddot{s}}_{lb}[i]$和${\ddot{s}}_{ub}[i]$通过车辆运动学模型参数给出，加加速度下、下限$jer{k}_{lb}$和$jer{k}_{ub}$通过调参经验得到。

• 连续性约束
  

只要令上、下限为0，上述条件就可转换为OSQP要求的$l\le Ax\le u$形式。

• 初始条件约束

$$s_0=s_{init}$$
$${\dot{s}}_0=v_{init}$$
$${\ddot{s}}_0=a_{init}$$

将上述表达式写成矩阵为：
$${\left[\begin{array}{c}{s}_{lb}[0]\\ ⋮\\ s_{lb}[n-1]\\ {\dot{s}}_{lb}[0]\\ ⋮\\ {\dot{s}}_{lb}[n-1]\\ {\ddot{s}}_{lb}[0]\\ ⋮\\ {\ddot{s}}_{lb}[n-1]\\ jer{k}_{lb}\Delta t\\ ⋮\\ jer{k}_{lb}\Delta t\\ 0.0\\ ⋮\\ 0.0\\ s_{init}\\ v_{init}\\ a_{init}\end{array}\right]}_{6n\ast 1}\le A_{6n\ast 3n}{\left[\begin{array}{c}{s}_0\\ ⋮\\ s_{n-1}\\ {\dot{s}}_0\\ ⋮\\ {\dot{s}}_{n-1}\\ {\ddot{s}}_0\\ {\ddot{s}}_1\\ ⋮\\ {\ddot{s}}_{n-1}\end{array}\right]}_{3n\ast 1}\le {\left[\begin{array}{c}{s}_{ub}[0]\\ ⋮\\ s_{ub}[n-1]\\ {\dot{s}}_{ub}[0]\\ ⋮\\ {\dot{s}}_{ub}[n-1]\\ {\ddot{s}}_{ub}[0]\\ ⋮\\ {\ddot{s}}_{ub}[n-1]\\ jer{k}_{ub}\Delta t\\ ⋮\\ jer{k}_{ub}\Delta t\\ 0.0\\ ⋮\\ 0.0\\ s_{init}\\ v_{init}\\ a_{init}\end{array}\right]}_{6n\ast 1}$$

**仿射矩阵$A_{6n\ast 3n}$**的表达式如下所示（空白部分的元素为0）：
$$A={\left[\begin{array}{cccccccccccc}1& & & & & & & & & & & \\ & 1& & & & & & & & & & \\ & & \ddots & & & & & & & & & \\ & & & 1_{n\ast n}& & & & & & & & \\ & & & & 1& & & & & & & \\ & & & & & 1& & & & & & \\ & & & & & & \ddots & & & & & \\ & & & & & & & 1_{n\ast n}& & & & \\ & & & & & & & & 1& & & \\ & & & & & & & & & 1& & \\ & & & & & & & & & & \ddots & \\ & & & & & & & & & & & 1_{n\ast n}\\ & & & & & & & & -1& 1& & \\ & & & & & & & & & \ddots & \ddots & \\ & & & & & & & & & & -1& 1_{(n-1)\ast n}\\ & & & & -1& 1& & & -\frac{\Delta t}{2}& -\frac{\Delta t}{2}& & \\ & & & & & \ddots & \ddots & & & \ddots & \ddots & \\ & & & & & & -1& 1_{(n-1)\ast n}& & & -\frac{\Delta t}{2}& -{\frac{\Delta t}{2}}_{(n-1)\ast n}\\ -1& 1& & & -\Delta t& & & & -\frac{\Delta t^2}{3}& -\frac{\Delta t^2}{6}& & \\ & \ddots & \ddots & & & \ddots & & & & \ddots & \ddots & \\ & & -1& 1_{(n-1)\ast n}& & & -\Delta t_{(n-1)\ast n}& & & & -\frac{\Delta t^2}{3}& -{\frac{\Delta t^2}{6}}_{(n-1)\ast n}\\ 1& 0& \cdots & 0_{1\ast n}& & & & & & & & \\ & & & & 1& 0& \cdots & 0_{1\ast n}& & & & \\ & & & & & & & & 1& 0& \cdots & 0_{1\ast n}\end{array}\right]}_{6n\ast 3n}$$

注意：上式中的下标$1_{n\ast n}$表示某块矩阵的行列数是$n\ast n$。仿射矩阵$A$中的系数也就是PiecewiseJerkProblem的CalculateAffineConstraint函数（在文件modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc中)的系数。仿射矩阵不是对称矩阵，OSQP不要求使用上三角矩阵的形式存储。

优化求解采用OSQP库来实现。

二、流程图

三、主要代码解读

• 调用入口

  Status PiecewiseJerkSpeedOptimizer::Process(
           const PathData& path_data, 
           const TrajectoryPoint& init_point, 
           SpeedData* const speed_data)

• 如果抵达终点， 直接返回OK

  if (reference_line_info_->ReachedDestination()) {
    return Status::OK();
  }

• 如果无路径点，返回错误

  if (path_data.discretized_path().empty()) {
    const std::string msg = "Empty path data";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

• 获取ST图数据和车辆参数

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

  const auto& veh_param =
      common::VehicleConfigHelper::GetConfig().vehicle_param();

• 设置状态变量的初始值

  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对象

 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());

• 根据前面动态规划获取到的初始位移点，更新位移范围（即上、下限）。

   // 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));

• 根据ST图中的限速决策数据，更新速度范围（即上、下限）

 // 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));

• 求解QP问题

      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);
  }

• 输出结果，如果速度曲线的总时长小于FLAGS_fallback_total_time = 3s，则使用SpeedProfileGenerator::FillEnoughSpeedPoints(speed_data)在后面补齐速度全部为0的点。

  // 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();