Apollo二次规划算法(piecewise jerk speed optimizer)解析

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo二次规划算法(piecewise jerk speed optimizer)解析

Apollo里面最重要的模块就是感知和规划模块，规划模块最重要的是路径规划和速度规划任务，对应ROS机器人里面的局部规划。Apollo的规划模块首先根据当前的情况进行多场景（scenario）调度，每个场景又包含一个或者多个阶段（stage），每个阶段又由多个具有独立功能的小模块任务（task）完成。前面介绍了路径规划piecewise jerk speed optimizer这个任务，这篇文章就解读一下速度规划piecewise jerk speed optimizer这个任务。SL规划保证车辆的横向偏移足够平滑，ST规划保证车辆的前进方向速度变化足够平滑。

Content:
  1. Apollo 中二次规划问题的构造
  2. Apollo代码的实现

Apollo 中二次规划问题的构造

首先列出代价函数：

(1)   \[ cost=\sum_{i=0}^{n-1}{w_s(s_i-s_{ref})^2+(w_v+p_i)(\dot s_i-v_{ref})^2+ w_a\ddot s_i^2+w_j\dddot s_i^2} \]

其中：

(2)   \[ x=\left\{ 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} \right\} \]

(3)   \[ \dddot s_i=\frac{\ddot s_{i+1}-\ddot s_i}{\Delta t} \]

带入\dddot s_i：

    \begin{align*} cost & = \sum_{i=0}^{n-1}(w_s s_i^2 + w_s s_{ref}^2 - 2w_s s_i s_{ref}) \\ & + \sum_{i=0}^{n-1}[(w_v+p_i) \dot s_i^2 + (w_v+p_i) v_{ref}^2 - 2(w_v+p_i)v_{ref}\dot{s}_i] \\ & + \sum_{i=0}^{n-1}(w_a \ddot s_i^2) \\ & + \sum_{i=0}^{n-1}(w_j \frac{\ddot s_{i+1}^2 + \ddot s_{i}^2 - 2 \ddot s_{i+1} \ddot s_{i}}{\Delta t^2}) \\ \end{align*}

注意到w_s s_{ref}^2和(w_v+p_i) v_{ref}^2为常数，略去：

    \begin{align*} cost & = \sum_{i=0}^{n-2}(w_s s_i^2) + (w_s+w_{s-end})s_{n-1}^2 - \sum_{i=0}^{n-1}(2w_s s_i s_{ref}) \\ & + \sum_{i=0}^{n-2}[(w_v+p_i) \dot s_i^2] + (w_v+p_{n-1}+w_{v-end})\dot s_{n-1}^2 - \sum_{i=0}^{n-1}(2w_vv_{ref}\dot{s}_i) \\ & + \sum_{i=0}^{n-2}(w_a \ddot s_i^2) + w_a \ddot s_{n-1}^2 \\ & + \sum_{i=0}^{n-2}(w_j \frac{\ddot s_{i+1}^2 + \ddot s_{i}^2 - 2 \ddot s_{i+1} \ddot s_{i}}{\Delta t^2}) - w_j \frac{\ddot s_{n}^2 + \ddot s_{n-1}^2 - 2 \ddot s_{n} \ddot s_{n-1}}{\Delta t^2} \\ \end{align*}

    \begin{align*} cost & = \sum_{i=0}^{n-2}(w_s s_i^2) + (w_s+w_{s-end})s_{n-1}^2 - \sum_{i=0}^{n-1}(2w_s s_i s_{ref}) \\ & + \sum_{i=0}^{n-2}[(w_v+p_i) \dot s_i^2] + (w_v+p_{n-1}+w_{v-end})\dot s_{n-1}^2 - \sum_{i=0}^{n-1}(2w_vv_{ref}\dot{s}_i) \\ & + \sum_{i=0}^{n-2}(w_a \ddot s_i^2) + w_a \ddot s_{n-1}^2 \\ & + \sum_{i=0}^{n-2}(w_j \frac{2\ddot s_{i}^2 - 2 \ddot s_{i+1} \ddot s_{i}}{\Delta t^2}) - \frac{w_j}{\Delta t^2} \ddot s_0^2 + \frac{w_j}{\Delta t^2} \ddot s_{n-1}^2 + w_j \frac{(\ddot s_{n} - \ddot s_{n-1})^2 }{\Delta t^2} \\ \end{align*}

