Camila de Oliveira Cunha - 201965120AI

Giovanni Tomasco Andrade - 201869015B

Robótica Móvel - ENE122

Trabalho Para Casa - TPC 3

Questão 1

Tendo em vista satisfazer as tarefas definidas para o trabalho, as Rotinas foram implementadas em MATLAB
e desenvolvidas de modo a obter um cenário satisfatório a partir de um ajuste inteligente dos valores do
controlador PID.

0.1 Simulação

Dois cenários foram simulados: sem permissão de locomoção em ambos os sentidos e com permissão
de locomoção em ambos os sentidos. Para o primeiro cenário, depois de 864 iterações, os valores ob-
tidos são: Kp = 2, 5922, Ki = 1, 5152 s−1 , Kd = −0, 9909 s, Tempo Total de Simulação = 66, 30 s e
Erro Médio Final = 33, 980 mm. E o resultado da simulação e o erro estão nas imagens.

Figura 1: Animação resultante com restrição de locomoção em dois sentidos

Figura 2: Erro x Tempo(Erro medio final: 33.980 mm)

Para o segundo cenário, depois de 848 iterações, os valores obtidos são: Kp = 18, 9575, Ki = −0, 0076 s−1 ,
Kd = 0, 3872 s, Tempo Total de Simulação = 60, 40 s e Erro Médio Final = 7, 310 mm. E o resultado da
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

simulação e o erro estão nas imagens.

Figura 3: Animação resultante sem restrição de locomoção em dois sentidos

Figura 4: Erro x Tempo(Erro medio final: 7.310 mm)

Códigos para o trabalho

ajustaAngulo.m

1 function ajusta_angulo = ajustaAngulo(angulo)

2 ajusta_angulo = mod(angulo, 2*pi);
3 if(ajusta_angulo>pi)
4 ajusta_angulo = ajusta_angulo−2*pi;
5 end

Rz2D.m

1 % Rotacao :
2 function M2 = Rz2D(M1,th)
3 Rz = [cos(th) −sin(th) 0
4 sin(th) cos(th) 0
5 0 0 1 ];
6 M2 = Rz*M1;
7 end
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

T2D.m

1 % Translacao :
2 function M2 = T2D(M1,DeltaX,DeltaY)
3 T = [1 0 DeltaX
4 0 1 DeltaY
5 0 0 1 ];
6 M2 = T*M1;
7 end

Simulacao.m

1 function[delErro,tAmostragem] = Simulacao (K, dinamica, plotSimu)

2
3 %Parametros do controlador PID
4 Kp = K(1);
5 Ki = K(2);
6 Kd = K(3);
7
8 Pini = [0 0 deg2rad(0)];%Posicao inicial do robo
9 P = Pini;%Posicao atual do robo
10
11 %Pontos Objetivos
12 caminho = [ 4 −4 −3 2 ; % x
13 3 −3 2 −2]; % y
14
15 Obj=[]; %Posicao objetivo do robo
16
17 tt = 0; %Tempo total da simulacao[s]
18 aux = 0; %Variavel auxiliar para plot
19 dt = 0.1; %Tempo de amostragem[s]
20 delDist = 0.03; %Distancia do objetivo [m]
21 Erro = []; %Erro da posicao do robo ate o caminho otimo
22
23 for j=1:length(caminho)
24 Pini = P;
25 Obj = [caminho(1,j) caminho(2,j)];
26 %Coeficientes da reta:
27 a = Obj(2)−Pini(2);
28 b = Pini (1)−Obj(1);
29 c = Obj(1)*Pini(2)−Pini(1)*Obj(2);
30
31 vmax = 0.5; %Velocidade linear maxima [m/s]
32 wmax = deg2rad(180); %Velocidade angular maxima[rad/s]
33 intAlpha = 0; %Integral do erro do controlador PID (alpha)
34 difAlpha = 0; %Derivada do erro do controlador PID (alpha)
35 alphaAntigo = 0; %Variavel para o calculo do termo diferencial do PID (alpha_{ t 1 })
36
37 t = 0; %Tempo atual da simulacao do trecho[s]
38 tmax = 10*sqrt((Obj(1)−Pini(1))^2 + (Obj(2) − Pini (2))^2)/vmax; %Tempo maximo de ...
sim ula cao [ s ] : 10x o tempo do caminho otimo com vmax
39
40 %Erros realimentados
41 delX = Obj(1) − P(1);
42 delY = Obj(2) − P(2);
43
44 rho = sqrt(delX^2 + delY^2); %Di s t a n ci a
45 gamma = ajustaAngulo(atan2(delY,delX)); %Angulo da p o si c a o do robo ao o bj e ...
t i v o
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

