百度apollo planning代码学习-Apollo\modules\planning\math\piecewise_jerk\PiecewiseJerkPathProblem类代码详解

概述

PiecewiseJerkPathProblem类是apollo planning模块下modules\planning\math\piecewise_jerk\piecewise_jerk_path_problem.cc/.h实现

从类名来看，应该是PiecewiseJerkPathProblem是分段匀加加速度运动路径问题

从代码来看PiecewiseJerkPathProblem类主要是实现：
对横向路径二次规划优化问题进行构建，主要是构建P矩阵和q矩阵，对横向d,d’,d’‘,d’''以及d偏离dref的距离按照一定的权重计算cost求出最优的路径这个优化问题的构建，但约束并不在该类里设置，而是在调用时通过该类的基类对优化进行设置。

简而言之，该类是路径优化的二次规划问题里的P,q矩阵的构建

*以一个计算示例来说明osqp问题的设置及求解(待上传20220819)

参考链接：
Apollo路径优化

piecewise_jerk_path_problem.h

#pragma once

#include <utility>
#include <vector>

#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"

namespace apollo {
namespace planning {

/*
 * @brief:
 * FEM stands for finite element method.FEM代表有限元方法？
 * This class solve an optimization problem:这个类是求解一个优化问题
 * 每一个s对应的横向偏移合起来就是路径path
 * x
 * |
 * |                       P(s1, x1)  P(s2, x2)
 * |            P(s0, x0)                       ... P(s(k-1), x(k-1))
 * |P(start)
 * |
 * |________________________________________________________ s
 *
 * we suppose s(k+1) - s(k) == s(k) - s(k-1)
 * 我们认为相邻离散点的纵向间隔是一样的
 *
 * Given the x, x', x'' at P(start),  The goal is to find x0, x1, ... x(k-1)
 * 给定初始的横向状态d0,d0',d0'',目标就是求解出最优的d0,d1,...dk-1使得整个路径最平滑
 * which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
 */
//公有继承PiecewiseJerkProblem分段加加速度问题类
class PiecewiseJerkPathProblem : public PiecewiseJerkProblem {
 public:
 //分段加加速度路径优化问题类带参构造函数，输入参数离散点数量也是输入的横向边界采样点的数量，delta_s离散点的纵向间隔1.0m，x_init横向的初始状态d0,d0',d0''
  PiecewiseJerkPathProblem(const size_t num_of_knots, const double delta_s,
                           const std::array<double, 3>& x_init);

  virtual ~PiecewiseJerkPathProblem() = default;

 protected:
 //构建二次规划问题的P矩阵，以CSC稀疏矩阵的格式储存到输入的3个指针数组里
 //P_indices P矩阵行索引数组
  //P_indptr P矩阵列索引数组
  //P_data P矩阵上面行列索引对应的元素值的数组
  void CalculateKernel(std::vector<c_float>* P_data,
                       std::vector<c_int>* P_indices,
                       std::vector<c_int>* P_indptr) override;
//构建二次规划问题的q矩阵,构建结果储存到输入的指针q里
  void CalculateOffset(std::vector<c_float>* q) override;
};

}  // namespace planning
}  // namespace apollo

piecewise_jerk_path_problem.cc

#include "modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h"

#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {
//类的带参构造函数
//输入参数：num_of_knots离散点的数量，调用时输入的是横向边界里的元素的数量，横向边界{(d0_lower,d0_upper),(d1_lower,d1_upper)...}
//输入参数：x_init初始的状态(s,s',s''或d,d',d'')
//输入参数：delta_s各个横线边界或者是离散点之间对应的纵向间隔s=1.0m
PiecewiseJerkPathProblem::PiecewiseJerkPathProblem(
    const size_t num_of_knots, const double delta_s,
    const std::array<double, 3>& x_init)
    //这些参数用来初始化另一个类PiecewiseJerkProblem
    : PiecewiseJerkProblem(num_of_knots, delta_s, x_init) {}

//计算内核，又是QP问题，输入又是一个CSC格式的P矩阵，求得的P矩阵按CSC格式存入输入的3个参数中,CSC稀疏矩阵储存，仅储存对角线元素以及其平行的对角元素，省略掉大量的0元素，对于大型矩阵的运算速度大大增加
 //P_indices P矩阵行索引数组
  //P_indptr P矩阵列索引数组
  //P_data P矩阵上面行列索引对应的元素值的数组
