image thumbnail
from Structured MIMO H-infinity design for a dual-stage platform using HIFOO and Hinfstruct by Zdenek Hurak
Code for advanced control design accompanying a journal paper submission. Can serve as a benchmark.

dual_stage_structured_MIMO_Hinf_control.m
%% Structured MIMO Hinf controller design for an experimental single-axis dual-stage platform using HIFOO and HINFSTRUCT
% _Martin Rezac and Zdenek Hurak_
% This m-file first creates a linear state-space model of a real experimental 
% motion control platform where two electrical motors are used to rotate a payload around
% a single (vertical) axis. This simple device was built to analyze control 
% schemes for inertial stabilization platforms. More details can be found
% in the paper 
%
% * M. Rezac and Z. Hurak. Structured MIMO H∞ Design for Dual-Stage Inertial 
% Stabilization: Case Study for HIFOO and Hinfstruct Solvers. Mechatronics,
% Vol.X, No.Y, 2013. A draft submitted after a minor revision can be
% downloaded <http://aa4cc.dce.fel.cvut.cz/sites/default/files/downloads/publications/structured-dual-mechatronics.pdf here>.
%
% A rigid plate freely rotatable around a vertical axis carries a standard 
% DC brush-type rotary motor, which via a belt transmission drives the secondary
% stage carrying a DC voice-coil motor, which pushes the payload. 
% The voice-coil motor enjoys a very small friction, which is a very plausible
% property for inertial stabilization, but its angular stroke is limited to 
% (+-5 deg). Therefore, the (geared) DC brush-type rotary motor is used to 
% keep the keep the angular devitation close to zero.
% The key disturbance is the angular velocity of the freely rotatable plate
% upon which the stator of the DC brushed motor resides. This mimicks the
% unwanted angular motion of a mobile carrier (car, aircraft, ship).
% The <http://youtu.be/F5N3WkDDRZM video> can help understanding the
% principle.
%
% Having two measured outputs and two control inputs, the control design
% can either be accomplished by successively closing SISO loops or
% designing a MIMO controller. The latter generally cannot include some
% resctrictions on the structure and the order of the controller. With two
% solvers HIFOO and Hinstruct, both providing identical functionality and
% comparable performance, the task of structured MIMO control design can be 
% by minimizing H-infinity norm of properly defined generalized plant.
%
% You are encouraged to use PUBLISH functionality in Matlab to obtain a
% nicely formatted version of this code.
%
% But first we need to create the mathematical model of the physical plant

%% Physical parameters
% The subscript 1 refers to the voice-coil motor (VCM), the subscript 2
% refers to the rotary brushed motor.

R1  =  3.6;                   % [Ohm] 
L1  =  1.1e-3;                % [H]
kt1 =  5.11;                  % [N/A]
J1   = 10e-3;                 % [kg*m^2]
b1  =  0.125;                 % [N*m*s/rad]
r   =  0.065;                 % [m]
n1  =  74;                    % [-]  
n2  =  4;                     % [-]  
R2  =  3.43;                  % [Ohm] 
L2  =  0.65e-3;               % [H]
kt2 =  56.6e-3;               % [Nm/A]
Jmp =  (67.8e-7+15e-7)*n1^2;  % [kg*m^2]
bp  =  0.48;                  % [N*m/(rad/s)]
k   =  250;                   % [N*m/(rad)]
b   =  0.9;                   % [N*m/(rad/s)]
J2  =  25e-3;                 % [kg*m^2]
voltageVCM_max = 7;           % [V]
voltageM_max = 32;        % [V]

%% Linear state-space model

