【附C++源代码】模型预测控制(MPC)公式推导以及算法实现,Model Predictive control介绍
2022年的第一篇博客,首先祝大家新年快乐!
提示:本篇博客主要集中在对MPC的理解以及应用。这篇博客可以作为你对MPC控制器深入研究的一个开始,起到抛砖引玉,带你快速了解其原理的作用。
这篇博客将介绍一下模型预测控制器(MPC)的公式、推导以及C++代码的实现。
主要内容如下:
- 从一个简单的线性系统开始,对MPC控制器公式进行推导;
- 根据推导出来的结论,对一个具体的系统进行控制,使用C++对MPC进行实现;
- 通过实验找到参数对该系统的影响。
1. 模型预测控制(MPC)公式推导
(1). 系统方程建模
假设,我们有以下的一个线性离散系统:
xk+1=Axk+Buk(1)\pmb{x_{k+1}} = \pmb A \pmb{x_k} + \pmb B \pmb{u_k} \tag{1} xk+1xk+1xk+1=AAAxkxkxk+BBBukukuk(1)
其中,x\pmb xxxx是系统的状态变量;u\pmb uuuu是对系统控制的输入量;A,B\pmb A,BAAA,B均为定值矩阵,用来对状态和控制量进行转换。
上式(1)是状态的一步传递。
假设我们以状态xk,k\pmb x_{k,k}xxxk,k为基础,然后以控制量uk,k\pmb u_{k,k}uuuk,k,带入式子(1)将计算得到xk+1,k\pmb x_{k+1,k}xxxk+1,k,然后再将计算出来的xk+1,k\pmb x_{k+1,k}xxxk+1,k和uk+1,k\pmb u_{k+1,k}uuuk+1,k带入(1)式,又得到了xk+2,k\pmb x_{k+2,k}xxxk+2,k,以此类推,就可以得到在kkk时刻的时候,对未来NNN个状态的预测。
为了方便大家的理解,我把上述过程通过公式的方式写出来:
xx,x=I∗xx,x+0∗uk,kxx+1,x=A∗xx,x+B∗uk,kxx+2,x=A∗xx+1,x+B∗uk+1,k=A∗(A∗xx,x+B∗uk,k)+B∗uk+1,k⋮xx+N,x=ANxk,k+AN−1Buk,k+AN−1Buk+1,k+⋯(3)\pmb x_{x,x} = \pmb I* \pmb x_{x,x} + \pmb 0 * \pmb u_{k,k} \\ \pmb x_{x+1,x} = \pmb A* \pmb x_{x,x} + \pmb B * \pmb u_{k,k} \\ \pmb x_{x+2,x} = \pmb A* \pmb x_{x+1,x} + \pmb B * \pmb u_{k+1,k} = \pmb A* (\pmb A* \pmb x_{x,x} + \pmb B * \pmb u_{k,k}) + \pmb B * \pmb u_{k+1,k} \\ \vdots \\ \pmb x_{x+N,x} = \pmb A^{N} \pmb x_{k,k} + \pmb A^{N-1} \pmb B \pmb u_{k,k} + \pmb A^{N-1} \pmb B u_{k+1, k} + \cdots \tag{3} xxxx,x=III∗xxxx,x+000∗uuuk,kxxxx+1,x=AAA∗xxxx,x+BBB∗uuuk,kxxxx+2,x=AAA∗xxxx+1,x+BBB∗uuuk+1,k=AAA∗(AAA∗xxxx,x+BBB∗uuuk,k)+BBB∗uuuk+1,k⋮xxxx+N,x=AAANxxxk,k+AAAN−1BBBuuuk,k+AAAN−1BBBuk+1,k+⋯(3)
上述过程中的控制量uk\pmb u_kuuuk很关键,只有知道了它的值,我们才能使用上面的过程进行状态的预测。而这个uk\pmb u_kuuuk便是我们通过MPC求解得到的。
我们将上述的(3)式写成一个向量和矩阵的形式,则如下:
[xk,kxk+1,kxk+2,k⋮xk+N,k]=[IAA2⋮AN]xk+[00⋯0B0⋯0ABB⋯0AN−1NAN−2⋯B][uk,kuk+1,kuk+2,k⋮uk+N−1,k](4)\left[ \begin{matrix} \pmb x_{k,k} \\ \pmb x_{k+1,k} \\ \pmb x_{k+2,k} \\ \vdots \\ \pmb x_{k+N,k} \end{matrix} \right] = \left[ \begin{matrix} \pmb I \\ \pmb A \\ \pmb A^2 \\ \vdots \\ \pmb A^N \end{matrix} \right] \pmb x_k + \left[ \begin{matrix} \pmb 0 & \pmb 0 & \cdots & \pmb 0 \\ \pmb B & \pmb 0 & \cdots & \pmb 0 \\ \pmb A \pmb B & \pmb B & \cdots & \pmb 0 \\ \pmb A^{N-1} \pmb N & \pmb A^{N-2} & \cdots & \pmb B \\ \end{matrix} \right] \left[ \begin{matrix} \pmb u_{k,k} \\ \pmb u_{k+1,k} \\ \pmb u_{k+2,k} \\ \vdots \\ \pmb u_{k+N-1,k} \end{matrix} \right] \tag{4} ⎣⎢⎢⎢⎢⎢⎡xxxk,kxxxk+1,kxxxk+2,k⋮xxxk+N,k⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡IIIAAAAAA2⋮AAAN⎦⎥⎥⎥⎥⎥⎤xxxk+⎣⎢⎢⎡000BBBAAABBBAAAN−1NNN000000BBBAAAN−2⋯⋯⋯⋯000000000BBB⎦⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡uuuk,kuuuk+1,kuuuk+2,k⋮uuuk+N−1,k⎦⎥⎥⎥⎥⎥⎤(4)
将上式进一步简写得到:
Xk=Mxk+CUk(5)\pmb X_k = \pmb M \pmb x_k + \pmb C \pmb U_k \tag{5} XXXk=MMMxxxk+CCCUUUk(5)
在此,再次强调,式子(5)是通过kkk时刻的已知状态xk\pmb x_kxxxk,通过NNN个控制量uk+i,k\pmb u_{k+i,k}uuuk+i,k对未来的NNN个状态进行预测而得到的,务必要理解这一点。
(2).构建误差函数
想象一下,我们有了对未来的NNN个预测状态,xk,k\pmb x_{k,k}xxxk,k、xk+1,k\pmb x_{k+1,k}xxxk+1,k、xk+2,k\pmb x_{k+2,k}xxxk+2,k、xk+3,k\pmb x_{k+3,k}xxxk+3,k …
而在多数的控制任务中,我们同样也有一串连续的已知参考状态,rk\pmb r_{k}rrrk、rk+1\pmb r_{k+1}rrrk+1、rk+2\pmb r_{k+2}rrrk+2、rk+3\pmb r_{k+3}rrrk+3 …
对于控制任务,控制器要做的就是通过对系统输入控制量,从而使得状态接近参考状态。
这样说可能有一些抽象,我们以一个一维的直线跟踪为例子,通过图示进行解释:
上图xxx轴为要跟踪的参考直线,我们以窗口N=6进行状态量预测,得到在kkk时刻的6个预测量,如上图中的红点所示,各个预测量对应的参考量为x轴的蓝点。
很明显,当红点与对应的蓝点之间的距离越接近的时候,控制的效果肯定就是越好。按照这个思路,我们可以将控制过程,写成一个代价函数,然后通过优化的方式进行求解。
假设我们的参考状态始终为r=[0,0]\pmb r = [0,0]rrr=[0,0],那么每一个预测状态的误差就为e=xk,k−rk=xk,ke = \pmb x_{k,k} - \pmb r_{k} = \pmb x_{k,k}e=xxxk,k−rrrk=xxxk,k。类似的可以写出整个窗口之内的误差为:
J=xk+N,kTFxk+N,k+∑i=0N−1xk+i,kTQxk+i,k(6)\pmb J = \pmb x_{k+N, k}^T \pmb F \pmb x_{k+N, k} + \sum_{i=0}^{N-1} \pmb x_{k+i, k}^T \pmb Q \pmb x_{k+i, k} \tag{6} JJJ=xxxk+N,kTFFFxxxk+N,k+i=0∑N−1xxxk+i,kTQQQxxxk+i,k(6)
上式(6)中的F\pmb FFFF和Q\pmb QQQQ分别表示误差的权重,它们都为对角矩阵,其中的数值越大,在优化的时候就越倾向于将当前这个式子的值减少的越多。
式子(6)中只是对状态的误差进行优化,这似乎有一些不合理,因为这完全没有考虑到控制量,如果将控制量也加入到(6)式中,可以得到:
J=xk+N,kTFxk+N,k+∑i=0N−1(xk+i,kTQxk+i,k+uk+i,kTRuk+i,k)(7)\pmb J = \pmb x_{k+N, k}^T \pmb F \pmb x_{k+N, k} + \sum_{i=0}^{N-1} (\pmb x_{k+i, k}^T \pmb Q \pmb x_{k+i, k} + \pmb u_{k+i,k}^T \pmb R \pmb u_{k+i,k})\tag{7} JJJ=xxxk+N,kTFFFxxxk+N,k+i=0∑N−1(xxxk+i,kTQQQxxxk+i,k+uuuk+i,kTRRRuuuk+i,k)(7)
因此就得到了完整的误差函数J\pmb JJJJ,我们的目的是通过优化(7)式,得到使整个误差函数最小的控制量u\pmb uuuu.
以上过程牵扯到一些最优化的思想,如果你觉得难以理解,可以看一些优化相关的理论。
为了计算方便,将(7)式子写成矩阵的形式:
J=xkTGxk+UkTHUk+2UkTExk(8)J = \pmb x_k^T \pmb G\pmb x_k + \pmb U_k^T \pmb H \pmb U_k + 2\pmb U_k^T \pmb E \pmb x_k \tag{8} J=xxxkTGGGxxxk+UUUkTHHHUUUk+2UUUkTEEExxxk(8)
其中:G=MTQ‾M\pmb G = \pmb M^T \overline{\pmb{Q}}\pmb MGGG=MMMTQQQMMM,E=CTQ‾M\pmb E = \pmb C^T \overline{\pmb{Q}}\pmb MEEE=CCCTQQQMMM,H=CTQ‾CR‾\pmb H = \pmb C^T \overline{\pmb{Q}}\pmb C \overline{\pmb{R}}HHH=CCCTQQQCCCRRR
Q‾=[Q0⋯000Q⋯00Q⋯⋮⋮⋮⋱000⋯F]\overline{\pmb{Q}} = \left[ \begin{matrix} \pmb Q & \pmb 0 & \cdots & \pmb 0 & \pmb 0 \\ \pmb 0 & \pmb Q & \cdots \\ \pmb 0 & \pmb 0 & \pmb Q & \cdots \\ \vdots & \vdots & \vdots & \ddots \\ \pmb 0 & \pmb 0 & \pmb 0 & \cdots & \pmb F \\ \end{matrix} \right]QQQ=⎣⎢⎢⎢⎢⎢⎡QQQ000000⋮000000QQQ000⋮000⋯⋯QQQ⋮000000⋯⋱⋯000FFF⎦⎥⎥⎥⎥⎥⎤
R‾=[R0⋯000R⋯00R⋯⋮⋮⋮⋱000⋯R]\overline{\pmb{R}} = \left[ \begin{matrix} \pmb R & \pmb 0 & \cdots & \pmb 0 & \pmb 0 \\ \pmb 0 & \pmb R & \cdots \\ \pmb 0 & \pmb 0 & \pmb R & \cdots \\ \vdots & \vdots & \vdots & \ddots \\ \pmb 0 & \pmb 0 & \pmb 0 & \cdots & \pmb R \\ \end{matrix} \right]RRR=⎣⎢⎢⎢⎢⎢⎡RRR000000⋮000000RRR000⋮000⋯⋯RRR⋮000000⋯⋱⋯000RRR⎦⎥⎥⎥⎥⎥⎤
(3).代价函数求解
对于式子(7)它是一个典型的二次规划问题,它是一个高维度的凸函数,它有且只有一个极值点,并且还是它的最小值。对于最小值的求解可以使用现成的优化库,例如OSQP等等进行求解。
式子(7)是一个无约束的最优化问题,也可以直接求导,导数为零的点即为最优控制量。也就是(7)式的最优解为:
U∗=H−1(−E∗xk)(9)\pmb U^* = \pmb H^{-1} (-\pmb E * \pmb x_k) \tag{9} UUU∗=HHH−1(−EEE∗xxxk)(9)
(4).如何使用控制量U∗U^*U∗
通过(9)式子,得到了NNN个预测的控制量,但是实际使用的时候,只取第一个控制量,作为kkk时刻的控制量。然后计算得到K+1K+1K+1时刻的状态,再以k+1k+1k+1时刻的状态,重复上述过程进行预测控制的计算。
2.具体例子以及代码实现
为了更方便的理解上面的过程,我引入一个实际的例子。
首先对公式(1)进行实例化,得到如下具体系统:
xk+1=Axk+Buk\pmb{x_{k+1}} = \pmb A \pmb{x_k} + \pmb B \pmb{u_k} xk+1xk+1xk+1=AAAxkxkxk+BBBukukuk
[x1,k+1x2,k+1]=[10.102][x1,kx2,k]+[00.5]∗uk(10)\left[\begin{matrix} x_{1,k+1} \\ x_{2,k+1} \end{matrix}\right] =\left[\begin{matrix} 1 & 0. 1 \\ 0 & 2 \end{matrix}\right] \left[\begin{matrix} x_{1,k} \\ x_{2,k} \end{matrix}\right]+ \left[\begin{matrix} 0 \\ 0.5 \end{matrix}\right] * \pmb u_k \tag{10} [x1,k+1x2,k+1]=[100.12][x1,kx2,k]+[00.5]∗uuuk(10)
并且,假设系统的预测区间N=3N=3N=3
对应的Q\pmb QQQQ、R\pmb RRRR、F\pmb FFFF矩阵分别如下:
Q=[1001]\pmb Q = \left[\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}\right] QQQ=[1001]
F=[2002]\pmb F = \left[\begin{matrix} 2 & 0 \\ 0 & 2 \end{matrix}\right] FFF=[2002]
R=[0.1]\pmb R = \left[\begin{matrix} 0.1 \end{matrix}\right] RRR=[0.1]
初始状态为:
xinit=[55]\pmb x_{init} = \left[\begin{matrix} 5 \\ 5 \end{matrix}\right] xxxinit=[55]
将上面的几个参数值代入到第1节中的公式(4)(5)(8)中,构建所需要的大矩阵,然后使用(9)式即可完成最优控制量的求解。
* 有一点再次强调,计算出来的控制量一共N个,但是我们只取第一个作为当前时刻的控制量,然后传入控制系统中。
如果大家对于矩阵的应用不是很熟悉,可能在计算每个矩阵维度的时候,会有一些困难。为了大家的理解,我把(10)所示的系统,在预测窗口N=3N=3N=3时的各个矩阵的维度都列出来,方便代价在推导公式的时候,进行验证。
Q\pmb QQQQ:2x2
F\pmb FFFF:2x2
R\pmb RRRR:1x1
xk\pmb x_kxxxk:2x1
A\pmb AAAA:2x2
B\pmb BBBB:2x1
M\pmb MMMM:8x2
C\pmb CCCC:8x3
Q‾\overline{\pmb Q}QQQ:8x8
R‾\overline{\pmb R}RRR:3x3
G\pmb GGGG:2x2
E\pmb EEEE:3x2
H\pmb HHHH:3x3
MPC的代码片段如下:(代码中的字母和这篇博客的字母是一一对应的)
Eigen::VectorXd RunMPC(unsigned int N, Eigen::VectorXd &init_x, Eigen::MatrixXd &A, Eigen::MatrixXd &B,Eigen::MatrixXd &Q, Eigen::MatrixXd &R, Eigen::MatrixXd &F) {unsigned int num_state = init_x.rows();unsigned int num_control = B.cols();Eigen::MatrixXd C, M;C.resize((N + 1) * num_state, num_control * N);C.setZero();M.resize((N + 1) * num_state, num_state);Eigen::MatrixXd temp;temp.resize(num_state, num_state);temp.setIdentity();M.block(0, 0, num_state, num_state).setIdentity();for (unsigned int i = 1; i <= N; ++i) {Eigen::MatrixXd temp_c;temp_c.resize(num_state, (N + 1) * num_control);temp_c << temp * B, C.block(num_state * (i - 1), 0, num_state, C.cols());C.block(num_state * i, 0, num_state, C.cols())= temp_c.block(0, 0, num_state, temp_c.cols() - num_control);temp = A * temp;M.block(num_state * i, 0, num_state, num_state) = temp;}Eigen::MatrixXd Q_bar, R_bar;Q_bar.resize(num_state * (N + 1), num_state * (N + 1));Q_bar.setZero();for (unsigned int i = 0; i < N; ++i) {Q_bar.block(num_state * i, num_state * i, num_state, num_state) = Q;}Q_bar.block(num_state * N, num_state * N, num_state, num_state) = F;R_bar.resize(N * num_control, N * num_control);R_bar.setZero();for (unsigned int i = 0; i < N; ++i) {R_bar.block(i * num_control, i * num_control, num_control, num_control) = R;}Eigen::MatrixXd G = M.transpose() * Q_bar * M;Eigen::MatrixXd E = C.transpose() * Q_bar * M;Eigen::MatrixXd H = C.transpose() * Q_bar * C + R_bar;return H.inverse() * (-E * init_x);
}
>>完整代码,点击进入<<
3.实验结果
对于公式(10)所示的系统,我们来探讨Q\pmb QQQQ、R\pmb RRRR、F\pmb FFFF,以及预测区间NNN对控制器的影响。
公式(10)所示的系统,其状态量有2个,其中x2x_2x2收敛的比较快,不利于我们观察,所以下图只是绘制了第一个状态量变量的变化情况。
(1). 预测区间N的影响
(2). 权重矩阵F的影响
(3). 权重矩阵Q的影响
(4). 权重矩阵R的影响
上面的过程只是为了帮助大家去理解MPC各个权重参数的大致作用,所得出的结论并一定适合别的系统。
4. 总结
本文从一个离散的线性系统开始,对MPC进行推导,完整的探索了整个过程。
如果大家对优化理论和控制理论有一些了解的话,对于MPC的理解是比较容易的,它并不复杂。
我们在上述的过程中,构建了一个代价函数J\pmb JJJJ,它的最优值求解是一个二次规划问题,在不考虑约束的情况下,直接求导就能得到最优的控制量。
但是实际应用中,可能还会引入一些约束。例如,在自动驾驶情境中,我们对四轮汽车的系统,控制它的速度和转角,但是这些控制量很明显有实际意义,汽车的转角有一个范围,速度也有范围,因此我们在求解类似这样的控制问题时,需要对代价函数加入约束。此时就变成了带约束的二次规划问题了,就不能再通过求导得到最优值了,只能通过优化迭代的方式找到最优值。
5.参考资料
(1). MPC模型预测控制器
【附C++源代码】模型预测控制(MPC)公式推导以及算法实现,Model Predictive control介绍相关推荐
- 无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
- Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
- 模型预测控制_模型预测控制(MPC)算法之一MAC算法
引言 随着自动驾驶技术以及机器人控制技术的不断发展及逐渐火热,模型预测控制(MPC)算法作为一种先进的控制算法,其应用范围与领域得到了进一步拓展与延伸.目前提出的模型预测控制算法主要有基于非参数模型的 ...
- 了解模型预测控制2--什么是模型预测控制(MPC)
本节,我们将讨论模型预测控制器的工作原理. 在控制问题中,控制器的目标是计算被控对象的输入,使得被控对象输出遵循期望的参考信号.模型预测控制器计算此输入的策略是预测未来. ...
- Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制 前言 模型预测控制 预测模型 线性化 单车模型 滚动优化 反馈矫正 总结 前言 非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
- 基于模型预测控制(MPC)的车道保持控制实现方法
车辆保持的目的是通过检测到车辆与道路中心线的横向偏差和横摆角偏差来控制车辆的方向盘的转角,最终使车辆行驶在道路中心线上. MATLAB 2018b中有一个关于车道保持的案例,本次设计模型控制算法部分与 ...
- 基于模型预测控制(MPC)的悬架系统仿真分析
目录 前言 1.悬架系统 2.基于MPC的悬架系统仿真分析 2.1 simulink模型 2.2仿真结果 2.2.1 随机C级路面 2.2.2 正弦路面 2.3 结论 3 总结 前言 模型预测控制是无 ...
- 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
- 模型预测控制(Model predictive control,MPC)
模型预测控制( MPC ) 是一种先进的过程控制方法,用于在满足一组约束条件的同时控制过程.自 1980 年代以来,它一直在化工厂和炼油厂的加工工业中使用.近年来,它还被用于电力系统平衡模型[1]和电 ...
最新文章
- AI 医疗公司“战疫”在前线
- vue中的浏览量_vue中前进刷新、后退缓存用户浏览数据和浏览位置的实践
- git stage 暂存_什么是Git?下载和安装Git
- How to create a simple 2D graphics program?
- 服务器响应为4.7.0,454 4.7.0 临时身份验证失败 - Exchange | Microsoft Docs
- 大于3小于4的整数bleem_比三大,比四小的整数是存在的吗?
- C++通过系统版本号获取windows系统版本
- 2017-2018-1 20155320 实验三——实时系统
- 【CNN】很细的讲解什么以及为什么是卷积(Convolution)!
- 多行文字省略(涵盖标点符号,中英文等复杂字符串)
- 火星坐标系、WGS84坐标系、百度坐标系和Web墨卡托坐标系相互转换(基于Python实现)
- iOS 强制屏幕实现旋转功能
- 计算机图形点阵表示实例,计算机图形学的应用实例(计算机图形作业)精选.doc
- ICCMO微信公众账号开发系列(1)接入微信公众账号
- 伦敦国王学院计算机申请要求,伦敦大学国王学院教育中计算机应用文学硕士研究生申请要求及申请材料要求清单...
- H5左滑动不能单独滑动问题和上下滚动不了问题的解决办法
- 文献记录(part107)--Detecting Meaningful Clusters From High-Dimensional Data ...
- 听说Python成为世界性语言了? Python是怎么构建世界?字符串在哪里?(三)
- matlab倒立摆模型,线性倒立摆模型(LIP)Matlab建模.PDF
- 快递查询(快递单号智能识别/快递公司+快递单号)-完整提供 Demo 代码示例及数据专业且全面的 API 查询接口