自编Matlab代码实现MPC基本算法

自编Matlab代码实现MPC的基本算法

本文属于入门级的教程，适合刚入坑的萌新看。在写之前我在网上找了一下相关的文章，发现没有一个写得很直观的答案，刚好最近在研究MPC，于是决定自己写一个，针对最简单的线性约束系统，推导其优化的过程原理，揭示其简单的结构。另外关于MPC的理论，可以自行参考相关教材，这里不再赘述。

首先要介绍一下一个基于Matlab的工具MPT3，这是一个优化工具箱，同样也能调用MPC，感兴趣的同学可以自己点进去看一下。今天要用的模型也是来自这里面的一个demo，可以参考这个网址https://www.mpt3.org/UI/RegulationProblem

一、模型准备

这里用最简单的线性模型来进行解释，用差分方程进行描述如下：
$$x^{+}=Ax+Bu$$
其中，$A=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$， $B=\left[\begin{array}{c}1\\ 0.5\end{array}\right]$. 初始状态$x_0={\left[\begin{array}{cc}4& 0\end{array}\right]}^{\prime }$. 然后是状态量以及控制量的约束限制：
$$\left[\begin{array}{c}-5\\ -5\end{array}\right]\le x\le \left[\begin{array}{c}5\\ 5\end{array}\right],-1\le u\le 1$$
一般的线性MPC中用的是二次优化，分别包含了针对状态量的代价函数$x_k^{\prime }Q{x}_k$项以及针对控制量的代价函数$u_k^{\prime }R{u}_k$:
$$J_{(k,N_p)}=\mathrm{m}\mathrm{i}\mathrm{n}\sum _{t=k}^{k+N_p}(x_t^{\prime }Q{x}_t+u_t^{\prime }R{u}_t)$$
表示的是在时刻$t$，滚动时域步长为$N_p$的优化目标函数，目的是要让状态量$x$以及控制量$u$尽量的小，即使用最少的能量使系统状态量尽快稳定到零点（或者是稳定到某一个状态，这也是一般优化问题的目的）。其中代价系数矩阵的取值分别为：
$$Q=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],R=1$$
其取值可以根据设计要求在状态量与控制量的大小之间平衡。预测时域的长度选为$N_p=5$，模型及参数准备完毕开始进行下一步。

二、两个关键问题

2.1 未来状态量的变量替换

为了使用Matlab中的quadprog函数进行在线二次优化，求解Matlab中的优化问题，需要对优化目标进行转换，变换为关于控制量$u_t$的优化函数，回顾优化目标函数，里面包含了在$N_p$步后的未来系统状态量$x_t$，这里需要将其描述为关于优化目标$u_t$的函数。由于是线性系统，通过不停的迭代可以很容易得到$x_t$关于$u_t$的表达式：
$$\begin{gathered} x_{k+1|k}=A{x}_k+B{u}_k \\ {x}_{k+2|k}=A{x}_{k+1|k}+B{u}_{k+1}=A^2{x}_k+AB{u}_k+B{u}_{k+1} \\ \dots \dots \\ {x}_{k+N_p|k}=A{x}_{k+N_p-1|k}+B{u}_{k+N_p-1}=A^{N_p}{x}_k+A^{N_p-1}B{u}_k+\dots \dots +AB{u}_{k+N_p-2}+B{u}_{k+N_p-1} \end{gathered}$$
如果感兴趣可以自己尝试推导一下。然后将其描述为矩阵形式：
$$\left[\begin{array}{c}{x}_{k+1|k}\\ x_{k+2|k}\\ ...\\ x_{k+N_p|k}\end{array}\right]=\left[\begin{array}{c}A\\ A^2\\ ...\\ A^{N_p}\end{array}\right]x_k+\left[\begin{array}{cccc}B& 0& ...& 0\\ AB& B& ...& 0\\ ...& ...& ...& ...\\ A^{N_p-1}B& A^{N_p-2}B& ...& B\end{array}\right]\left[\begin{array}{c}{u}_k\\ u_{k+1}\\ ...\\ u_{k+N_p-1}\end{array}\right]$$
简化表示为：
$$X=\tilde{A}{x}_k+\tilde{B}U$$
其中$X=[x_{k+1\mid k}^{\prime },x_{k+2\mid k}^{\prime },...,x_{k+N_{\text{p}}\mid k}^{\prime }{]}^{\prime },U=[u_k^{\prime },u_{k+1}^{\prime },...,u_{k+N_{\text{p}}-1}^{\prime }{]}^{\prime }$，$\tilde{A},\tilde{B}$为对应的矩阵，这样就将将来的状态量表示为了关于优化目标$U$的表达式，可以代入优化目标中进行求解了。这里值得注意的是在Matlab中，通过适当的技巧可以得到$A_t,B_t$，具体方法可以参考后文的代码。

