百度apollo planning代码学习-Apollo\modules\planning\lattice\trajectory_generation\LateralOSQPOptimizer类详解

概述

参考链接：
Apollo 轨迹二次规划数学推导 - 知乎
Solver settings — OSQP documentation
LateralOSQPOptimizer类是apollo planning模块下modules\planning\lattice\trajectory_generation\lateral_osqp_optimizer.cc/.h实现

从类名来看，应该是横向OSQP优化器类？

从代码来看LateralOSQPOptimizer类主要是实现：

对接下来要走的参考线按照s=1.0m间隔采样得到离散的横向边界点对集合d_bounds{(d0min,d0max),(d1min,d1max),…}作为输入加上初始的横向偏移状态输入，然后按照事先设计好的目标函数来设置osqp问题，主要是设置P，q, A, l , u矩阵，然后可以返回求解的最优结果X，X代表在d_bounds约束下，同时还满足其他各种等式不等式约束情况下求得的一条最优横向偏移路径(每个横向偏移点对应着相应的Frenet系纵向坐标s,s每间隔1.0m采样)，其实最后求出来的就是一条path? {(s0,d0,d0’,d0’‘),(s1,d1,d1’,d1’‘),…} 各个纵向坐标点对应的横向偏移状态.求得的最优路径(d,d’,d’')序列塞到类成员vector opt_d_，opt_d_prime_，opt_d_pprime_里。

目标函数包括 横向偏移项(偏离原本参考道路中心线)，横向偏移一阶导，二阶导，到障碍物的距离项。

*将一个简单的小例子具体说明代码的计算流程及原理，见附录

*Lattice palnner，该算法基于Frenet坐标系，通过采样的方式生成轨迹

lateral_osqp_optimizer.h

#pragma once

#include <utility>
#include <vector>

#include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
#include "modules/planning/lattice/trajectory_generation/lateral_qp_optimizer.h"
#include "osqp/osqp.h"

namespace apollo {
namespace planning {

//LateralOSQPOptimizer 类公有继承基类LateralQPOptimizer
class LateralOSQPOptimizer : public LateralQPOptimizer {
 public:
  LateralOSQPOptimizer() = default;

  virtual ~LateralOSQPOptimizer() = default;

//优化函数？
//输入参数：d_state横向状态(d,d',d''),调用时发现其实是初始的横向状态
//输入参数：优化求解横向时delta_s纵向间隔采样？调用时是1.0m
//输入参数: d_bounds是不同s对应的横向上下边界的vector数组，first是下界，second是上界
//其实这个函数主要是对系数矩阵A,还有上下界矩阵lb,ub，q矩阵设置，调用设置P矩阵的函数，最后进行求解取出最优解，存入继承自基类的类成员opt_d_，opt_d_prime_，opt_d_pprime_
//求解出最优X=[d0,d1,...dn,d0',...dn',d0'',...,dn'']^T(代表转置，X为列向量)
  bool optimize(
      const std::array<double, 3>& d_state, const double delta_s,
      const std::vector<std::pair<double, double>>& d_bounds) override;

 private:
 //计算核心？主要就是设置osqp问题里的P矩阵 1/2*X^T*P*X 
//P矩阵里主要储存的都是对d,d',d''项的惩罚系数的权重设置
//详见附录里的计算示例就明白这一系列的原理，因为要化成二次规划标准形式所以引入了系数2
//2.0 * FLAGS_weight_lateral_offset...
  void CalculateKernel(const std::vector<std::pair<double, double>>& d_bounds,
                       std::vector<c_float>* P_data,
                       std::vector<c_int>* P_indices,
                       std::vector<c_int>* P_indptr);
};

}  // namespace planning
}  // namespace apollo

lateral_osqp_optimizer.cc

#include "modules/planning/lattice/trajectory_generation/lateral_osqp_optimizer.h"

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

namespace apollo {
namespace planning {

//优化函数？
//输入参数：d_state横向状态(d,d',d''),调用时发现其实是初始的横向状态
//输入参数：优化求解横向时delta_s纵向间隔采样？调用时是1.0m
//输入参数: d_bounds是不同s对应的横向上下边界的vector数组，first是下界，second是上界
//其实这个函数主要是对系数矩阵A,还有上下界矩阵lb,ub，q矩阵设置，调用设置P矩阵的函数，最后进行求解取出最优解，存入继承自基类的类成员opt_d_，opt_d_prime_，opt_d_pprime_
//求解出最优X=[d0,d1,...dn,d0',...dn',d0'',...,dn'']^T(代表转置，X为列向量)
bool LateralOSQPOptimizer::optimize(
    const std::array<double, 3>& d_state, const double delta_s,
    const std::vector<std::pair<double, double>>& d_bounds) {
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  //计算二次规划标准形式中的矩阵P，输入参数是不同s处对应的横向上下界这样一个vector数组
  //P矩阵以CSC稀疏矩阵的格式储存，P_indices行索引数组，P_indptr列索引数组，P_data同样下标下P矩阵中P_indices行P_indptr列元素的值，稀疏矩阵只储存那些有用的元素，大部分为0的元素就不储存
  CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
  //类成员delta_s_， delta_s为1.0m，说明横向边界是s每隔1.0m采样一次
  delta_s_ = delta_s;
  //横向边界采样点的个数
  const int num_var = static_cast<int>(d_bounds.size());
  //每个横向采样点对应的状态有3个元素d,d',d''
  //所以总共就有3*横向边界采样点的个数的元素，代表二次规划中X的行数
  const int kNumParam = 3 * static_cast<int>(d_bounds.size());
  //约束的个数，包括不等式及等式，实际上总共就有2个kNumParam个约束
  //不等式和等式约束合并成lb<=AX<=ub，等式约束就是上下界下等
  // lb<=X<=ub  X有kNumParam个元素，每个元素都有上下界，就有kNumParam个约束
  // X里连续性等式约束还有kNumParam个，见附录的例子就明白了
  //一对lb,ub称为一个约束
  const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3;
  //lb,ub自然里面就有kNumConstraint个元素，约束有多少对，lb,ub就有多少个元素
  //初始化定义lb,ub数组
  c_float lower_bounds[kNumConstraint];
  c_float upper_bounds[kNumConstraint];

  //prime_offset其实就是代表X中第一个一阶项d0'的下标，详见附录例子就明白了
  const int prime_offset = num_var;
  //pprime_offset其实就是代表X中第一个二阶项d0''的下标，详见附录例子就明白了
  const int pprime_offset = 2 * num_var;

//columns用来储存lb<=AX<=ub里系数矩阵A的，columns[i] i代表列下标
//columns[i]里塞入的pair(int j, float k) j代表行的小标，k代表i行j列的值
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  //columns数组里的元素个数，也就是A的总列数，其实就是X矩阵的行数kNumParam
  columns.resize(kNumParam);

//constraint_index就是一个临时的A矩阵行下标索引，初始从0开始
  int constraint_index = 0;

  //二次规划标准形式里的X=[d0,d1,...dn,d0',...dn',d0'',...,dn'']^T(代表转置，X为列向量)
  // d_i+1'' - d_i''
  //这一块for循环就是为了针对横向偏移的对纵向s的三阶导dddL/ds = (di+1''-di'')/ds，代表横向jerk，对横向jerk进行约束
  for (int i = 0; i + 1 < num_var; ++i) {
    //代表状态变量X里di''对应的A系数矩阵中系数为-1
    columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
    //代表状态变量X里di+1''对应的A系数矩阵中系数为1
    //这样的AX不就代表d_i+1'' - d_i''了?
    columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);

    //这一块for循环主要就是设置矩阵A,lb,ub来实现对jerk的限制，(di+1''-di'')/delta_s<=d'''max
    //(di+1''-di'')<=d'''max*delta_s
    //lateral_third_order_derivative_max就是d'''max在配置文件中被设置为0.1
    //最大值<=max,最小值>=-max
    lower_bounds[constraint_index] =
        -FLAGS_lateral_third_order_derivative_max * delta_s_;
    upper_bounds[constraint_index] =
        FLAGS_lateral_third_order_derivative_max * delta_s_;
    ++constraint_index;
  }

//这个for循环是为了设置连续性等式约束(1) d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')=0
//lb,ub中相应值设置为0
// d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
//这个for循环为了实现这个连续性等式约束(1)，设置lb<=AX<=ub里的A矩阵以及lb,ub里的相应元素
  for (int i = 0; i + 1 < num_var; ++i) {
    //columns代表A矩阵里的列,prime_offset代表二次规划目标函数自变量X里第一个一阶项的下标d0'
    //pprime_offset代表二次规划目标函数自变量X里第一个二阶项的下标d0''
    //代表-di'，自然在A矩阵里相应的系数设为-1
    columns[prime_offset + i].emplace_back(constraint_index, -1.0);
    //代表di+1'，自然在A矩阵里相应的系数设为1
    columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
    //代表- 0.5 * ds * (d_i'' + d_i+1'')，自然在A矩阵里相应的系数设置
    columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
    columns[pprime_offset + i + 1].emplace_back(constraint_index,
                                                -0.5 * delta_s_);
//因为是等式约束d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')=0
//其上下界均设置为0 lb,ub相应行都设置为0
    lower_bounds[constraint_index] = 0.0;
    upper_bounds[constraint_index] = 0.0;
    ++constraint_index;
  }

//这个for循环为了实现这个连续性等式约束(2)，设置lb<=AX<=ub里的A矩阵以及lb,ub里的相应元素
  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
  for (int i = 0; i + 1 < num_var; ++i) {
  //columns代表A矩阵里的列,这一项代表-di的系数-1,在A矩阵里设置相应的系数，di由二次规划标准形式里的X贡献
    columns[i].emplace_back(constraint_index, -1.0);
  //columns代表A矩阵里的列,这一项代表di+1的系数1,在A矩阵里设置相应的系数，di+1由二次规划标准形式里的X贡献
    columns[i + 1].emplace_back(constraint_index, 1.0);
    //columns代表A矩阵里的列,这一项代表- d_i' * ds的系数-ds,在A矩阵里设置相应的系数，di'由二次规划标准形式里的X贡献,prime_offset代表二次规划目标函数自变量X里第一个d0'一阶项的下标
    columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
    //columns代表A矩阵里的列,这一项代表- 1/3 * d_i'' * ds^2的系数- 1/3 * ds^2,在A矩阵里设置相应的系数，di''由二次规划标准形式里的X贡献,pprime_offset代表二次规划目标函数自变量X里第一个二阶项d0''的下标
    columns[pprime_offset + i].emplace_back(constraint_index,
                                            -delta_s_ * delta_s_ / 3.0);
    //columns代表A矩阵里的列,这一项代表- 1/6 * d_i+1'' * ds^2的系数- 1/6 * ds^2,在A矩阵里设置相应的系数，di+1''由二次规划标准形式里的X贡献,pprime_offset代表二次规划目标函数自变量X里第一个二阶项d0''的下标
    columns[pprime_offset + i + 1].emplace_back(constraint_index,
                                                -delta_s_ * delta_s_ / 6.0);

//因为是等式约束d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2=0
//其上下界均设置为0 lb,ub相应行都设置为0
    lower_bounds[constraint_index] = 0.0;
    upper_bounds[constraint_index] = 0.0;
    ++constraint_index;
  }

//这个是等式约束，就是横向位置的初始状态约束 d0=输入的d_state[0]
//在A矩阵中相应的系数设置为1，代表d0
  columns[0].emplace_back(constraint_index, 1.0);
  //因为是等式约束 d0=输入的d_state[0]
  //所以上下边界矩阵都设置为d_state[0]    d_state[0]<=d0<=d_state[0]
  //将等式约束转化为不等式约束了
  lower_bounds[constraint_index] = d_state[0];
  upper_bounds[constraint_index] = d_state[0];
  ++constraint_index;

//这个是等式约束，就是横向位置对纵向s一阶导的初始状态约束 d0'=输入的d_state[1]
//在A矩阵中相应的系数设置为1，代表d0'
  columns[prime_offset].emplace_back(constraint_index, 1.0);
  //因为是等式约束 d0'=输入的d_state[1]
  //所以上下边界矩阵都设置为d_state[1]    d_state[1]<=d0'<=d_state[1]
  //将等式约束转化为不等式约束了
  lower_bounds[constraint_index] = d_state[1];
  upper_bounds[constraint_index] = d_state[1];
  ++constraint_index;

//这个是等式约束，就是横向位置对纵向s二阶导的初始状态约束 d0''=输入的d_state[2]
//在A矩阵中相应的系数设置为1，代表d0''
  columns[pprime_offset].emplace_back(constraint_index, 1.0);
  //因为是等式约束 d0''=输入的d_state[2]
  //所以上下边界矩阵都设置为d_state[2]    d_state[2]<=d0''<=d_state[2]
  //将等式约束转化为不等式约束了
  lower_bounds[constraint_index] = d_state[2];
  upper_bounds[constraint_index] = d_state[2];
  ++constraint_index;

//定义了一个大值LARGE_VALUE 2.0用来作为对一阶导，二阶导d',d''的边界约束[-2,2]
  const double LARGE_VALUE = 2.0;
  //对二次规划目标函数里的自变量X的每一个元素d0,d1,...di,d0',d1',...,d0'',...di''
  for (int i = 0; i < kNumParam; ++i) {
    //A矩阵里第i+1列，constraint_index行元素塞入1，对应X里的d/d'/d''
    columns[i].emplace_back(constraint_index, 1.0);
    if (i < num_var) {
      //如果是0阶项d， di靠输入参数的d_bounds横向边界采样数组里的边界来约束
      //first代表下边界，second代表上边界
      lower_bounds[constraint_index] = d_bounds[i].first;
      upper_bounds[constraint_index] = d_bounds[i].second;
    } else {
    //如果是一阶或者二阶项d' or d''直接给约束[-2,2]
    //据此设置ub,lb矩阵
      lower_bounds[constraint_index] = -LARGE_VALUE;
      upper_bounds[constraint_index] = LARGE_VALUE;
    }
    ++constraint_index;
  }

  CHECK_EQ(constraint_index, kNumConstraint);

  // change affine_constraint to CSC format
  //改变仿射约束到CSC稀疏矩阵格式？其实就是系数矩阵A的储存方法
  //其实就是把上边代表约束系数矩阵A的的columns转化成A矩阵的稀疏阵形式储存
  //把columns里的有用的或非0的元素挨个按行索引，列索引，值依次存入3个vector
  //而不是构建一个超大矩阵，节省储存空间
  //A_indices A矩阵行索引数组
  //A_indptr  A矩阵列索引数组
  //A_data    A矩阵上面行列索引对应的元素值的数组
  //这3个vector只储存有效的或非0的数据
  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  int ind_p = 0;
  for (int j = 0; j < kNumParam; ++j) {
    A_indptr.push_back(ind_p);
    for (const auto& row_data_pair : columns[j]) {
      A_data.push_back(row_data_pair.second);
      A_indices.push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  A_indptr.push_back(ind_p);

  // offset，这里设置的是二次规划标准形式中的q矩阵，主要是X的一次项
  //q矩阵里主要是对应X里0阶项di的系数，1阶项，2阶项的系数全部塞入0
  //代表-2*d*dobs
  //dobs代表车辆为了避障实际路径对参考线的偏离距离，这一项主要为了惩罚对参考线的偏移，其实也代表到障碍物的距离
  //weight_lateral_obstacle_distance就代表这一项惩罚项的权重
  double q[kNumParam];
  for (int i = 0; i < kNumParam; ++i) {
    if (i < num_var) {
      q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
             (d_bounds[i].first + d_bounds[i].second);
    } else {
      q[i] = 0.0;
    }
  }

  // Problem settings问题设置
  //对二次规划问题进行一些初始设置
  //OSQPSettings类是osqp库里的，需包含头文件 "osqp/osqp.h"
  //osqp问题初始设置类 settings 
  OSQPSettings* settings =
      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));

  // Define Solver settings as default
  //定义求解器的设置用默认参数
  osqp_set_default_settings(settings);
  //在默认参数的基础上修改这些参数
  settings->alpha = 1.0;  // 松弛因子？[0,2]默认值1.6
  settings->eps_abs = 1.0e-05; //求解的绝对精度
  settings->eps_rel = 1.0e-05; //求解的相对精度
  settings->max_iter = 5000;//最大求解迭代次数
  settings->polish = true;//性能润色？
  settings->verbose = FLAGS_enable_osqp_debug;//是否打印输出？

  // Populate data设置osqp问题的相关数据
  // OSQPData类是osqp库里的，需包含头文件 "osqp/osqp.h"
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  //n代表二次规划问题自变量X的维度 X是n*1的矩阵
  data->n = kNumParam;
  //m代表二次规划问题等式不等式约束合并的个数 代表A矩阵的行数
  data->m = kNumConstraint;
  //二次规划问题的P矩阵，以系数矩阵格式储存
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
                       P_indices.data(), P_indptr.data());
 //二次规划问题的q矩阵
  data->q = q;
  //二次规划问题约束的系数矩阵A，以CSC系数矩阵格式输入，lb<=Ax<=ub
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
                       A_indices.data(), A_indptr.data());
  //二次规划问题自变量X的下边界矩阵l
  data->l = lower_bounds;
  //二次规划问题自变量X的上边界矩阵u
  data->u = upper_bounds;

  // Workspace
  //osqp的工作空间设置，把数据data和设置settings装入工作空间
   // OSQPWorkspace类是osqp库里的，需包含头文件 "osqp/osqp.h"
  OSQPWorkspace* work = nullptr;
  // osqp_setup(&work, data, settings);
  work = osqp_setup(data, settings);

  // Solve Problem求解工作空间work里的二次规划问题
  osqp_solve(work);

  // extract primal results从工作空间work中提取出原始解solution
  //求解出的最优解X按照0阶di,1阶di',二阶di''依次塞入数组opt_d_，opt_d_prime_，opt_d_pprime_
  for (int i = 0; i < num_var; ++i) {
    opt_d_.push_back(work->solution->x[i]);
    opt_d_prime_.push_back(work->solution->x[i + num_var]);
    opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
  }
  //最后强行把终端的一阶d'，二阶d''归0？为什么？希望到达这个位置时没有横向速度和横向加速度？
  opt_d_prime_[num_var - 1] = 0.0;
  opt_d_pprime_[num_var - 1] = 0.0;

  // Cleanup清空osqp工作空间
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return true;
}

