二次规划---以百度Apollo对qpOASES的使用

在上一篇博客中对二次规划和qpoase进行了介绍,　但是对qpOASES库的使用还是不够了解，这里通过百度Apollo规划模块中对qpOASES的使用对这个库进行再次了解。后续也将研究研究apollo对osqp库的使用方法。

apollo中对osqp的用处可能不止一处，对active_set_spline_1d_solver.cc进行分析。

变量定义依据

基于qpoase对二次规划形式的描述，我们采用的变量名称都依据下图。


H：hessian矩阵 $H\in {\mathbf{R}}^{\mathbf{n}\mathbf{V}\times \mathbf{n}\mathbf{V}}$
g：梯度向量 $g\in {\mathbf{R}}^{\mathbf{n}\mathbf{V}}$
A：约束矩阵 $A\in {\mathbf{R}}^{\mathbf{n}\mathbf{C}\times \mathbf{n}\mathbf{V}}$
lb ub：上下限边界向量 $lb,ub\in {\mathbf{R}}^{\mathbf{n}\mathbf{V}}$
lbA ubA：上下限约束向量 $lb,ub\in {\mathbf{R}}^{\mathbf{n}\mathbf{C}}$。 这个向量中不仅包含了不等式，也包括了等式的约束,将上限和下限设置为相同数值即可表示为等式约束！
nWSR： 最大迭代次数
cputime：如果输入不为空，就会输出整个初始化和求解所花的时间。

结构分析

apollo是通过osqp和qpoase两种库对样条曲线进行求解。在方程的建立过程中，使用了spline1d存储样条，使用spline1dConstraint来存储所有的约束条件，包含等式约束、不等式约束、边界约束等；使用spline1dKernel存储了优化目标方程中的hissen矩阵H和线性矩阵g。

代码分析

方程建立

由于qpoase所需的输入都是一维的，因此需要对矩阵、各约束进行转换。以下就对求解过程中的各个参数的赋值和转换进行了分析。

• 传入各矩阵

  const MatrixXd& kernel_matrix = kernel_.kernel_matrix();　　//二维hissen矩阵，定义为MatrixXd::Zero(total_params_, total_params_)
  const MatrixXd& offset = kernel_.offset();　　　　　　　　　　//　线性矩阵ｇ，实则一个向量　定义为MatrixXd::Zero(total_params_, 1);
  const MatrixXd& inequality_constraint_matrix =
      constraint_.inequality_constraint().constraint_matrix();　//不等式约束矩阵
  const MatrixXd& inequality_constraint_boundary =
      constraint_.inequality_constraint().constraint_boundary();　//不等式约束边界，实则向量
  const MatrixXd& equality_constraint_matrix =
      constraint_.equality_constraint().constraint_matrix();     //等式约束矩阵
  const MatrixXd& equality_constraint_boundary =
      constraint_.equality_constraint().constraint_boundary();  //等式约束边界，实则向量

• 传入矩阵转化为qpoase格式

1. 优化目标转化：Hessian矩阵和g矩阵
   

  const auto kNumOfMatrixElements = kernel_matrix.rows() * kernel_matrix.cols();  //二维矩阵的总大小
  double h_matrix[kNumOfMatrixElements];  // NOLINT 

  const auto kNumOfOffsetRows = offset.rows(); 
  double g_matrix[kNumOfOffsetRows];  // NOLINT ， 与offset一般大
  int index = 0;

  for (int r = 0; r < kernel_matrix.rows(); ++r) {  // kernel_matrix.rows() == kernel_matrix.cols() == offset.rows()
    g_matrix[r] = offset(r, 0);
    for (int c = 0; c < kernel_matrix.cols(); ++c) {
      h_matrix[index++] = kernel_matrix(r, c);          //h矩阵为逐行赋值
    }
  }

3. 边界条件转化
   

  double lower_bound[num_param];  // NOLINT
  double upper_bound[num_param];  // NOLINT

  const double l_lower_bound_ = -kMaxBound;
  const double l_upper_bound_ = kMaxBound;
  for (int i = 0; i < num_param; ++i) { //没有对x的边界约束，默认为最大最下数值1e3，当然按照说明，这里赋值为空指针页可以
    lower_bound[i] = l_lower_bound_;
    upper_bound[i] = l_upper_bound_;
  }

