1 序言

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

2 MPC控制

2.1 建立车辆模型

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

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

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

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

2.2 MPC 求解

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

其中：
Ht=M1=KT∗QQ∗K+RRH_{t}=M1=K^{^{T}}\ast QQ\ast K+ RRHt=M1=KT∗QQ∗K+RR
Gt=M2=KT∗QQ∗(M−Ref)=KT∗QQ∗E(t)G_{t}=M2=K^{^{T}}\ast QQ\ast (M-Ref)=K^{^{T}}\ast QQ\ast E(t)Gt=M2=KT∗QQ∗(M−Ref)=KT∗QQ∗E(t)

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

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_tMatrix 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_vMatrix 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 powerMatrix 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_mmatrix_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_rrfor (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 problemconst 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 problemconst 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: QPOASESMatrix 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;
}

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

Apollo代码学习(七)—MPC与LQR比较
Apollo代码学习-MPC与LQR比较前言研究对象状态方程工作时域目标函数求解方法前言 Apollo中用到了PID.MPC和LQR三种控制器,其中,MPC和LQR控制器在状态方程的形式 ...
搭建百度unit2.0测试代码（Java）
1.主类:App.java package haidong.haidong;import java.io.BufferedReader; import java.io.InputStreamReade ...
Apollo代码学习(五)—横纵向控制
Apollo代码学习-横纵向控制前言纵向控制横向控制前馈控制注意反馈控制总结补充 2018.11.28 前言在我的第一篇博文:Apollo代码学习(一)-控制模块概述中,对横纵向控制 ...
Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制前言模型预测控制预测模型线性化单车模型滚动优化反馈矫正总结前言非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
OpenCV与图像处理学习八——图像边缘提取（Canny检测代码）
OpenCV与图像处理学习八--图像边缘提取(Canny检测代码) 一.图像梯度 1.1 梯度 1.2 图像梯度二.梯度图与梯度算子 2.1模板卷积 2.2 梯度图 2.3 梯度算子 2.3.1 R ...
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
VTM10.0代码学习5：coding_unit()cu_pred_data()
此系列是为了记录自己学习VTM10.0的过程和锻炼表达能力,主要是从解码端进行入手.由于本人水平有限,出现的错误恳请大家指正,欢迎与大家一起交流进步. 上一篇博客(VTM10.0代码学习4)讲述了将语 ...
VTM10.0代码学习10：EncGOP_compressGOP()
此系列是为了记录自己学习VTM10.0的过程,目前正在看编码端.主要的参考文档有JVET-S2001-vH和JVET-S2002-v1.由于本人水平有限,出现的错误恳请大家指正,欢迎与大家一起交流进步 ...
html5代码好学吗,0基础能学习Html5吗?Html5好学吗？
原标题:0基础能学习Html5吗?Html5好学吗? 0基础可以学习Html5吗?这两年一直是被挂在嘴边的话题,随着人们对用户体验的要求越来越高,前端开发技术难度越来越大,所以对于IT从业者来讲,前端 ...

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