Code covered by the BSD License

# Simulation of Control Systems for a Mobile Robot Platform

Control design for Field Survey Mobile Robot using lead-lag compensation, PID and Fuzzy Logic

Mobile_Robot_Simulation

## Contents

```% m-code number one
%==========================================================================
% Design and Simulation of Control Systems for a Field Survey Mobile Robot Platform using lead-lag compensation, PID and Fuzzy Logic
% To customize this code you need to:
% 1- Change the values of DC motor constants
% 2- Change the zeros of lead-lag compensation
% 3- Change the gains of PID controller
% 4- Change the rules of Fuzzy logic controller
%==========================================================================

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

## Initialization....

```clc;
close all

% DC motor constants
R=4.67 ;            %Terminal Resistance Ohm
L=170e-3;           %Terminal Inductance H
J=42.6e-6;          %Motor Inertia Kg.m2
kt=14.7e-3;         %Torque Constant Nm/Amp

% Transfer Function
num=[kt];
den=[J*L J*R+b*L b*R+ke*kt];
disp('Open loop Transfer function without controller')
TF_DC=tf(num,den)

num_std=[kt/(J*L)];
den_std=[1 (J*R+b*L)/(J*L) (b*R+ke*kt)/(J*L)];
disp('Standard format Open loop Transfer Function without controller')
TF_std=tf(num_std,den_std)

[numclp,denclp]=cloop(num,den,-1);
disp('Closed loop Transfer Function without controller')
tf(numclp,denclp)

% Open loop response without controller
figure;
subplot(2,1,1);
step(num,den,0:0.1:1),grid on
title('Open loop response without controller')
hold on

% Closed loop response without controller
subplot(2,1,2)
step(numclp,denclp,0:0.1:1),grid on
title('Closed loop response without controller')

% State Spapce representation
A=[-(R/L) -(ke/L);(kt/J) -(b/J)];
B=[1/L; 0];
C=[0 1];
D=0;
disp('State Space representation:')
SYS_DC=ss(A,B,C,D)

% Checking Controlability and Observability
if det(ctrb(A,B))==0
disp('          ----------> System is NOT Controllable <----------')
else
disp('          ----------> System is Controllable <----------')
end

if det(obsv(A,C))==0
disp('          ----------> System is NOT Observable   <----------')
else
disp('          ----------> System is Observable   <----------')
end

% Drawing Root locus
figure
rlocus(num,den),grid on
title('Root Locus without controller')

% Design Criteria
Ts=0.1;         % Settling time<0.5 second
PO=0.05;        % Overshoot<5%

abs(roots([1+(((-log(PO))/pi)^2) 0 -(((-log(PO))/pi)^2)])); % Damping ratio
Damp=ans(1);
Wn=4/(Ts*Damp); % Natural frequency

disp('Desired Damping ratio is:'),Damp
disp('Desired Natural Frequency is:'),Wn

% Desired Characteristic Equation:
dend=[1 2*Wn*Damp Wn^2];
disp('Desired Characteristic Equation is:'),dend

% Desired Poles location
Dp=roots(dend);
disp('Desired Pole locations:'),Dp

% From root locus and the location of desired closed loop pole, it can be
% found that a lead compensator is needed to shift the current root locus to right.
```
```Open loop Transfer function without controller

Transfer function:
0.0147
--------------------------------------
7.242e-006 s^2 + 0.000207 s + 0.000437

Standard format Open loop Transfer Function without controller

Transfer function:
2030
---------------------
s^2 + 28.58 s + 60.34

Closed loop Transfer Function without controller

Transfer function:
0.0147
-------------------------------------
7.242e-006 s^2 + 0.000207 s + 0.01514

State Space representation:

a =
x1        x2
x1    -27.47  -0.08647
x2     345.1     -1.11

b =
u1
x1  5.882
x2      0

c =
x1  x2
y1   0   1

d =
u1
y1   0

Continuous-time model.
----------> System is Controllable <----------
----------> System is Observable   <----------

