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(' heta (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 ( heta)'), 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(' heta (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(' heta (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') %验证闭环系统极点