2.2 优化目标函数的转换

在完成了未来状态量的变量替换之后，进一步将优化目标函数替换为全部关于优化目标$u_t$以及当前状态量$x_k$的表达式。将前文中的优化目标函数重新表示为：
$$J_{(k,N_p)}=\mathrm{m}\mathrm{i}\mathrm{n}{X}^{\prime }\tilde{Q}X+U^{\prime }\tilde{R}U$$
其中$\tilde{Q},\tilde{R}$是适当增广变换之后的优化代价系数矩阵，感兴趣的同学可以自己推导。具体的变化方法在后文Matlab代码中可见。总之就是尽量将优化目标简化，使其能够直接传递到quadprog函数的参数中。参考Matlab中quadprog函数的形式，调用方法为：
$$\begin{gathered} [x,fval,\mathrm{e}\mathrm{x}\mathrm{i}\mathrm{t}\mathrm{f}\mathrm{l}\mathrm{a}\mathrm{g}]=quadprog(H,f,A,b,Aeq,beq,lb,ub,x_0,options) \\ \underset{x}{\min }\frac{1}{2}{x}^{\prime }Hx+f^{\prime }x \\ s.t.Ax\le b \\ Aeq\ast x=beq \\ lb\le x\le ub \end{gathered}$$
为了将优化目标函数表示为标准形式，将$x_t$的表达式代入其中得到：
$$\begin{gathered} J_{(k,N_p)}=(\tilde{A}{x}_k+\tilde{B}U{)}^{\prime }{Q}_t(\tilde{A}{x}_k+\tilde{B}U)+U^{\prime }{R}_{t}U \\ =U^{\prime }({\tilde{B}}^{\prime }{Q}_t\tilde{B}+R_t)U+2x_k^{\prime }{\tilde{A}}^{\prime }\tilde{Q}\tilde{B}U+x_k^{\prime }{\tilde{A}}^{\prime }\tilde{Q}\tilde{A}{x}_k \end{gathered}$$
其中，$U$为优化目标，第三项与优化目标无关，故可以将其省略掉，所以有
$$\begin{gathered} H=2({\tilde{B}}^{\prime }\tilde{Q}\tilde{B}+\tilde{R}) \\ f=(2x_k^{\prime }{\tilde{A}}^{\prime }\tilde{Q}\tilde{B}) \end{gathered}$$
再根据状态量$x_t$的约束得到A,b（注意这里的A要与系统系数矩阵区别开来）的表达式：
$$\begin{gathered} -5\le \tilde{A}{x}_k+\tilde{B}U\le 5 \\ \Rightarrow \{\begin{array}{c}\tilde{B}U\le 5-\tilde{A}{x}_k\\ -\tilde{B}U\le 5+\tilde{A}{x}_k\end{array} \\ \Rightarrow \left[\begin{array}{c}\tilde{B}\\ -\tilde{B}\end{array}\right]U\le \left[\begin{array}{c}5-\tilde{A}{x}_k\\ 5+\tilde{A}{x}_k\end{array}\right] \end{gathered}$$
故可得到：
$$A=\left[\begin{array}{c}\tilde{B}\\ -\tilde{B}\end{array}\right],b=\left[\begin{array}{c}5-\tilde{A}{x}_k\\ 5+\tilde{A}{x}_k\end{array}\right]$$
同样地，可以由输入的约束轻易得到$lb,ub$。至此准备工作完成，可以开始用Matlab实现MPC的优化过程了。

三、Matlab在线优化及代码

clear; clc; close all;
% 线性系统系数矩阵
A=[1 1; 0 1]; B=[1; 0.5];
% 初始状态量-如果不能在下一步回到约束范围内，则会造成无解
x0=[4; 0];
% 预测步长
Np=5;
% 优化目标参数，加权矩阵
Q=eye(2); R=1;
% 转化为用控制量ut表示的，关于状态量的推导方程的矩阵
At=[]; Bt=[]; temp=[];
% 转换后的加权矩阵
Qt=[]; Rt=[];

% 加权矩阵的计算过程，以及推导方程矩阵的叠加过程
for i=1:Np
        At=[At; A^i];
        Bt=[Bt zeros(size(Bt,1), size(B,2));
            A^(i-1)*B temp];
        temp=[A^(i-1)*B temp];
        
        Qt=[Qt zeros(size(Qt,1),size(Q,1));
            zeros(size(Q,1),size(Qt,1)) Q];
        Rt=[Rt zeros(size(Rt,1),size(R,1));
            zeros(size(R,1),size(Rt,1)) R];
end
% 控制量ut的上下限
lb=-1*ones(Np,1);
ub=1*ones(Np,1);
% 控制量ut的初始值
u0=zeros(Np,1);
% 转换后的优化目标函数矩阵，循环优化函数中H后的表达式为优化目标的另一项
H=2*(Bt'*Qt*Bt + Rt);
% 转换后的优化中的不等式约束左边系数矩阵，后面循环中的bi为不等式右边
Ai=[Bt; -Bt];
% 声明u来保存每一步采用的控制量
u=[];
x=x0;
xk=x0;


for k=1:50
    
    % 关于ut的不等式约束，实际上约束的是状态量，常数4就是状态量约束的上下边界
    bi=[5-At*xk; 5+At*xk];
    % 一切准备就绪，进行二次优化
    [ut, fval, exitflag]=quadprog(H,(2*xk'*At'*Qt*Bt)',Ai,bi,[],[],lb,ub,u0);
    % 显示求解结果是否正常
    fprintf('%d\n', exitflag)

    % 采用优化得到的控制量的第一个元素作为实际作用的控制量，代入到原系统中得到下一个时刻的状态量
    u(k) = ut(1);
    x(:, k+1) = A*x(:, k) + B*u(k);
    xk = x(:, k+1);
    % 对优化初始值进行修改，采用预测值的后段作为下一步的初始值
    u0 = [ut(2:Np); ut(Np)];
    
end


figure();
plot(x');
legend('x_1','x_2');

figure();
plot(u);
legend('u');

以上为全部的代码，以下为仿真得到的结果：

四、总结

本文只是针对线性系统的MPC做了仿真，并且简化了很多内容。在后期大家可以根据自身的需求，在此基础上进行修改，比如考虑非线性系统，增加输出约束，增加终端代价函数和终端约束来保证稳定性等。或者进一步考虑跟踪，改变预测步长$N_p$来观察其对优化性能的影响，改变优化目标中的代价矩阵观察控制量的变化等等。尽情探索吧，如果有时间我会再更新进一步关于MPC的内容。最后欢迎大家来一起讨论！

更新

应一些朋友的要求，在本文的基础上又更新了一篇关于定点跟踪的文章——自编Matlab代码实现MPC定点跟踪，基本原理不变，对本文的代码做了一些修改，增加了一个作图的功能，感兴趣的朋友可以看看。