figure =

13

Desired Damping ratio is:

Damp =

0.6901

Desired Natural Frequency is:

Wn =

57.9620

Desired Characteristic Equation is:

dend =

1.0e+003 *

0.0010    0.0800    3.3596

Desired Pole locations:

Dp =

-40.0000 +41.9476i
-40.0000 -41.9476i

```

## Designing lead compensator to meet the desired Settling time and Overshoot

------------------------------------------------------------------------%

```z1=16;          % Assuming zero of the first lead compensator

% Finding pole of the first lead compensator
num=num/den(1);
den=den/den(1);
ANS=inv([den(1) -dend(1) 0;den(2) -dend(2) num(1);den(3) -dend(3) num(1)*z1])*[dend(2)-den(2);dend(3)-den(3);0];

disp('Pole of the first lead compensator is:')
p1=ANS(1)
c=ANS(2);
disp('Gain of the first lead compensator is:')
K=ANS(3)

% TF of the first lead compensator G1(s)=K(s+z1)/(s+p1)
disp('Transfer function of the first lead compensator to improve Ts and PO%:')

% DC motor Transfer function with lead compensator 1
disp('DC motor Transfer function with lead compensator')
TF=tf(NUM,DEN)
% Root locus with lead compensator 1
figure
rlocus(TF),grid on
title('Root locus with lead compensator 1')

% Open loop responce of the system with lead compensator 1
figure
step(TF,0:0.1:5),grid on
title('Open loop response with lead compensator 1')

% Closed loop responce of the system with lead compensator 1
[numc,denc]=cloop(NUM,DEN);
figure
step(numc,denc,0:0.1:5),grid on
title('Closed loop response with lead compensator 1 that improves Ts & PO%')

z2=5;     % Assuming zero of the 2nd lead compensator
SSE=0.001;  % Steady State Error design criteria

% Solving for pole of the 2nd lag compensator
disp('pole of the 2nd lead compensator')
numlag2=[1 z2];
denlag2=[1 p2];

disp('The 2nd lag compensator Transfer function to improve SSE:')
tf(numlag2,denlag2)

% DC motor transfer function with lead compensator that improves Ts, PO% & SSE
disp('Open loop TF of the DC motor with final lead compensator (improved Ts, PO% & SSE) ')
tf(NumDC,DenDC)

% Root locus with final lead compensator
figure
rlocus(NumDC,DenDC), grid on
title('Root locus with final lead compensator')

% Closed loop TF of the DC motor with final lead-lag compensator
[NumCLP,DenCLP]=cloop(NumDC,DenDC);
disp('closed loop TF of the DC motor with final lead compensator (improved Ts, PO% & SSE) ')
tf(NumCLP,DenCLP)
figure
step(NumCLP,DenCLP,0:0.1:5), grid on
title('Closed loop response with final lead compensator')

```
```Pole of the first lead compensator is:

p1 =

64.4925

Gain of the first lead compensator is:

K =

1.2326

Transfer function of the first lead compensator to improve Ts and PO%:

Transfer function:
1.233 s + 19.72
---------------
s + 64.49

DC motor Transfer function with lead compensator

Transfer function:
2502 s + 4.003e004
-------------------------------
s^3 + 93.07 s^2 + 1904 s + 3891

figure =

13

figure =

13

figure =

13

pole of the 2nd lead compensator

p2 =

0.0564

The 2nd lag compensator Transfer function to improve SSE:

Transfer function:
s + 5
-----------
s + 0.05643

Transfer function:
1.233 s^2 + 25.88 s + 98.6
--------------------------
s^2 + 64.55 s + 3.64

Open loop TF of the DC motor with final lead compensator (improved Ts, PO% & SSE)

Transfer function:
2502 s^2 + 5.254e004 s + 2.002e005
-------------------------------------------
s^4 + 93.13 s^3 + 1909 s^2 + 3999 s + 219.6

figure =

13

closed loop TF of the DC motor with final lead compensator (improved Ts, PO% & SSE)

