百度Apollo5.0控制模块代码学习（八）MPC控制

1 序言

对MPC控制算法，参照论文《Model Predictive Control of a Mobile Robot Using Linearization》进行整理。其中很多内容还参考引用了下列文章：
https://blog.youkuaiyun.com/u013914471/article/details/83824490
https://blog.youkuaiyun.com/weixin_41399470/article/details/91353459

2 MPC控制

2.1 建立车辆模型

百度Apollo中采用的方向盘动力学模型如下：
系统变量分别为横向偏差，横向车速，航向角偏差，航向角偏差变化率，纵向偏差，速度偏差。
状态矩阵，控制矩阵，扰动矩阵分别如下：

采用双线性变换离散法将矩阵离散化：
在离散化的同时，将车辆方向盘动力学模型中最后一项合并成了$\tilde{C}$（$\dot{\phi }$为转向车速，heading_error_rate就是转向车速），因此将车辆动力学模型离散化为：

系数的输出方程为：y(k)=Dx(k)。

本项目中预测时域为10，对于第k个阶段，可以得出如下方程：
经过一系列变换，最终可以得到如下的车辆动力学方程：
于代码中的对应关系为：
matrix_aa: ${\Psi }_t$
matrix_k: ${\Phi }_t$
matrix_cc： ${{\rm Y}}_t$

2.2 MPC 求解

通过找到预测时域内最优的控制量，来使得性能函数最优，建立如下的二次规划函数：

其中：
$H_t=M1=K^{{}^T}\ast QQ\ast K+RR$
$G_t=M2=K^{{}^T}\ast QQ\ast (M-Ref)=K^{{}^T}\ast QQ\ast E(t)$

根据以上条件，可求出一个最优的控制序列$△U$。
将序列中的第一个控制量作为实际的控制输入增量作用于系统，可得：
$u(t)=u(t-1)+△u_t^{\ast }$

3 MPC代码分析

bool SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b,
                    const Matrix &matrix_c, const Matrix &matrix_q,
                    const Matrix &matrix_r, const Matrix &matrix_lower,
                    const Matrix &matrix_upper,
                    const Matrix &matrix_initial_state,
                    const std::vector<Matrix> &reference, const double eps,
                    const int max_iter, std::vector<Matrix> *control,
                    std::vector<Matrix> *control_gain,
                    std::vector<Matrix> *addition_gain) {
  if (matrix_a.rows() != matrix_a.cols() ||
      matrix_b.rows() != matrix_a.rows() ||
      matrix_lower.rows() != matrix_upper.rows()) {
    AERROR << "One or more matrices have incompatible dimensions. Aborting.";
    return false;
  }

对时域进行设置。

  size_t horizon = static_cast<size_t>(reference.size());

更新参考序列轨迹matrix_t，Ref=matrix_t，参考序列轨迹就为传进来的参考序列。J的大小小于预测时域大小。

  // Update augment reference matrix_t
  Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1);
  for (size_t j = 0; j < horizon; ++j) {
    matrix_t.block(j * reference[0].size(), 0, reference[0].size(), 1) =
        reference[j];
  }

更新预测控制序列matrix_v，Ctr=matrix_v，相当与初始化为0。

  // Update augment control matrix_v
  Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1);
  for (size_t j = 0; j < horizon; ++j) {
    matrix_v.block(j * (*control)[0].rows(), 0, (*control)[0].rows(), 1) =
        (*control)[j];
  }

初始化序列matrix_a_power，AP=matrix_a_power=matrix_a的i次方。

  std::vector<Matrix> matrix_a_power(horizon);
  matrix_a_power[0] = matrix_a;
  for (size_t i = 1; i < matrix_a_power.size(); ++i) {
    matrix_a_power[i] = matrix_a * matrix_a_power[i - 1];
  }

更新矩阵matrix_k ，K=matrix_k，K矩阵在求解二次规划时用到。

  Matrix matrix_k =
      Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * horizon);
  matrix_k.block(0, 0, matrix_b.rows(), matrix_b.cols()) = matrix_b;
  for (size_t r = 1; r < horizon; ++r) {
    for (size_t c = 0; c < r; ++c) {
      matrix_k.block(r * matrix_b.rows(), c * matrix_b.cols(), matrix_b.rows(),
                     matrix_b.cols()) = matrix_a_power[r - c - 1] * matrix_b;
    }
    matrix_k.block(r * matrix_b.rows(), r * matrix_b.cols(), matrix_b.rows(),
                   matrix_b.cols()) = matrix_b;
  }

初始化矩阵M，Q，R等。

  // Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr,
  // vector of matrix A power
  Matrix matrix_m = Matrix::Zero(matrix_b.rows() * horizon, 1);
  Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows());
  Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols());
  Matrix matrix_ll = Matrix::Zero(horizon * matrix_lower.rows(), 1);
  Matrix matrix_uu = Matrix::Zero(horizon * matrix_upper.rows(), 1);
  Matrix matrix_cc = Matrix::Zero(horizon * matrix_c.rows(), 1);
  Matrix matrix_aa = Matrix::Zero(horizon * matrix_a.rows(), matrix_a.cols());
  matrix_aa.block(0, 0, matrix_a.rows(), matrix_a.cols()) = matrix_a;

