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

yuan〇

已于 2023-09-07 21:48:18 修改

阅读量1.3k

点赞数 8

分类专栏： Apollo 文章标签：自动驾驶决策规划算法 apollo 人工智能

于 2023-09-04 17:01:52 首次发布

本文链接：https://blog.csdn.net/sinat_52032317/article/details/132651462

版权

Apollo 专栏收录该内容

46 篇文章 219 订阅

订阅专栏

文章目录

TASK系列解析文章
前言
PIECEWISE_JERK_SPEED_OPTIMIZER功能简介
PIECEWISE_JERK_SPEED_OPTIMIZER相关配置
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_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问题的标准类型定义：

$\frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. LB \leq x \leq UB \\ A_{eq}x = b_{eq} \\ Ax \leq b$

优化变量

在这里插入图片描述

速度规划的坐标系

注意：速度规划的 $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,\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}}\}$

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

设计目标函数

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

尽可能贴合决策时制定的st曲线： $\left| {{s_i} - {s_{i - ref}}} \right| \downarrow$
确保舒适的体感，尽可能降低加速度/加加速度： $\left| {{{\ddot s}_{i + 1}}} \right| \downarrow$ ， $\left| {{{s'''}_{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{aligned} 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^{'''}}_{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{aligned}$

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

为了统一格式，方便与代码对照，在此对目标函数进行变换：
$\begin{aligned} 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_{\color{red}i=0}^{\color{red}n-2}w_{ddds}{s^{'''}}_{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{aligned}$

注： $p_i$ 代表penalty，为曲率 $\kappa$ 与曲率的权重 $w_i$ 的乘积。

接着，对目标函数按阶次整理可得：
$\begin{aligned} 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_{\color{red}i=0}^{\color{red}n-2}w_{ddds}{s^{'''}}_{i \to i + 1}^2 \end{aligned}$

用 $s^{'''}=\frac{{{{s''}_{i + 1}} - {{s''}_i}}}{{\Delta t}}$ 代入目标函数，
$\begin{aligned} \sum_{\color{red}{i=0}}^{\color{red}{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_{\color{red}i=1}^{\color{red}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_{\color{red}i=0}^{\color{red}n-2}\frac{s_i^{\prime\prime}\cdot s_{i+1}^{\prime\prime}}{\Delta t^2} \end{aligned}$

$\begin{aligned} 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_{\color{red}i=1}^{\color{red}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_{\color{red}i=0}^{\color{red}n-2}\frac{s_i^{\prime\prime}\cdot s_{i+1}^{\prime\prime}}{\Delta t^2}] \end{aligned}$

约束条件

接下来谈谈约束的设计。
要满足的约束条件：
• 主车必须在道路边界内，同时不能和障碍物有碰撞 ${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{aligned} s_{i+1}^{\prime\prime} &=s_i''+\int_0^{\Delta t}s_{i\to i+1}^{\prime\prime\prime}dt=s_i''+s_{i\to i+1}^{\prime\prime\prime}*\Delta t \\ s_{i+1}^{\prime} &=s_i^{\prime}+\int_0^{\Delta t}\boldsymbol{s''}(t)dt=s_i^{\prime}+s_i^{\prime\prime}*\Delta t+\frac12*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^2 \\ &= s_i^{\prime}+\frac12*s_i^{\prime\prime}*\Delta t+\frac12*s_{i+1}^{\prime\prime}*\Delta t\\ s_{i+1} &=s_i+\int_0^{\Delta t}\boldsymbol{s'}(t)dt \\ &=s_i+s_i^{\prime}*\Delta t+\frac12*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^3\\ &=s_i+s_i^{\prime}*\Delta t+\frac13*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i+1}^{\prime\prime}*\Delta t^2 \end{aligned}$

• 起点约束： $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 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$ , $u pp er b o u n d$ , $l o w er b o u n d$ 均和路径规划部分一致：
$\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]$

$\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]$

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

二次项系数矩阵 $H$

$\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]$

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$

$\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]$

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

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

输入：

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\leq s \leq pathlength$ ，此时还未考虑ST boundary； $\dot s$ 的约束为 $0\leq \dot s \leq v_{max}$ ， $v_{max}$ 选出FLAGS_planning_upper_speed_limit和起始点速度之中最大的一个； $\ddot s$ 的约束为 $a_{dec-max}\leq \ddot s \leq a_{acc-max}$ ，即在最大减速度和最大加速度范围内； $s^{'''}$ 的约束为 $s^{'''}_{lb}\leq s^{'''} \leq s^{'''}_{ub}$ ， $j er k$ 的约束由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();

yuan〇

关注

8
点赞
踩
8

收藏

觉得还不错? 一键收藏
打赏
3
评论
【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的函数会依次调用task_list中的TASK，本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限，文章可能有纰漏的地方，还请批评斧正。在配置文件中，我们可以看到LaneFollow所需要执行的所有task。本文将继续介绍LaneFollow的第13个TASK——产生平滑速度规划曲线根据ST图的可行驶区
复制链接

扫一扫