Transfer function:
2502 s^2 + 5.254e004 s + 2.002e005
----------------------------------------------------
s^4 + 93.13 s^3 + 4411 s^2 + 5.654e004 s + 2.004e005

figure =

13

```

## --------------------------------PID control------------------------------%

PID control gain, using trial and error

```kp=0.6844;
ki=0.5975;
kd=0.0119;

% PID transfer function
numPID=[kd kp ki];
denPID=[1 0];

% Open loop TF of DC motor with PID controller
num_DC_PID=conv(num,numPID);
den_DC_PID=conv(den,denPID);
disp('Open loop TF of DC motor with PID controller')
tf(num_DC_PID,den_DC_PID)

% Closed loop TF of DC motor with PID controller
[NumPID_CLP,DenPID_CLP]=cloop(num_DC_PID,den_DC_PID);
disp('Closed loop TF of DC motor with PID controller')
tf(NumPID_CLP,DenPID_CLP)
figure
step(NumPID_CLP,DenPID_CLP), grid on
title('Closed loop response of DC with PID Control')
%-------------------------End of PID control------------------------------%
```
```Open loop TF of DC motor with PID controller

Transfer function:
24.15 s^2 + 1389 s + 1213
-------------------------
s^3 + 28.58 s^2 + 60.34 s

Closed loop TF of DC motor with PID controller

Transfer function:
24.15 s^2 + 1389 s + 1213
-------------------------------
s^3 + 52.74 s^2 + 1450 s + 1213

figure =

13

```

## Response Plots....

Open loop response of the DC motor without controller

```figure;
subplot(4,2,1);
step(num,den,0:0.1:1),grid on
title('Open loop response without controller')
% Closed loop response of the DC motor without controller
subplot(4,2,2)
step(numclp,denclp,0:0.1:1),grid on
title('Closed loop response without controller')

% Open loop responce of the DC motor with lead compensator 1
subplot(4,2,3)
step(TF,0:0.1:5),grid on
title('Open loop response with lead compensator 1')
% Closed loop responce of the DC motor with lead compensator 1
subplot(4,2,4)
step(numc,denc,0:0.1:5),grid on
title('Closed loop response with lead compensator 1 that improves Ts & PO%')

% Open loop responce of the DC motor with final lead-lag compensator
subplot(4,2,5)
step(NumDC,DenDC)
title('Open loop response with final lead-lag compensator')
% Closed loop response of the DC motor with final lead-lag compensator
subplot(4,2,6)
step(NumCLP,DenCLP,0:0.1:5), grid on
title('Closed loop response with final lead-lag compensator')

% Open loop responce of the DC motor with PID controller
subplot(4,2,7)
step(num_DC_PID,den_DC_PID)
title('Open loop response with PID controller')

% Closed loop response of the DC motor with PID controller
subplot(4,2,8)
step(NumPID_CLP,DenPID_CLP), grid on
title('Closed loop response with PID Control')
```

## Root Locus Plot...

```% Drawing Root locus
figure
subplot(2,2,1)
rlocus(num,den),grid on
title('Root Locus without controller')

% Root locus with lead compensator 1
subplot(2,2,2)
rlocus(TF),grid on
title('Root locus with lead compensator 1')

% Root locus with final lead-lag compensator
subplot(2,2,3)
rlocus(NumDC,DenDC), grid on
title('Root locus with final lead-lag compensator')

% Root locus with PID
subplot(2,2,4)
rlocus(num_DC_PID,den_DC_PID), grid on
title('Root locus with PID controller')
```
```figure =

13

```

## Bode plots...

```% Bode plot, Determining gain and phase margin
figure
subplot(2,2,1)
margin(numclp,denclp), grid on %
title('Bode plot of closed loop TF without controller')

subplot(2,2,2)
margin(numc,denc), grid on %
title('Bode plot of closed loop TF with lead compensator')