46 alpha = ajustaAngulo(gamma − P(3)); %angulo e n t r e a f r e n t e do robo e o o ...

bj e t i v o
47
48 while(rho>delDist) && (t≤ tmax)
49 %Inc remen ta o tempo
50 t = t + dt;
51 tt = tt + dt;
52
53 %E r ro s r ealim e n ta do s
54 delX = Obj(1) − P(1);
55 delY = Obj(2) − P(2);
56
57 %Di s t a n ci a
58 rho = sqrt(delX^2 + delY^2);
59 %Angulo da posicao do robo ao objetivo
60 gamma = ajustaAngulo(atan2(delY,delX));
61 %angulo entre a frente do robo e o objetivo
62 alpha =ajustaAngulo(gamma − P(3));
63 %Atualizacao dos paramtros
64 difAlpha = alpha − alphaAntigo;
65 alphaAntigo = alpha;
66 intAlpha = intAlpha + alpha;
67
68 %Lei de controle: velocidade linear
69 v = min(rho,vmax);
70
71 if isequal (dinamica,1)
72 if ((alpha>pi/2) || (alpha<−pi/2))
73 v = −v;
74 alpha = ajustaAngulo(alpha+pi);
75 end
76 end
77
78 %Velocida de angular
79 w = Kp*alpha + Ki*intAlpha + Kd*difAlpha;
80 %Lei de controle: velocidade angular
81 w = sign(w)*min(abs(w),wmax);
82
83 %Atualizacao posicao do robo
84 delS = v*dt;
85 delTheta = w*dt;
86 delP = [delS*cos(P(3)+delTheta), delS*sin(P(3)+delTheta),delTheta];
87 pAnterior = P;
88 P = P + delP;
89 P(3) = ajustaAngulo(P(3));
90
91 %Historico de erros(distancia do ponto a reta)
92 Erro = [Erro; abs(a*P(1) + b*P(2) + c)/sqrt(a^2+b^2)];
93
94 if isequal(plotSimu,1)
95 %Dimensoes do robo
96 Corpo = [ 100.0, 227.5, 227.5, 100.0, −200.0, −227.5, −227.5, −200.0; ...
−190.5, −50.0, 50.0, 190.5, 190.5, 163.0, −163.0, −190.5]/500;
97 Corpo = [ Corpo ; [ 1 1 1 1 1 1 1 1 ]];
98 RodaE = [ 97.5, 97.5, −97.5, −97.5; 170.5, 210.5, 210.5, 170.5]/500;
99 RodaE = [RodaE; [ 1 1 1 1 ]];
100 RodaD = [ 97.5, 97.5, −97.5, −97.5;−170.5 , −210.5 , −210.5 , −170.5]/500;
101 RodaD = [ RodaD; [ 1 1 1 1 ]];
102 l = 1/2*(381/500);
103 corCarro = 'y';
104 corMarcador = 'og';
105 corCaminho = 'g';
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

106 corPontoObjetivo ='ob';

