cp7_1.m

离散控制系统设计的MATLAB 代码

%%%%%%%%%%% Comprehensive Problem 7.1 %%%%%%%%%%%
%   Discrete-Time Control Problems using        %
%       MATLAB and the Control System Toolbox   %
%   by J.H. Chow, D.K. Frederick, & N.W. Chbat  %
%         Brooks/Cole Publishing Company        %
%                September 2002                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ball and Beam System              
% System has 4 states, 1 input, and 2 outputs
% 4 states : ball's position and velocity, and
%            wheel's angular position and velocity
% 1 input  : motor voltage (u)
% 2 output : ball's position (xi), and 
%            wheel's angular position (theta)

disp('CP7.1: Ball and Beam')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part A : Load model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear; close all

% Load continuous and discrete-time systems
if ~exist('bbeam.mat'), 
    dbbeam
elseif exist('bbeam.mat') == 2, 
    load bbeam 
end

Ts = 0.02;                          % in dbbeam already
disp('Loaded dbbeam')
disp('*******>'); pause; disp(' ')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part B : Inner (motor) Open_Loop - Margins
%          Closed_Loop - Response to a Unit Step Input 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% K1(z) proportional control 
disp('Closed-loop system response - K1(z) prop. controller'); disp(' ')
Gm_vec = []; Pm_vec = []; trm_vec = []; Mom_vec = [];
ks = 0.5;                           % gain step
Kp1 = 0.5:ks:10;
figure, xlabel('Time (sec)'), ylabel('Response (rad)'), grid
title('Step response, Kp1 from 0.5 to 10')
hold on
dtime = [0:0.02:3]';
for i = 1:length(Kp1) 
    Km = Kp1(i);
    Gm_OL = Gz*Km;                  % Open Loop (servo motor)
    Gm_OL_mtr = minreal(Gm_OL(2,1));  
    [Gm,Pm,Wcg,Wcp] = margin(Gm_OL_mtr);
    Gm_vec(i) = Gm;
    Pm_vec(i) = Pm;
    Gm_CL = feedback(Gm_OL_mtr,1,1,1);
    ym = step(Gm_CL,dtime);         % response due to wheel angle step
    [Mom,tpm,trm,tsm,essm] = kstats(dtime,ym,1);
    trm_vec(i) = trm;               % Rise Time of inner CL
    Mom_vec(i) = Mom;               % Overshoot of inner CL
    plot(dtime,ym,'o')
    drawnow
    pause(1)                        % pause for 1 second
end
Gm_vec_dB = 20*log10(Gm_vec);
text(1,0.5,'Kp1 = 0.5')
text(0.2,1.15,'Kp1 = 10')
hold off
disp('*******>'); pause
disp('Display gain margin and phase margin as a function of proportional gain.')

% Plot Gm and Pm as a fcn of Kp1
figure; plot(Kp1,Gm_vec_dB,Kp1,Pm_vec,'--')
grid;
xlabel('Proportional Gain')
ylabel('Gain Margin (dB) and Phase Margin (deg)');
title('Gain Margin (blue) and Phase Margin (green) vs Proportional Gain of Servo Motor')
format bank; disp(' ')
disp(['Kp1                -->    ',num2str(Kp1)])
disp(['Gain Margin (dB)   --> ',num2str(Gm_vec_dB)])
disp(['Phase Margin (deg) --> ',num2str(Pm_vec)]);
format; disp(' ')
disp('*******>'); pause
% Plot trm_vec and Mom_vec as a fcn of Kp1
figure; plot(Kp1,trm_vec,Kp1,Mom_vec,'--')
grid; 
xlabel('Proportional Gain')
ylabel('Rise Time (sec) and Maximum Overshoot (Percent)')
title('Rise Time (blue) and Max Overshoot (green) vs Kp1')
format bank; disp(' ')
disp(['Kp1             --> ',num2str(Kp1)])
disp(['Rise Time (s)   --> ',num2str(trm_vec)])
disp(['Max % Overshoot --> ',num2str(Mom_vec)]);
format; disp(' ')
disp('Pick Kp1 = 6.5 for Km(z)'); 
disp('*******>'); pause

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part C : Outer (ball) Loop with a PD Controller 
% Margins and Step Response Performance Due To
% 0.1m Input Command 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OL - PD controller - compute margins
disp('OL - Outer (ball) Loop - PD Controller'); disp(' ')
ks = 0.25;                            % gain step size
Kp2 = [0.4:0.4:2];                    % proportional gain for Gb(z) controller
Kd  = [0.4:0.4:2];                    % derivative gain for Gb(z) controller
lenKp2 = length(Kp2); lenKd = length(Kd);
%
Kp1 = 6.5;
Gm_OL = Gz*Kp1;                       % Open Loop (servo motor)
Gm_CL = feedback(Gm_OL,1,1,2);
Gmb_mat = []; Pmb_mat = []; trb_mat = []; Mob_mat = []; ang_mat = [];
dtime = [0:0.02:25]';
for i = 1:lenKp2, 
    yall = [];
    for j = 1:lenKd,
        Kb = Kp2(i)*tf([(1+Kd(j)/Ts) -Kd(j)/Ts],[1 0],Ts);
        Gb_OL = Gm_CL*Kb;
        [Gm,Pm,Wcg,Wcp] = margin(Gb_OL(1,1)); % margins
        Gmb_mat(i,j) = Gm;
        Pmb_mat(i,j) = Pm;
        Gb_CL = feedback(Gb_OL,1,1,1); % CLoop
        yb    = step(Gb_CL,dtime);     % step response
        yall = [yall yb(:,1)];
        [Mob,tpb,trb,tsb,essb] = kstats(dtime,0.1*yb(:,1),0.1);
        trb_mat(i,j) = trb;            % Rise Time of outer (b&b) CL
        Mob_mat(i,j) = Mob;            % Overshoot of outer CL
        ang_mat(i,j) = 0.1*max(yb(:,2));
        drawnow
        pause(0.5)
    end
    figure
    plot(dtime(1:10:1251),yall(1:10:1251,:),'o')
    xlabel('Time (sec)'), ylabel('Response (m)'), grid
    title(['Closed-loop system step response for Kd from 0.4 to 2, Kp = ', num2str(Kp2(i))])
    legend('Kd = 0.4','Kd = 0.8','Kd = 1.2','Kd = 1.6','Kd = 2.0')
end

Gmb_mat_dB = 20*log10(Gmb_mat);

figure; [ctrb,htrb] = contour(Kd,Kp2,trb_mat);
clabel(ctrb,htrb); xlabel('Kd'); 
ylabel('Kp2'); hold on; 
title(['Solid: Rise Time, Dashdot: Max Wheel Angle, Dashed: Phase Margin'])
[cang,hang] = contour(Kd,Kp2,ang_mat,'-.');
clabel(cang,hang); grid
[cPmb,hPmb] = contour(Kd,Kp2,Pmb_mat,':');
clabel(cPmb,hPmb)
hold off

disp('For a design with max wheel angle < 1 rad and a phase margin')
disp('of at least 45 deg, the best rise time that can be achieved')
disp('is about 2.8 s, with Kp2 = 0.85 and Kd = 1.2')

disp('*******>'); pause

% Done
disp('End of Comprehensive Problem CP7.1')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%