void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
                                               std::vector<c_int>* P_indices,
                                               std::vector<c_int>* P_indptr) {
  //将离散点的数量赋值给n
  const int n = static_cast<int>(num_of_knots_);
  //则变量的个数应该就是二次规划QP问题里1/2X^T*P*X+q^T*X变量X的元素的数量，n个离散时间采样点，每个时刻对应的状态有3个(s,s',s''或d,d',d'')
  const int num_of_variables = 3 * n;
  //非零元素的个数 = X的元素的数量 + n - 1 = 4n-1?
  const int num_of_nonzeros = num_of_variables + (n - 1);
  //定义了一个columns来储存P矩阵的各列
  std::vector<std::vector<std::pair<c_int, c_float>>> columns(num_of_variables);
  int value_index = 0;

  // x(i)^2 * (w_x + w_x_ref[i]), w_x_ref might be a uniform value for all x(i)
  // or piecewise values for different x(i)
  //这一项是构建cost函数里对横向偏移量项及避障时导致横向boundary的中心线对参考线的偏移项构成， Wd*∑d^2 + Wdref*∑(d-(dmin+dmax)/2)^2这两项之和把d^2项系数合并就是这个for循环干的事。注：Wdref*∑dmax^2这是一个常数项，求cost函数最小值时可以直接忽略，scale_factor_[0],scale_factor_[1],scale_factor_[2]都是1，weight_x_ref_vec_基本也是个常数项，调用时好像都一样
  //设置P矩阵中
  for (int i = 0; i < n - 1; ++i) {
    columns[i].emplace_back(i, (weight_x_ + weight_x_ref_vec_[i]) /
                                   (scale_factor_[0] * scale_factor_[0]));
    ++value_index;
  }
  // x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x)
  // x(n-1)^2 * (w_x + w_x_ref[n-1])这个其实跟上面的for循环干的一件事，只是因为上面的for循环没有到第n项(下标n-1)，，但不同的是，这里针对第n个离散横向采样点偏移的平方加了一个终端状态的权重weight_end_state_[0],但是貌似weight_end_state_[0],weight_end_state_[1],weight_end_state_[2]全部为0，直接把第n项横向偏移的平方惩罚项合并到上面那个for循环算了
  columns[n - 1].emplace_back(
      n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /
                 (scale_factor_[0] * scale_factor_[0]));
  ++value_index;

  // x(i)'^2 * w_dx
  //这个for循环是设置P矩阵中每个离散采样点d'^2的惩罚项的权重，对横向偏移量变化率的权重
  for (int i = 0; i < n - 1; ++i) {
    columns[n + i].emplace_back(
        n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
    ++value_index;
  }
  // x(n-1)'^2 * (w_dx + w_end_dx) 本来是想对对终端的一阶横向状态单独处理，但是实际上weight_end_state_[1]=0，所以完全终端状态的一阶横向状态可以和上面的for循环合并
  columns[2 * n - 1].emplace_back(2 * n - 1,
                                  (weight_dx_ + weight_end_state_[1]) /
                                      (scale_factor_[1] * scale_factor_[1]));
  ++value_index;

	//横向离散采样点的纵向间隔delta_s_ 通常为1.0m，这里求了个平方是做什么的？
  auto delta_s_square = delta_s_ * delta_s_;
  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
  //这个是对横向状态二阶d''项平方进行惩罚，weight_dddx_ / delta_s_square其实是拆分了一部分加加速度的平方项 加加速度平方项cost=weight_dddx_ ∑(d''(k+1)-d''(k))/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;
  }
  //横向加加速度平方项cost(部分)以及横向加速度的平方项cost，其实weight_end_state_[2]=0，注意这里的加速度和加加速度都是横向d对纵向位置s求导
  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循环实现写入P矩阵
  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, num_of_nonzeros);

  int ind_p = 0;
  //P_indices P矩阵行索引数组
  //P_indptr P矩阵列索引数组
  //P_data P矩阵上面行列索引对应的元素值的数组
  //将上述写入columns的数据按CSC稀疏矩阵格式储存矩阵P
  for (int i = 0; i < num_of_variables; ++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);
}

//Wdref*∑(d-(dmin+dmax)/2)^2计算路径因为避障车道左右边界中心线偏离道路参考线的cost中的交叉项由二次规划的q^T*X项实现，也就是这里的计算offset,由于1/2X^TPX中的1/2为了化成1，所以q^T*X的权重要乘以个2
void PiecewiseJerkPathProblem::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, 0.0);

  if (has_x_ref_) {
    for (int i = 0; i < n; ++i) {
      q->at(i) += -2.0 * weight_x_ref_vec_.at(i) * x_ref_[i] / scale_factor_[0];
    }
  }

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

}  // namespace planning
}  // namespace apollo

以一个例子说明P，q矩阵是如何构建的
约束条件呢？

约束条件