ROBTICA PROBABILSTICA

RELATRIO AULA 5

1. INTRODUO
Um sistema pode ser definido como um dispositivo abstrato que recebe
entradas e produz sadas como resposta a essas entradas. Em sistemas de
controle em malha aberta, o sinal de sada no exerce ao no sinal do controle,
portanto no h comparao ou realimentao com a sada de referncia e se
houver uma perturbao, o objetivo desejado no ser atingido.

J em sistemas de controle em malha fechada, as sadas e entradas so

comparadas de forma a produzir um sinal de erro, que quanto mais prximo de
zero melhor ser o resultado do objetivo desejado. Portanto, o sinal de sada
possui um efeito direto na ao do controle.

Esse relatrio tem por objetivo abordar, de forma resumida, o

desenvolvimento de um algoritmo PID (Proporcional-Integral-Derivativo) de
controle em malha fechada.

2. CONTROLADOR
Antes da implementao do controlador PID, foi feita a implementao de
um controlador P (Proporcional) clssico que dado pela figura abaixo, onde o
bloco r o objetivo a ser cumprido pelo sistema, o bloco e o erro entre a sada
da planta e a referncia (no nosso caso a diferena entre a posio atual e a
posio objetivo do rob).

O bloco Planta o prprio sistema que se quer controlar. No caso, o

prprio rob, dado pelo seu modelo cinemtico e O bloco u, que dado pelas
velocidades linear e angular, o sinal de controle que movimenta os estados da

Planta. O bloco KP prprio controlador proporcional que uma matriz com 6

parmetros. Encontrar tantos parmetros muito trabalhoso.
Para diminuir esse esforo, optou-se por representar o rob em
Coordenadas Polares.

O modelo cinemtico do rob, no novo sistema de coordenadas polares,

representado por:

O algoritmo implementado para o controlador P encontra-se em anexo.

A atividade realizada (controlador PID) levou em considerao a
modelagem do controle Proporcional, desconsiderando o ngulo de chegada do
rob ao objetivo () e considerando apenas o alinhamento do rob com o ponto
objetivo (). Foi utilizada a seguinte lei de controle:

Para sintonizar os parmetros (Kp, Ki e Kd), utilizou-se o Twiddle, que

um algoritmo criado pelo Professor Sebastian Thrun da Universidade de
Stanford. Esse mtodo adapta os parmetros do controlador minimizando o erro
mdio produzido pela planta num determinado objetivo.
3. DESENVOLVIMENTO
Foram implementados trs algoritmos: controlador PID, clculo do erro e
twiddle. Nesse tpico, eles sero apresentados e comentados de forma
resumida.

3.1. CONTROLADOR PID

Esse algoritmo foi baseado no controlador proporcional, porm, como dito
no captulo anterior, desconsiderou-se o ngulo de chegada do rob ao objetivo
().
%% CONTROLADOR PID
clear all
close all
clc
handler = MoveService('','http://127.0.0.1:4950/');
P.x = 0;
P.y = 0;
P.th = 0;
setPosition(handler,P);
%Variaveis auxiliares
T1 = [];
T2 = [];
dt=0;
dposicao
= [0 0 0]';
soma_alfa
= 0;
sub_alfa
= 0;

%deltat
%deltaS

delta1 = 50; %em relacao ao ro

%Parametros PID
KP = 1;
KI = 0;
KD = 0;
Xr = [P.x P.y P.th]';
Xg = [4300 3200 0]';

%Posicao inicial do robo

%Posicao objetivo

dx = Xg(1,1) - Xr(1,1);
dy = Xg(2,1) - Xr(2,1);
dth = Xg(3,1) - Xr(3,1);

As variveis ro, gama e alfa foram implementadas de acordo com as

equaes abaixo:

ro = sqrt(dx^2 + dy^2);
gama = rad2deg(atan2(dy,dx));
alfa = gama - Xr(3,1);

tic
while (abs(ro)>delta1)
posicao = getPosition(handler);
dx = Xg(1,1) - posicao.x;
dy = Xg(2,1) - posicao.y;
dth = Xg(3,1) - posicao.th;
alfa_ant = alfa;
ro = sqrt(dx^2 + dy^2);
gama = rad2deg(atan2(dy,dx));
alfa = gama - posicao.th;

A funo ajusta ngulo serve para manter o ngulo sempre no intervalo

(-180,180]. Ela foi implementada da seguinte maneira:

%Ajusta angulo alfa

alfa = mod(alfa,360);
if (alfa > 180)
alfa = alfa - (360);
end

A condio abaixo feita para que o rob no precise manobrar para se

orientar em direo ao ponto objetivo. Portanto, se pertencer ao intervalo I2,
os ngulos sero alterados, conforme as figuras abaixo:

if (((alfa > -180) && (alfa < -90)) || ((alfa > 90) && (alfa <= 180)))
ro = -ro;
alfa = alfa + 180;
%Ajusta angulo alfa
alfa = mod(alfa,360);

if (alfa > 180)

