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

TASK系列解析文章

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块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_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_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_OPTIMIZERpiecewise_jerk_speed_optimizer_config {acc_weight: 1.0jerk_weight: 3.0kappa_penalty_weight: 2000.0ref_s_weight: 10.0ref_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 , … , 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$
• 末端惩罚项，使末端$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={\dot{s}}_{init}$,${\ddot{s}}_0={\ddot{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 problemif (!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}[0]\\ ⋮\\ {s^{\prime }}_{lb}[n-1]\\ {s^{\prime \prime }}_{lb}[0]\\ ⋮\\ {s^{\prime \prime }}_{lb}[n-1]\\ {s^{\prime \prime \prime }}_{lb}[0]\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}[0]\\ ⋮\\ s_{ub}[n-1]\\ {s^{\prime }}_{ub}[0]\\ ⋮\\ {s^{\prime }}_{ub}[n-1]\\ {s^{\prime \prime }}_{ub}[0]\\ ⋮\\ {s^{\prime \prime }}_{ub}[n-1]\\ {s^{\prime \prime \prime }}_{ub}[0]\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_reffor (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];}
}

设定OSQP求解参数

OSQPSettings* PiecewiseJerkSpeedProblem::SolverDefaultSettings() {// Define Solver default settingsOSQPSettings* 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 STBoundarystd::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 sides_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_lowerif (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_sstd::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_sSpeedPoint sp;reference_speed_data.EvaluateByTime(curr_t, &sp);const double path_s = sp.s();x_ref.emplace_back(path_s);// get curvaturePathPoint path_point = path_data.GetPathPointWithPathS(path_s);penalty_dx.push_back(std::fabs(path_point.kappa()) *config.kappa_penalty_weight());// get v_upper_boundconst 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 problemif (!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 outputconst 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 stoppedif (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();