%           i1,                  w1,               i2,                  wp,               w2,            phip,             phi2,     phi1,             
% ---------------------------------------------------------------------------------------------------------------------------------------                
A = [   -R1/L1,           -r*kt1/L1,                0,                   0,         r*kt1/L1,               0,                0,        0;
      r*kt1/J1,              -b1/J1,                0,                   0,            b1/J1,               0,                0,        0;
             0,                   0,           -R2/L2,          -n1*kt2/L2,                0,               0,                0,        0;
             0,                   0,       kt2*n1/Jmp,         (-b-bp)/Jmp,         b*n2/Jmp,          -k/Jmp,         k*n2/Jmp,        0;
     -kt1*r/J2,                   0,                0,                b/J2,       (-b*n2)/J2,            k/J2,         -k*n2/J2,        0;
             0,                   0,                0,                   1,                0,               0,                0,        0;
             0,                   0,                0,                   0,                1,               0,                0,        0;
             0,                   1,                0,                   0,                0,               0,                0,        0; ];
         
B = [voltageVCM_max/L1 0 0;
     0 0 0;0, voltageM_max/L2, n2*n1*kt2/L2; 
     0 0 bp*n2/(n1^2*Jmp); 
     0 0 0;
     0 0 0; 
     0 0 0; 
     0 0 0];

C2 = [0, 1, 0, 0, 0, 0, 0, 0; 
      0, 0, 0, 0, 0, 0, -1, 1; 
      0, 0, 0, 0, 1, 0, 0, 0];
  
C = [0, 1, 0, 0, 0, 0, 0, 0; 
    0, 0, 0, 0, 0, 0, -1, 1];

D = zeros(2,3);
D2 = zeros(3,3);
 
Gds = ss(A,B,C,D);
set(Gds,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}'});  
Gds = minreal(Gds);

Gds_ext = ss(A,B,C2,D2);
set(Gds_ext,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}','\omega_2'});  
Gds_ext = minreal(Gds_ext);


%% Design of a structured MIMO controller for a dual-stage model using HIFOO and Hinfstruct
% Some preliminary stuff
st = tf('s');
fmin=1e-4;
fmax=1e4;

%% Creating the weighting filters for H-infinity synthesis

MS=5;AS=0.001;f=50;
W1=(st/MS+f)/(st+AS*f);
MS=10;AS=0.025;f=1;
W2=(st/MS+f)/(st+AS*f)*100;
AS=1.2;MS=0.6;f=3;
Wc=(st/MS+f)/(st+AS*f)*AS;
Wu1 = 1;
Wu2 = 1;

%% Creating the generalized plant P
w1ref = icsignal(1);wc = icsignal(1);u1 = icsignal(1);u2 = icsignal(1);
zew1 = icsignal(1);zew2 = icsignal(1); zu1 = icsignal(1);zu2 = icsignal(1);
ew1 = icsignal(1); ephi = icsignal(1);
w1 = icsignal(1); 

MP = iconnect;
MP.Equation{1} = equate([w1;ephi], Gds*[u1;u2;Wc*wc]);
MP.Equation{2} = equate(ew1,w1ref - w1);
MP.Equation{3} = equate(zew1,W1*ew1);
MP.Equation{4} = equate(zew2,W1*ephi);
MP.Equation{5} = equate(zu1,Wu1*u1);
MP.Equation{6} = equate(zu2,Wu2*u2);
MP.Input = [w1ref; wc; u1; u2];
MP.Output = [zew1; zew2; zu1; zu2; w1ref; ew1; ephi];
P = MP.system;

% group inputs and outputs    
NU = 2; NY = 3;
%P = ss(minreal(P));
[r,c]=size(P);
P.InputGroup = struct('U1',1:c-NU,'U2',c-NU+1:c);
P.OutputGroup = struct('Y1',1:r-NY,'Y2',r-NY+1:r);

%% Create the initial controller (for hifoo and hinfstruct)

% K2 controller in state space (init for HIFOO and hinfstruct)
INIT.a = zeros(5);
INIT.d = [ 0 1 0; 0 0 10;];
INIT.b = [ 0 0 0; 0 8 0; 0 0 0; 0 0 2; 0 0 0;];
INIT.c = [ 0 12.5 0 0 0; 0 0 0 2.5 0];
K2 = ss(INIT.a,INIT.b,INIT.c,INIT.d);

%K2=[0 , 1+100/tf('s'), 0; 0 , 0 ,  (10+5/tf('s'))];

