【DR_CAN-MPC學習筆記】3&4.詳細的MPC模組化例子和matlab代碼

上一篇部落格：【DR_CAN-MPC學習筆記】2.最優化數學模組化推導

參照二次規劃一般形式，詳細推導了MPC的數學模型，即最小化代價函數的表達式，最終推導結果為：

DR_CAN的視訊：

【MPC模型預測控制器】3_一個詳細的模組化例子

【MPC模型預測控制器】3

 【MPC模型預測控制器】4_完整案例講解 - Octave代碼

【MPC模型預測控制器】4

離散系統狀态空間一般形式：

其中

為狀态向量（n×1），

為輸入向量（p×1），

為系統狀态矩陣（n×n），

為系統輸入矩陣（n×p）。

單輸入二階系統的例子：

上式中，

，

，n = 2，p = 1

系統狀态向量和輸入向量的關系：

• 

  表示在k時刻預測 k＋1 時刻的系統狀态。
• 由于

  

  由

  

  決定，是以不需要

  

  ，是以

  

  比

  

  少一個次元。
• 因為初始值

  

  和

  

  均為 n×1 向量，是以

  

  為 (N+1)n×1 向量。同理可推出

  

  為 Np×1 向量。

• 

  為 (N+1)n×n 矩陣。

• 

  矩陣上面所有的 0 與初始狀态

  

  有關（n×1矩陣），

  

  均為 n×p 矩陣，是以

  

  為 (N+1)n×Np 矩陣。

具體可參考上一篇部落格的推導（【DR_CAN-MPC學習筆記】2.最優化數學模組化推導）：

分析過程：

回到單輸入二階系統的例子，

，

，n = 2，p = 1，假設預測區間 N=3 ，

整理一下次元：

	=			+
(N+1)n×1	=	(N+1)n×n	n×1	+	(N+1)n×Np	Np×1
8×1	=	8×2	2×1	+	8×3	3×1

對于系統輸出方程：

，參考值

，誤差

，代價函數為：

代價函數 =

誤差權重和 +

輸入權重和 +

終端誤差，其中

和

為權重系數矩陣且均為對角矩陣。

經過簡化後消去變量 

（簡化過程參考【DR_CAN-MPC學習筆記】2.最優化數學模組化推導）：

由上式可見，

隻包含了初始狀态項

和輸入項

，對

進行最優化可以得到輸入項

。

矩陣

與 

有關：

 其中

和

是原來兩個權重矩陣

和

的增廣形式：

矩陣計算較為複雜，可用程式設計求解。

例子代碼：

控制目标：設計合适的

使得

随着

的增加，趨近于0。引入誤差

：

為目标值，控制目标為令誤差接近0。

狀态矩陣（n×k）：

...

輸入矩陣（p×k）：

...

狀态方程：

例如：k=1 時，

表示第2列

回到單輸入二階系統的例子：

為系統狀态變量權重矩陣，

為系統輸入變量權重矩陣，

為終端權重矩陣。

DR_CAN給出了Octave代碼，在Matlab中也可以運作。下面的代碼是我在此基礎上修改後的單輸入例子的代碼，後面也有介紹如何修改為多輸入系統。

傳送門：（二輸入系統）【MPC模型預測控制器】4_Octave代碼

代碼一共由三個部分組成，分别為主程式：MPC_Test.m，以及兩個函數：MPC_Matrices.m和Prediction.m

MPC_Test.m

設定初始參數：

，

...

%% 清屏
clear; 
close all; 
clc;

%% 加載 optim package,若使用matlab，則注釋掉此行
pkg load optim;

%% 第一步，定義狀态空間矩陣
%% 定義狀态矩陣 A, n x n 矩陣
A = [1 0.1; 0 2];
n= size (A,1); % 計算矩陣第一個次元的長度

%% 定義輸入矩陣 B, n x p 矩陣
B = [0; 0.5];
p = size(B,2); % 計算矩陣第二個次元的長度

%% 定義Q矩陣，n x n 矩陣
Q=[1 0; 0 1];

%% 定義F矩陣，n x n 矩陣
F=[1 0; 0 1];

%% 定義R矩陣，p x p 矩陣
R=[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]; % 初始狀态不為0，控制目标為0

%% 定義輸入矩陣 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") 

%% 作者：DR_CAN https://www.bilibili.com/read/cv16891782 出處：bilibili
           

分析：

注釋掉以下行，即輸入設為0，可單獨運作：

N=5;
[E,H]=MPC_Matrices(A,B,Q,R,F,N);
U_K(:,k) = Prediction(X_K(:,k),E,H,N,p);
           

得到：

由上圖可見，

均趨于無窮，因為初始狀态不為0且輸入為0.

接下來設定合适的輸入使得狀态值趨于0.

代價函數：

與初始狀态有關，不影響代價函數，是以控制目标為最小化

.

預測了N項，但結果隻取第一項

.

矩陣

在之前已經分析過了：

矩陣的計算用 MPC_Matrices 函數解決，即代碼：

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

功能：輸入矩陣 A,B,Q,R,F 和預測區間 N ，輸出矩陣 E,H。具體過程參考下面的 MPC_Matrices.m 檔案。接下來進行預測：

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

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 

%%作者：DR_CAN https://www.bilibili.com/read/cv16891782 出處：bilibili
           

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 = U_k(1:p,1); % 取第一個結果

end 

%%作者：DR_CAN https://www.bilibili.com/read/cv16891782 出處：bilibili
           

分析：

quadprog：matlab自帶的最優化函數

運作結果：

由上圖所示，狀态值

趨于0.

以上為單輸入系統的例子。

二輸入例子：

代碼修改一下，也可以用于多輸入，比如：

修改矩陣 A,B,R,Q,F,R ，使得

變化得更快（通過對矩陣Q的設定），且降低能耗減小

初始值（系統的輸入一般是耗能的部分，通過對矩陣R的設定）

修改後的 MPC_Test.m：（其餘不變）

%% 清屏
clear; 
close all; 
clc;

%% 加載 optim package,若使用matlab，則注釋掉此行
% pkg load optim;

%% 第一步，定義狀态空間矩陣
%% 定義狀态矩陣 A, n x n 矩陣
% A = [1 0.1; 0 2];
A = [1 0.1; -1 2];
n= size (A,1); % 計算矩陣次元

%% 定義輸入矩陣 B, n x p 矩陣
% B = [0; 0.5];
B=[0.2 1; 0.5 2];
p = size(B,2);

%% 定義Q矩陣，n x n 矩陣
% Q=[1 0; 0 1];
Q=[100 0; 0 1]; % 更加看重x_1的變化

%% 定義F矩陣，n x n 矩陣
% F=[1 0; 0 1];
F=[100 0; 0 1];

%% 定義R矩陣，p x p 矩陣
% R=[0.1];
R=[1 0; 0 0.1]; % 減小能耗，減小輸入u_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") 

%% 作者：DR_CAN https://www.bilibili.com/read/cv16891782 出處：bilibili
           

由上圖所示，

迅速趨近于0. 相比于 R=[0.1 0; 0 0.1] 時，R=[1 0; 0 0.1] 時的輸入

減小，但 

趨近于0的速度變緩。由此也可以看出MPC的最優化的決策結果不是絕對的。

 PS：

騰訊雲開學季優惠：

【騰訊雲】多款雲産品1折起，買雲伺服器送免費機器，最長免費續3個月

【騰訊雲】爆款雲伺服器限時體驗20元起，更多上雲必備産品低至1元