MPC入门与Matlab实现
本文为B站视频《你还在用PID？MPC模型预测控制，从公式到代码！》的学习笔记，强烈推荐去看这位大佬的视频，链接放在了最后，别忘了给大佬一键三连哈
MPC入门与Matlab实现
- 前言
- 1. 模型
- 2. 预测
- 3. 滚动优化
-
- 参考轨迹
- 优化目标
- 4. 误差补偿
- matlab实现
前言
模型预测控制(MPC)是一类特殊的控制。它的当前控制动作是在每一个采样瞬间通过求解一个有限时域开环最优控制问题而获得。过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用。这是它与那些使用预先计算控制律的算法的最大不同。本质上模型预测控制求解一个开环最优控制问题。它的思想与具体的模型无关，但是实现则与模型有关。 (百度百科)模型预测控制的结构图如下（来源论文[1]）：
因此，模型预测控制一共包括了以下四个部分：模型，预测，滚动优化以及误差补偿。
模型顾名思义指的是被控系统的模型，预测指的是根据系统输出以及当前状态预测之后的状态，优化指使用常见的优化方法找到满足约束条件的最优值，之所称为滚动优化，是因为每run一步，需要将下一时刻的预测值作为当前时刻的预测值，依次类推，就好比“滚动一样”。误差补偿是因为系统是存在静态误差的，即当进入下一步时会发现，预测值并不等于下一次的实际值，这是就需要对这个误差进行补偿。下面详细介绍各个部分的含义。
首先需要定义两个变量：
P:预测步长，即每次要预测多少步， y ( k + 1 ) , y ( k + 2 ) , . . . , y ( k + P − 1 ) y(k+1),y(k+2),...,y(k+P-1) y(k+1),y(k+2),...,y(k+P−1)
M:控制步长，即之后M步的控制量， Δ u ( k ) , Δ u ( k + 1 ) , . . . , Δ u ( k + M − 1 ) \Delta{u}(k),\Delta{u}(k+1),...,\Delta{u}(k+M-1) Δu(k),Δu(k+1),...,Δu(k+M−1)
1. 模型
参考视频的内容，这里也以线性系统为例，即一个单位阶跃响应：
根据叠加原理我们可以得到k时刻的系统可以表示为（论文[1]）：
y ( k ) = ∑ i = 1 P − 1 a i Δ u ( k − i ) + a p Δ u ( k − p ) ( j = 1 , 2 , ⋯ , n ) \begin{align}y(k)=\sum_{i=1}^{P-1} a_{i} \Delta u(k-i)+a_{p} \Delta u(k-p)(j=1,2, \cdots, n)\end{align} y(k)=i=1∑P−1aiΔu(k−i)+apΔu(k−p)(j=1,2,⋯,n)
用matlab程序表示如下：
%1.1获取阶跃响应模型
[stepresponse,t]=step(sys1,ts:ts:(P)*ts);
%1.2创建动态矩阵A，矩阵大小为P*M
A(:,1)=stepresponse(1:P);
for i=1:P
for j=2:M
if i>=j
A(i,j)=A(i-1,j-1);
end
end
end
2. 预测
之后的P步预测估计可以写成（论文[1]）：
y ^ ( k + j ) = ∑ i = 1 P − 1 a i Δ u ( k + j − i ) + a p Δ u ( k + j − p ) ( j = \hat{y}(k+j)=\sum_{i=1}^{P-1} a_{i} \Delta u(k+j-i)+a_{p} \Delta u(k+j-p)(j= y^(k+j)=∑i=1P−1aiΔu(k+j−i)+apΔu(k+j−p)(j=
1 , 2 , ⋯ , n ) 1,2, \cdots, n) 1,2,⋯,n)
为了方便计算，写成矩阵的形式如下（论文[1]）：
[ y ^ ( k + 1 ) y ^ ( k + 2 ) ⋮ y ^ ( k + n ) ] = [ a 1 0 a 2 a 1 ⋮ ⋱ a n a n − 1 ⋯ a 1 ] [ Δ u ( k ) Δ u ( k + 1 ) ⋮ Δ u ( k + m − 1 ) ] + {\left[\begin{array}{c}\hat{y}(k+1) \\ \hat{y}(k+2) \\ \vdots \\ \hat{y}(k+n)\end{array}\right]=\left[\begin{array}{cccc}a_{1} & & & 0 \\ a_{2} & a_{1} & & \\ \vdots & & \ddots & \\ a_{n} & a_{n-1} & \cdots & a_{1}\end{array}\right]\left[\begin{array}{c}\Delta u(k) \\ \Delta u(k+1) \\ \vdots \\ \Delta u(k+m-1)\end{array}\right]+} ⎣ ⎡y^(k+1)y^(k+2)⋮y^(k+n)⎦ ⎤=⎣ ⎡a1a2⋮ana1an−1⋱⋯0a1⎦ ⎤⎣ ⎡Δu(k)Δu(k+1)⋮Δu(k+m−1)⎦ ⎤+
[ y 0 ( k + 1 ) y 0 ( k + 2 ) ⋮ y 0 ( k + n ) ] {\left[\begin{array}{c}y_{0}(k+1) \\ y_{0}(k+2) \\ \vdots \\ y_{0}(k+n)\end{array}\right] } ⎣ ⎡y0(k+1)y0(k+2)⋮y0(k+n)⎦ ⎤
进而可以写成：
Y ^ 0 = A ⋅ Δ u + Y 0 \begin{align}\hat{Y}_0=A\cdot\Delta{u}+Y_0 \end{align} Y^0=A⋅Δu+Y0
即：新的预测输出=控制变化量+原预测输出
matlab:
%计算增量化控制
Y0 = Y0 + A * DU;
3. 滚动优化
参考轨迹
首先需要确定参考轨迹（期望）
使用一阶低通滤波得到期望的轨迹为：
w ( k + i ) = α i y ( k ) + ( 1 − α i ) y t a r g e t w(k+i)=\alpha^iy(k)+(1-\alpha^i)y_{target} w(k+i)=αiy(k)+(1−αi)ytarget
α \alpha α越大，预测的轨迹月缓慢。
用matlab程序表示如下：
%参考轨迹
for i=1:P
W(i,1) = alpha^i * y(k) + (1 - alpha^i) * target;
end
优化目标
这里使用二次规划，matlab可以使用quadprog()函数求解，也可以通过对目标函数求导等于0得到最优值。
目标一：离目标越近越好
J 1 = ∑ i = 1 P − 1 [ y ( k + i ) − w ( k + i ) ] 2 \begin{align}J_1=\sum_{i=1}^{P-1}[y(k+i)-w(k+i)]^2\end{align} J1=i=1∑P−1[y(k+i)−w(k+i)]2
目标二：能量越小越好（耗费能量直接与控制量u相关，因此要最小化控制量u）
J 2 = ∑ j = 1 M − 1 [ Δ u ( k + j − 1 ) ] 2 \begin{align}J_2=\sum_{j=1}^{M-1}[\Delta{u}(k+j-1)]^2\end{align} J2=j=1∑M−1[Δu(k+j−1)]2
所以优化的目标函数可以写成：
J = ∑ i = 1 P − 1 [ y ( k + i ) − w ( k + i ) ] 2 ⋅ q + ∑ j = 1 M − 1 [ Δ u ( k + j − 1 ) ] 2 ⋅ r \begin{align}J=\sum_{i=1}^{P-1}[y(k+i)-w(k+i)]^2\cdot{q}+\sum_{j=1}^{M-1}[\Delta{u}(k+j-1)]^2\cdot{r} \end{align} J=i=1∑P−1[y(k+i)−w(k+i)]2⋅q+j=1∑M−1[Δu(k+j−1)]2⋅r
其中q和r为对应的权重系数，如果更关注与到达目标点就让q大一点，如果关注与节能，就让r大一点。
表示成二次规划的形式为：
J = ( Y − W ) T ⋅ Q ⋅ ( Y − W ) + Δ U T ⋅ R ⋅ Δ U \begin{align} J=(Y-W)^T \cdot{Q}\cdot(Y-W)+\Delta{U}^T\cdot{R}\cdot{\Delta{U}} \end{align} J=(Y−W)T⋅Q⋅(Y−W)+ΔUT⋅R⋅ΔU
根据 Δ J Δ U = 0 \frac{\Delta{J}}{\Delta{U}}=0 ΔUΔJ=0可得：
Δ U = ( A T Q A + R ) − 1 ⋅ A T ⋅ ( W − Y 0 ) \begin{align} \Delta{U}=(A^TQA+R)^{-1}\cdot{A}^T\cdot(W-Y_0) \end{align} ΔU=(ATQA+R)−1⋅AT⋅(W−Y0)
Δ U = [ Δ u ( k ) Δ u ( k + 2 ) ⋮ Δ u ( k + m − 1 ) ] \Delta{U}=\left[\begin{array}{c}\Delta u(k) \\ \Delta u(k+2) \\ \vdots \\ \Delta u(k+m-1)\end{array}\right] ΔU=⎣ ⎡Δu(k)Δu(k+2)⋮Δu(k+m−1)⎦ ⎤，选取第一行 Δ u ( k ) \Delta{u}(k) Δu(k)作为系统的输入。
matlab:
%求解最优值
DU = (A'*Q*A+R)^-1*A'*Q*(W-Y0);
u(k) = u(k-1) + DU(1,1);
4. 误差补偿
在k时刻，我们预测到了P个输出： y ( k + 1 ) , y ( k + 2 ) , . . . , y ( k + P − 1 ) y(k+1),y(k+2),...,y(k+P-1) y(k+1),y(k+2),...,y(k+P−1)
假设现在位于k+1时刻，则预测的误差可以表示为：
e ( k + 1 ) = y ( k + 1 ) − y ^ ( k + 1 ) e(k+1)=y(k+1)-\hat{y}(k+1) e(k+1)=y(k+1)−y^(k+1)
由于在ｋ＋１时刻实际的测量值仅为该时刻的值，而后面时间序列的测量值无法测得，因此需要引入加权的方法来预测未来误差，以此补偿模型预测出现的误差，可以得到校正后的预测向量:
y c o r ( k + 1 ) = y ^ p ( k + 1 ) + H e ( k + 1 ) \boldsymbol{y}_{c o r}(k+1)=\hat{\boldsymbol{y}}_{p}(k+1)+\boldsymbol{H} \boldsymbol{e}(k+1) ycor(k+1)=y^p(k+1)+He(k+1)
其中 H 为误差校正矩阵：
H = [ h ( 1 ) h ( 2 ) ⋮ h ( p ) ] H={\left[\begin{array}{c}h(1) \\ h(2) \\ \vdots \\ h(p)\end{array}\right] } H=⎣ ⎡h(1)h(2)⋮h(p)⎦ ⎤
在ｋ＋１时刻，由于时间基点的变动，预测的未来时间点移到ｋ＋２，…，ｋ＋ｐ＋１。初始预测值也相应移位，其初始预测值:
Y ^ 0 ( k + 1 ) = S ⋅ Y ^ c o r ( k + 1 ) \hat{Y}_0(k+1)=S\cdot\hat{Y}_{cor}(k+1) Y^0(k+1)=S⋅Y^cor(k+1)
其中S为移位矩阵：
S = [ 0 1 ⋯ 0 0 ⋱ ⋱ ⋮ ⋮ ⋱ 0 1 0 ⋯ 0 1 ] \boldsymbol{S}=\left[\begin{array}{cccc}0 & 1 & \cdots & 0 \\ 0 & \ddots & \ddots & \vdots \\ \vdots & \ddots & 0 & 1 \\ 0 & \cdots & 0 & 1\end{array}\right] S=⎣ ⎡00⋮01⋱⋱⋯⋯⋱000⋮11⎦ ⎤
matlab:
%误差补偿，修正轨迹
Y_cor = Y0 + H * (y(k) - Y0(1,1));
%移位
Y0 = S * Y_cor;
matlab实现
最后附上作者的完整matlab程序（若有侵权，请联系删除）：
clc;clear;
%%
%创建系统，初始化部分参数
steps=100; % 仿真步数
ts=0.01; % 采样周期
ad = [1.00027180509492,0.00625101658363726,-0.000298104527325984,-0.000592137149727941,-0.000195218555764740;-0.00625101658365004,0.879670596217866,0.0123995907573806,0.00942892684037583,-0.00775386215642799;-0.000298104527325549,-0.0123995907573839,0.999169855139624,-0.0148759276100900,0.000129671924415677;0.000592137149728420,0.00942892684037156,0.0148759276100894,0.998913472148301,0.0286900249744246;-0.000195218555764543,0.00775386215643324,0.000129671924425366,-0.0286900249744255,0.999703452784522];
bd = [-0.023307871208778;-0.314731276263951;-0.008803109981206;0.016810972019614;0.005019051193557];
cs = [0.023307871208772,-0.314731276263952,0.008803109981209,0.016810972019614,-0.005019051193548];
ds = 0;
sys1 = ss(ad,bd,cs,ds,ts);
xs0=[0,0,0,0,0]';
%%
%MPC关键参数
P = 10;%预测步长
M = 5;%控制步长
q = 1;%Q矩阵权重
r = 10;%R矩阵权重
h = 0.5;%H矩阵权重
alpha = 0.2;%期望轨迹的平滑度（范围为0~1），越小，响应越快
target = 1;%目标值
%矩阵初始化
A=zeros(P,M);%动态矩阵
Q=eye(P,P)*q;%Q矩阵
R=eye(M,M)*r;%R矩阵
H=ones(P,1)*h;%H矩阵
S=zeros(P,M);%移位矩阵
DU=zeros(M,1);
for i=1:P-1
S(i,i+1)=1;
end
S(P,P)=1;
W=zeros(P,1);%期望轨迹
Y0=zeros(P,1);%预测输出轨迹
Y_cor=zeros(P,1);%预测输出轨迹修正值
%% 1.模型
%1.1获取阶跃响应模型
[stepresponse,t]=step(sys1,ts:ts:(P)*ts);
%1.2创建动态矩阵A，矩阵大小为P*M
A(:,1)=stepresponse(1:P);
for i=1:P
for j=2:M
if i>=j
A(i,j)=A(i-1,j-1);
end
end
end
%% 2.预测
xs1=ad*xs0;
y(1)=cs*xs0;
u(1) = 0;
for k=2:3*steps
xs1=ad*xs0+bd*u(k-1);
y(k)=cs*xs0+ds*u(k-1);
xs0=xs1;
if k < steps
target = 1;
elseif (k-steps)*Q*A+R)^-1*A'*Q*(W-Y0);
u(k) = u(k-1) + DU(1,1);
%使用quadprog()的办法
%Z1 = A'*Q*A+R;
%Z2 = A'*Q*(-W);
%[x,fval,exitflag,output,lambda] = quadprog(Z1,Z2);
%u(k)=u(k-1)+x(1,1);
end
%%
% 绘制图形
figure(1);
subplot(211);
plot(y,'linewidth',2);
hold on;plot(ref,'linewidth',2);
title('系统输出');
xlabel('t');
ylabel('y');
ylim([-1.5 1.5])
grid on;
subplot(212);
plot(u,'linewidth',2);
title('控制输入');
xlabel('t');
ylabel('u');
grid on;
参考：
[1]仝小龙,张立广,王娜丹.模型预测控制在纯滞后对象中的研究[J].电子测量技术,2018,41(23):12-17.DOI:10.19651/j.cnki.emt.1801892.
[2] 你还在用PID？MPC模型预测控制，从公式到代码！