MPC模型预测控制原理和代码

一. 介绍模型预测控制(MPC)原理

简要解释一下最优控制最优控制的目标是在一定的约束条件下达到最优的系统表现，那么要让系统达到最优表现，一般是通过定义损失函数J，通过最小化损失函数J来达到最优控制，对于单入单出(SISO)系统来说，损失函数J上面已经定义了，多入多出(MIMO)系统的损失函数和SISO系统的区别就是单入单出系统的损失函数里面的q和r是实数，MIMO系统的损失函数J里面的Q和R是矩阵，它们用来控制误差u和输入u的权重，e和u分别代表误差和输入；

第二和第三部分是推导部分，自己推一推吧，没法解释很清楚；

第四部分是状态空间方程的矩阵的维度的计算；

上面就是MPC模型预测控制的原理，主要还是要自己推导一下，下面给出模型预测控制的Python代码以及matlab代码；

二. Python代码

import numpy as np
import scipy.linalg
from cvxopt import solvers, matrix
import matplotlib.pyplot as plt

A = np.array([[1, 1], [-1, 2]])
n = A.shape[0]

B = np.array([[1, 1],[1, 2]])
p = B.shape[1]

Q = np.array([[1, 0],[0, 1]])
F = np.array([[1, 0],[0, 1]])
R = np.array([[1, 0],[0, 0.1]])

k_steps = 100

X_k = np.zeros((n, k_steps))

X_k[:,0] = [10, -10]

U_k = np.zeros((p, k_steps))

N = 5

def cal_matrices(A,B,Q,R,F,N):

    n = A.shape[0]
    p = B.shape[1]

    M = np.vstack((np.eye((n)), np.zeros((N*n,n))))
    C = np.zeros(((N+1)*n,N*p))
    tmp = np.eye(n)

    for i in range(N):
        rows = i * n + n
        C[rows:rows+n,:] = np.hstack((np.dot(tmp, B), C[rows-n:rows, 0:(N-1)*p]))
        tmp = np.dot(A, tmp)
        M[rows:rows+n,:] = tmp

    Q_bar_be = np.kron(np.eye(N), Q)
    Q_bar = scipy.linalg.block_diag(Q_bar_be, F)
    R_bar = np.kron(np.eye(N), R)

    G = np.matmul(np.matmul(M.transpose(),Q_bar),M)
    E = np.matmul(np.matmul(C.transpose(),Q_bar),M)
    H = np.matmul(np.matmul(C.transpose(),Q_bar),C) + R_bar

    return H, E

def Prediction(M,T):

    sol = solvers.qp(M,T)
    U_thk = np.array(sol["x"])
    u_k = U_thk[0:2, :]

    return u_k

M, C = cal_matrices(A,B,Q,R,F,N)
M = matrix(M)

for k in range(1,k_steps):
    x_kshort = X_k[:, k - 1].reshape(2, 1)
    u_kshort = U_k[:, k - 1].reshape(2, 1)
    T = np.dot(C,x_kshort)
    T = matrix(T)
    for i in range(2):
        U_k[i:,k-1] = Prediction(M,T)[i,0]

    X_knew = np.dot(A,x_kshort) + np.dot(B,u_kshort)

    for j in range(2):
        X_k[j:,k] = X_knew[j,0]

print(X_k)

三 .Matlab代码

%% 加载 optim package,若使用matlab，则注释掉此行

pkg load optim;

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

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

%% 定义状态矩阵 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

这就是模型预测控制MPC的原理以及代码实现，欢迎大家观看学习。