欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

Apollo MPC OSQP Solver

程序员文章站 2022-07-12 13:16:49
...

Apollo MPC算法之前使用qpOASES Solver, 现在替换为OSQP, OSQP 开发者自己做个Benchmark, 测试Solver性能,结果现实OSQP的运行效率是比qpOASES要高的, 我们自己也做过测试,对于同样的MPC实时优化问题, OSQP的求解速度的确比qpOASES要快很多。
OSQP Benchmark
使用qpOASES Solver构造二次规划问题的形式与OSQP Solver构造二次规划问题的形式不相同,关于qpOASES Solver是如何构造二次规划问题的可以参考:
qpOASES QP Formulation

关于Apollo使用的基于道路进行线性化的车辆动力学公式以及离散化方式请参考:
Linearized Lateral Error Dynamics
现在假设在采样时刻 kk 我们已经得到如下车辆Error Dynamics 线性系统状态方程
xk+1=A(k)xk+B(k)uk+C(k) x_{k + 1} = A(k)x_{k} +B(k)u_{k} + C(k)
同时, 我们状态量和输入量的上下限xminx_{min}, xmaxx_{max}, uminu_{min}, umaxu_{max}已经规定好, Q(k)Q(k)R(k)R(k)kk时刻的状态惩罚矩阵和输入惩罚矩阵。AkA_{k}, BkB_{k}CkC_{k}分别为kk时刻 Linearized Error Dynamics的系统矩阵, 输入矩阵和干扰矩阵。

kk时刻,将这些量带入初始化OSQP Solver:


MpcOsqp::MpcOsqp(const Eigen::MatrixXd &matrix_a,
                 const Eigen::MatrixXd &matrix_b,
                 const Eigen::MatrixXd &matrix_q,
                 const Eigen::MatrixXd &matrix_r,
                 const Eigen::MatrixXd &matrix_initial_x,
                 const Eigen::MatrixXd &matrix_u_lower,
                 const Eigen::MatrixXd &matrix_u_upper,
                 const Eigen::MatrixXd &matrix_x_lower,
                 const Eigen::MatrixXd &matrix_x_upper,
                 const Eigen::MatrixXd &matrix_x_ref, const int max_iter,
                 const int horizon, const double eps_abs)
    : matrix_a_(matrix_a),
      matrix_b_(matrix_b),
      matrix_q_(matrix_q),
      matrix_r_(matrix_r),
      matrix_initial_x_(matrix_initial_x),
      matrix_u_lower_(matrix_u_lower),
      matrix_u_upper_(matrix_u_upper),
      matrix_x_lower_(matrix_x_lower),
      matrix_x_upper_(matrix_x_upper),
      matrix_x_ref_(matrix_x_ref),
      max_iteration_(max_iter),
      horizon_(horizon),
      eps_abs_(eps_abs) {
  state_dim_ = matrix_b.rows();
  control_dim_ = matrix_b.cols();
  ADEBUG << "state_dim" << state_dim_;
  ADEBUG << "control_dim_" << control_dim_;
  num_param_ = state_dim_ * (horizon_ + 1) + control_dim_ * horizon_;
}

参数对应表格:

参数 代码变量名称
AkA_{k} matrix_a
BkB_{k} matrix_b
QkQ_{k} matrix_q
RkR_{k} matrix_r
xkx_{k} matrix_initial_x
xmaxx_{max} matrix_x_upper
xminx_{min} matrix_x_lower
umaxu_{max} matrix_u_upper
uminu_{min} matrix_u_lower

优化目标函数和约束

Apollo MPC使用如下优化目标函数,
u0=minxk,ukk=0N(xkxr)TQ(xkxr)+k=0N1ukTRukxk+1=Axk+Bukxminxkxmaxuminxkumaxx0=xˉ u_{0} ^{*} = \min_{x_{k}, u_{k}} \sum_{k=0}^{N} (x_k-x_r)^T Q (x_k-x_r) + \sum_{k=0}^{N - 1}u_k^T R u_k \\ x_{k+1} = A x_k + B u_k \\ x_{\rm min} \le x_k \le x_{\rm max} \\ u_{\rm min} \le x_k \le u_{\rm max} \\ x_{0} = \bar{x}
其中NN为prediction horizon。

OSQP 二次规划标准形式

