Apollo中osqp库的使用

OSQP是一种求解QP问题的数值优化开源软件，其使用我们可以分为以下四步：

1. 设定OSQP求解参数
2. 计算QP系数矩阵
3. 构造OSQP求解器
4. 获取优化结果

1. 设定OSQP求解参数

OSQPSettings* PiecewiseJerkProblem::SolverDefaultSettings() {
  // Define Solver default settings
  OSQPSettings* settings =
      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->polish = true;
  settings->verbose = FLAGS_enable_osqp_debug;
  settings->scaled_termination = true;
  return settings;
}

2. 计算QP系数矩阵

// 2.计算QP系数矩阵
// 0.5 * x'Px + q' * x
OSQPData* PiecewiseJerkProblem::FormulateProblem() {
  // calculate kernel
  // 目标函数的二次项系数矩阵 P
  // P_data 存储了P矩阵中的数据;P_indices存储了数据的索引;P_indptr存储了列指针。
  // 这些数据结构是为了构建稀疏矩阵。
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  CalculateKernel(&P_data, &P_indices, &P_indptr);

  // calculate affine constraints
  // 线性不等式约束
  // lower_bounds 和 upper_bounds 存储了约束条件的上下界。
  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  std::vector<c_float> lower_bounds;
  std::vector<c_float> upper_bounds;
  CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
                            &upper_bounds);

  // calculate offset
  // 偏置项，向量 q
  std::vector<c_float> q;
  CalculateOffset(&q);

  // 通过动态内存分配分配OSQPData结构体，并将计算得到的数据填充到这个结构体中。
  // 最后，将问题的维度、稀疏矩阵 P、向量 q、稀疏矩阵 A、约束的下界和上界存储在
  // OSQPData 结构体中，并将这个结构体返回
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));

  CHECK_EQ(lower_bounds.size(), upper_bounds.size());

  size_t kernel_dim = 3 * num_of_knots_;
  size_t num_affine_constraint = lower_bounds.size();

  data->n = kernel_dim;
  data->m = num_affine_constraint;
  data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data),
                       CopyData(P_indices), CopyData(P_indptr));
  data->q = CopyData(q);
  data->A =
      csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),
                 CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));
  data->l = CopyData(lower_bounds);
  data->u = CopyData(upper_bounds);
  return data;
}

3. 构造OSQP求解器

bool PiecewiseJerkProblem::Optimize(const int max_iter) {
  OSQPData* data = FormulateProblem();

  OSQPSettings* settings = SolverDefaultSettings();
  settings->max_iter = max_iter;

  // lcx
  // 3.构造OSQP求解器
  OSQPWorkspace* osqp_work = nullptr;
  osqp_work = osqp_setup(data, settings);
  // osqp_setup(&osqp_work, data, settings);

  osqp_solve(osqp_work);

  auto status = osqp_work->info->status_val;

  if (status < 0 || (status != 1 && status != 2)) {
    AERROR << "failed optimization status:\t" << osqp_work->info->status;
    osqp_cleanup(osqp_work);
    FreeData(data);
    c_free(settings);
    return false;
  } else if (osqp_work->solution == nullptr) {
    AERROR << "The solution from OSQP is nullptr";
    osqp_cleanup(osqp_work);
    FreeData(data);
    c_free(settings);
    return false;
  }

4. 获取优化结果

  // 4.获取优化结果
  // extract primal results
  x_.resize(num_of_knots_);
  dx_.resize(num_of_knots_);
  ddx_.resize(num_of_knots_);
  for (size_t i = 0; i < num_of_knots_; ++i) {
    x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0];
    dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1];
    ddx_.at(i) =
        osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2];
  }

  // Cleanup
  osqp_cleanup(osqp_work);
  FreeData(data);
  c_free(settings);
  return true;