H Infinity contrl ---LMI方法（Yalmip 求解）和Riccati 方法
參考書籍：Yu, Hai-Hua_ Duan, Guangren - LMIs in control systems _ analysis, design and applications (2013, CRC Press) - libgen.lc(一書的附錄有詳細介紹LMI的編寫方式-有很多例子)
參考網站：http://www.doc88.com/p-8718494012962.html
有問題可以上google論壇--在matlab 問答模塊可以搜到網址--可以提問相關問題，
關於LMI 本身求解參考：
線性矩陣不等式(LMI)的_MATLAB求解- http://www.doc88.com/p-7874379836838.html
Matlab中LMI(線性矩陣不等式)工具箱使用教程 https://www.doc88.com/p-304944065894.html
線性矩陣不等式的LMI工具箱求解-LMI三個函數的應用實例；
https://www.doc88.com/p-3925042294217.html
https://www.doc88.com/p-660150804881.html
https://blog.csdn.net/qq_28093585/article/details/69358180
ceshi_Hinf_lmi
clc;clear; close all; A = [2 1 -2;1 -1 -3; 4 0 -1]; B = [1 0;0 3;3 1]; E1 = [1 0.2 -0.5]'; C = [2 1 -0.5]; D = [1 1]; E2 = [0.05] P = 10e+7; X0 = [8.7047 -6.5321 4.2500; -6.5321 8.8601 -4.2821; 4.2500 -4.2821 5.0227]; W0 = [-5.2786 2.9364 -7.7991; -3.4736 -0.8733 6.0925]; W =P*W0; X = X0*P; K1 =W*inv(X) Ac =A-B*K1; Bc = B; Cc = C; Dc = D; sys_cl = ss(Ac,Bc,Cc,Dc); step(sys_cl) % figure(1) % t = 0:0.01:15; % r =[0.5*ones(size(t)); % 0.5*ones(size(t))]; % [y,t,x]=lsim(sys_cl,r,t); % plot(t,y) % % % K2 = [1.2850 0.5978 -2.0283; % -1.4101 -0.7807 1.4861] % Ac =A-B*K2; % Bc = B; % Cc = C; % Dc = D; % sys_cl = ss(Ac,Bc,Cc,Dc); % % figure(2) % t = 0:0.01:15; % r =[0.5*ones(size(t)); % 0.5*ones(size(t))]; % [y,t,x]=lsim(sys_cl,r,t); % plot(t,y)
%連續H2 optimal control clc;clear;close all; %% 書籍：LMIs in control systems _ analysis, design and applications中428頁中例題的結果-- % 連續H2 state feedback control % Step 1. Describe this LMIs in MATLAB 網址：http://www.doc88.com/p-8718494012962.html %{ % Step 1. Describe this LMIs in MATLAB % specify parameter matrices A = [-3 -2 1;1 2 1;1 -1 -1]; B1 = [1; 0; 1]; B2 = [2 0;0 2;0 1]; C = [1 0 1;0 1 0]; D = eye(2); % define the LMI system setlmis ([]) % initialing % declare the LMI variable X = lmivar(1, [3 1]); W = lmivar(2,[2,3]); Z = lmivar(1, [2 1]); rou = lmivar(1, [1 1]); % #1 LMI, left-hand side lmiterm ( [1 1 1 X],A, 1, 's'); lmiterm ( [1 1 1 W],B2, 1, 's'); lmiterm ( [1 1 1 0],B1*B1'); % #2 LMI, left-hand side lmiterm ( [2 1 1 Z],1,-1); % the (1,1) block lmiterm ( [2 1 2 W],D,1); % the (1,2) block lmiterm ( [2 1 2 X],C,1); % the (1,2) block lmiterm ( [2 2 2 X],1,-1); % the (2,2) block % #3 LMI, left-hand side lmiterm ( [3 1 1 Z],[1 0],[1 0]'); lmiterm ( [3 1 1 Z],[0 1],[0 1]'); % #3 LMI, right-hand side lmiterm ( [3 1 1 rou],1,-1); lmisys=getlmis; % finishing description % Step 2. Solve this LMI problem and use dec2mat to get the % corresponding matrix X % c = mat2dec(lmisys,0,0,ones(2)); % obtain vector c n=decnbr(lmisys);%獲取LMI中決策變量的個數 c=zeros(n,1); % for jj=1:n % [Zj]=defcx(lmisys,jj,Z); % c(jj)=trace(Zj) % end for jj=1:n [pj]=defcx(lmisys,jj,rou); c(jj)=(pj); end options=[1e-5 0 0 0 1]; %[copt,xopt] = mincx(lmisys,c); [copt,xopt] = mincx(lmisys,c,options); % solve the optimization problem X = dec2mat(lmisys,xopt,1); % extract matrix X W = dec2mat(lmisys,xopt,2); % extract matrix W Z = dec2mat(lmisys,xopt,3); % extract matrix Z K = W*inv(X); % get the feedback gain %} %注：求出的結果為：[-1.00000000000000,5.44822811876415e-18,-1.00000000000000; % 1.75680133525483e-16,-1.00000000000000,-7.35314080057690e-18] % 書中的結果是[-1 0 -1;0 -1 0] %% %{ %% 書籍：LMIs in control systems _ analysis, design and applications中259頁中例題的結果 % Step 1. Describe this LMIs in MATLAB 網址：http://www.doc88.com/p-8718494012962.html % specify parameter matrices A = [-3 -2 1;1 2 1;1 -1 -1]; B1 = [3; 0; 1];%干擾矩陣 B2 = [2 0;0 2;0 1]; %控制矩陣 C = [1 0 1;0 1 1]; D = [1 1;0 1]; % define the LMI system setlmis ([]) % initialing % declare the LMI variable X = lmivar(1, [3 1]); W = lmivar(2,[2,3]); Z = lmivar(1, [2 1]); rou = lmivar(1, [1 1]); % #1 LMI, left-hand side lmiterm ( [1 1 1 X],A, 1, 's'); lmiterm ( [1 1 1 W],B2, 1, 's'); lmiterm ( [1 1 1 0],B1*B1'); % #2 LMI, left-hand side lmiterm ( [2 1 1 Z],1,-1); % the (1,1) block lmiterm ( [2 1 2 W],D,1); % the (1,2) block lmiterm ( [2 1 2 X],C,1); % the (1,2) block lmiterm ( [2 2 2 X],1,-1); % the (2,2) block % #3 LMI, left-hand side lmiterm ( [3 1 1 Z],[1 0],[1 0]'); lmiterm ( [3 1 1 Z],[0 1],[0 1]'); % #3 LMI, right-hand side lmiterm ( [3 1 1 rou],1,-1); lmisys=getlmis; % finishing description % Step 2. Solve this LMI problem and use dec2mat to get the % corresponding matrix X % c = mat2dec(lmisys,0,0,ones(2)); % obtain vector c n=decnbr(lmisys);%獲取LMI中決策變量的個數 c=zeros(n,1); % for jj=1:n % [Zj]=defcx(lmisys,jj,Z); % c(jj)=trace(Zj) % end for jj=1:n pj=defcx(lmisys,jj,rou); c(jj)=(pj); end %options=[1e-5 0 0 0 1]; [copt,xopt] = mincx(lmisys,c); %[copt,xopt] = mincx(lmisys,c,options) % solve the optimization problem X = dec2mat(lmisys,xopt,1) % extract matrix X W = dec2mat(lmisys,xopt,2) % extract matrix W Z = dec2mat(lmisys,xopt,3) % extract matrix Z K = W*inv(X) % get the feedback gain %} %注：求出的結果為：K =[-0.911192567348860,1.75193380502826,-0.249064838067320; % -0.106341894142934,-1.90039834212258,-0.701758897247713]; % pou =3.349135e-04; %書中的結果：[-0.9112 1.7519 -0.2491; -0.1063 -1.9004 -0.7018]; %% 采用yample優化工具箱的求出結果； %示例 https://github.com/eoskowro/LMI % Arizona State University % MAE 598 LMIs in Control Systems %{ clear;close all; clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % LMI for Opimal FSF H_2 Control % Bring in Data A = [-1 1 0 1 0 1;... -1 -2 -1 0 0 1;... 1 0 -2 -1 1 1;... -1 1 -1 -2 0 0;... -1 -1 1 1 -2 -1;... 0 -1 0 0 -1 -3]; B = [0 -1 -1;... 0 0 0;... -1 1 1;... -1 0 0;... 0 0 1;... -1 1 1]; C = [0 1 0 -1 -1 -1;... 0 0 0 -1 0 0;... 1 0 0 0 -1 0]; D = zeros(3); % Determine sizes ns = size(A,1); % number of states nai = size(B,2); % number of actuator inputs nmo = size(C,1); % number of measured outputs nd = nai+nmo; % number of disturbances nro = nmo+nai; % number of regulated outputs eta = 0.0001; % 9 Matrix Representation B1 = [B zeros(ns,nmo)]; B2 = B; C1 = [C;... zeros(nai,ns)]; C2 = C; D11 = [D zeros(nai);... zeros(nmo) zeros(nmo)]; D12 = [D;... eye(nmo)]; D21 = [D eye(nmo)]; D22 = D; % Settings opt = sdpsettings('verbose',0,'solver','sedumi'); %Create/alter solver options structure. % Define Variables gam= sdpvar(1); X = sdpvar(6); W = sdpvar(6); Z = sdpvar(3,6); % Define matricies mat1 = [A B2]*[X;Z] + [X Z']*[A';B2']+ B1*B1'; mat2 = [X (C1*X+D12*Z)';C1*X+D12*Z W]; mat3 = trace(W); % Define Constraints F = []; F = [F, X>=eta*eye(6)]; F = [F, mat1<=-eta*eye(6)]; F = [F, mat2>=eta*eye(12)]; F = [F, mat3<=gam]; % Optimization Problem optimize(F,gam,opt); gam= sqrt(gam); disp('The H-2 gain is: '); disp(value(gam)); K= value(Z)*inv(value(X)); sys = ss(A,B,C,D) eig(A+B2*K) %求出的系統是穩定的的 %} %% %使用上面書中例題的參數的采用Yamiple求解-采用的公式也與上面的相同 A = [-3 -2 1;1 2 1;1 -1 -1]; B1 = [3; 0; 1];%干擾矩陣 B2 = [2 0;0 2;0 1]; %控制矩陣 C1 = [1 0 1;0 1 1]; D12 = [1 1;0 1]; eta = 0.000001; %當減小eta的值時，求出的K值幾乎和書中例題結果一樣 % Settings opt = sdpsettings('verbose',0,'solver','sedumi'); %選定那種求解器 % Define Variables gam= sdpvar(1); X = sdpvar(3); W = sdpvar(2,2); Z = sdpvar(2,3); % Define matricies mat1 = [A B2]*[X;Z] + [X Z']*[A';B2']+ B1*B1'; mat2 = [X (C1*X+D12*Z)';C1*X+D12*Z W]; mat3 = trace(W); % Define Constraints F = []; F = [F, X>=eta*eye(3)]; F = [F, mat1<=-eta*eye(3)]; F = [F, mat2>=eta*eye(5)]; F = [F, mat3<=gam]; % Optimization Problem optimize(F,gam,opt); gam= sqrt(gam); disp('The H-2 gain is: '); disp(value(gam)); K = value(Z)*inv(value(X)); X = value(X) Z = value(Z) W = value(W) %注意這里的W和Z與上面方法的W和Z剛好是相反的 gama =value(gam) sys = ss(A,B2,C1,D12) eig(A+B2*K) %求出的系統是穩定的的 %注：當eta = 0.000001;時得出的K值為：[-0.911539661339165,1.74872193348681,-0.247991715575816;-0.105965874043457,-1.89693739748588,-0.702917300903666] % 幾乎和書中例題一樣但是求出的gamma值相差較大：書中259頁給出的時3.3491*10^(-4) % 而這種方法求出的gamma值為： 0.0184 %{ %% 連續的H2 optimal control --表達公式不同采用Yamiple方法 % 參考的：LMI Properties and Applications in Systems, stabillity and Control % theory(好)70頁公式（4.4，4.6，4.7） clear; clc; A = [-3 -2 1; 1 2 1; 1 -1 -1]; B1 = [1 0.2 0]'; B2 = [0.4 1 0.6]'; C1 = [1.2 0.5 0.3]; D12 = 1; P = sdpvar(3); Z = sdpvar(1); F = sdpvar(1,3); mu = sdpvar(1); mat1 = [A*P+P*A'+B2*F+F'*B2' P*C1'+F'*D12'; (P*C1'+F'*D12')' -eye(1)]; mat2 = [Z B1'; B1 P]; Fd = [mat1<=0; mat2>=0; P>=0; Z>=0 ;trace(Z)<=mu]; optimize(Fd, mu); Kd = value(F)*inv(value(P)) H2_norm = sqrt(value(mu)) %} %{ %% 連續的H2全狀態反饋 --LMI原始方法-只是表達式與上面的形式不同【驗證】 % 參考的：LMI Properties and Applications in Systems, stabillity and Control % theory(好)70頁公式（4.4，4.6，4.7） A = [-3 -2 1;1 2 1;1 -1 -1]; B1 = [1; 0; 1]; B2 = [2 0;0 2;0 1]; C1 = [1 0 1;0 1 0]; D12 = eye(2);D11=[0;0]; %構建矩陣變量 [M,N] =size(A); setlmis([]); P=lmivar(1,[N,1]); %N階對稱滿塊 Z=lmivar(1,[1,1]); F=lmivar(2,[2,N]); rou=lmivar(1,[1,1]); %描述LMI %第一個LMI lmiterm([1,1,1,P],A,1,'s'); %AP+(AP)' lmiterm([1,1,1,F],B2,1,'s'); %B2*F+(B2*F)' lmiterm([1,1,2,P],1,C1'); %P*C1 lmiterm([1,1,2,-F],1,D12'); %F'*D12' lmiterm([1,2,2,0],-eye(2)); %I %第二個LMI lmiterm([-2,1,1,Z],1,1);%Z lmiterm([-2,1,2,0],B1');%B1' lmiterm([-2,2,2,P],1,1);%P %第三個LMI --跡的表達形式 % lmiterm ([3 1 1 Z],[1 0],[1 0]'); % lmiterm ([3 1 1 Z],[0 1],[0 1]'); lmiterm ([3 1 1 Z],1,1); lmiterm ([-3 1 1 rou],1,1); lmisys=getlmis ;%獲取lmi信息 n=decnbr(lmisys); %得出決策變量的個數 c=zeros(n,1); for j=1:n bj=defcx(lmisys, j, rou); c(j)=bj; end % options=[1e-5, 0, 0, 0, 0]; %計算精度 [copt,xopt]=mincx(lmisys,c ); % P=dec2mat(lmisys, xopt, P); % Z=dec2mat(lmisys, xopt, Z); % W=dec2mat(lmisys, xopt, W); X = dec2mat(lmisys,xopt,1); F = dec2mat(lmisys,xopt,2); Z = dec2mat(lmisys,xopt,3); K = F*inv(P) gamma=sqrt(copt) %} %{ %% 連續的H2 optimal control clear; clc; A = [-3 -2 1;1 2 1;1 -1 -1]; B1 = [1; 0; 1]; B2 = [2 0;0 2;0 1]; C1 = [1 0 1;0 1 0]; D12 = eye(2);D11=[0;0]; P = sdpvar(3); Z = sdpvar(1); F = sdpvar(2,3); musqr = sdpvar(1); mat1 = [A*P+P*A'+B2*F+F'*B2' P*C1'+F'*D12'; (P*C1'+F'*D12')' -eye(2)]; mat2 = [Z B1'; B1 P]; Fd = [mat1 <= 0; mat2>=0; P>=0; Z>=0 trace(Z)<=musqr]; optimize(Fd, musqr); Kd = value(F)*inv(value(P)) H2_norm = sqrt(value(musqr)) %} %{ %% 離散的H2 ---采用傳統的LMI方法 % 參考的：LMI Properties and Applications in Systems, stabillity and Control % theory(好)70頁公式（下半部的公式） clear; clc; A = [0.1 0.2 0.3; 0.4 0.5 0.6; 0.7 0.8 0.9]; B1 = ones(3,1); B2 = ones(3,1); C1 = ones(1,3); D12 = 1; %構建矩陣變量 [M,N] =size(A); setlmis([]); P=lmivar(1,[N,1]); %N階對稱滿塊 Z=lmivar(1,[1,1]); F=lmivar(2,[1,N]); rou=lmivar(1,[1,1]); %描述LMI %第一個LMI lmiterm([-1,1,1,P],1,1); %P lmiterm([-1,1,2,P],A,1); %AP lmiterm([-1,1,2,F],B2,1); %B2*F lmiterm([-1,1,3,0],B1); %B1 lmiterm([-1,2,2,P],1,1); %P lmiterm([-1,2,3,0],0); %0 lmiterm([-1,3,3,0],eye(1)); %I %第二個LMI lmiterm([-2,1,1,Z],1,1);%Z lmiterm([-2,1,2,P],C1,1);%C1*P lmiterm([-2,1,2,F],D12,1);%d112*F lmiterm([-2,2,2,P],1,1); %P %第三個LMI --跡的表達形式 % lmiterm ([3 1 1 Z],[1 0],[1 0]'); % lmiterm ([3 1 1 Z],[0 1],[0 1]'); lmiterm ([3 1 1 Z],1,1); lmiterm ([-3 1 1 rou],1,1); % 兩個約束條件 lmiterm([-4,1,1,P],1,1); %P正定P>0 lmiterm([-5,1,1,Z],1,1); %Z正定Z>0 lmisys=getlmis ;%獲取lmi信息 n=decnbr(lmisys); %得出決策變量的個數 c=zeros(n,1); for j=1:n bj=defcx(lmisys, j, rou); c(j)=bj; end % options=[1e-5, 0, 0, 0, 0]; %計算精度 [copt,xopt]=mincx(lmisys,c ); %注意獲取參數時要按照定義變量的順序進行 % P=dec2mat(lmisys, xopt, P); % Z=dec2mat(lmisys, xopt, Z); % W=dec2mat(lmisys, xopt, W); P = dec2mat(lmisys,xopt,1); F = dec2mat(lmisys,xopt,3); K = F*inv(P) gamma=sqrt(copt) %}
H Infinity control LMI 方法
參考：H∞控制器的設計 - 百度文庫 (baidu.com)
程序：
% 測試--------使用RICCATI進行H infinity的狀態反饋設計 %系統參數參見網址：https://wenku.baidu.com/view/c3f834b2ac51f01dc281e53a580216fc710a5358.html?rec_flag=default clc;clear;close all; A =[0 1 0 0; 0 0 -1 0; 0 0 0 1; 0 0 11 0]; B1 =[0 1;0 1;1 -3; 2 1]; B2 =[0 2 3 1; 1 8 2 1; 0 2 3 4; -1 -1 4 2]; %使用網站原來的參數C1的值，求不出riccari的增益K值，為此對其作了些改動 C1=[2 0 0 0; 0 0.02 0 0; 0 0 0.04 0; 0 0 0 0.01; 0 0 0 0]; D11=[1 2;1 4]; D12 =[0 0 0 0 1]'; C2=eye(4); D21=0;D22=0; %% 判斷系統是否滿足基於Riccati方程的假設條件 sys =ss(A,B2,C2,D22); P0 = pole(sys) Cc =rank(ctrb(A,B2)) Ob =rank(obsv(A,C2)) S =D12'*[C1 D12] %% 利用Riccati方程求解 H infinity 在增益K, A'*X+X*A+X*(B1*B1'-B2*B2')*X+C'*C=0 % A'*X+X*A-X*[B1 B2]*[-I 0;0 I]*[B1';B2']*X+C1'*C1=0 B0 =[B1 B2]; V=[-1,-1,-1,1,1,1]; R=diag(V); C =C1; X =care(A,B0,C'*C,R) K =-B2'*X root =eig(A+K) %判斷閉環系統是否穩定；看其特征值是否都有負實部
仿真結果：
ceshi_Hinf_lmi-- discrete time
clc;clear; close all; A = [2 1 -2;1 -1 -3; 4 0 -1]; B = [1 0;0 3;3 1]; E1 = [1 0.2 -0.5]'; C = [2 1 -0.5]; D = [1 1]; E2 = [0.05] P = 10e+7; X0 = [8.7047 -6.5321 4.2500; -6.5321 8.8601 -4.2821; 4.2500 -4.2821 5.0227]; W0 = [-5.2786 2.9364 -7.7991; -3.4736 -0.8733 6.0925]; W =P*W0; X = X0*P; K1 =W*inv(X) Ac =A-B*K1; Bc = B; Cc = C; Dc = D; sys_cl = ss(Ac,Bc,Cc,Dc); step(sys_cl) % figure(1) % t = 0:0.01:15; % r =[0.5*ones(size(t)); % 0.5*ones(size(t))]; % [y,t,x]=lsim(sys_cl,r,t); % plot(t,y) % % % K2 = [1.2850 0.5978 -2.0283; % -1.4101 -0.7807 1.4861] % Ac =A-B*K2; % Bc = B; % Cc = C; % Dc = D; % sys_cl = ss(Ac,Bc,Cc,Dc); % % figure(2) % t = 0:0.01:15; % r =[0.5*ones(size(t)); % 0.5*ones(size(t))]; % [y,t,x]=lsim(sys_cl,r,t); % plot(t,y)
daolibai_LMI_method----discrete time Hinf state feedback control
% daolibai_LMI_method--Hinf control clear;clc; A = [0 1 0 0;0 -0.0618 -0.7167 0;0 0 0 1;0 0.2684 31.6926 0]; B1 = [0;-2.6838;0;118.6765]; B2 = [0;0.8906;0;-2.6838]; C1 = [0.00001 0 0 0;0 -0.0001 0 0;0 0 -0.01 0;0 0 0 -0.01]; D11 = [0;0;0]; D12 = [0;0;0]; n=length(A); %構建矩陣變量 setlmis([]); [X,n,sX] = lmivar(1,[4,1]); %4階的對稱滿塊 [W,n,sW] = lmivar(2,[1,4]); %4*1的矩陣 %描述LMI lmiterm([1 1 1 W],A); lmiterm([1 1 1 X],B2,'s'); lmiterm([3 1 1 0],B1); lmiterm([1 2 1 X],B2,1); lmiterm([1 2 2 A],D21,2); lmiterm([1 3 3 0],-1); lmiterm([2 2 1 W],-1); lmiterm([-2 2 1 X],1,1); lmisys = getlmis; [tmin,xfeas]=feasp(lmisys) if(tmin<0) disp('Feasible'); else sys=[]; return end X = dec2mat(lmisys,xfeas,X) W = dec2mat(lmisys,xfeas,W) K = W*inv(X)
daolibai_LMI_and_riccati--discrete time Hinf state feedback control
clear,clc M=1; %小車質量 m=0.1; %倒立擺質量 L=0.5; %擺的長度 g=9.8 ;%重力加速度 J=m*L^2/3; %轉動慣量 k=J*(M+m)+m*M*L^2 ; k1=(M+m)*m*g*L/k ; k2=-m^2*g*L^2/k ; k3=-m*L/k ; k4=( J+m*L^2 ) /k ; %中間變量 %------PlantA------------ A=[0,0,1,0 ;0,0,0,1 ;k1,0,0,0 ;k2,0,0,0]; % B1=[0; 0; 0.09; 0.11 ]; B1=[0;0;0.5;0.5] ; B2=[0; 0; k3; k4]; C1=[1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1; ] ; % C=[1,0,0,0;0,0,0,0] ; N=length(A); D11=0; D12=[0; 0; 0;0; 1]; %狀態方程各個矩陣 %D12=0; % -----直接求出K--------LMI求解 gamma =4; %根據所得的次優gamma重新求解K % [X,W]=LMIC(A,B1,B2,C1,D11,D12,gamma,N) % K_sub=W*inv(X) [X,W,gamma]=LMID(A,B1,B2,C1,D11,D12,N) K_opt=W*inv(X) % A=[0,1,0,0;10.78,0,0,0;0,0,0,1;-0.98,0,0,0]; % B1=[0;0;0.09;0.11]; % B2=[0;-1;0;0.5]; % N=length(A); % D12=[0]; % C=[1,0,0,0]; % D11=[]; % gamma=4; %% -----LMI求解------------ % gamma=2; % K=LMIC(A,B1,B2,C,D11,D12,gamma,N) %LMI求解 %------------------- % K=[51.1721, 3.6803, 9.2785, 7.9564]; %% ------PlantB------------ A=[0 1 0 0; 0 -0.0883 0.6293 0; 0 0 0 1; 0 -0.2357 27.8285 0] ; B1=[0 2.3566 0 104.2027]'; B2=[0 0.8832 0 2.3566]'; C= [0.064 0 0 0; 0 1e-3 0 0; 0 0 0.11 0; 0 0 0 0.01; 0 0 0 0]; D12=[0;0;0;0;1]; %% -----Raccati方程求解---可行-------- % B=[B1,B2]; % C=C1; % R=[-1, 0; 0, 1]; % X_ric=care(A, B, C'*C, R) ; % K_ric=-B2'*X_ric %控制器 % K=[ 112.1078 99.9032 -364.5474 -72.8227]; % X=[0.1253,0.0042,-0.6266,-0.0266; % 0.0042,0.565,0.0156,-0.3558; % -0.6266,0.0156,3.6427,-0.1747; % -0.0266,-0.3558,-0.1747,0.5286]; % W=[0.4583,0.0057,-0.1069,-0.8101]; % K=W*inv(X) % function [X,W]=LMIC(A,B1,B2,C1,D11,D12,gamma,N) %次優函數調用 % %{ % 程序功能： % 1、利用LMI求解倒立擺 % 2、狀態反饋控制，求控制器 % 3、參考文獻：[1]呂 申,武俊峰.基於 LMI 優化的魯棒控制器設計[J].工業儀表與自動化裝置,2017,3:123-128. % %} % % %構建矩陣變量 % setlmis([]); % [X,n,Sx]=lmivar(1,[N,1]); %4階的對稱滿塊 % [W,n,Sw]=lmivar(2,[1,N]); %4*1的矩陣 % % lmiterm([1,1,1,X],A,1,'s'); %AX+(AX)' % lmiterm([1,1,1,W],B2,1,'s'); %B2*W+(B2*W)' % lmiterm([1,1,2,0], B1); %C1*X % lmiterm([1,1,3,X],1,C1' ); %D12*W % lmiterm([1,1,4,-W],1,D12' ); %W'*D12' % lmiterm([1,2,2,0],-gamma^2);%-gamma % lmiterm([1,2,3,0],D11'); %D12' % lmiterm([1,3,3,0], -1 ); %-I % % %------------------------------------------ % lmiterm([-2,1,1,X],1,1); %X正定 % %------------------------------------------ % lmisys=getlmis ;%獲取lmi信息 % %求解可行解X,W % [tmin, xfeas]=feasp(lmisys); % if(tmin<0) % disp('Feasible'); % else % sys=[]; % return % end % X=dec2mat(lmisys, xfeas, X); % W=dec2mat(lmisys, xfeas, W); % end
example LMI (yalmip)--discrete time Hinf state feedback control
clc; clear; clc; %% System parameters A = [0.9641 0.0025 -0.0004 -0.0452; 0.0044 0.9036 -0.0188 -0.3841; 0.0096 0.465 0.9435 0.2350; 0.0005 0.0024 0.0969 1.0120]; Bd1= [0.0440 0.0164; 0.4898 -0.7249; -0.5227 0.4172; -0.0266 0.0214]; Bd2 =[0.0440 0.0164; 0.4898 -0.7249; -0.5227 0.4172; -0.0266 0.0214]; Cd1 = ones(1,4); Cd2 = eye(4); Dd12 = [0.002 0.002]; Dd11 = [1 1]; Dd22 = zeros(4,2); Ts =0.1; %% Calculate the H infinity state feedback gain P = sdpvar(4); Z = sdpvar(1); Fd = sdpvar(2,4); gam = sdpvar(1); mat1 = [P A*P+Bd2*Fd Bd1 zeros(4,1); (A*P+Bd2*Fd)' P zeros(4,2) P*Cd1'-Fd'*Dd12'; Bd1' zeros(2,4) gam*eye(2) Dd11'; zeros(1,4) (P*Cd1'-Fd'*Dd12')' Dd11 gam*eye(1)]; F = [mat1 >= 0; P>=0]; optimize(F, gam); Kd = value(Fd)*inv(value(P)) Hinf_norm = (value(gam)) L1 =eig(A-Bd2*Kd) %% simulation % sysd = ss(A,Bd2,Cd2,Dd22); % feedin = [1 2]; % feedout = [1 2 3 4]; % sys_cl = feedback(sysd,Kd,feedin,feedout) % % Ac = A-Bd2*Kd; % Bc = Bd2; % Cc = Cd2; % Dc = Dd22; % sys_cl = ss(Ac,Bc,Cc,Dc) % T = 0:0.1:10; % step(sys_cl,T) % clear; % clc; A = [0.1 0.2 0.3; 0.4 0.5 0.6; 0.7 0.8 0.9]; Bd1 = ones(3,1); Bd2 = ones(3,1); Cd1 = ones(1,3); Dd12 = 1; Dd11 = 0; P = sdpvar(3); Z = sdpvar(1); Fd = sdpvar(1,3); gam = sdpvar(1); mat1 = [P A*P+Bd2*Fd Bd1 zeros(3,1); (A*P+Bd2*Fd)' P zeros(3,1) P*Cd1'-Fd'*Dd12'; Bd1' zeros(1,3) gam*eye(1) Dd11'; zeros(1,3) (P*Cd1'-Fd'*Dd12')' Dd11 gam*eye(1)]; F = [mat1 >= 0; P>=0]; optimize(F, gam); Kd = value(Fd)*inv(value(P)) Hinf_norm = (value(gam)) L2 =eig(A-Bd2*Kd)
H2 狀態反饋控制--模型參數例子來於書本：robust control design _269頁--三個例子分開運行
clc;clear;close all; %文中有三個例子，需要分開運行才可以 %% 模型參數例子來於書本：robust control design _269頁 %{ A=[-5 2 -4 ;0 -3 0; 0 7 -1]; B1=[7 -3 1]'; B2=[6 8 -5]'; C1=[-2 9 4]; C2=[6 3 -1]; D11=[0]; D12=[1]; D21=[2]; D22=[0]; G=ss(A,B2,C2,D22); P =ltisys(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]); P1 =pck(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]); [Aa,Bb,Cc,Dd]=branch(P); sys=ss(Aa,Bb,Cc,Dd); po=pole(sys) nmeas = 1; ncont = 1; [K,CL,gamma] = h2syn(sys,nmeas,ncont); [Ak,Bk,Ck,Dk]=branch(K); GK=ss(Ak,Bk,Ck,Dk); K_tf=zpk(); [Knum,Kden]=ss2tf(Ak,Bk,Ck,Dk); K_tf=tf(Knum,Kden) % [Aa,Bb,Cc,Dd]=branch(P); % sys=ss(Aa,Bb,Cc,Dd); % P_tf=zpk(ss(Aa,Bb,Cc,Dd)) step(feedback(G*GK,-1),30) % clsys =slft(P,K); % splot(clsys,'st') %} %{ %% matlab幫助中的h2syn函數的例子 A = [5 6 -6 6 0 5 -6 5 4]; B = [0 4 0 0 1 1 -2 -2 4 0 0 -3]; C = [-6 0 8 0 5 0 -2 1 -4 4 -6 -5 0 -15 7]; D = [0 0 0 0 0 0 0 1 0 0 0 0 0 0 3 6 8 0 -7 0]; P = ss(A,B,C,D); P1 = pck(A,B,C,D); pole(P) nmeas = 2; ncont = 1; [K,CL,gamma] = h2syn(P,nmeas,ncont); pole(CL) step(CL) clsys =slft(P,K); splot(clsys,'st') [A,B1,B2,C1,C2,D11,D12,D21,D22]=branch(P) %} %% 原來舊函數的應用--https://wenku.baidu.com/view/d79bb3cb0722192e44 A =[0 1 0 0; -5000 -100/3 500 100/3; 0 -1 0 1; 0 100/3 -4 -60]; B =[0 25/3 0 -1]'; C=[0 0 1 0]; D=0; G =ss(A,B,C,D); W1=[0 100; 1 1]; W2=1e-5; W3=[1 0; 0 1000]; T_ss=augtf(G,W1,W2,W3); [Aa,Bb,Cc,Dd]=branch(T_ss); [Aa,Bb,Cc,Dd]=ssdata(T_ss); [a,b1,b2,c1,c2,d11,d12,d21,d22]=branch(T_ss);%當使用augtf函數獲取增廣系統矩陣表達式后， %可用branch函數獲得各個增光系統矩陣的參數 P =rt2smat(T_ss) %變換成系統矩陣P K_h2=h2lqg(T_ss) %獲得H2最優控制器 [AK0,BK0,CK0,DK0]=branch(K_h2); [AK0,BK0,CK0,DK0]=ssdata(K_h2);%兩個函數均可實現這個功能 K_H2_tf=zpk(ss(AK0,BK0,CK0,DK0)) K_Hinf=hinf(T_ss) %獲得H00優控制器 [AK1,BK1,CK1,DK1]=branch(K_Hinf); K_Hinf_tf=zpk(ss(AK1,BK1,CK1,DK1)) [gamma,K_opt]=hinfopt(T_ss); %獲得H00最優控制器 [AK2,BK2,CK2,DK2]=branch(K_opt); K_opt_tf=zpk(ss(AK2,BK2,CK2,DK2)) % 繪制閉環節約響應下系統的開環bode圖 bode(G*K_H2_tf,'-',G*K_Hinf_tf,'--',G*K_opt_tf,':') % 閉環階躍響應曲線 figure(1) step(feedback(G,K_H2_tf,-1),'r-') figure(2) step(feedback(G*K_H2_tf,1),'r-') % step(feedback(G*K_H2_tf,1),'r-',feedback(G*K_Hinf_tf,1),'g--',feedback(G*K_opt_tf,1),':') %系統矩陣變換函數 -參見：https://wenku.baidu.com/view/d79bb3cb0722192e44 function P =rt2smat(A,B1,B2,C1,C2,D11,D12,D21,D22) if nargin ==1, [A,B1,B2,C1,C2,D11,D12,D21,D22]=branch(A); end n =length(A); P=[A,B1,B2;C1,D11,D12;C2,D21,D22]; P(size(P,1)+1,size(P,2)+1)=-inf; m=size(P,2); P(1,m)=n; end %}
H_infinity_looping_shape_control---參考文章：BACHELOR’S FINAL PROJECT（有代碼有幾種控制方法lqgLQRHOOPID）
clear;clc; close all; %% State-space model A=[-0.38 0.60 -0.36 -9.80; -0.98 -10.65 16.74 -0.21; 0.18 -5.39 -16.55 0; 0 0 1 0]; B=[-0.36; -3.62; -141.57; 0]; C4=[0 0 0 1]; %longitudinal speed u D=0; sys=ss(A,B,C4,D); %% Define system with uncertainties a11=ureal('a11',1,'Percentage',10); a12=ureal('a12',1,'Percentage',10); a13=ureal('a13',1,'Percentage',10); a14=ureal('a14',1,'Percentage',10); a21=ureal('a21',1,'Percentage',10); a22=ureal('a22',1,'Percentage',5); a23=ureal('a23',1,'Percentage',5); a24=ureal('a24',1,'Percentage',10); a31=ureal('a31',1,'Percentage',10); a32=ureal('a32',1,'Percentage',5); a33=ureal('a33',1,'Percentage',5); a34=ureal('a34',1,'Percentage',10); a41=ureal('a41',1,'Percentage',10); a42=ureal('a42',1,'Percentage',10); a43=ureal('a43',1,'Percentage',10); a44=ureal('a44',1,'Percentage',10); uA=[-0.38*a11 0.60*a12 -0.36*a13 -9.80*a14; -0.98*a21 -10.65*a22 16.74*a23 -0.21*a24; 0.18*a31 -5.39*a32 -16.55*a33 0; 0 0 1*a43 0]; b1=ureal('b1',1,'Percentage',10); b2=ureal('b2',1,'Percentage',5); b3=ureal('b3',1,'Percentage',5); b4=ureal('b4',1,'Percentage',10); uB=[-0.36*b1; -3.62*b2; -141.57*b3; 0]; usys=ss(uA,uB,C4,D); %% H-infinity loop shaping s=tf('s'); %Define s as the Laplace variable %Singular value diagram to obtain the crossover frequency (Wc) figure(1) sigma(sys) %Wc=6.11 rad/s (Value seen on figure 1) Wc=6.11; G=Wc/(s); %desired loopshape % Compute the optimal loop shaping controller K [K,CL,GAM] = loopsyn(sys,G); [K.A,K.B,K.C,K.D]=ssdata(K); %得出控制器狀態空間表達式參數 %% Closed-loop system L=K*sys; %Adding disturbance to the system (這句注釋的不對) L_close=feedback(L,1); %Closed-loop figure(2) step(L_close); str=sprintf('Step response of the pitch angle \\theta (H-infinity loopshaping)'); title(str); ylabel('\theta (rad)'); %System information S_final = stepinfo(L_close) [y,t]=step(100*L_close); sserror_final=100-y(end); %Display gain and phase margins figure(3) margin(L_close); %% Controller simplification %Hankel singular values of K figure(4) hsv = hankelsv(K); semilogy(hsv,'*--'), grid title('Hankel singular values of K (\theta)'), xlabel('Order') %Reduce controller from 7 to 5 states Kr = reduce(K,5); order(Kr) %check simplification %Closed-loop responses comparison (K and Kr) Lr=Kr*sys; Lr_close=feedback(Lr,1); figure(5) step(L_close,'b',Lr_close,'r-.'); str=sprintf('Original and simplified controller (K & Kr) comparison for \\theta'); title(str); ylabel('\theta (rad)'); legend('K','Kr','Location','Southeast') %% Plot step response with uncertainties uLr=Kr*usys; uLr_close=feedback(uLr,1); %Closed loop system with uncertainties figure(6); step(uLr_close); str=sprintf('Step response of the pitch angle \\theta (H-infinity loopshaping, uncertain plant)'); title(str); ylabel('\theta (rad)')
H2_optimal_control_fixed_wing-----基於Ｈ２最優控制的小型無人機飛行姿態控制器設計
clc;clear;close all; %{ %% State-space system A=[-0.38 0.60 -0.36 -9.80; -0.98 -10.65 16.74 -0.21; 0.18 -5.39 -16.55 0; 0 0 1 0]; B2=[-0.36; -3.62; -141.57; 0]; B1 =[0 0.6 0 0.2]'; %干擾矩陣 C1 =[0.3 0 0 0; %這個C1矩陣怎樣選擇 0 0.4 0 0; 0 0 0.1 0; 0 0 0 0.02] C2= [0 0 0 1]; D11=zeros(4,1); D12=[0 0 0 1]'; D21=[1]; D22 =[0]; G =ss(A,B2,C2,D22); P =ltisys(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]) [A1,B1,C1,D1]=branch(P) nmeas =1; %測量輸出個數 ncont =1; %控制輸入個數 gmin =0.1; gmax=18; tol =0.0001 % [K,CL,gamma] = hinfsyn(P,nmeas,ncont) %這里為什么不能使用這個函數會出錯？ [K,CL,gamma] = hinfsyn(P,nmeas,ncont,gmin,gmax,tol); %% 畫出階躍響應信號 figure(1) clsys =slft(P,K); %得到閉環系統 spol(clsys); %該函數返回閉環系統的極點 splot(clsys,'st',1);%畫出階躍響應曲線 % %% 畫出控制信號 % figure(2) % clsys =slft(K,P); %得到閉環系統 % spol(clsys); %該函數返回閉環系統的極點 % splot(clsys,'st');%畫出階躍響應曲線 [Ak,Bk,Ck,Dk]=branch(K) sys=ss(Ak,Bk,Ck,Dk) K_tf=zpk(ss(Ak,Bk,Ck,Dk)) [Knum2,Kden2]=ss2tf(Ak,Bk,Ck,Dk) K_tf1=tf(Knum2,Kden2) figure(3) step(feedback(G*K_tf,-1),40,'-') %% G =ss(A,B2,C2,D22); s=tf('s') W1=(100)/(10*s+1); W2=1e-6; W3=(10*s)/(s+0.000008); T_ss=augtf(G,W1,W2,W3); [Aa,Bb,Cc,Dd]=branch(T_ss); [Aa,Bb,Cc,Dd]=ssdata(T_ss); K_Hinf=hinf(T_ss) %獲得H00優控制器 [AK1,BK1,CK1,DK1]=branch(K_Hinf); [a,b1,b2,c1,c2,d11,d12,d21,d22]=branch(T_ss) figure(4) step(feedback(G,K_Hinf,-1),40,'-') %} %% 例子二： 系統參數數據-來自論文：基於Ｈ２最優控制的小型無人機飛行姿態控制器設計 A =[-0.136 11.52 -9.8 0; -0.0551 -0.2059 0 0.98; 0 0 0 1; 0.026 -0.66 0 -0.5534]; B =[0.34 0.001; -0.00167 -0.0063; 0 0; -0.000304 -0.00985]; C =[ 0 0 0 1]; % C=eye(4); D=[0 0]; % D =[0 0 0 0;0 0 0 0]'; G =ss(A,B,C,D); num=[0.008 5]; den =[1 0.0005]; w1 =tf(num,den); p1=w1; W1s=w1*eye(4) W2s=1e-5*eye(2); W2s=tf(W2s) p2=W2s; num1=[8 0.1]; den1 =[0.00001 40]; w2 =tf(num1,den1); p3=w2; % num2=[1 1.667e-5]; den2 =[0.00001 40]; w3 =tf(num2,den2); % W3s=[0 0 0 0; % 0 w2 0 0; % 0 0 0 0; % 0 0 0 w3]; % T_ss=augtf(G,W1s,W2s,W3s); T_ss=augtf(G,p1,p2,p3); [Aa,Bb,Cc,Dd]=branch(T_ss); [Aa,Bb,Cc,Dd]=ssdata(T_ss) [a,b1,b2,c1,c2,d11,d12,d21,d22]=branch(T_ss);%當使用augtf函數獲取增廣系統矩陣表達式后， %可用branch函數獲得各個增光系統矩陣的參數 P =rt2smat(T_ss) %變換成系統矩陣P K_h2=h2lqg(T_ss) %用H2lqg函數獲得H2最優控制器 nmeas = 1; ncont = 2; [K,CL,gamma] = h2syn(T_ss,nmeas,ncont) %用H2syn函數獲得H2最優控制器 % [] = h2syn(T_ss,nmeas,ncont) %用H2syn函數獲得H2最優控制器 [AK0,BK0,CK0,DK0]=branch(K_h2); [AK0,BK0,CK0,DK0]=ssdata(K_h2);%兩個函數均可實現這個功能 K_H2_tf=zpk(ss(AK0,BK0,CK0,DK0)) figure(1) bode(G*K_H2_tf,'-'); figure(2) % feedin=[2]; % feedout=[ 4]; % step(feedback(G*K_H2_tf,1,feedin,feedout),'r-') figure(1) step(feedback(G*K_H2_tf,1),'r-',6) figure(2) step(feedback(G*K,1),'r-',6) %% 問題 采用H2syn函數時 求出gamma 是無窮代表啥意思 %% 系統進行離散化 Ts=0.1; %采樣時間 Gd=c2d(G,Ts,'z') %采用零階保持器的方法對系統離散化 T_ssd=c2d(T_ss,Ts,'z') %廣義系統的離散化 P1 =rt2smat(T_ssd) [Aad,Bbd,Ccd,Ddd]=branch(T_ss); [ad,bd1,bd2,cd1,cd2,dd11,dd12,dd21,dd22]=branch(T_ssd); % dd11=zeros(4,1); % Pd =ltisys(ad,[bd1,bd2],[cd1,cd2],[dd11,dd12,dd21,dd22]) Pd =mksys(ad,bd1,bd2,cd1,cd2,dd11,dd12,dd21,dd22,'tss') Kd_h2=dh2lqg(Pd) %獲得離散的H2最優控制器，這里使用的是命令dh2lqg [AK0d,BK0d,CK0d,DK0d]=branch(Kd_h2); Kd_h2_tf=zpk(ss(AK0d,BK0d,CK0d,DK0d,Ts)) %將控制器轉換成零極點白表達式 figure(3) bode(Gd*Kd_h2_tf,'-'); %畫出系統伯德圖 figure(4) step(feedback(Gd*Kd_h2_tf,1),'r-',6) %} %系統矩陣變換函數 -參見：https://wenku.baidu.com/view/d79bb3cb0722192e44 function P =rt2smat(A,B1,B2,C1,C2,D11,D12,D21,D22) if nargin ==1, [A,B1,B2,C1,C2,D11,D12,D21,D22]=branch(A); end n =length(A); P=[A,B1,B2;C1,D11,D12;C2,D21,D22]; P(size(P,1)+1,size(P,2)+1)=-inf; m=size(P,2); P(1,m)=n; end
H infinty 狀態反饋-基於Riccati方程方法
%小型固定翼無人機俯仰控制——系統參數參見文章：BACHELOR’S FINAL PROJECT(好done) % H infinty 狀態反饋-基於Riccati方程方法 clear close all; clc %% State-space system A=[-0.38 0.60 -0.36 -9.80; -0.98 -10.65 16.74 -0.21; 0.18 -5.39 -16.55 0; 0 0 1 0]; B2=[-0.36; -3.62; -141.57; 0]; B1 =[0 0.86 0 4.2]'; %干擾矩陣 C1 =[0.1 0 0 0; %這個C1矩陣怎樣選擇 0 0.04 0 0; 0 0 0.02 0; 0 0 0 0.01; 0 0 0 0] C2= [0 0 0 1];% 測量矩陣 % D11 =[0]; D11 =[0;0;0;0;0]; D12 =[0;0;0;0;1]; D21=0; D22=0; P =ltisys(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]) %% 判斷系統是否滿足基於Riccati方程的假設條件 sys =ss(A,B2,C2,D22); P0 = pole(sys) Cc =rank(ctrb(A,B2)) Ob =rank(obsv(A,C2)) S =D12'*[C1 D12] %% 利用Riccati方程求解H infinity 在增益K, A'*X+X*A+X*(B1*B1'-B2*B2')*X+C'*C=0 % A'*X+X*A-X*[B1 B2]*[-I 0;0 I]*[B1';B2']*X+C1'*C1=0 B =[B1 B2]; R =[-1 0;0 1]; C =C1; X =care(A,B,C'*C,R) K =-B2'*X root =eig(A-K) %判斷閉環系統是否穩定；看其特征值是否都有負實部 [Aa,Bb,Cc,Dd]=branch(P) sys=ss(Aa,Bb,Cc,Dd) P_tf=zpk(ss(Aa,Bb,Cc,Dd)) % feedin=[1]; feedout=[1 2 3 4]; step(feedback(sys,K,feedin,feedout,-1)); % clsys1 =slft(P,K); %得到閉環系統 % spol(clsys1,'st') %驗證閉環系統極點