OSQP优化问题的标准形式如下:
minx    12xTPx+qTxsubject to lAcxu \min_{x}~~~~\dfrac{1}{2}x^{T}Px + q^{T}x \\ subject~to~ l \leq A_{c}x \leq u
上述二次规划问题只包括不等式约束不包括等式约束,对于等式约束Aeqx=beqA_{eq}x = b_{eq},应该对其进行如下变换,将其化为不等式约束:
beqAeqxbeq b_{eq} \leq A_{eq}x \leq b_{eq}
对于Apollo模型预测控制问题,等式约束来自于系统的状态方程xk+1=A(k)xk+B(k)uk+C(k)x_{k + 1} = A(k)x_{k} + B(k)u_{k} + C(k)。 目前开源出的代码中, OSQP等式约束中是忽略了CkC_{k}这一项的, 只用了约束xk=Akxk+Bkukx_{k} = A_{k}x_{k} + B_{k}u_{k}
针对于OSQP求解器的该种接口形式,需要对MPC优化问题进行重新构造,从而适配接口,也就是说要求出对应于上节提到优化问题的Hessian Matrix PP, Gradient Vector qq, Equality Constraint Matrix AcA_{c}和Equality Constraint Vectors lluu
OSQP Solver中的矩阵数据采取CSC格式,参考博客如下:
CSC Format

为了防止大家混淆变量,我们重新梳理一下变量和的含义, 上一节中提到的车辆Linearized Error Dynamics中的x(k)x(k)包括横向和纵向误差变量一共六个状态变量, kk为当前采样时刻。
x(k)=[elat(k)e˙lat(k)eheading(k)estation(k)espeed(k)] x(k) = \begin{bmatrix} e_{lat}(k) \\ \dot{e}_{lat}(k) \\ e_{heading}(k) \\ e_{station}(k) \\ e_{speed}(k) \end{bmatrix}
输入uku_{k}包括方向盘转角δ\delta和纵向加速度aa
u(k)=[δ(k)a(k)] u(k) = \begin{bmatrix} \delta(k) \\ a(k) \end{bmatrix}
OSQP solver中xx为二次规划问题的决策变量,由当前时刻的状态变量,当前时刻的输入,预测的状态变量和输入构成。
x=[x(k)x(k+1)x(k+N)u(k)u(k+1)u(k+N1)] x = \begin{bmatrix} x(k) \\ x(k + 1) \\ \vdots \\x(k + N) \\ u(k) \\ u(k + 1) \\ \vdots \\ u(k + N - 1) \end{bmatrix}
其中NN为预测步长。
Hessian矩阵PP形式如下:
P=diag(Q,Q,...,Q,R,...,R) P = \text{diag}(Q, Q, ..., Q, R, ..., R)

void MpcOsqp::CalculateKernel(std::vector<c_float> *P_data,
                              std::vector<c_int> *P_indices,
                              std::vector<c_int> *P_indptr) {
  // col1:(row,val),...; col2:(row,val),....; ...
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(num_param_);
  int value_index = 0;
  // state and terminal state
  for (size_t i = 0; i <= horizon_; ++i) {
    for (size_t j = 0; j < state_dim_; ++j) {
      // (row, val)
      columns[i * state_dim_ + j].emplace_back(i * state_dim_ + j,
                                               matrix_q_(j, j));
      ++value_index;
    }
  }
  // control
  const size_t state_total_dim = state_dim_ * (horizon_ + 1);
  for (size_t i = 0; i < horizon_; ++i) {
    for (size_t j = 0; j < control_dim_; ++j) {
      // (row, val)
      columns[i * control_dim_ + j + state_total_dim].emplace_back(
          state_total_dim + i * control_dim_ + j, matrix_r_(j, j));
      ++value_index;
    }
  }
  CHECK_EQ(value_index, num_param_);

  int ind_p = 0;
  for (size_t i = 0; i < num_param_; ++i) {
    // TODO(SHU) Check this
    P_indptr->emplace_back(ind_p);
    for (const auto &row_data_pair : columns[i]) {
      P_data->emplace_back(row_data_pair.second);    // val
      P_indices->emplace_back(row_data_pair.first);  // row
      ++ind_p;
    }
  }
  P_indptr->emplace_back(ind_p);
}

Gradient Vector 构造形式入下:
q=[QxrQxrQxr00] q = \begin{bmatrix} -Q x_r \\ -Q x_r \\ \vdots \\ -Q x_r \\ 0\\ \vdots\\ 0 \end{bmatrix}

