MCP教程

1. MPC教程

% =============MPC教程-基于差分模型============= %
clear all;clc;close all;
%% 状态空间模型与模型参数
%-----------矩阵赋值-----------%
% A = [];B = [];C = [];
% A = input("请输入A矩阵:\n A = ");
% B = input("请输入B矩阵:\n B = ");
% C = input("请输入C矩阵:\n C = ");
% 
% %-----------参数赋值-----------%
% Np = 20; Nc = 10;interval = 0.01;
% Np = input("请输入预测步长Np:\n Np = ");
% Nc = input("请输入控制步长Nc:\n Nc = ");
% interval = input("请输入采样间隔interval:\n interval = ");
% times = input("请输入仿真时长times:\n times = ");
% A = [0 1 0;-1 -3 0;1 0 0]; B = [0;1;0]; C = [0 0 1];
% A = [0 1;0 0]; B = [0;1]; C = [1 0];
A = [-1 0 0; 0 -3 0;3 3 -5]; B = [1 0;0 1;1 1]; C = [1 0 0;0 1 0];
Np = 20; Nc = 5; interval = 0.05; times = 60;

temp_Nx = size(A);
Nx = temp_Nx(1);      % 状态变量个数
temp_Nu = size(B);    
Nu = temp_Nu(2);      % 控制变量个数
temp_Nm = size(C);
Nm = temp_Nm(1);      % 输出变量个数 
t = 0:interval:times;
initial_value = 2;
X(:,1) = initial_value*ones(Nx,1);
deltaX(:,1) = [zeros(Nx,1);initial_value*ones(Nm,1)];

% 状态空间模型离散化
Ak = A*interval + eye(Nx);
Bk = B*interval;

% for i = 2:1:length(t)
%    temp_x = X(:,i-1);
%    u = -4*(sum(temp_x));
%    X(:,i) = Ak*X(:,i-1)+Bk*u;
% end
% figure 
% plot(t,X)
%-----------增广矩阵赋值-----------%
Au = [Ak zeros(Nx,Nm);C*Ak eye(Nm)];
Bu = [Bk; C*Bk];
Cu = [zeros(Nm,Nx) eye(Nm)];

%% 跟踪值设定
r = 4*ones(length(t),1);
%% 预测模型参数赋值
F = cell(Np,1);
%-----------F矩阵赋值-----------%
for i = 1:1:Np         % 计算预测方程矩阵
  F{i,1} = Cu*Au^i;
end
F = cell2mat(F);

%-----------PHI矩阵赋值-----------%
PHI = cell(Np,Nc);
for i = 1:1:Np
   for j = 1:1:Nc
       if (j<=i)
           PHI{i,j} = Cu*Au^(i-j)*Bu;
       else
           PHI{i,j} = zeros(Nm,Nu);
       end
   end
end
PHI = cell2mat(PHI);

%-----------权值矩阵赋值-----------%
R = 0.002*eye(Nc*Nu);

%% 迭代计算
XX = ones(Nm*Np,1);              % 设定值向量
old_U = zeros(Nu,1);
for i = 2:1:length(t)
    U = inv(PHI'*PHI + R)*PHI'*(XX*r(i)- F*deltaX(:,i-1));
    u = U(1:Nu,1) + old_U;
    old_U = u;
    X(:,i) = Ak*X(:,i-1) + Bk*u;
    deltaX(:,i) = [X(:,i) - X(:,i-1);C*X(:,i)];
end
%% 画图
len = size(C);
figure 
plot(t,r,'g','LineWidth',2)
hold on
plot(t,X(1:len(1),:),'LineWidth',1)
grid minor