alfa = alfa - (360);
end
end
soma_alfa = soma_alfa + alfa; %Somatorio
sub_alfa = alfa - alfa_ant;
%alfa(t) - alfa(t-1)
%Definicao - frente ou tras
if (ro>0)
v = 100; %velocidade em mm/s
else
v = -100;
end
w = KP*alfa + KI*(soma_alfa) + KD*(sub_alfa); %velocidade angular
%Limitador de velocidade angular
if w > 40
w = 40;
end
if w < -40
w = -40;
end
setVel(handler,v);
setRotVel(handler,w);
%Modelo cinematico incremental
dt = toc;
tic
dposicao = [v*dt*cosd(Xr(3,1)+((w*dt)/2)); v*dt*sind(Xr(3,1)+((w*dt)/2)); w*dt];
Xr = Xr+dposicao;
%Grafico
T1 = [T1,[posicao.x; posicao.y]];
plot(T1(1,:),T1(2,:),'b*'); %Plota
xlabel('Eixo X');
ylabel('Eixo Y');
end
stop(handler);

3.1. FUNO CALCULA ERRO

Esse algoritmo uma funo que tem por objetivo achar o erro, que no
caso a distncia em que o rob est de uma reta (menor caminho entre dois
pontos), e jogar esse erro no Twiddle, para achar os melhores valores de
parmetros.
Nesse caso, o ponto usado foi x=1000 e y=1000, conforme a figura abaixo,
mas poderia ser qualquer outro ponto.

Esta funo baseada no algoritmo anterior, porm, nela o simulador no

foi utilizado e sim o modelo cinemtico. A equao abaixo foi utilizada para obter
o erro.

%% FUNCAO CALCULA ERRO

function e = calcula_erro(k);
Xr = [0 0 0]';
Xg = [1000 1000 0]';
%Variaveis auxiliares
dposicao = [0 0 0]';
dt = 0.1;
time = 0;
e = 0;
a = 0;
soma_alfa = 0;
sub_alfa = 0;

%deltaS
%deltat
%Erro

delta1 = 50; %em relacao ao ro

%Parametros PID
KP = k(1);
KI = k(2);
KD = k(3);
dx = Xg(1,1) - Xr(1,1);
dy = Xg(2,1) - Xr(2,1);
ro = sqrt(dx^2 + dy^2);
gama = rad2deg(atan2(dy,dx));
alfa = gama - Xr(3,1);
tic

while (abs(ro)>delta1) && (a==0)

dx = Xg(1,1) - Xr(1,1);
dy = Xg(2,1) - Xr(2,1);
dth = Xg(3,1) - Xr(3,1);
alfa_ant = alfa;
ro = sqrt(dx^2 + dy^2);
gama = rad2deg(atan2(dy,dx));
alfa = gama - Xr(3,1);
%Ajusta angulo alfa
alfa = mod(alfa,360);
if (alfa > 180)
alfa = alfa - 360;
end
if (((alfa > -180) && (alfa < -90)) || ((alfa > 90) && (alfa <= 180)))
ro = -ro;
alfa = alfa + 180;
%Ajusta angulo alfa
alfa = mod(alfa,360);
if (alfa > 180)
alfa = alfa - 360;
end
end
%Definicao - frente ou tras
if (ro>0)
v = 100; %velocidade em mm/s
else
v = -100;
end
soma_alfa = soma_alfa + alfa; %Somatorio
sub_alfa = alfa - alfa_ant;
%alfa(t) - alfa(t-1)
w = KP*alfa + KI*(soma_alfa) + KD*(sub_alfa);
%Limitador de velocidade angular
if w > 40
w = 40;
end
if w < -40
w = -40;
end
%Modelo cinematico incremental
dt=toc;
tic
dposicao = [v*dt*cosd(Xr(3,1)+((w*dt)/2)); v*dt*sind(Xr(3,1)+((w*dt)/2)); w*dt];

Xr = Xr+dposicao;
%Coeficientes da equacao geral da reta
aa = Xr(2,1) - Xg(2,1);
bb = Xg(1,1) - Xr(1,1);
cc = Xr(1,1)*Xg(2,1) -Xr(2,1)*Xg(1,1);
%Distancia do ponto a reta
d = (abs(aa*Xr(1,1) + bb*Xr(2,1) + cc))/(sqrt(aa^2 + bb^2));
e = e + d;
time = time + dt;
%Condicao de saida do loop
if time > 50
a = 1;
end
end

3.2. FUNO TWIDDLE

Essa funo tem por objetivo sintonizar o controlador. Foi utilizado =1%.
%% TWIDDLE
clear all
close all
clc
k = [0 0 0];
dk = [1 1 1];
best_erro = inf;
threshold = 2;

%Grupo de parametros inicial

%Vetor de variacao diferencial
%Melhor erro
%Limiar a ser ajustado

while (sum(dk)>threshold)
for i=1:3
k(i) = k(i) + dk(i);
e = calcula_erro(k)
if e < best_erro
best_erro = e;
dk(i) = dk(i)*1.01;
else
k(i) = k(i) - 2*dk(i);
e = calcula_erro(k)
if e < best_erro
best_erro = e;
dk(i) = dk(i)*1.01;
else
k(i) = k(i) + dk(i);
dk(i) = dk(i)*0.99;

end
end
end
end

Os parmetros (K1, K2 e K3) encontrados nesse algoritmo (figura abaixo)

foram usados no primeiro algoritmo (controlador PID).

4. RESULTADOS

PID usando o simulador (x=4300 ; y=3200)

PID usando o simulador (x=-1300 ; y=1200)

ANEXO
%% Controlador P
clear all
close all
clc
handler = MoveService('','http://127.0.0.1:4950/');
P.x = 0;
P.y = 0;
P.th = 0;
setPosition(handler,P);
.
.
.
end
stop(handler);