// reference is always zero
void MpcOsqp::CalculateGradient() {
  // populate the gradient vector
  gradient_ = Eigen::VectorXd::Zero(
      state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
  for (size_t i = 0; i < horizon_ + 1; i++) {
    gradient_.block(i * state_dim_, 0, state_dim_, 1) =
        -1.0 * matrix_q_ * matrix_x_ref_;
  }
  ADEBUG << "Gradient_mat";
  ADEBUG << gradient_;
}

Equality Constraint Matrix构造如下, 包括上下两个部分,上部分对应等式约束,下部分对应不等式约束, 每个部分又分为左右两部分, 左部分对应于决策变量中的状态量x(k)x(k)部分,右部分对应决策变量中输入量u(k)u(k)部分。
Ac=[I000000AI00B000AI00B0000I00BI0000000I0000000I0000000I0000000I0000000I0000000I] A_c = \left[ \begin{array}{ccccc|cccc} -I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ A & -I & 0 & \cdots & 0 & B & 0 & \cdots & 0 \\ 0 & A & -I & \cdots & 0 & 0 & B & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & -I & 0 & 0 & \cdots & B\\ \hline I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & I & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & 0 & I & \cdots & 0 & 0 & 0 & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & I & 0 & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & I & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & 0 & I & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & I \end{array} \right]

// equality constraints x(k+1) = A*x(k)
void MpcOsqp::CalculateEqualityConstraint(std::vector<c_float> *A_data,
                                          std::vector<c_int> *A_indices,
                                          std::vector<c_int> *A_indptr) {
  constexpr double kEpsilon = 1e-6;
  // block matrix
  Eigen::MatrixXd matrix_constraint = Eigen::MatrixXd::Zero(
      state_dim_ * (horizon_ + 1) + state_dim_ * (horizon_ + 1) +
          control_dim_ * horizon_,
      state_dim_ * (horizon_ + 1) + control_dim_ * horizon_);
  Eigen::MatrixXd state_identity_mat = Eigen::MatrixXd::Identity(
      state_dim_ * (horizon_ + 1), state_dim_ * (horizon_ + 1));
  ADEBUG << "state_identity_mat" << state_identity_mat;

  matrix_constraint.block(0, 0, state_dim_ * (horizon_ + 1),
                          state_dim_ * (horizon_ + 1)) =
      -1 * state_identity_mat;
  ADEBUG << "matrix_constraint";
  ADEBUG << matrix_constraint;

  Eigen::MatrixXd control_identity_mat =
      Eigen::MatrixXd::Identity(control_dim_, control_dim_);

  for (size_t i = 0; i < horizon_; i++) {
    matrix_constraint.block((i + 1) * state_dim_, i * state_dim_, state_dim_,
                            state_dim_) = matrix_a_;
  }
  ADEBUG << "matrix_constraint with A";
  ADEBUG << matrix_constraint;

  for (size_t i = 0; i < horizon_; i++) {
    matrix_constraint.block((i + 1) * state_dim_,
                            i * control_dim_ + (horizon_ + 1) * state_dim_,
                            state_dim_, control_dim_) = matrix_b_;
  }
  ADEBUG << "matrix_constraint with B";
  ADEBUG << matrix_constraint;

  Eigen::MatrixXd all_identity_mat =
      Eigen::MatrixXd::Identity(num_param_, num_param_);

  matrix_constraint.block(state_dim_ * (horizon_ + 1), 0, num_param_,
                          num_param_) = all_identity_mat;
  ADEBUG << "matrix_constraint with I";
  ADEBUG << matrix_constraint;

  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(num_param_ + 1);
  int value_index = 0;
  // state and terminal state
  for (size_t i = 0; i < num_param_; ++i) {  // col
    for (size_t j = 0; j < num_param_ + state_dim_ * (horizon_ + 1);
         ++j)  // row
      if (std::fabs(matrix_constraint(j, i)) > kEpsilon) {
        // (row, val)
        columns[i].emplace_back(j, matrix_constraint(j, i));
        ++value_index;
      }
  }
  ADEBUG << "value_index";
  ADEBUG << value_index;
  int ind_A = 0;
  for (size_t i = 0; i < num_param_; ++i) {
    A_indptr->emplace_back(ind_A);
    for (const auto &row_data_pair : columns[i]) {
      A_data->emplace_back(row_data_pair.second);    // value
      A_indices->emplace_back(row_data_pair.first);  // row
      ++ind_A;
    }
  }
  A_indptr->emplace_back(ind_A);
}

Constraint Vector形式如下:
l=[x000xminxminuminumin]u=[x000xmaxxmaxumaxumax] l = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{min}\\ \vdots\\ x_{min}\\ u_{min}\\ \vdots\\ u_{min}\\ \end{bmatrix} \quad u = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{max}\\ \vdots\\ x_{max}\\ u_{max}\\ \vdots\\ u_{max}\\ \end{bmatrix}


void MpcOsqp::CalculateConstraintVectors() {
  // evaluate the lower and the upper inequality vectors
  Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(
      state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
  Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(
      state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
  for (size_t i = 0; i < horizon_; i++) {
    lowerInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,
                          control_dim_, 1) = matrix_u_lower_;
    upperInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,
                          control_dim_, 1) = matrix_u_upper_;
  }
  ADEBUG << " matrix_u_lower_";
  for (size_t i = 0; i < horizon_ + 1; i++) {
    lowerInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_lower_;
    upperInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_upper_;
  }
  ADEBUG << " matrix_x_lower_";

  // evaluate the lower and the upper equality vectors
  Eigen::VectorXd lowerEquality =
      Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1), 1);
  Eigen::VectorXd upperEquality;
  lowerEquality.block(0, 0, state_dim_, 1) = -1 * matrix_initial_x_;
  upperEquality = lowerEquality;
  lowerEquality = lowerEquality;
  ADEBUG << " matrix_initial_x_";

  // merge inequality and equality vectors
  lowerBound_ = Eigen::MatrixXd::Zero(
      2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
  lowerBound_ << lowerEquality, lowerInequality;
  ADEBUG << " lowerBound_ ";
  upperBound_ = Eigen::MatrixXd::Zero(
      2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
  upperBound_ << upperEquality, upperInequality;
  ADEBUG << " upperBound_";
}

求解器主函数部分如下

bool MpcOsqp::Solve(std::vector<double> *control_cmd) {
  ADEBUG << "Before Calc Gradient";
  CalculateGradient();
  ADEBUG << "After Calc Gradient";
  CalculateConstraintVectors();
  ADEBUG << "MPC2Matrix";

  OSQPData *data = Data();
  ADEBUG << "OSQP data done";
  ADEBUG << "OSQP data n" << data->n;
  ADEBUG << "OSQP data m" << data->m;
  for (int i = 0; i < data->n; ++i) {
    ADEBUG << "OSQP data q" << i << ":" << (data->q)[i];
  }
  ADEBUG << "OSQP data l" << data->l;
  for (int i = 0; i < data->m; ++i) {
    ADEBUG << "OSQP data l" << i << ":" << (data->l)[i];
  }
  ADEBUG << "OSQP data u" << data->u;
  for (int i = 0; i < data->m; ++i) {
    ADEBUG << "OSQP data u" << i << ":" << (data->u)[i];
  }

  OSQPSettings *settings = Settings();
  ADEBUG << "OSQP setting done";
  OSQPWorkspace *osqp_workspace = osqp_setup(data, settings);
  ADEBUG << "OSQP workspace ready";
  osqp_solve(osqp_workspace);

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

  size_t first_control = state_dim_ * (horizon_ + 1);
  for (size_t i = 0; i < control_dim_; ++i) {
    control_cmd->at(i) = osqp_workspace->solution->x[i + first_control];
    ADEBUG << "control_cmd:" << i << ":" << control_cmd->at(i);
  }

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

针对上述二次规划问题构造形式我觉得可以做出如下改进:

  1. 如果我们改变目标函数形式,变为如下形式的话
    u0=minxk,ukk=0N(xkxr)TQ(xkxr)+k=0N1ukur)TRukur)xk+1=Axk+Bukxminxkxmaxuminxkumaxx0=xˉ u_{0} ^{*} = \min_{x_{k}, u_{k}} \sum_{k=0}^{N} (x_k-x_r)^T Q (x_k-x_r) + \sum_{k=0}^{N - 1}(u_k - u_r)^T R (u_k - u_r) \\ x_{k+1} = A x_k + B u_k \\ x_{\rm min} \le x_k \le x_{\rm max} \\ u_{\rm min} \le x_k \le u_{\rm max} \\ x_{0} = \bar{x}
    我们需要变换 Gradient Vector
    q=[QxrQxrQxrRurRur] q = \begin{bmatrix} -Q x_r \\ -Q x_r \\ \vdots \\ -Q x_r \\ -R u_r\\ \vdots\\ -R u_r \end{bmatrix}
  2. 干扰矩阵CC也应该考虑, 此时应该修改Constraint Vector:
    l=[x0CCxminxminuminumin]u=[x0CCxmaxxmaxumaxumax] l = \begin{bmatrix} -x_0 \\ -C \\ \vdots \\ -C \\ x_{min}\\ \vdots\\ x_{min}\\ u_{min}\\ \vdots\\ u_{min}\\ \end{bmatrix} \quad u = \begin{bmatrix} -x_0 \\ -C \\ \vdots \\-C \\ x_{max}\\ \vdots\\ x_{max}\\ u_{max}\\ \vdots\\ u_{max}\\ \end{bmatrix}