//计算核心？主要就是设置osqp问题里的P矩阵 1/2*X^T*P*X 
//P矩阵里主要储存的都是对d,d',d''项的惩罚系数的权重设置
//详见附录里的计算示例就明白这一系列的原理，因为要化成二次规划标准形式所以引入了系数2
//2.0 * FLAGS_weight_lateral_offset...
void LateralOSQPOptimizer::CalculateKernel(
    const std::vector<std::pair<double, double>>& d_bounds,
    std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
    std::vector<c_int>* P_indptr) {
  const int kNumParam = 3 * static_cast<int>(d_bounds.size());
  P_data->resize(kNumParam);
  P_indices->resize(kNumParam);
  P_indptr->resize(kNumParam + 1);

  for (int i = 0; i < kNumParam; ++i) {
    if (i < static_cast<int>(d_bounds.size())) {
      P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset +
                      2.0 * FLAGS_weight_lateral_obstacle_distance;
    } else if (i < 2 * static_cast<int>(d_bounds.size())) {
      P_data->at(i) = 2.0 * FLAGS_weight_lateral_derivative;
    } else {
      P_data->at(i) = 2.0 * FLAGS_weight_lateral_second_order_derivative;
    }
    P_indices->at(i) = i;
    P_indptr->at(i) = i;
  }
  P_indptr->at(kNumParam) = kNumParam;
  CHECK_EQ(P_data->size(), P_indices->size());
}

}  // namespace planning
}  // namespace apollo