注意上式最后一项(\ddot s_{n} - \ddot s_{n-1})^2为常数，略去：

(4)   \begin{align*} cost & = \sum_{i=0}^{n-2}{[{w_ss_i^2}+(w_v+p_i) \dot s_i^2+(w_a+\frac{2w_j}{\Delta t^2}) \ddot s_i^2-\frac{2w_j}{\Delta t^2}\ddot s_i \ddot s_{i+1}]} \\ & + (w_s+w_{s-end})s_{n-1}^2+(w_v+p_{n-1}+w_{v-end})\dot s_{n-1}^2 \\ & - \frac{w_j}{\Delta t^2} \ddot s_0^2 + (w_a+\frac{w_j}{\Delta t^2}) \ddot s_{n-1}^2 \\ & + \sum_{i=0}^{n-1}{[-2w_s s_i s_{ref}-2w_vv_{ref}\dot{s}_i]} \end{align*}

(5)   \[ P_s=\begin{bmatrix} w_s &0 &\ldots &0 \\ 0 &w_s &\ldots &0 \\ \vdots &\vdots &\ddots &\vdots \\ 0 &0 &\ldots &w_s+w_{s-end} \end{bmatrix}\in R^{n \times n} \]

(6)   \[ P_{\dot s}=\begin{bmatrix} w_{v}+p_0 &0 &\ldots &0 \\ 0 &w_{v}+p_1 &\ldots &0 ​\\ \vdots &\vdots &\ddots &\vdots\\ 0 &0 &\ldots &w_{v}+p_{n-1}+w_{v-end} \end{bmatrix}\in R^{n \times n} \]

(7)   \[ P_{\ddot s}=\begin{bmatrix} w_{a}+\frac{w_{j}}{\Delta t^2} &0 &\ldots &0 &0 \\ -\frac{2w_{j}}{\Delta t^2} &w_{a}+\frac{2w_{j}}{\Delta t^2} &\ldots &0 &0 \\ \vdots &\vdots &\ddots &\vdots &\vdots \\ 0 &0 &\ldots &w_{a}+\frac{2w_{j}}{\Delta t^2} &0\\ 0 &0 &\ldots &-\frac{2w_{j}}{\Delta t^2} &w_{a}+\frac{w_{j}}{\Delta t^2}+w_{j-end} \end{bmatrix}\in R^{n \times n} \]

(8)   \[ P=\begin{bmatrix} P_s &0 &0\\ 0 &P_{\dot s} &0\\ 0 &0 &P_{\ddot s} \end{bmatrix}\in R^{3n \times 3n} \]

(9)   \[ q=\begin{bmatrix} -2w_s s_{ref_0} \\ -2w_s s_{ref_1} \\ \vdots \\ -2w_s s_{ref_{n-1}} - 2w_{s-end} w_{s-end-ref_{n-1}} \\ -2w_v v_{ref_0}\\ -2w_v v_{ref_1}\\ \vdots\\ -2w_v v_{ref_{n-1}} - 2w_{v-end} w_{v-end-ref_{n-1}}\\ 0 \\ \vdots\\ - 2w_{j-end} w_{j-end-ref_{n-1}} \end{bmatrix}^T\in R^{1 \times 3n} \]

其中，公式8中从上到下依次对应P_s ， P_{\dot s} ， P_{\ddot s}。
这样代价函数1就由上面的P，q，x表示了。

然后现在构建约束条件l\le Ax\le u：
这个和上文Apollo二次规划算法(piecewise jerk path optimizer)解析中的一致，只是把l对s的限制修改为s对t的限制。

Apollo代码的实现

同上文一样，只是构建P和Q不一样：
构建P:
参见公式8。

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 参见公式5
  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)  参见公式6
  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)''   参见公式7
  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:
参见公式9。

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

本作品采用知识共享署名 4.0 国际许可协议进行许可。

发表回复