107 if(aux==1)
108 delete([cp,re,rd,ed,ee,m]);
109 end
110
111 % Corpo
112 RoboC = T2D(Rz2D(Corpo,P(3)),P(1),P(2));
113 % Roda Esquerda
114 RoboE = T2D(Rz2D(RodaE,P(3)),P(1),P(2));
115 % Roda Direita
116 RoboD = T2D(Rz2D(RodaD,P(3)),P(1),P(2));
117 %Ponto objetivo
118 pOb = plot(Obj(1),Obj(2), corPontoObjetivo, 'linewidth',2, 'markersize',5);
119 %Plot
120 cp = fill(RoboC(1,:),RoboC(2,:),corCarro); % co rpo
121 re = fill(RoboE(1,:),RoboE(2,:),corCarro); % roda esquerda
122 rd = fill(RoboD(1,:),RoboD(2,:),corCarro);% rodad i r e i t a
123 %Eixo das rodas
124 ed = plot([P(1) P(1)+l*cos(P(3)+pi/2)],[P(2) ...
P(2)+l*sin(P(3)+pi/2)],'k','linewidth',2);
125 ee = plot([P(1) P(1)+l*cos(P(3)−pi/2)],[P(2) ...
P(2)+l*sin(P(3)−pi/2)],'k','linewidth',2);
126 % Posicao atual do robo :
127 m = plot(P(1),P(2),corMarcador,'linewidth',2,'markersize',5);
128 %Rastro do caminho percorrido
129 r= plot([pAnterior(1) P(1)],[pAnterior(2) P(2)],corCaminho, 'linewidth' ,2);
130 title(sprintf( 'tTrecho = %.2f s, tTotal =%.2f s ,v= %.2f m/s,rho=%.2f m, ...
Erro=%.3f mm ', t,tt,v,rho,Erro(end)*10^3));
131
132 if (aux==0)
133 aux = 1;
134 end
135 drawnow;
136 end
137 end
138 end
139
140 if isequal(plotSimu,1)
141 %Lista de erros ao longo da simulacao e tempo de amostragem
142 delErro = Erro;
143 %Tempo de amostragem
144 tAmostragem = dt;
145 else
146 %Desempenho do algo ritmo nesta iteracao: soma dos erros registrados
147 delErro = sum(Erro);
148 end
149 end

Twiddle.m

1 clc ; clear all; close all;

2
3 %Permissao para movimento em ambos os sentidos
4 permissao=0;
5
6 % Parametros Kp, Ki e Kd
7 K = [ 0 0 0 ];
8 dK = [ 1 1 1 ];
9 ksi = 1/100;
10
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

11 % Limiar de finalizacao
12 ∆ = 0.001;
13
14 %Obtem o p rim ei r o e r r o
15 Ebest = Simulacao(K, permissao, 0);
16
17 %Historico de erros ao longo da simulacao principal
18 historicoErros= [];
19
20 %% LOOP DE OTIMIZACAO
21 k = 0;
22 while sum(dK)>∆
23 k = k+1; %Contador de iteracoes
24 for i = 1:length(K)
25 K(i) = K(i)+dK(i); %Procura pelos melhores parametros
26 Erro = Simulacao (K , permissao, 0); %Calcula o erro
27 if Erro < Ebest
28 Ebest = Erro;
29 dK(i) = dK(i) * ( 1 + ksi);
30 else
31 K(i) = K(i) − 2 * dK(i); %Procura pelos melhores parametros na direcao ...
contrario
32 Erro = Simulacao (K , permissao, 0); %Calcula o erro
33 if Erro < Ebest
34 Ebest = Erro ;
35 dK(i) = dK(i) * ( 1 + ksi);
36 else
37 K(i)=K(i)+dK(i); %Caso nenhum dos sentidos funcionar
38 dK(i) = dK(i)*(1−ksi);
39 end
40 end
41 end
42 fprintf('Iteracao %i : Melhor Erro = %.5f m, soma(dK) = %.5f \n ',k,Ebest,sum(dK))
43 end
44 fprintf( 'Parametros PID: P = %.4f, I = %.4f, D = %.4f\n',K(1),K(2),K(3))
45
46
47 %% PLOT DA Simulacao
48 figure(1)
49 hold on ; grid on;
50 xlabel('X [m]');
51 ylabel ('Y [m]');
52 axis ([ −5 5 −5 5])
53
54 [historicoErros,dt]=Simulacao(K,permissao,1);
55
56
57 %% GRAFICO DE ERRO x TEMPO
58 %Conversao para milimetros[mm]
59 historicoErros= historicoErros*10^3;
60 %Erro medio final [mm]
61 erroMedioFinal= mean(historicoErros);
62 %Calculo do tempo total de simulacao
63 tempoTotal = length(historicoErros)*dt;
64 %Tempo discretizado
65 tempo = dt:dt:tempoTotal;
66 fprintf( 'Tempo total de Simulacao: %.2f s , Erro medio final: %.3f mm. \n', ...
tempoTotal, erroMedioFinal);
67 figure(2)
68 grid on; hold on;
69 %Ajuste de limite dos eixos
70 axis([0 tempoTotal 0 max(historicoErros)*1.1]);
 Camila de Oliveira Cunha - 201965120AI
Giovanni Tomasco Andrade - 201869015B
Robótica Móvel - ENE122

71
72 %ajuste de legenda dos eixos e titulo
73 ylabel('Erro [mm]') ; xlabel( 'Tempo [ s ]') ; title(sprintf('Erro x Tempo(Erro medio ...
final: %.3f mm)' , erroMedioFinal))
74 plot(tempo, historicoErros, 'k ', 'linewidth',1);