附录

一个简单的计算示例
例：
假设输入Optimize函数的3个参数为：
1.初始横向偏移状态d_state={d0,d0’,d0’'};
2.横向路径优化求解时离散采样长度Δs(纵向每前进1.0m采样一个点，该函数调用时确实是这么多)delta_s，这个也是第3个参数离散的采样长度；
3.横向路径离散采样点的上下边界对集合d_bounds，假设只有3对边界，d_bounds={(-3,3),(-3,3),(-3,3)}，代表在三个点出横向偏移必须在[-3,3]m内
我们根据输入的参数求出对应的P,q,Ac,l,u,x矩阵

P矩阵及X矩阵

q矩阵主要是惩罚车辆到障碍物的距离中的交叉项

l<=AcX<=u


Ac, l , u矩阵都是18行对应着18个约束
1~2行代表对横向jerk的边界不等式约束，最大0.1，这里的横向jerk，指的是对纵向s求导，而非实际常说针对时间求导的jerk；
3~6行代表横向路径的一二阶运动学连续性等式约束；
7~9行代表初始横向状态d0,d0’,d0’'的等式约束；
10~18行代表对X矩阵中每一个元素的边界不等式约束；

CSC稀疏矩阵的储存
上面已经看到，光是d_bounds为3就已经产生了18*9维度的Ac矩阵，实际上d_bounds个数远多3，矩阵的储存表示非常不方便，因此采用CSC稀疏矩阵的储存方式，P矩阵，A矩阵其中都包含大量的0元素，为对角阵或条带矩阵，稀疏矩阵意义就在于只储存这些非零或有意义的元素，即对角线及对角线附近与对角线平行的斜线上的元素，这些有意义的元素用3个vector数组来表示:
vector data代表值；
vector indptr代表指代列索引；
vector indices指代行索引；