subplot(2,2,3)
margin(NumCLP,DenCLP), grid on %
title('Bode plot of closed loop TF with final lead-lag compensator')

subplot(2,2,4)
margin(NumPID_CLP,DenPID_CLP), grid on %
title('Bode plot of closed loop TF with PID controller')
```
```figure =

13

```

## Simulation result

```[A1,B1,C1,D1]=tf2ss(NumCLP,DenCLP); % For the lead compensator
[A2,B2,C2,D2]=tf2ss(NumPID_CLP,DenPID_CLP); % For the PID
t = 0:0.01:5;
sys_PID=ss(A2,B2,C2,D2);
u1=zeros(1,501);

for i=1:100
u1(1,i)=2;
end
for i=100:200
u1(1,i)=3;
end
for i=200:300
u1(1,i)=2;
end
for i=300:400
u1(1,i)=1;
end
for i=400:501
u1(1,i)=2;
end

figure
subplot(3,1,1)
hold on
lsim(sys_PID,u1,t)

subplot(3,1,2)
[u,t] = gensig('square',3,10,0.1);
lsim(sys_PID,u,t)
hold on

subplot(3,1,3)
% Response to Sine function
[u,t] = gensig('sin',3,10,0.1);
lsim(sys_PID,u,t)
hold on

%------------------ Begin of Fuzzy Logic Controller FIS ------------------%
%========================================================
% Fuzzy logic controller for DC motor Speed control
% ramin.sh@ufl.edu
% Aug.01.2012,
% University of Florida
% Gainesville, Florida
%========================================================

% DC motor constants
R=4.67 ;            %   Terminal Resistance Ohm
L=170e-3;           %   Terminal Inductance H
J=42.6e-6;          %   Motor Inertia Kg.m2
kt=14.7e-3;         %   Torque Constant Nm/Amp

% Transfer Function
num=[kt];
den=[J*L J*R+b*L b*R+ke*kt];
disp('Open loop Transfer function without controller')
TF_DC=tf(num,den)

%----------------------------- Begin of FIS ---------------------------%
f=newfis('DCm'); % New FIS

% Adding variable error (e) as input 1 with range [-2,2]

f = addmf(f,'input',1,'NVB','trapmf',[-2.6 -2 -1.8 -1.5]);
f = addmf(f,'input',1,'NB','trapmf',[-2 -1.6 -1.4 -1]);
f = addmf(f,'input',1,'NM','trapmf',[-1.5 -1.1 -0.9 -0.5]);
f = addmf(f,'input',1,'NS','trapmf',[-1 -0.6 -0.4 0]);
f = addmf(f,'input',1,'Z','trapmf',[-0.5 -0.1 0.1 0.5]);
f = addmf(f,'input',1,'PS','trapmf',[0 0.4 0.6 1]);
f = addmf(f,'input',1,'PM','trapmf',[0.5 0.9 1.1 1.5]);
f = addmf(f,'input',1,'PB','trapmf',[1 1.4 1.6 2]);
f = addmf(f,'input',1,'PVB','trapmf',[1.5 1.9 2.1 2.649]);

figure = figure('Color',[1 1 1]);
subplot(3,1,1,'Parent',figure);
plotmf(f,'input',1);
title('Input variable (e)')
xlabel('e');
hold on

% Adding variable Change of error (de) as input 2 with range [-3,3]

f = addmf(f,'input',2,'NVB','trapmf',[-4 -3 -1.9 -1.5]);
f = addmf(f,'input',2,'NB','trapmf',[-2 -1.6 -1.4 -1]);
f = addmf(f,'input',2,'NM','trapmf',[-1.5 -1.1 -0.9 -0.5]);
f = addmf(f,'input',2,'NS','trapmf',[-1 -0.6 -0.4 0]);
f = addmf(f,'input',2,'Z','trapmf',[-0.5 -0.2 0.2 0.5]);
f = addmf(f,'input',2,'PS','trapmf',[0 0.4 0.6 1]);
f = addmf(f,'input',2,'PM','trapmf',[0.5 0.9 1.1 1.5]);
f = addmf(f,'input',2,'PB','trapmf',[1 1.4 1.6 2]);
f = addmf(f,'input',2,'PVB','trapmf',[1.5 1.9 3 4]);

