MPC模型预测控制数学推导以及MatLab实现

最优化控制

研究动机：在一定的约束条件下达到最优的系统表现。

关于最优的，举个车变道的例子，从表面上来看，轨迹1行车轨迹很平滑，很舒适，没有什么急转弯；轨迹2是快速的，但是假如前面有了障碍物，也需要一种快速的紧急避障能力，所以关于最优的，还得分析特定的情况。

SISO系统

轨迹跟踪的性能表示：${\int }_0^t\exp (2)dt$–>其结果越小，追踪的就越好。
（关于为什么采用$exp(2)$的解释：误差有正有负，$exp(2)$可以把正负误差都统计上，外加绝对值也是可行的。至于为什么是积分结果越小，追踪的就好的解释：控制器在变化，看哪个控制器的误差积分最小，哪个控制器最优）

输入：${\int }_0^t{u}^{2}dt$->积分结果越小，输入越小。

物理条件：能耗最低的，可以用很小的能耗达到所需条件。

所以可以构建一个Cost Function，如$J={\int }_0^t{qe}^2+{ru}^{2}dt$。LQR控制

最优化的过程就是对$u$进行设计，使得$min(J)$。

$q>>r$:则说明更看重误差
$r>>q$:则说明更看重输入

MIMO系统

公式太难打了，看我手写图吧。

需要注意的是，上方我写的符号冲突了，$y_1-r_1$当中的$r_1$是参考值（$r_2$同理），看上方的控制框图中的$r$。而下方的$u^{T}Ru$中的$r_1,r_2$是调节参数。

MPC基本概念

通过模型来预测系统在某一未来时间段内的表现来进行优化控制，多用于数位控制。

常用的是离散形式的表达式：$X_{k+1}=A{X}_k+B{u}_k$

MPC的步骤主要有三步：

在k时刻：

Step1:估计/测量读取当前系统状态:$y_k$
下图中的参数，分别是输入$u$以及输出$y$，$r$是其参考值。

Step2:基于$u_k,u_{k+1}......u_{k+N}$来进行最优化可以看出，下方当中的控制区间当中$u_k$等参数的选择，就是一个最优化的过程。

其Cost Function就是如下：
$J=\sum _k^{N-1}{E}_k^{T}Q{E}_K+u_k^{T}R{u}_k+E_N^{T}F{E}_N$
其中最后一项，$E_N^{T}F{E}_N$代表的是最终时刻的最末一点误差的代价函数。

要做的就是把代价函数最优化找到它的最小值。

Step3:这一步是最关键的，上方说过了基于$u_k,u_{k+1}......u_{k+N}$来进行最优化，但是在MPC当中，算了这么多，只取$u_k$，看N步走一步的思想，这是因为预测的模型很难完美描述现实当中的系统，在现实当中的系统可能会有扰动。那是不是之前的计算就白算了呢？没有白算，若不计算后面的$u_{k+1}$之类的项目，就无法保证$k$时刻能达到最优。

例如，你预测的是蓝色线段，但是实际的可能是蓝色线段上方的一个红色线段，有扰动。

滚动优化

进行刚刚的一个轮回选择完$u_k$进行优化后，就是在发现实际有偏差之后，以当前的实际为基础，把之前整个黄色框图整体向未来移动一步，再进行优化设计（之前预测的蓝色线段就不要了）。

所以MPC对计算性能有要求，因为每一步都要进行最优化的计算。

最优化建模

以下公式太多，采用手写笔记替代，必要时会进行文字说明。

二次规划

因为二次规划的求解已经非常成熟了（MatLab/Python），所以可以把模型化成这样的一般形式，这样的话就可以扔到里面进行计算了。

关于为什么是$u_{k+N-1}$而$x_{k+N}$等维度不一致的问题，因为$x_{k+N}$是由$u_{k+N-1}$推出来的，所以少了一个。

记住这个绿色的$X_k$以及蓝色的$u_k$。

就是将初始条件代入$AX+BU$的方程中，计算出下一时刻后，就把新的时刻值再代入到之前的方程，再计算出下一时刻。反复计算，直到计算出$X(k+N|k)$，预测空间结束。

化简结果：

得到了绿色的$X_k=M{x}_k+C{u}_k$，把这个公式代入到下图当中的$J$中进行化简。
记住，这里的$Q,R$都是对角矩阵。

因为$J$是一个数，$1\ast 1$的形式，所以中间两项转置也就是本身，可以进行合并。
就是说J是1乘1的矩阵，它的这几个分式子也只能是1乘1的加起来才是1乘1的，所以转置前后相等。

算了这么久，得到了新的代价函数：$J=x_k^{T}G{x}_k+2x_k^{T}E{u}_k+u_k^{T}H{u}_k$，最开始的一项只是初始状态，对最优化部分不影响，有了模型后就能进行模型预测控制部分了。
二次规划的一般形式为：$min{z}^{T}Qz+c^{T}z$
与二次规划的一般形式进行对比，发现是一模一样的