%% HIFOO design
ordR2 = 2;
ordK1 = 2;
ordF = 1;
OPTIONS.struct.a = blkdiag(ones(ordF),ones(ordK1),ones(ordR2));
OPTIONS.struct.b = blkdiag(ones(ordF,1),ones(ordK1,1),ones(ordR2,1));
OPTIONS.struct.c = [zeros(1,ordF), ones(1,ordK1), zeros(1,ordR2); ones(1,ordF), zeros(1,ordK1), ones(1,ordR2)];
OPTIONS.struct.d = [ 0 1 0; 1 0 1];
OPTIONS.nrand = 3;

tic
[K1_hifo, F, VIOL, LOC] = hifoo(P,ordR2+ordK1+ordF,OPTIONS,INIT);
toc
 
%% Hinfstruct design
ordR2 = 2;
ordK1 = 2;
ordF = 1;

blk = ltiblock.ss('ssblock',5,2,3);
blk.a.Value = INIT.a;
blk.d.Value = INIT.d;
blk.b.Value = INIT.b;
blk.c.Value = INIT.c;
blk.a.Free = OPTIONS.struct.a;
blk.b.Free = OPTIONS.struct.b;
blk.c.Free = OPTIONS.struct.c;
blk.d.Free = OPTIONS.struct.d;

options = hinfstructOptions
%options = hinfstructOptions('Display','iter' )
options = hinfstructOptions('RandomStart',3)
tic
[K4,gamma,info] = hinfstruct(P,blk,options);
toc
K4 = ss(K4);
      
%% Standard (unstructured, unconstrained) H-infinity design
[K3,CL,GAM] = hinfsyn(P,3,2,'METHOD','lmi');
K3 = minreal(K3);
  
%% Building closed-loop transfer functions
K1 = K1_hifo; 
stability = {'NO'; 'YES'};

% create open and closed loop tf
MG = iconnect;
MG.Equation{1} = equate([w1;ephi], Gds*[u1;u2;wc]);
MG.Equation{2} = equate(ew1,w1ref - w1);
MG.Input = [w1ref; wc; u1; u2];
MG.Output = [w1ref; ew1; ephi];
G = MG.system;

MG.Equation{3} = equate([u1;u2], K2*[w1ref; ew1; ephi]);
MG.Input = [w1ref;wc];
MG.Output = [ew1; ephi; u1; u2];
CL2= minreal(MG.system);
disp(strcat('isstable(CL2): ',stability(isstable(CL2)+1)))
disp(strcat('isstable(K2): ',stability(isstable(K2)+1)))

MG.Equation{3} = equate([u1;u2], K1*[w1ref; ew1; ephi]);
CL= minreal(MG.system);
disp(strcat('isstable(CL) hiffo: ',stability(isstable(CL)+1)))
disp(strcat('isstable(K1) hiffo: ',stability(isstable(K1)+1)))

MG.Equation{3} = equate([u1;u2], K3*[w1ref; ew1; ephi]);
CL3= (MG.system);
disp(strcat('isstable(CL3): ',stability(isstable(CL3)+1)))
disp(strcat('isstable(K3): ',stability(isstable(K3)+1)))

MG.Equation{3} = equate([u1;u2], K4*[w1ref; ew1; ephi]);
CL4= (MG.system);
disp(strcat('isstable(CL4) hinfstruct: ',stability(isstable(CL4)+1)))
disp(strcat('isstable(K4): hinfstruct',stability(isstable(K4)+1)))

%% Printing the values of the resulting Hinf norms

disp(['hifoo = ',num2str(norm(lft(P,K1),inf))])
disp(['PI = ', num2str(norm(lft(P,K2),inf))])
disp(['hinf = ',num2str(norm(lft(P,K3),inf))])
disp(['hinfstruct = ',num2str(norm(lft(P,K4),inf))])


%% Ploting resulting responses
set(CL,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL2,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL3,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL4,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(Gds_ext,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}','\omega_2'});  
set(K1,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});  
set(K2,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});  
set(K3,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});  
set(K4,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});  

