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; %deltat

dposicao = [0 0 0]'; %deltaS

soma_alfa = 0;

sub_alfa = 0;

delta1 = 50; %em relacao ao ro

%Parametros PID

KP = 1;

KI = 0;

KD = 0;

Xr = [P.x P.y P.th]'; %Posicao inicial do robo

Xg = [4300 3200 0]'; %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

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]'; %deltaS

dt = 0.1; %deltat

time = 0;

e = 0; %Erro

a = 0;

soma_alfa = 0;

sub_alfa = 0;

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)

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]; %Grupo de parametros inicial

dk = [1 1 1]; %Vetor de variacao diferencial

best_erro = inf; %Melhor erro

threshold = 2; %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);