image thumbnail

Simulation of tractor active suspension

by

 

Design and analysis of full-state feedback controller for a tractor active suspension

m_file_Tractor_suspension.m
%==========================================================================
% <<Run this file after running the simulation suspmod.mdl>>
% Controller design for tractor active suspension system
% To customize this code you need to modify
%
% 1- System parameters, m1,m2,k1,k2,b1,b2
% 2- Controller matrix gains, K_control
%==========================================================================

%++++++++++++++++++++++++++++++++++
%   Aug.29.2012, Ramin Shamshiri  +
%   ramin.sh@ufl.edu              +
%   Dept of Ag & Bio Eng          +
%   University of Florida         +
%   Gainesville, Florida          +
%++++++++++++++++++++++++++++++++++


% Initializing

clc
close all
% clear all

% System parameters
m1 = 700;       % kg
m2 = 90;        % kg
k1 = 62000;     % N/m
k2 = 570000;    % N/m
b1 = 500;      % N.s/m
b2 = 22500;     % N.s/m

%% Transfer function-----


nump=[(m1+m2) b2 k2];
denp=[(m1*m2) (m1*(b1+b2))+(m2*b1) (m1*(k1+k2))+(m2*k1)+(b1*b2) (b1*k2)+(b2*k1) k1*k2];
G1=tf(nump,denp);

%  consider the disturbance input W(s) only, we set U(s) = 0

num1=[-(m1*b2) -(m1*k2) 0 0];
den1=[(m1*m2) (m1*(b1+b2))+(m2*b1) (m1*(k1+k2))+(m2*k1)+(b1*b2) (b1*k2)+(b2*k1) k1*k2];
G2=tf(num1,den1);
%------------------


%% Dynamic model in State Space form

%  consider the control input U(s) only, we set R(s) = 0
d1=(m1*(b1+b2)+b1*m2)/(m1*m2);
d2=(m1*(k1+k2)+b1*(b1+b2)+(m2*k1)-b1^2)/(m1*m2);
d3=((k1+k2)*b1+(b1+b2)*k1-2*b1*k1)/(m1*m2);
d4=(k1*k2)/(m1*m2);

n1=0;
n2=(m1+m2)/(m1*m2);
n3=b2/(m1*m2);
n4=k2/(m1*m2);

% State Space for system 1, R(s) = 0
A11=[-d1     -d2     -d3     -d4
    1       0       0       0
    0       1       0       0
    0       0       1       0];
B11=[1 0 0 0]';
C11=[n1 n2 n3 n4];
D11=0;
sys_G1=ss(A11,B11,C11,D11);


% State Space for system 2, U(s) = 0

N1=(-m1*b2)/(m1*m2);
N2=(-m1*k2)/(m1*m2);
N3=0;
N4=0;


A21=[-d1     -d2     -d3     -d4
    1       0       0       0
    0       1       0       0
    0       0       1       0];
B21=[1 0 0 0]';
C21=[N1 N2 N3 N4];
D21=0;
sys_G2=ss(A21,B21,C21,D21);
%------------------

%% Checking Controlability and Observability for system 1
if det(ctrb(A11,B11))==0
    disp('          ----------> System 1 is NOT Controllable <----------')
else
    disp('          ----------> System 1 is Controllable <----------')
end
    

if det(obsv(A11,C11))==0
    disp('          ----------> System 1 is NOT Observable   <----------')
else
    disp('          ----------> System 1 is Observable   <----------')
end
%-----------------------

% Checking Controlability and Observability for system 2
if det(ctrb(A21,B21))==0
    disp('          ----------> System 2 is NOT Controllable <----------')
else
    disp('          ----------> System 2 is Controllable <----------')
end
    

if det(obsv(A21,C21))==0
    disp('          ----------> System 2 is NOT Observable   <----------')
else
    disp('          ----------> System 2 is Observable   <----------')
end
%-----------------------



%% Response without controller

% Root locus
figure
subplot(2,2,1)
rlocus(sys_G1)
title('Root Locus system 1, without controller')

subplot(2,2,2)
rlocus(sys_G2)
title('Root Locus system 2, without controller')