%%
% CL ref -> e 
figure(51)
subplot(2,2,1);
step(CL2(1,1),'g',0:0.0001:0.3);  title(''); hold on
step(CL3(1,1),'c',0:0.0001:0.3);  title(''); hold on
step(CL4(1,1),'m-.',0:0.0001:0.3);  title(''); hold on
step(CL(1,1),'k--',0:0.0001:0.3);  title(''); hold off
title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5); 
ylabel('angle rate [rad/s]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best' )
subplot(2,2,2);
step(-CL2(1,2),'g',0:0.0001:0.3);  title(''); hold on
step(CL3(1,2),'c',0:0.0001:0.3);  title(''); hold on
step(-CL4(1,2),'m-.',0:0.0001:0.3);  title(''); hold on
step(-CL(1,2),'k--',0:0.0001:0.3);  title(''); hold off
axis([0,0.3,-0.4,1]);
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5); 
ylabel('angle rate [rad/s]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best')    
subplot(2,2,3);
step(CL2(2,1),'g',0:0.0001:5.1);  title(''); hold on
step(CL3(2,1),'c',0:0.0001:5);  title(''); hold on
step(CL4(2,1),'m-.',0:0.0001:5);  title(''); hold on
step(CL(2,1),'k--',0:0.0001:5.1);  title(''); hold off
axis([0, 5, -0.02, 0.06])
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5); 
ylabel('angle [rad]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best') 
subplot(2,2,4);
step(CL2(2,2),'g',0:0.0001:5.1);  title(''); hold on
step(CL3(2,2),'c',0:0.0001:5);  title(''); hold on
step(CL4(2,2),'m-.',0:0.0001:5);  title(''); hold on
step(CL(2,2),'k--',0:0.0001:5.1);  title(''); hold off
axis([0, 5, -0.02, 0.06])
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5); 
ylabel('-angle [rad]'); grid on
xlabel('time'); 
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
legend('PI','H_{\infty}','Hinfstruct','HIFOO','Location','Best') 
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k');
h = findall(gcf,'type','line'); set(h,'linewidth',2.5); 
 
%%
figure(11), bodemag(CL2(1:2,1:2),'g',{fmin,fmax}); grid on; hold on;
axis([1e-4,1e4,-100,20]);
bodemag(CL3(1:2,1:2),'c',{fmin,fmax});
axis([1e-4,1e4,-100,20]);
bodemag(CL4(1:2,1:2),'k--',{fmin,fmax});
axis([1e-4,1e4,-100,20]);
bodemag(CL(1:2,1:2),'m-.',{fmin,fmax});
bodemag([1/W1 1/W1;1/W2 1/W2],'r--',{fmin,fmax}); hold off;
legend('PI','H_{\infty}','HIFOO','Hinfstruct','1/W_x','Location','Best');title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
axis([1e-4,1e4,-100,20]);h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5); 
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k'); 
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);  
%%

figure(41),bodemag(CL2(3:4,1:2),'g',{fmin,fmax}); grid on; hold on;
bodemag(CL3(3:4,1:2),'c',{fmin,fmax});
bodemag(CL4(3:4,1:2),'k--',{fmin,fmax});
bodemag(CL(3:4,1:2),'m-.',{fmin,fmax});
bodemag(tf([1/Wu1 1/Wu1;1/Wu2 1/Wu2]),'r--',{fmin,fmax}); hold off;
legend('PI','H_{\infty}','HIFOO','Hinfstruct','1/W_{ux}','Location','Best');title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
axis([1e-4,1e4,-100,20]);h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5); 
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k'); 
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);  


%%
figure(31), bodemag(K1,'k--',{0.1*fmin,fmax}); grid on; hold on
bodemag(K2,'g',{0.1*fmin,fmax}); hold on
bodemag(K4,'m-.',{0.1*fmin,fmax}); hold on
bodemag(K3,'c',{0.1*fmin,fmax}); hold off
legend('HIFOO','PI','Hinfstruct','H_{\infty}','Location','Best');title(''); 
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5); 
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);

Contact us