MATLAB Examples

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 b=47.3e-6; % Viscous-friction coefficient Nms/rad kt=14.7e-3; %Torque Constant Nm/Amp ke=14.7e-3; %Back emf V-sec/rad % 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% SSE=0.001; % Steady state error<0.1% 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) numlead1=K*[1 z1]; denlead1=[1 p1]; disp('Transfer function of the first lead compensator to improve Ts and PO%:') tf(numlead1,denlead1) % DC motor Transfer function with lead compensator 1 disp('DC motor Transfer function with lead compensator') NUM=conv(numlead1,num); DEN=conv(denlead1,den); 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%') % Improving SSE by adding a second lead compensator 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') p2=(1+((K*z1*num(1)/denlead1(2))/den(3)))*z2*SSE numlag2=[1 z2]; denlag2=[1 p2]; Numlead=conv(numlead1,numlag2); Denlead=conv(denlead1,denlag2); disp('The 2nd lag compensator Transfer function to improve SSE:') tf(numlag2,denlag2) disp('The overal lead compensator transfer function (lead1*lag2):') tf(Numlead,Denlead) % DC motor transfer function with lead compensator that improves Ts, PO% & SSE NumDC=conv(Numlead,num); DenDC=conv(Denlead,den); 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') %--------------------End of lead compensator Design------------------------% ```
```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 The overal lead compensator transfer function (lead1*lag2): 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_lead=ss(A1,B1,C1,D1); 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) lsim(sys_lead,u1,t) 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 lsim(sys_lead,u,t) subplot(3,1,3) % Response to Sine function [u,t] = gensig('sin',3,10,0.1); lsim(sys_PID,u,t) hold on lsim(sys_lead,u,t) %------------------ Begin of Fuzzy Logic Controller FIS ------------------% %======================================================== % Fuzzy logic controller for DC motor Speed control % Author: Ramin Shamshiri © % 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 b=47.3e-6; % Viscous-friction coefficient Nms/rad kt=14.7e-3; % Torque Constant Nm/Amp ke=14.7e-3; % Back emf V-sec/rad % 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=addvar(f,'input','e',[-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=addvar(f,'input','de',[-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=addvar(f,'output','control',[-5,5]); 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,'Z','trimf',[-0.8333 0 0.8333]); 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 = addrule(f,rulelist); 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]); ```