初始化矩阵matrix_aa。

  for (size_t i = 1; i < horizon; ++i) {
    matrix_aa.block(i * matrix_a.rows(), 0, matrix_a.rows(), matrix_a.cols()) =
        matrix_a * matrix_aa.block((i - 1) * matrix_a.rows(), 0,
                                   matrix_a.rows(), matrix_a.cols());
  }

初始化矩阵M，ll，uu，qq，rr。（ll，uu为上下边界）

  // Compute matrix_m
  matrix_m.block(0, 0, matrix_a.rows(), 1) = matrix_a * matrix_initial_state;
  for (size_t i = 1; i < horizon; ++i) {
    matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) =
        matrix_a *
        matrix_m.block((i - 1) * matrix_a.rows(), 0, matrix_a.rows(), 1);
  }

  // Compute matrix_ll, matrix_uu, matrix_qq, matrix_rr
  for (size_t i = 0; i < horizon; ++i) {
    matrix_ll.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) =
        matrix_lower;
    matrix_uu.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) =
        matrix_upper;
    matrix_qq.block(i * matrix_q.rows(), i * matrix_q.rows(), matrix_q.rows(),
                    matrix_q.rows()) = matrix_q;
    matrix_rr.block(i * matrix_r.rows(), i * matrix_r.rows(), matrix_r.cols(),
                    matrix_r.cols()) = matrix_r;
  }

更新矩阵CC。

  matrix_cc.block(0, 0, matrix_c.rows(), 1) = matrix_c;
  for (size_t i = 1; i < horizon; ++i) {
    matrix_cc.block(i * matrix_c.rows(), 0, matrix_c.rows(), 1) =
        matrix_cc.block((i - 1) * matrix_c.rows(), 0, matrix_c.rows(), 1) +
        matrix_aa.block((i - 1) * matrix_a.rows(), 0, matrix_a.rows(),
                        matrix_a.cols()) *
            matrix_c;
  }

分别计算矩阵M1，M2。

  // Update matrix_m1, matrix_m2, convert MPC problem to QP problem
  const Matrix matrix_m1 = static_cast<Matrix>(
      matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr);
  const Matrix matrix_m2 = static_cast<Matrix>(
      matrix_k.transpose() * matrix_qq * (matrix_m + matrix_cc - matrix_t));

对于无约束问题，更新相应的矩阵。

  // Update matrix_m0, matrix_ctrl_gain, matrix_add_gain, obtain the analytical
  // control gain matrix, corresponding to the unconstraint QP problem
  const Matrix matrix_m0 = static_cast<Matrix>(
      -1 * matrix_m1.inverse() * matrix_k.transpose() * matrix_qq);
  const Matrix matrix_ctrl_gain = static_cast<Matrix>(matrix_m0 * matrix_aa);
  const Matrix matrix_add_gain = static_cast<Matrix>(matrix_m0 * matrix_cc);

  // Format in qp_solver
  /**
   * *           min_x  : q(x) = 0.5 * x^T * Q * x  + x^T c
   * *           with respect to:  A * x = b (equality constraint)
   * *                             C * x >= d (inequality constraint)
   * **/

对边界进行约束。

  // TODO(QiL) : change qp solver to box constraint or substitute QPOASES
  // Method 1: QPOASES
  Matrix matrix_inequality_constrain_ll =
      Matrix::Identity(matrix_ll.rows(), matrix_ll.rows());
  Matrix matrix_inequality_constrain_uu =
      Matrix::Identity(matrix_uu.rows(), matrix_uu.rows());
  Matrix matrix_inequality_constrain =
      Matrix::Zero(matrix_ll.rows() + matrix_uu.rows(), matrix_ll.rows());
  matrix_inequality_constrain << matrix_inequality_constrain_ll,
      -matrix_inequality_constrain_uu;
  Matrix matrix_inequality_boundary =
      Matrix::Zero(matrix_ll.rows() + matrix_uu.rows(), matrix_ll.cols());
  matrix_inequality_boundary << matrix_ll, -matrix_uu;
  Matrix matrix_equality_constrain =
      Matrix::Zero(matrix_ll.rows() + matrix_uu.rows(), matrix_ll.rows());
  Matrix matrix_equality_boundary =
      Matrix::Zero(matrix_ll.rows() + matrix_uu.rows(), matrix_ll.cols());

求解二次规划，结果存放在result中。

  std::unique_ptr<QpSolver> qp_solver(new ActiveSetQpSolver(
      matrix_m1, matrix_m2, matrix_inequality_constrain,
      matrix_inequality_boundary, matrix_equality_constrain,
      matrix_equality_boundary));
  auto result = qp_solver->Solve();
  if (!result) {
    AERROR << "Linear MPC solver failed";
    return false;
  }

预测控制序列矩阵matrix_v 就是二次规划求出的结果。无约束条件下的控制增益矩阵，控制增加矩阵。

  matrix_v = qp_solver->params();

  for (size_t i = 0; i < horizon; ++i) {
    (*control)[i] =
        matrix_v.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1);
  }

  for (size_t i = 0; i < horizon; ++i) {
    (*control_gain)[i] = matrix_ctrl_gain.block(i * (*control_gain)[0].rows(),
                                                0, (*control_gain)[0].rows(),
                                                (*control_gain)[0].cols());
  }

  for (size_t i = 0; i < horizon; ++i) {
    (*addition_gain)[i] = matrix_add_gain.block(
        i * (*addition_gain)[0].rows(), 0, (*addition_gain)[0].rows(), 1);
  }

  return true;
}