MPC建模

状态空间的离散表达形式：

一个SISO系统的例子：

各向量维度

维度的问题需要清楚的了解。

$X_k$维度：$(N+1)n\ast 1$

$U_k$维度：$Np\ast 1$

$M$的维度：$(N+1)n\ast n$

$C$的维度：$(N+1)n\ast Np$

之前最优化建模当中推得的表达式为：$X(k)=Mx(k)+CU(k)$

接下来，就用SISO的例子，具体算一下。

可以化简成以下形式。

现在只包含了输入项以及初始状态项，所以对其进行最优化，就能得到输入的一个最优化结果了。以上是非常关键的一点。

---

代码实现

%% 清屏

~~~~~~~~~~~~~~~~~~~~~~~~~~~MPC_Test.m~~~~~~~~~~~~~~~~~~~~~~~~~~

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%% 清屏

clear ; 

close all; 

clc;

%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 第一步，定义状态空间矩阵

%% 定义状态矩阵 A, n x n 矩阵

A = [1 0.1; -1 2];

n= size (A,1);

%% 定义输入矩阵 B, n x p 矩阵

B = [ 0.2 1; 0.5 2];

p = size(B,2);

%% 定义Q矩阵，n x n 矩阵

Q=[100 0;0 1];

%% 定义F矩阵，n x n 矩阵

F=[100 0;0 1];

%% 定义R矩阵，p x p 矩阵

R=[1 0 ;0 .1];

%% 定义step数量k

k_steps=100; 

%% 定义矩阵 X_K， n x k 矩 阵

X_K = zeros(n,k_steps);

%% 初始状态变量值， n x 1 向量

X_K(:,1) =[20;-20];

%% 定义输入矩阵 U_K， p x k 矩阵

U_K=zeros(p,k_steps);



%% 定义预测区间K

N=5;

%% Call MPC_Matrices 函数 求得 E,H矩阵 

[E,H]=MPC_Matrices(A,B,Q,R,F,N);



%% 计算每一步的状态变量的值

for k = 1 : k_steps 

%% 求得U_K(:,k)

U_K(:,k) = Prediction(X_K(:,k),E,H,N,p);

%% 计算第k+1步时状态变量的值

X_K(:,k+1)=(A*X_K(:,k)+B*U_K(:,k));

end



%% 绘制状态变量和输入的变化

subplot  (2, 1, 1);

hold;

for i =1 :size (X_K,1)

plot (X_K(i,:));

end

legend("x1","x2")

hold off;

subplot (2, 1, 2);

hold;

for i =1 : size (U_K,1)

plot (U_K(i,:));

end

legend("u1","u2")

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

~~~~~~~~~~~~~~~~~~~~~~~MPC_Matrices.m~~~~~~~~~~~~~~~~~~~~

function  [E , H]=MPC_Matrices(A,B,Q,R,F,N)



n=size(A,1);   % A 是 n x n 矩阵, 得到 n

p=size(B,2);   % B 是 n x p 矩阵, 得到 p

%%%%%%%%%%%%

M=[eye(n);zeros(N*n,n)]; % 初始化 M 矩阵. M 矩阵是 (N+1)n x n的， 

                         % 它上面是 n x n 个 "I", 这一步先把下半部

                         % 分写成 0 

C=zeros((N+1)*n,N*p); % 初始化 C 矩阵, 这一步令它有 (N+1)n x NP 个 0

% 定义M 和 C 

tmp=eye(n);  %定义一个n x n 的 I 矩阵

%　更新Ｍ和C

for i=1:N % 循环，i 从 1到 N

    rows =i*n+(1:n); %定义当前行数，从i x n开始，共n行 

    C(rows,:)=[tmp*B,C(rows-n, 1:end-p)]; %将c矩阵填满

    tmp= A*tmp; %每一次将tmp左乘一次A

    M(rows,:)=tmp; %将M矩阵写满

end 



% 定义Q_bar和R_bar

Q_bar = kron(eye(N),Q);

Q_bar = blkdiag(Q_bar,F);

R_bar = kron(eye(N),R); 



% 计算G, E, H

G=M'*Q_bar*M; % G: n x n

E=C'*Q_bar*M; % E: NP x n

H=C'*Q_bar*C+R_bar; % NP x NP 



end

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

~~~~~~~~~~~~~~~~~~~~~~~Prediction.m~~~~~~~~~~~~~~~~~~~~~

function u_k= Prediction(x_k,E,H,N,p)

U_k = zeros(N*p,1); % NP x 1

U_k = quadprog(H,E*x_k);

u_k = U_k(1:p,1); % 取第一个结果

end

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 

可以看出随着K的增加，是逐渐趋向于0的。要侧重于$x_1,x_2,u_1,u_2$可以修改代码当中对应的权重系数。
以上不光适用于SISO，也适用于MIMO。