subplot(312)
plotmf(f,'input',2);
title('Input variable (de)')
xlabel('de');
hold on

% Adding Output variable Control with range [-5,5] volt

f = addmf(f,'output',1,'NVB','trapmf',[-6.667 -5 -3.167 -2.5]);
f = addmf(f,'output',1,'NB','trapmf',[-3.333 -2.667 -2.333 -1.667]);
f = addmf(f,'output',1,'NM','trapmf',[-2.5 -1.833 -1.5 -0.8333]);
f = addmf(f,'output',1,'NS','trapmf',[-1.667 -1 -0.6667 0]);
f = addmf(f,'output',1,'PVB','trapmf',[2.5 3.167 5 6.667]);
f = addmf(f,'output',1,'PB','trapmf',[1.667 2.333 2.667 3.333]);
f = addmf(f,'output',1,'PM','trapmf',[0.8333 1.5 1.833 2.5]);
f = addmf(f,'output',1,'PS','trapmf',[0 0.6667 1 1.667]);

subplot(313)
plotmf(f,'output',1);
title('Output Variable (Control)')
xlabel('Control');
hold off

rulelist=[
1 1 8 1 1
1 2 8 1 1
1 3 8 1 1
1 4 7 1 1
1 5 9 1 1
1 6 9 1 1
1 7 3 1 1
1 8 2 1 1
1 9 2 1 1
2 1 8 1 1
2 2 8 1 1
2 3 7 1 1
2 4 9 1 1
2 5 3 1 1
2 6 3 1 1
2 7 3 1 1
2 8 2 1 1
2 9 2 1 1
3 1 8 1 1
3 2 7 1 1
3 3 9 1 1
3 4 3 1 1
3 5 3 1 1
3 6 2 1 1
3 7 2 1 1
3 8 2 1 1
3 9 6 1 1
4 1 7 1 1
4 2 9 1 1
4 3 9 1 1
4 4 3 1 1
4 5 3 1 1
4 6 2 1 1
4 7 2 1 1
4 8 6 1 1
4 9 6 1 1
5 1 9 1 1
5 2 9 1 1
5 3 3 1 1
5 4 2 1 1
5 5 2 1 1
5 6 2 1 1
5 7 6 1 1
5 8 6 1 1
5 9 5 1 1
6 1 9 1 1
6 2 3 1 1
6 3 3 1 1
6 4 2 1 1
6 5 6 1 1
6 6 6 1 1
6 7 5 1 1
6 8 5 1 1
6 9 4 1 1
7 1 3 1 1
7 2 3 1 1
7 3 2 1 1
7 4 6 1 1
7 5 6 1 1
7 6 5 1 1
7 6 4 1 1
7 8 4 1 1
7 9 4 1 1
8 1 3 1 1
8 2 2 1 1
8 3 2 1 1
8 4 6 1 1
8 5 5 1 1
8 6 5 1 1
8 7 4 1 1
8 8 1 1 1
8 9 1 1 1
9 1 2 1 1
9 2 2 1 1
9 3 6 1 1
9 4 5 1 1
9 5 5 1 1
9 6 4 1 1
9 7 4 1 1
9 8 1 1 1
9 9 1 1 1
];

f.defuzzMethod='mom';
f.type='mamdani';
f.andMethod='min';
f.orMethod='max';
f.impMethod='min';
f.aggMethod='max';

%----------------------------- End of FIS ---------------------------%
```
```figure =

13

Open loop Transfer function without controller

Transfer function:
0.0147
--------------------------------------
7.242e-006 s^2 + 0.000207 s + 0.000437

```
```Index exceeds matrix dimensions.

Error in ==> Mobile_Robot_Simulation at 418
figure = figure('Color',[1 1 1]);
```