【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_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$到$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}$.
$$x=\{s_0,s_1,\dots ,s_{n-1},\dot{s_0},\dot{s_1},\dots ,\dot{s_{n-1}},\ddot{s_0},\ddot{s_1},\dots ,\ddot{s_{n-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$
• 在转弯时减速行驶, 曲率越大，速度越小，减小向心加速度： ∣ s ˙ i 2 κ s i ∣ ↓ \left| \dot{s}_i^2\kappa_{s_i} \right| \downarrow ​s˙i2​κsi​​ ​↓
• 末端惩罚项，使末端$s,v,a$趋于预期

最后会得到以下目标函数：
$$\begin{array}{rl}minf& =\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$代表penalty，为曲率$\kappa$与曲率的权重$w_i$的乘积。

接着，对目标函数按阶次整理可得：
$$\begin{array}{rl}minf& =\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}minf& =\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)$$• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
$$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 }(t)\right)$$
• 连续性约束：
$$\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}{\mathbf{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}{\mathbf{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}$$

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

可以看到和路径规划部分的约束条件基本一致，因此在约束矩阵$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$均和路径规划部分一致：
l b = [ s l b [ 0 ] ⋮ s l b [ n − 1 ] s ′ l b [ 0 ] ⋮ s ′ l b [ n − 1 ] s ′ ′ l b [ 0 ] ⋮ s ′ ′ l b [ n − 1 ] s ′ ′ ′ l b [ 0 ] ⋅ Δ t ⋮ s ′ ′ ′ l b [ n − 2 ] ⋅ Δ t 0 ⋮ 0 s i n i t s ′ i n i t s ′ ′ i n i t ] , u b = [ s u b [ 0 ] ⋮ s u b [ n − 1 ] s ′ u b [ 0 ] ⋮ s ′ u b [ n − 1 ] s ′ ′ u b [ 0 ] ⋮ s ′ ′ u b [ n − 1 ] s ′ ′ ′ u b [ 0 ] ⋅ Δ t ⋮ s ′ ′ ′ u b [ n − 2 ] ⋅ Δ t 0 ⋮ 0 s i n i t s ′ i n i t s ′ ′ i n i t ] lb = \left[ {\begin{array}{ccccccccccccccc}{{s_{lb}}[0]}\\ \vdots \\{{s_{lb}}[n - 1]}\\{{{s'}_{lb}}[0]}\\ \vdots \\{{{s'}_{lb}}[n - 1]}\\{{{s''}_{lb}}[0]}\\ \vdots \\{{{s''}_{lb}}[n - 1]}\\{{{s'''}_{lb}}[0] \cdot \Delta t}\\ \vdots \\{{{s'''}_{lb}}[n - 2] \cdot \Delta t}\\0\\ \vdots \\0\\{{s_{init}}}\\{{{s'}_{init}}}\\{{{s''}_{init}}}\end{array}} \right],ub = \left[ {\begin{array}{ccccccccccccccc}{{s_{ub}}[0]}\\ \vdots \\{{s_{ub}}[n - 1]}\\{{{s'}_{ub}}[0]}\\ \vdots \\{{{s'}_{ub}}[n - 1]}\\{{{s''}_{ub}}[0]}\\ \vdots \\{{{s''}_{ub}}[n - 1]}\\{{{s'''}_{ub}}[0] \cdot \Delta t}\\ \vdots \\{{{s'''}_{ub}}[n - 2] \cdot \Delta t}\\0\\ \vdots \\0\\{{s_{init}}}\\{{{s'}_{init}}}\\{{{s''}_{init}}}\end{array}} \right] lb= ​slb​[0]⋮slb​[n−1]s′lb​[0]⋮s′lb​[n−1]s′′lb​[0]⋮s′′lb​[n−1]s′′′lb​[0]⋅Δt⋮s′′′lb​[n−2]⋅Δt0⋮0sinit​s′init​s′′init​​ ​,ub= ​sub​[0]⋮sub​[n−1]s′ub​[0]⋮s′ub​[n−1]s′′ub​[0]⋮s′′ub​[n−1]s′′′ub​[0]⋅Δt⋮s′′′ub​[n−2]⋅Δt0⋮0sinit​s′init​s′′init​​ ​

A = [ 1 0 ⋯ 0 0 ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ 0 1 1 0 ⋯ 0 0 ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ 0 1 1 0 ⋯ 0 0 ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ 0 1 − 1 1 ⋱ ⋱ − 1 1 − 1 1 ⋱ ⋱ − 1 1 − Δ t 2 − Δ t 2 ⋱ ⋱ − Δ t 2 − Δ t 2 − 1 1 ⋱ ⋱ − 1 1 − Δ t ⋱ − Δ t − Δ t 2 3 − Δ t 2 6 ⋱ ⋱ − Δ t 2 3 − Δ t 2 6 1 1 1 ] A = \left[ {\begin{array}{ccccccccccccccc}{\begin{array}{ccccccccccccccc}1&0& \cdots &0\\0& \ddots &{}& \vdots \\ \vdots &{}& \ddots &0\\0& \cdots &0&1\end{array}}&{}&{}\\{}&{\begin{array}{ccccccccccccccc}1&0& \cdots &0\\0& \ddots &{}& \vdots \\ \vdots &{}& \ddots &0\\0& \cdots &0&1\end{array}}&{}\\{}&{}&{\begin{array}{ccccccccccccccc}1&0& \cdots &0\\0& \ddots &{}& \vdots \\ \vdots &{}& \ddots &0\\0& \cdots &0&1\end{array}}\\{}&{}&{\begin{array}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}\\{}&{\begin{array}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}&{\begin{array}{ccccccccccccccc}{ - \frac{{\Delta t}}{2}}&{ - \frac{{\Delta t}}{2}}&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - \frac{{\Delta t}}{2}}&{ - \frac{{\Delta t}}{2}}\end{array}}\\{\begin{array}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}&{\begin{array}{ccccccccccccccc}{ - \Delta t}&{}&{}&{}\\{}& \ddots &{}&{}\\{}&{}&{ - \Delta t}&{}\end{array}}&{\begin{array}{ccccccccccccccc}{ - \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}{ccccccccccccccc}1&{}&{}&{}\\{}&{}&{}&{}\\{}&{}&{}&{}\end{array}}&{\begin{array}{ccccccccccccccc}{}&{}&{}&{}\\1&{}&{}&{}\\{}&{}&{}&{}\end{array}}&{\begin{array}{ccccccccccccccc}{}&{}&{}&{}\\{}&{}&{}&{}\\1&{}&{}&{}\end{array}}\end{array}} \right] A= ​10⋮0​0⋱⋯​⋯⋱0​0⋮01​−1​1⋱​⋱−1​1​1​​​​​10⋮0​0⋱⋯​⋯⋱0​0⋮01​−1​1⋱​⋱−1​1​−Δt​⋱​−Δt​​1​​​​​10⋮0​0⋱⋯​⋯⋱0​0⋮01​−1​1⋱​⋱−1​1​−2Δt​​−2Δt​⋱​⋱−2Δt​​−2Δt​​−3Δt2​​−6Δt2​⋱​⋱−3Δt2​​−6Δt2​​1​​​​​ ​

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

二次项系数矩阵$H$

H = 2 ⋅ [ w s − r e f 0 ⋯ 0 0 ⋱ ⋮ ⋮ w s − r e f 0 0 ⋯ 0 w s − r e f + w e n d − s w d s − r e f + p 0 0 ⋯ 0 0 ⋱ ⋮ ⋮ w d s − r e f + p n − 2 0 0 ⋯ 0 w d s − r e f + p n − 1 + w e n d − s w d d s + w d d d s Δ t 2 0 ⋯ ⋯ 0 − 2 ⋅ w d d d s Δ t 2 w d d s + 2 ⋅ w d d d s Δ t 2 ⋮ 0 − 2 ⋅ w d d d s Δ t 2 ⋱ ⋮ ⋮ ⋱ w d d s + 2 ⋅ w d d d s Δ t 2 0 0 ⋯ 0 − 2 ⋅ w d d d s Δ t 2 w d d s + w d d d s Δ t 2 + w e n d − d d s ] H = 2 \cdot \left[ {\begin{array}{ccccccccccccccc}{\begin{array}{ccccccccccccccc}{{w_{s - ref}}}&0& \cdots &0\\0& \ddots &{}& \vdots \\ \vdots &{}&{{w_{s - ref}}}&0\\0& \cdots &0&{{w_{s - ref}} + {w_{end - s}}}\end{array}}&{}&{}\\{}&{\begin{array}{ccccccccccccccc}{{w_{ds - ref}} + {p_0}}&0& \cdots &0\\0& \ddots &{}& \vdots \\ \vdots &{}&{{w_{ds - ref}} + {p_{n - 2}}}&0\\0& \cdots &0&{{w_{ds - ref}} + {p_{n - 1}} + {w_{end - s}}}\end{array}}&{}\\{}&{}&{\begin{array}{ccccccccccccccc}{{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}}}}&{}&{}& \vdots \\0&{ - 2 \cdot \frac{{{w_{ddds}}}}{{\Delta {t^2}}}}& \ddots &{}& \vdots \\ \vdots &{}& \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] H=2⋅ ​ws−ref​0⋮0​0⋱⋯​⋯ws−ref​0​0⋮0ws−ref​+wend−s​​​wds−ref​+p0​0⋮0​0⋱⋯​⋯wds−ref​+pn−2​0​0⋮0wds−ref​+pn−1​+wend−s​​​wdds​+Δt2wddds​​−2⋅Δt2wddds​​0⋮0​0wdds​+2⋅Δt2wddds​​−2⋅Δt2wddds​​⋯​⋯⋱⋱0​⋯wdds​+2⋅Δt2wddds​​−2⋅Δt2wddds​​​0⋮⋮0wdds​+Δt2wddds​​+wend−dds​​​ ​

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 = [ − 2 ⋅ w s − r e f ⋅ s 0 − r e f ⋮ − 2 ⋅ w s − r e f ⋅ s ( n − 2 ) − r e f − 2 ⋅ w s − r e f ⋅ s ( n − 1 ) − r e f − 2 ⋅ w e n d − s ⋅ s e n d − 2 ⋅ w d s − r e f ⋅ s ˙ r e f ⋮ − 2 ⋅ w d s − r e f ⋅ s ˙ r e f − 2 ⋅ w e n d − d s ⋅ s ¨ e n d − 2 ⋅ w d s − r e f ⋅ s ˙ r e f 0 ⋮ 0 − 2 ⋅ w e n d − d d s ⋅ s ¨ e n d ] q = \left[ {\begin{array}{ccccccccccccccc}{ - 2 \cdot {w_{s - ref}} \cdot {s_{0 - ref}}}\\ \vdots \\{ - 2 \cdot {w_{s - ref}} \cdot {s_{(n - 2) - ref}}}\\{ - 2 \cdot {w_{s - ref}} \cdot {s_{(n - 1) - ref}} - {\rm{2}} \cdot {w_{end - s}} \cdot {s_{end }}}\\- 2 \cdot {w_{ds - ref}} \cdot {\dot s_{ref}}\\ \vdots \\- 2 \cdot {w_{ds - ref}} \cdot {\dot s_{ref}}\\{ - {\rm{2}} \cdot {w_{end - ds}} \cdot {\ddot s_{end}}}- 2 \cdot {w_{ds - ref}} \cdot {\dot s_{ref}}\\0\\ \vdots \\0\\{ - {\rm{2}} \cdot {w_{end - dds}} \cdot {\ddot s_{end }}}\end{array}} \right] q= ​−2⋅ws−ref​⋅s0−ref​⋮−2⋅ws−ref​⋅s(n−2)−ref​−2⋅ws−ref​⋅s(n−1)−ref​−2⋅wend−s​⋅send​−2⋅wds−ref​⋅s˙ref​⋮−2⋅wds−ref​⋅s˙ref​−2⋅wend−ds​⋅s¨end​−2⋅wds−ref​⋅s˙ref​0⋮0−2⋅wend−dds​⋅s¨end​​ ​

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

设定OSQP求解参数

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便是基于二次规划的速度优化算法的主流程了，主要包含以下步骤：

1. 判断是否抵达终点，如果抵达终点，直接return。
2. 检查路径是否为空
3. 设置相关参数
4. 更新STBoundary
5. 更新速度边界和参考s
6. 速度优化
7. 输出

输入：

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

输出：
得到最优的速度，信息包括$opt\_s,opt\_ds,opt\_dds$。在Process函数中最终结果保存到了speed_data中。

设置相关参数

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

这部分需要注意一下：
可以看到，在速度规划方面，$s$的约束为$0\le s\le pathlength$，此时还未考虑ST boundary；$\dot{s}$的约束为$0\le \dot{s}\le v_{max}$，$v_{max}$选出FLAGS_planning_upper_speed_limit和起始点速度之中最大的一个；$\ddot{s}$的约束为$a_{dec-max}\le \ddot{s}\le a_{acc-max}$，即在最大减速度和最大加速度范围内；$s^{{}^{\prime \prime \prime }}$的约束为$s_{lb}^{{}^{\prime \prime \prime }}\le s^{{}^{\prime \prime \prime }}\le s_{ub}^{{}^{\prime \prime \prime }}$，$jerk$的约束由FLAGS_longitudinal_jerk_lower_bound和FLAGS_longitudinal_jerk_upper_bound两个参数决定；${\dot{s}}_{ref}$参考速度取决于当前路径的巡航速度。

更新STBoundary

遍历每个点，根据不同障碍物类型，更新STBoundary 。

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

其中涉及到这个函数GetUnblockSRange，用途是获取未被阻塞段的s的范围。

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

更新速度边界和参考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));

速度优化

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

输出

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