3. 约束条件转换

由于存储等式约束和不等式约束两种，apollo在增加时先后增加了这两种约束

 double affine_constraint_matrix[num_param * num_constraint];  // NOLINT 大小为参数个数乘以约束个数
  double constraint_lower_bound[num_constraint];                // NOLINT 大小为约束条件个数
  double constraint_upper_bound[num_constraint];                // NOLINT 大小为约束条件个数

  index = 0;
  // 首先增加等式约束
  for (int r = 0; r < equality_constraint_matrix.rows(); ++r) {  
    constraint_lower_bound[r] = equality_constraint_boundary(r, 0); // 上下限相同
    constraint_upper_bound[r] = equality_constraint_boundary(r, 0);

    for (int c = 0; c < num_param; ++c) {
      affine_constraint_matrix[index++] = equality_constraint_matrix(r, c);  //按照行顺序输入矩阵A中
    }
  }

  DCHECK_EQ(index, equality_constraint_matrix.rows() * num_param);
  //增加不等式约束
  const double constraint_upper_bound_ = kMaxBound;
  for (int r = 0; r < inequality_constraint_matrix.rows(); ++r) {
    constraint_lower_bound[r + equality_constraint_boundary.rows()] =   // 下限由不等式获取
        inequality_constraint_boundary(r, 0);
    constraint_upper_bound[r + equality_constraint_boundary.rows()] =   // 上限是最大值30，可能已经将所有上边界进行转化为下边界了
        constraint_upper_bound_;

    for (int c = 0; c < num_param; ++c) {
      affine_constraint_matrix[index++] = inequality_constraint_matrix(r, c);//按照行顺序输入矩阵A中
    }
  }

方法调用和求解

use_hot_start：当上一次有解且和这一次的约束 参数数量相同就不再进行初始化和init设置了。

1. qpoase参数设置

    sqp_solver_.reset(new ::qpOASES::SQProblem(num_param, num_constraint,  //设置变量个数 约束个数 Hessian矩阵类型未知
                                               ::qpOASES::HST_UNKNOWN));
    ::qpOASES::Options my_options;
    my_options.enableCholeskyRefactorisation = 1;           //设定Specifies the frequency of full refactorisation of proj. Hessian
    my_options.epsNum = FLAGS_default_active_set_eps_num;   //设定 比率测试的分子公差。
    my_options.epsDen = FLAGS_default_active_set_eps_den;   //设定比率测试的分母公差。
    my_options.epsIterRef = FLAGS_default_active_set_eps_iter_ref;   //设定迭代优化的早期终止公差
    sqp_solver_->setOptions(my_options);   //设定参数选项
    if (!FLAGS_default_enable_active_set_debug_info) {
      sqp_solver_->setPrintLevel(qpOASES::PL_NONE);   //设定不输出
    }

2. 初始化和求解

    ret = sqp_solver_->hotstart(  //如果能够热启动，使用部分上一次的搜索结果
        h_matrix, g_matrix, affine_constraint_matrix, lower_bound, upper_bound,
        constraint_lower_bound, constraint_upper_bound, max_iter);
    if (ret != qpOASES::SUCCESSFUL_RETURN) {
      DEBUG("Fail to hotstart spline 1d, will use re-init instead.");
      // 重新求解
      ret = sqp_solver_->init(h_matrix, g_matrix, affine_constraint_matrix,
                              lower_bound, upper_bound, constraint_lower_bound,
                              constraint_upper_bound, max_iter);
    }

3. 获取结果

  double result[num_param];  // NOLINT
  memset(result, 0, sizeof result);      //全为0
  sqp_solver_->getPrimalSolution(result);  //获取结果

  MatrixXd solved_params = MatrixXd::Zero(num_param, 1);
  for (int i = 0; i < num_param; ++i) {                 //结果转化为内部需要的一维格式
    solved_params(i, 0) = result[i];
    DEBUG("spline 1d solved param[" << i << "]: " << result[i]);
  }

  last_num_param_ = num_param;
  last_num_constraint_ = num_constraint;

  return spline_.SetSplineSegs(solved_params, spline_.spline_order());   //求取参数化的样条曲线

以上所分析的代码部分解决的问题原型来自于apollo中的速度规划,可见官方文档对算法的分析。