cp10_2.m

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

%%%%%%%%%%% Comprehensive Problem 10.2 %%%%%%%%%%
%   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                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverted Pendulum
% System has 4 states, 1 input, and 2 outputs
% 4 states : pendulum's angular position and velocity, and
%            cart's position and velocity
% 1 input  : motor voltage (u)
% 2 outputs: pendulum's angular position (theta)
%            cart's position (x) 

disp('CP10.2: Inverted Pendulum')

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

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

Ts = 0.01;  % in dstick already
disp('*******>'); pause; disp(' ');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part B : full state feedback control design
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Design of full-state control using pole-placement')
disp(' ')

% determining the desired pole locations
disp('The desired pendulum transient decay should be about 0.1 s')
disp('that is, the transient should decay to 5% after 10 time periods')
disp('of 0.01 s (Ts). So the 10th root of 0.05 should be a good ')
disp('starting point.')
p_pen = 0.05^(1/10)
disp('*******>'); pause; disp(' ')

disp('The desired cart position step response should have a rise time')
disp('about 1.5 s and an overshoot of less than 5%.')
disp('This will translate into a continuous-time oscillatory mode')
disp('with a period of 6 (4x1.5) s and a damping ratio of') 
disp('0.9 by the formula Mp = exp(-zeta/sqrt(1-zeta^2)*pi)')
disp('Then the continuous complex poles can be mapped into')
disp('discrete-time poles using z = exp(s*Ts).')
w = 2*pi/6
s = -cot(acos(0.9))*w + sqrt(-1)*w
p_cart = exp(s*Ts)
disp('*******>'); pause; disp(' ')

disp('desired poles are')
p_des = [p_pen; p_pen-0.01; p_cart; conj(p_cart)];
xy2p(p_des);
disp('*******>'); pause; disp(' ')

disp('Full-state-feedback gain') 
K = place(Gz.a,Gz.b,p_des)

disp('Closed-loop system model')
Gz_fs = ss(Gz.a - Gz.b*K, Gz.b, Gz.c, Gz.d, Ts)
disp('*******>'); pause; disp(' ')

disp('initial condition response with zero input')
disp('initial condition is [0 0 0.1 0], cart is 0.1 m off center')
x0 = [0 0 0.1 0]';
t = [0:1:200]'*Ts;
y = lsim(Gz_fs,0*t,t,x0);

figure
plot(t,y(:,1),'o',t,y(:,2),'*'); grid  
drawnow
xlabel('Time (s)')
ylabel('Amplitude')
legend('pendulum angle (rad)','Cart position (m)')
title('Full-state feedback design')
disp('*******>'); pause; disp(' ')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part C : derivative design 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% build derivative feedback
Gdps = tf([100 0],[1 100]);
Gdp = c2d(Gdps,Ts,'tustin');
Gdcs = tf([99 0],[1 99]); % rate filter with lowpass
Gdc = c2d(Gdcs,Ts,'tustin');

disp('Controllers for the servo and the ball are')
Kp = K(1) + K(2)*Gdp
Kc = K(3) + K(4)*Gdc
Kfd = [Kp Kc]
disp('*******>'); pause; disp(' ')

disp('Closed-loop system poles')
Gz_fd_CL = feedback(Gz*Kfd,[1 0;0 1]);
xy2p(pole(Gz_fd_CL));

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

disp('Initial condition for derivative control')
xfd20 = [0 0 0.1 0 0 0]'
t = [0:1:200]'*Ts;
y = lsim(Gz_fd_CL,t*[0 0],t,xfd20);

figure
plot(t,y(:,1),'o',t,y(:,2),'*'); grid  
drawnow
xlabel('Time (s)')
ylabel('Amplitude')
legend('pendulum angle (rad)','Cart position (m)')
title('Derivative feedback design')
disp('*******>'); pause; disp(' ')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part D : Observer design 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% assign observer eigenvalues at twice the decay rate of 
% the full-state feedback poles
pObs = [p_pen^2; (p_pen-0.01)^2; p_cart^2; conj(p_cart^2)];

disp('desired observer poles')
xy2p(pObs);

disp('observer gain')
L = place(Gz.a',Gz.c',pObs);
L = L'

disp('*****>');pause
A_f = Gz.a-Gz.b*K-L*Gz.c;             % controller system matrix
disp('Observer controller poles')
xy2p(eig(A_f));                       % controller poles
disp('*******>'); pause
KObs = ss(A_f,-L,-K,0,Ts);

%
GK = Gz*KObs;
%
GObsCL = feedback(GK,[1 0;0 1]);      % closed-loop system
disp('Closed-loop system poles')
xy2p(pole(GObsCL));                   % closed-loop system poles
disp('*******>'); pause

disp('Closed-loop system initial conditions')
xObs0 = [0 0 0.1 0 0 0 0 0]'
t = [0:1:200]'*Ts;
y = lsim(GObsCL,t*[0 0],t,xObs0);
figure, plot(t,y(:,1),'o',t,y(:,2),'*'); grid  
drawnow
xlabel('Time (s)')
ylabel('Amplitude')
legend('pendulum angle (rad)','Cart position (m)')
title('Initial condition response of observer controller')
disp('*****>');pause

disp('Disturbance to pendulum angle')
t = [0:1:200]'*Ts;
y = impulse(GObsCL(:,1),t);
figure, plot(t,y(:,1),'o',t,y(:,2),'*'); grid  
drawnow
xlabel('Time (s)')
ylabel('Amplitude')
legend('pendulum angle (rad)','Cart position (m)')
title('Impulse response on pendulum angle')
disp('*****>');pause

disp('Step command on cart position')
y = step(GObsCL(:,2),t);
figure, plot(t,0.1*y(:,1),'o',t,0.1*y(:,2),'*'); grid  
xlabel('Time (s)')
ylabel('Amplitude')
legend('pendulum angle (rad)','Cart position (m)')
title('Step response on cart position to move 0.1 m')

% Done
disp('End of Comprehensive Problem CP10.2')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%