% Open loop Step response of system 1, (R(s) = 0) without controller, Step= 0.25m
figure;
subplot(2,2,1);
step(0.25*sys_G1,0:0.01:15),grid on
title('Open loop step response of system 1 (R(s) = 0) without controller, Step= 0.25m')


% Open loop Step response of system 2, (U(s) = 0) without controller, Step= 0.25m
subplot(2,2,2);
step(0.25*sys_G2,0:0.01:15),grid on
title('Open loop Step response of system 2 (U(s) = 0) without controller, Step= 0.25m')

% Open loop Sin response of system 2, (U(s) = 0) without controller, Frequency= 0.25, Amplitude= 0.25m
[signal,time] = gensig('sin',1,15,0.01);
subplot(2,2,3);
lsim(0.25*sys_G2,0.25*signal,time),grid on
title('Open loop Sin response of system 2 (U(s) = 0) without controller, Frequency= 0.25, Amplitude= 0.25m')


%% State Space Controller design

Aa=[0                 1   0                                              0         0
   -(b1*b2)/(m1*m2)   0   ((b1/m1)*((b1/m1)+(b1/m2)+(b2/m2)))-(k1/m1)   -(b1/m1)   0
    b2/m2             0  -((b1/m1)+(b1/m2)+(b2/m2))                      1         0
    k2/m2             0  -((k1/m1)+(k1/m2)+(k2/m2))                      0         0
    0                 0   1                                              0         0];
Ba=[0                 0
    1/m1              (b1*b2)/(m1*m2)
    0                -(b2/m2)
    (1/m1)+(1/m2)    -(k2/m2)
    0                 0];
Ca=[0 0 1 0 0];
Da=[0 0];
sys=ss(Aa,Ba,Ca,Da);


K_control=[ 250 500 300 200 150 ];



%% Closed-loop response with controller
t = 0:0.01:15;
sys_cl = ss(Aa-Ba(:,1)*K_control,-0.25*Ba,Ca,Da);
subplot(2,2,4);
step(0.25*sys_cl*[0;1],t), grid on
title('Closed-Loop Response with controller Step=0.25m')


%% Simulation of response with controller

% Simulation for step bump
t = 0:0.01:15;
dis_step=zeros(1,1501);

for i=1:100
    dis_step(1,i)=0;
end
for i=100:150
    dis_step(1,i)=0.25;
end
for i=150:750
    dis_step(1,i)=0;
end
for i=750:800
    dis_step(1,i)=-0.25;
end
for i=800:1501
    dis_step(1,i)=0;
end






% Simulation for Sinus bump
t = 0:0.01:15;
dis_sin=zeros(1,1501);


for i=1:100
    dis_sin(1,i)=0;
end
for i=100:150
    dis_sin(1,i)=0.25*sin(i/(3*pi));
end
for i=150:750
    dis_sin(1,i)=0;
end
for i=750:800
    dis_sin(1,i)=-0.25*sin(i/(3*pi));
end
for i=800:1501
    dis_sin(1,i)=0;
end


% Step plot
figure
subplot(3,1,1)
plot(t,dis_step),grid on
hold on
plot(t,dis_sin),grid on
hold off

subplot(3,1,1);
lsim(sys_cl*[0;1],dis_step,t), grid on
subplot(3,1,3);
lsim(sys_G2,dis_step,t), grid on


% Sin plot
figure
subplot(3,1,1);
lsim(sys_cl*[0;1],dis_sin,t), grid on
subplot(3,1,1);
lsim(sys_G2,dis_sin,t), grid on



%% Other plots
figure
subplot(5,1,1)
plot(Control.time,Control.signals.values,'DisplayName','Control') 
title('Plot of Control')

subplot(5,1,2)
plot(x1.time,x1.signals.values,'DisplayName','x1')  % x1
title('Plot of x1')

subplot(5,1,3)
plot(dx1.time,dx1.signals.values,'DisplayName','dx1') % dx1
title('Plot of dx1')

subplot(5,1,4)
plot(dy.time,dy.signals.values,'DisplayName','dy')  % y
title('Plot of dy')


subplot(5,1,5)
plot(y.time,y.signals.values,'DisplayName','y')  % y
title('Plot of y')





%subplot(5,1,5)
% plot(inty.time,inty.signals.values,'DisplayName','inty') % dx1
% title('Plot of int(y)')













Contact us