Академический Документы
Профессиональный Документы
Культура Документы
Salvador
2010
Alexandre Costa Mota
Salvador
2010
Alexandre Costa Mota
Página
1 INTRODUÇÃO ............................................................................................... 14
7 VARIÁVEIS .................................................................................................... 47
8 METODOLOGIA ............................................................................................. 48
10 CONCLUSÃO ................................................................................................. 68
REFERÊNCIAS......................................................................................................... 69
Página
Página
DC Direct current
EEPROM Electrically-Erasable Programmable Read-Only Memory
GLSCENE Biblioteca 3D em OpenGL para Delphi
IEEE Institute of Electrical and Electronics Engineers
ISP In-System Programming
ODE Open Dynamics Engine
PDIP Plastic Dual In-line Package
PID Proportional–integral–derivative
PWM Pulse-width modulation
RAM Random Access Memory
RF Rádio freqüência
RISC Reduced Instruction Set Computer
ROM Read Only Memory
RS-232 Padrão para troca serial de dados binários
SFR Special Function Registers
SYNEDIT Editor de controle avançado multi-linha
UART Universal asynchronous receiver/transmitter
USART Universal Synchronous Asynchronous Receiver Transmitter
USB Universal Serial Bus
XML Extensible Markup Language
LISTA DE SÍMBOLOS
A maioria dos robôs não holonômicos não podem andar em uma direção perpendicular
às suas rodas motrizes, como por exemplo: um robô de tração diferencial pode andar para frente,
para trás, em uma curva, ou girar ao redor de algo, mas não pode andar de lado, porem os robôs
omni-direcionais são capazes de se movimentar em qualquer direção em um plano. Esse documento
busca propor um sistema de controle de movimento embarcado para um robô holonômico envolvendo
velocidades e trajetos simples como andar em linha, em circulo ou ir até um ponto-alvo sem
restrições. Esse documento também descreve o sistema eletrônico embarcado e os próprios
controles de velocidades e trajetória discretos desse sistema. Para validar os controladores são
realizados simulações num software de cálculos matemáticos e um software de simulação de robôs
móveis. No final do trabalho o robô tem a capacidade de descrever trajetórias de ações básicas como
andar em linha ou em círculo com velocidades controladas.
Palavras-Chave: Controle, trajetória, PID, sistema, autônomo, robótica, móvel, robôs holonômicos.
ABSTRACT
Most nonholonomic robots cannot move in a perpendicular direction to their drive wheels,
for example, a differential drive robot can move forward, backward, on a curve or turn around
something, but cannot move aside, but the omni-directional robots are able to move in any direction in
a plane. This paper aims to propose a embedded motion control system for a holonomic robot
involving speeds and simple paths as walking in line, in circle or go to a target without restrictions.
This document also describes the embedded electronics system and the discrete controls of speed
and trajectory of this robot. To validate the controllers are made simulations in mathematical software
and a simulation software for mobile robots. At the end of the work the robot has the ability to describe
trajectories of basic actions like move in line or in circle with controlled speeds.
Key-Words: Control, trajectory, PID, autonomous, system, mobile, robotic, holonomic robots.
14
1 INTRODUÇÃO
1.3 HIPÓTESE
1.4 OBJETIVOS
1.5 JUSTIFICATIVA
1.6 METODOLOGIA
2 A ROBÓTICA MÓVEL
Number of
Arrangement Description
Wheels
One steering wheel in the front, one traction wheel in the rear
02
Two-wheel differential drive with the center of mass (COM)
below the axle
connected wheels
robô com pelo menos três rodas deste tipo pode se deslocar em qualquer direção,
independentemente da sua orientação (FRANCO, 2007, p. 12).
2.3 LOCALIZAÇÃO
apenas os modelos de postura são necessários (LAGES, 1998, p. 10) por esse
motivo são esses os modelos utilizados nesse documento.
Os próximos tópicos abordam as equações de cinemática e dinâmica
para robôs omni-direcionais de três rodas.
A figura abaixo serve como base para a maioria das fórmulas presentes
posteriormente abordando características vetoriais presentes num robô omni-
direcional de três rodas (ver lista de símbolos).
(3)
A partir das velocidades lineares vx e vy é possível calcular as velocidades
lineares v e vn, pela equação abaixo (OLIVEIRA, 2007, p. 40):
cos
sin
0
sin
cos
0 ·
0 0 1
(4)
A partir da cinemática inversa é possível determinar as velocidades vx e
vy, tal como pode ser observado na seguinte equação (OLIVEIRA, 2007, p. 40):
cos
sin
0
sin
cos
0 ·
0 0 1
(5)
A relação entre as velocidades das rodas v0, v1 e v2, com as velocidades
do robô v, vn e ω é descrita pela equação abaixo (OLIVEIRA, 2007, p. 40):
# #
!sin " 3 % cos " 3 % & )
(
0 1 & ( .
# # (
sin " 3 % cos " 3 % & '
(6)
Sendo R a distancia do centro da roda j=[0,1,2] até sua extremidade, se
tem:
+ + · ,
(7)
Então, a equação de cinemática inversa do robô omni-direcional de três
rodas sendo 60º o ângulo de inclinação das rodas é dada por:
!√3 1 & )
1 2 2 (
0 (
1 & ·
, (
√3 1 (
2 &'
2
(8)
Segundo Oliveira (2007, p. 41), se pode obter a equação de cinemática
direta a partir da equação de cinemática inversa, resultando em:
32
1 1
! # 0 # )
2 · sin " % 2 · sin " % (
3 3
1 1 1 (
# # # (
2 · cos " % / 1 cos " % / 1 2 · cos " % / 1 ( ·
3 3 3
π (
1 cos " % 1 (
3
# # # (
2 · · cos " % / 1 · cos " % / 1 2 · · cos " % / 1'
3 3 3
(9)
Substituindo a equação (7) na equação (9), se obtêm:
!√3 0
√3 )
3 3 (
1 2 1 (
, ·
3 3 3 ((
1 1 1 (
3. & 3. & 3. & '
(10)
Os controladores de trajetória utilizam o sistema de coordenadas global,
ou seja, um sistema de coordenadas do meio em que o robô se encontra. Para se
localizar, o robô precisa saber a relação de sua posição atual com o ponto de
referência no sistema global, por isso, utilizam-se as equações (4) e (5) para esse
fim.
As equações (8) e (10) relacionam as velocidades angular das rodas nos
seus respectivos sistemas de coordenadas com as velocidades do robô no sistema
de coordenadas global, representado pela figura (4). Essas relações são importantes
para o controle de posicionamento do robô visto que a odometria é o principal meio
de obtenção de re-alimentação, obtendo valores relativos discretos de posição das
rodas.
1 2 34 354 364
(11)
1 2 34 354 364
(12)
7 2 8 859 869
(13)
As equações acima sugerem que a aceleração do robô segundo as
direções v, vn e o seu eixo de rotação é igual ao somatório das forças do robô,
subtraída das forças de atrito aplicadas nessas mesmas direções ou eixo de
rotação. As forças de atrito presentes no robô são, neste caso, designadas de atrito
viscoso ou dinâmico e atrito de Coulomb ou estático. Os atritos viscosos (FBv, FBvn,
TBω), apresentam-se como uma relação linear entre a força aplicada e a velocidade,
enquanto que os atritos de Coulomb (FCv, FCvn, TCω), apresentam uma amplitude
constante e sinal de modo a contrariar o movimento (OLIVEIRA, 2007, p. 44).
As equações para as forças de atrito viscoso são:
354 :4 ·
(14)
354 :4 · ;
(15)
359 :9 ·
(16)
As equações para as forças de atrito de Coulomb são:
364 <4 · =>?;
(17)
364 <4 · =>?;
(18)
869 <9 · =>?;
(19)
Atrito viscoso é uma força que surge quando um determinado corpo
apresenta movimento relativo, opondo-se a esse movimento (OLIVEIRA, 2007, p.
45). E atrito de Coulomb é presente quando o corpo se encontra na iminência do
movimento, ou seja, no princípio da atuação das forças aplicadas. Contudo a
34
explicação mais simples diz que é a força que o corpo terá de fazer para vencer o
seu estado de repouso (OLIVEIRA, 2007, p. 46).
Sendo 60º a inclinação das rodas, as relações entre as forças de tração e
torque do robô com as forças de tração nas rodas são descritas pelas seguintes
equações (OLIVEIRA, 2007, p. 47):
#
2 34 @ @ · sin " %
3
(20)
#
2 3 @ / @ / @ · cos " %
3
(21)
2 8 @ / @ / @ ·
(22)
A força de tração em cada roda é calculada através do torque nas rodas
j=[0,1,2] que, por sua vez, é determinado usando a corrente consumida pelo motor,
tal como é descrito nas seguintes equações (OLIVEIRA, 2007, p. 47):
8+
@+
,
(23)
8+ A · BC · >+
(24)
Como mostrado, a partir das equações (23) e (24) calcula-se a força
aplicada em cada roda a partir de uma entrada de corrente elétrica, essas forças são
inseridas nas equações (20), (21) e (22) para se obter as forças geradas pelo robô
no sistema de coordenadas local. Com as equações (14) a (19) calcula-se as forças
de atrito estático e dinâmico nas rodas. Por fim, as equações de força são inseridas
na equação de dinâmica geral relacionado forças e velocidades utilizadas pelos
controladores.
35
disco. A posição do eixo pode ser detectada por contagem de pulsos e a velocidade
pelo incremento desses pulsos variando no tempo (BATES, 2004, p. 214).
O sinal de feedback pode ser inserido em um microcontrolador que
monitora essa entrada pulsante e varia sua saída para controlar a velocidade e/ou
posição do motor (BATES, 2004, p. 214). A figura abaixo mostra esse sistema em
diagrama de blocos:
Specification XBee-Pro
Performance
Indoor/Urban Range Up to 300 ft. (90 m), up to 200 ft (60 m) International variant
Outdoor RF line-of-sight Range Up to 1 mile (1600 m), up to 2500 ft (750 m) international variant
Transmit Power Output 63mW (18dBm)* 10mW (10 dBm) for International variant
41
posição angular de cada eixo j = [0,1,2] no tempo discreto como mostra as equações
abaixo:
FG
,DE
2·#
(25)
FG ; FG ; 1
∆
+ ;
,DE
(26)
Assumindo que em um intervalo de tempo ta de amostra entre os
instantes n-1 e n pode-se obter a variação da posição e orientação do robô num
instante n tornando discreta a equação de cinemática direta obtendo a equação
abaixo:
!√3 0
√3 )
I ; 3 3 ( ∆
;
1 2 1 (
; , · · ∆
;
∆
; 3 3 3 (( ∆
;
1 1 1 (
3. & 3. & 3. & '
(27)
A figura abaixo mostra a contribuição das velocidades v e ω para vn = 0
sendo que o robô descreve um movimento curvilíneo:
(33)
O primeiro termo é o termo proporcional, em que Kp é a constante de
proporcionalidade, o segundo termo é o termo integrador onde Ki é a constante de
integração e o terceiro termo é o derivativo em que Kd é a constante de derivação.
Para o controle digital pode-se tornar todos os termos discretos obtendo a
equação abaixo (MICROCHIP TECHNOLOGY INC., 2004b, p. 3):
Q N; N; 1
<; B · N; / · 2 N; / 8P ·
8I Q
(34)
A sintonia de um controlador PID baseia-se em determinar as constantes
de proporcionalidade, integração e derivação. Para tal existem métodos
experimentais diversificados conforma o modelo do sistema. A seguir é descrito o
método Ziegler-Nichols de Malha Fechada.
Controlador K Ti Td
0,5 · B&T ∞
1
P 0
0,45 · B&T ·W
1,2 &T
PI 0
PID 0,6 · B&T 0,5 · W&T 0,125 · W&T
Fonte – Nunes e Lopes, 2001, p. 2.
47
7 VARIÁVEIS
8 METODOLOGIA
Robô
Raio 0,09 m
Altura 0,125 m
Massa 1,59 Kg
Encoder
Resolução 1000 pulsos/revolução
Média de Ruído 0
Desvio padrão do Ruído 0
Rodas
Raio 0,04 m
Largura 0,025 m
Massa 0,377 Kg
54
(35)
Encontrado o erro de velocidade, calcula-se a saída do controlador
conforme a equação (34). As constantes K, Ti e Td são obtidas experimentalmente
com a ajuda do método de sintonização Ziegler-Nichols mostrado no Capítulo 6.
Para saber o valor da velocidade atual de cada eixo utiliza-se a equação (7), sendo
que a velocidade angular instantânea das rodas j=[0,1,2] é calculada conforme a
equação (3), em que sua forma discreta no tempo é mostrada na equação abaixo:
56
∆
+ ;
+ ;
8Q
(36)
Para validar o controle de velocidade que depende da modelagem
cinemática do robô, o fluxograma abaixo mostra a seqüência de comandos do
SimTwo que são escritas no editor de código em Pascal Script detalhando as
funções Initialize e Control:
Do inglês “ir até o alvo”, essa função faz com que o robô se locomova de
onde estiver a uma dada velocidade nominal até um ponto dado (x, y) e permaneça
em uma dada orientação θ. Para isso parte-se do princípio que o robô sabe sua
posição e orientação atual, portanto é proposto um algoritmo que realiza essa
função. A figura abaixo mostra o exemplo de como funciona esse algoritmo:
Do inglês “seguir em uma linha”, essa função faz com que o robô siga por
uma dada reta imaginária que corta dois pontos (x1, y1), (x2, y2) dados no espaço
global com uma dada velocidade nominal. Parte-se do princípio que o robô sabe sua
localização atual e descreve um percurso para se aproximar da reta ao tempo que
segue por ela. Esta função ainda permite que uma dada orientação θ seja fixada no
robô ao tempo que esse descreve seu movimento.
Dados os dois pontos quaisquer, calculam-se as constantes fundamentais
a, b e c da reta que passa por estes. O controle proposto da referida função baseia-
se em um algoritmo PID discreto como descrito no Capítulo 6. O cálculo do erro para
esse controlador está na distância entre o ponto de localização do robô e a reta
calculada anteriormente. Esse cálculo pode ser visto na equação abaixo:
Y· /Z·/[
N;
√Y / Z
(37)
Depois de calculado o erro do sistema calcula-se a saída do controlador
conforme a equação (34) normalizada a um valor de velocidade resultante. Tal
velocidade é então distribuída em duas componentes e somadas duas a duas com
mais outras componentes que representam a velocidade nominal de entrada da
função. As equações abaixo mostram o somatório vetorial das velocidades:
\]^ · sin
_I`Q / abc · cos
_I`Q
(38)
59
\]^ · cos
_I`Q / abc · sin
_I`Q
(39)
A variável θLinha é o ângulo que a reta faz com a horizontal e é descrita
pela equação abaixo:
_I`Q tanf
(40)
Como a reta pode assumir diversas configurações no plano utiliza-se a
função computacional arco-tangente2 que abrange todos os quadrantes do espaço
cartesiano. A figura abaixo mostra o erro de distância do robô á reta:
Do inglês “seguir em círculo”, essa função faz com que o robô siga por um
círculo imaginário com centro em (xc, yc) e raio dado no espaço global com uma
dada velocidade nominal. Parte-se do princípio que o robô sabe sua localização
atual e descreve um percurso para se aproximar do círculo ao tempo que segue por
ele. Esta função ainda permite que uma dada orientação θ seja fixada no robô ao
tempo que esse descreve seu movimento.
O controlador da orientação segue o mesmo princípio descrito
anteriormente para a função FollowLine que também é igual a função GoToTarget.
O controle proposto da referida função baseia-se em um algoritmo PID
discreto como descrito no Capítulo 6. O cálculo do erro para esse controle está na
subtração do raio informado a função pela distancia do robô à reta ao centro da
curva. Tal reta é perpendicular ao raio do círculo como mostra a figura abaixo:
61
9 RESULTADOS OBTIDOS
O método Dead-Reckoning
Dead Reckoning descrito no Capítulo 5 foi implementado no
SimTwo a partir da função GoToTarget pois não se fazia necessário a utilização
desta para o controle de velocidade. O próprio simulador é capaz de fornecer a
posição e orientação do robô, assim pode-se
pode se calibrar os controladores PID de
posição com essa
ess função,
função o que garantiu precisão do controle. O gráfico abaixo
mostra um ensaio da função GoToTarget para uma posição inicial do robô (0, 0, 0º)
e posição final (0.5, 0.5, 45º) e velocidade nominal 0.5 m/s sem a utilização
utilização do
método Dead-Reckoning
Reckoning
Reckoning:
O ensaio
ensaio acima pode ser visto no modo de simulação 3D conforme a
figura abaixo:
66
Constantes PID Kp Ti Td
Controlador de Velocidade 4,5 0,25 0,006
Controlador GoToTarget 1 2 0
Controlador FollowLine 6 2 0,05
Controlador FollowCircle 10 10000 0,0001
10 CONCLUSÃO
REFERÊNCIAS
COSTA, Paulo José Cerqueira Gomes da. SimTwo. 2010. Disponível em:
<http://paginas.fe.up.pt/~paco/wiki/index.php?n=Main.SimTwo>. Acesso em: 10 de Junho
2010.
SOUZA, David José de. Desbravando o PIC: Ampliado e Atualizado para PIC
16F628A. 9ª ed. São Paulo: Érica, 2005. 267p.
72
// Constantes do Robô
const TENSAO_MAX = 12;
const DIST_AO_CENTRO = 0.1;
const TEMPO_DE_AMOSTRA = 0.04;
const RAIO = 0.04;
const ENCODER_PPR = 1000;
const ID_ROBOT_1 = 0;
const ID_RODA_0 = 0;
const ID_RODA_1 = 1;
const ID_RODA_2 = 2;
// Constantes de Controle de Velocidade
const KP = 4.5;
const TI = 0.25;
const TD = 0.006;
// Variáveis Globais do Robô
var RobotPos: TState2D; //Coordenadas de Posição do Robô (X, Y, Theta)
// Varáveis de Controle de Velocidade
var VelGlobal: TState2D; //Velocidade no Espaço (Vx, Vy, W)
var VRef: TState2D; //Velocidade no Robô (V, Vn, W)
var VrRef: TState2D; //Velocidade de Referência das Rodas (V0, V1, V2)
var Vr: TState2D; //Velocidade Atual das Rodas (V0, V1, V2)
var DTheta: TState2D; //Deslocamento Angular das Rodas (DTheta0, DTheta1,
DTheta2)
var Erro: TState2D; //Erro de Velocidade das Rodas (ErroV0, ErroV1, ErroV2)
var ErroA: TState2D; //Erro de Velocidade das Rodas (ErroAnteriorV0,
ErroAnteriorV1, ErroAnteriorV2)
var Derivada: TState2D; //Derivada do Erro de Velocidade das Rodas
(DerivadaV0, DerivadaV1, DerivadaV2)
var Integral: TState2D; //Integral do Erro de Velocidade das Rodas
(IntegralV0, IntegralV1, IntegralV2)
var U: TState2D; //Tensão nas Rodas (U0, U1, U2)
// Função de Controle da Velocidade do Robô
procedure VelControl(Robot: LongInt; VGlobalRef: TState2D);
begin
// Matriz de Rotação da Velocidade Global para a Velocidade Local
VRef.x:=Cos(RobotPos.angle)*VGlobalRef.x+Sin(RobotPos.angle)*VGlobalRef.y;
VRef.y:=Sin(RobotPos.angle)*VGlobalRef.x+Cos(RobotPos.angle)*VGlobalRef.y;
VRef.angle:=VGlobalRef.angle;
// Matriz Cinemática da Velocidade Local para a Velocidade Linear das Rodas
VrRef.x:=Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.angle;
VrRef.y:=(0)*VRef.x-(1)*VRef.y+DIST_AO_CENTRO*VRef.angle;
VrRef.angle:=Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.an
gle;
// Cálculo do Erro de Velocidade das Rodas
Erro.x := VrRef.x -Vr.x;
Erro.y := VrRef.y -Vr.y;
Erro.angle := VrRef.angle-Vr.angle;
// Cálculo da Derivada do Erro de Velocidade das Rodas
Derivada.x := (Erro.x -ErroA.x) /TEMPO_DE_AMOSTRA;
Derivada.y := (Erro.y -ErroA.y) /TEMPO_DE_AMOSTRA;
Derivada.angle := (Erro.angle-ErroA.angle)/TEMPO_DE_AMOSTRA;
// Cálculo da Integral do Erro de Velocidade das Rodas
Integral.x := Integral.x +TEMPO_DE_AMOSTRA*Erro.x;
Integral.y := Integral.y +TEMPO_DE_AMOSTRA*Erro.y;
Integral.angle := Integral.angle+TEMPO_DE_AMOSTRA*Erro.angle;
// Cálculo do Erro Anterior de Velocidade das Rodas
ErroA.x := Erro.x;
ErroA.y := Erro.y;
ErroA.angle := Erro.angle;
// PIDs de Tensão das Rodas
U.x := KP*(Erro.x +(1/TI)*Integral.x +TD*Derivada.x);
U.y := KP*(Erro.y +(1/TI)*Integral.y +TD*Derivada.y);
U.angle := KP*(Erro.angle+(1/TI)*Integral.angle+TD*Derivada.angle);
// Controle de Saturação dos PIDs
if(U.x>TENSAO_MAX)then //Saturação no Controlador da Roda 0
begin
75
U.x := TENSAO_MAX;
Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x;
end;
if(U.x<-TENSAO_MAX)then
begin
U.x := -TENSAO_MAX;
Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x;
end;
if(U.y>TENSAO_MAX)then //Saturação no Controlador da Roda 1
begin
U.y := TENSAO_MAX;
Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y;
end;
if(U.y<-TENSAO_MAX)then
begin
U.y := -TENSAO_MAX;
Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y;
end;
if(U.angle>TENSAO_MAX)then //Saturação no Controlador da Roda 2
begin
U.angle := TENSAO_MAX;
Integral.angle:=Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
if(U.angle<-TENSAO_MAX)then
begin
U.angle := -TENSAO_MAX;
Integral.angle:=Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
// Insere o Valor Controlado de Tensão nas Rodas
SetAxisVoltageRef(Robot, ID_RODA_0, U.x);
SetAxisVoltageRef(Robot, ID_RODA_1, U.y);
SetAxisVoltageRef(Robot, ID_RODA_2, U.angle);
end;
// Atualiza os dados de posição e orientação
procedure AtualizaDados;
begin
// Cálculo do Deslocamento Linear das Rodas
DTheta.x := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_0)/ENCODER_PPR;
DTheta.y := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_1)/ENCODER_PPR;
DTheta.angle := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_2)/ENCODER_PPR;
// Cálculo da Velocidade Linear das Rodas
Vr.x := (DTheta.x/TEMPO_DE_AMOSTRA)*RAIO;
Vr.y := (DTheta.y/TEMPO_DE_AMOSTRA)*RAIO;
Vr.angle := (DTheta.angle/TEMPO_DE_AMOSTRA)*RAIO;
// Cálculo de Posicionamento do Robô
RobotPos := GetRobotPos2D(ID_ROBOT_1);
end;
// This procedure is called periodicaly (default: 40 ms)
procedure Control;
begin
AtualizaDados;
AddTrailNode(ID_ROBOT_1, RobotPos.x, RobotPos.y, 0.001);
VelControl(ID_ROBOT_1, VelGlobal);
end;
// This procedure is called once when the script is started
procedure Initialize;
begin
SetRobotPos(ID_ROBOT_1, 0, 0, 0, 0);
RobotPos.x := 0;
RobotPos.y := 0;
RobotPos.angle := 0;
VrRef.x := 0;
VrRef.y := 0;
VrRef.angle := 0;
Vr.x := 0;
Vr.y := 0;
Vr.angle := 0;
DTheta.x := 0;
DTheta.y := 0;
DTheta.angle := 0;
76
Erro.x := 0;
Erro.y := 0;
Erro.angle := 0;
ErroA.x := 0;
ErroA.y := 0;
ErroA.angle := 0;
Derivada.x := 0;
Derivada.y := 0;
Derivada.angle := 0;
Integral.x := 0;
Integral.y := 0;
Integral.angle := 0;
U.x := 0;
U.y := 0;
U.angle := 0;
VRef.x := 0;
VRef.y := 0;
VRef.angle := 0;
VelGlobal.x := 0.1;
VelGlobal.y := 0.1;
VelGlobal.angle := 0.1;
end;
77
// Constantes do Robô
const TENSAO_MAX = 12;
const DIST_AO_CENTRO = 0.1;
const TEMPO_DE_AMOSTRA = 0.04;
const RAIO = 0.04;
const ENCODER_PPR = 1000;
const ID_ROBOT_1 = 0;
const ID_RODA_0 = 0;
const ID_RODA_1 = 1;
const ID_RODA_2 = 2;
// Constantes de Controle de Velocidade
const KP = 4.5;
const TI = 0.25;
const TD = 0.006;
// Constantes de Controle de Posição
const KP_TARGET = 1;
const TI_TARGET = 2;
// Constantes de Controle de Linha
const KP_LINE = 6;
const TI_LINE = 2;
const TD_LINE = 0.05;
// Constantes de Controle de Círculo
const KP_CIRCLE = 10;
const TI_CIRCLE = 10000;
const TD_CIRCLE = 0.0001;
// Variáveis Globais do Robô
var RobotPos: TState2D; //Coordenadas de Posição do Robô (X, Y, Theta)
var Deslocamento: TState2D; //Deslocamento de Posição do Robô (DX, DY,
DTheta)
// Varáveis de Controle de Velocidade
var VelGlobal: TState2D; //Velocidade no Espaço (Vx, Vy, W)
var VRef: TState2D; //Velocidade no Robô (V, Vn, W)
var VrRef: TState2D; //Velocidade de Referência das Rodas (V0, V1, V2)
var Vr: TState2D; //Velocidade Atual das Rodas (V0, V1, V2)
var DTheta: TState2D; //Deslocamento Angular das Rodas (DTheta0, DTheta1,
DTheta2)
var Erro: TState2D; //Erro de Velocidade das Rodas (ErroV0, ErroV1, ErroV2)
var ErroA: TState2D; //Erro de Velocidade das Rodas (ErroAnteriorV0,
ErroAnteriorV1, ErroAnteriorV2)
var Derivada: TState2D; //Derivada do Erro de Velocidade das Rodas
(DerivadaV0, DerivadaV1, DerivadaV2)
var Integral: TState2D; //Integral do Erro de Velocidade das Rodas
(IntegralV0, IntegralV1, IntegralV2)
var U: TState2D; //Tensão nas Rodas (U0, U1, U2)
// var iáveis de Controle de Posição
var VelTarget: TState2D; //Velocidade no Espaço para Controle de Posição
(Vx, Vy, W)
var ErroTarget: TState2D; //Erro ao Ponto-Alvo (ErroX, ErroY, ErroTheta)
var IntegralTarget: TState2D; //Integral do Erro de Posição (IntegralX,
IntegralY, IntegralTheta)
var Alvo: TState2D; //Coordenada Absoluta do Pont-Alvo (X, Y, Theta)
// Variáveis de Controle de Linha
var VelLine: TState2D; //Velocidade Resultante no Espaço para Controle de
Linha (Vx, Vy, W)
var VelPIDLine: Double; //Velocidade no Espaço para Controle de Linha (Vx,
Vy, W)
var ErroThetaLine: Double; //Erro de Angulação do Robô
var ErroLine: Double; //Erro de Distância à Linha
var ErroALine: Double; //Erro de Distância à Linha
var DerivadaLine: Double; //Derivada do Erro de Distância à Linha
var IntegralLine: Double; //Integral do Erro de Distância à Linha
var IntegralThetaLine: Double; //Integral do Erro de Angulação do Robô
var Ponto1: TState2D; //Coordenada Absoluta de um Ponto Qualquer (X, Y,
Theta)
var Ponto2: TState2D; //Coordenada Absoluta de um Ponto Qualquer (X, Y,
Theta)
78
begin
U.angle := TENSAO_MAX;
Integral.angle := Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
if(U.angle<-TENSAO_MAX)then
begin
U.angle := -TENSAO_MAX;
Integral.angle := Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
// Insere o Valor Controlado de Tensão nas Rodas
SetAxisVoltageRef(Robot, ID_RODA_0, U.x);
SetAxisVoltageRef(Robot, ID_RODA_1, U.y);
SetAxisVoltageRef(Robot, ID_RODA_2, U.angle);
end;
// Função de Controle de Posição do Robô
procedure GotoTarget(Robot: LongInt; VNom: Double; Target: TState2D);
begin
VNom := Abs(VNom);
// Cálculo do Erro de Posição do Robô
ErroTarget.x := Target.x-RobotPos.x;
ErroTarget.y := Target.y-RobotPos.y;
ErroTarget.angle := Target.angle-RobotPos.angle;
// Cálculo da Integral do Erro de Posição do Robô
IntegralTarget.x:=IntegralTarget.x+TEMPO_DE_AMOSTRA*ErroTarget.x;
IntegralTarget.y:=IntegralTarget.y+TEMPO_DE_AMOSTRA*ErroTarget.y;
IntegralTarget.angle:=IntegralTarget.angle+TEMPO_DE_AMOSTRA*ErroTarget.angl
e;
// PIs de Velocidade Global
VelTarget.x:=KP_TARGET*(ErroTarget.x+(1/TI_TARGET)*IntegralTarget.x);
VelTarget.y:=KP_TARGET*(ErroTarget.y+(1/TI_TARGET)*IntegralTarget.y);
VelTarget.angle:=KP_TARGET*(ErroTarget.angle+(1/TI_TARGET)*IntegralTarget.a
ngle);
// Controle de Saturação dos PIs
if(VelTarget.x > VNom)then //Saturação no Controlador de Vx
begin
VelTarget.x:=VNom;
IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x;
end;
if(VelTarget.x < -VNom)then
begin
VelTarget.x:=-VNom;
IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x;
end;
if(VelTarget.y > VNom)then //Saturação no Controlador de Vy
begin
VelTarget.y:=VNom;
IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y;
end;
if(VelTarget.y < -VNom)then
begin
VelTarget.y:=-VNom;
IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y;
end;
if(VelTarget.angle > VNom)then //Saturação no Controlador de W
begin
VelTarget.angle:=VNom;
IntegralTarget.angle:=IntegralTarget.angle-
TEMPO_DE_AMOSTRA*ErroTarget.angle;
end;
if(VelTarget.angle<-VNom)then
begin
VelTarget.angle:=-VNom;
IntegralTarget.angle:=IntegralTarget.angle-
TEMPO_DE_AMOSTRA*ErroTarget.angle;
end;
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelTarget);
end;
// Função de Seguir uma Linha
80
end
else
begin
SinalXPID := -1;
SinalyPID := -1;
end;
end
else // Linha Inclinada
begin
a := Tan(ThetaLinha);
b := -1;
c := -a*PontoA.x-b*PontoA.y;
if((a >= 0)and(ErroLine >= 0))then
begin
SinalX := FlagSentido;
SinalY := FlagSentido;
SinalXPID := -1;
SinalyPID := 1;
end
else if((a >= 0)and(ErroLine < 0))then
begin
SinalX := FlagSentido;
SinalY := FlagSentido;
SinalXPID := 1;
SinalyPID := 0;
end
else if((a < 0)and(ErroLine < 0))then
begin
SinalX := FlagSentido;
SinalY := -FlagSentido;
SinalXPID := -1;
SinalyPID := -1;
end
else if((a < 0)and(ErroLine >= 0))then
begin
SinalX := FlagSentido;
SinalY := -FlagSentido;
SinalXPID := 1;
SinalyPID := 0;
end;
end;
// PID de Controle de Linha
ErroLine := (a*RobotPos.x+b*RobotPos.y+c)/Sqrt(a*a+b*b);
ErroThetaLine := Angulo-RobotPos.angle;
DerivadaLine := (ErroLine-ErroALine)/TEMPO_DE_AMOSTRA;
IntegralLine := IntegralLine+TEMPO_DE_AMOSTRA*ErroLine;
IntegralThetaLine := IntegralThetaLine+TEMPO_DE_AMOSTRA*ErroThetaLine;
ErroALine := ErroLine;
VelPIDLine :=
KP_LINE*(ErroLine+(1/TI_LINE)*IntegralLine+TD_LINE*DerivadaLine);
// Controle de Saturação do PID
if(VelPIDLine>VNom)then
begin
VelPIDLine := VNom;
IntegralLine := IntegralLine-TEMPO_DE_AMOSTRA*ErroLine;
end;
if(VelPIDLine<-VNom)then
begin
VelPIDLine := -VNom;
IntegralLine := IntegralLine-TEMPO_DE_AMOSTRA*ErroLine;
if(VelLine.angle > VNom)then //Saturação no Controlador de W
begin
VelLine.angle := VNom;
IntegralThetaLine := IntegralThetaLine-TEMPO_DE_AMOSTRA*ErroThetaLine;
end;
if(VelLine.angle<-VNom)then
begin
VelLine.angle := -VNom;
IntegralThetaLine := IntegralThetaLine-TEMPO_DE_AMOSTRA*ErroThetaLine;
end;
82
end;
// Cálculo das Componentes de Velocidade
VelLine.x:=SinalXPID*Abs(VelPIDLine)*Sin(Abs(ThetaLinhaNorm))+SinalX*VNom*C
os(Abs(ThetaLinhaNorm));
VelLine.y:=SinalYPID*Abs(VelPIDLine)*Cos(Abs(ThetaLinhaNorm))+SinalY*VNom*S
in(Abs(ThetaLinhaNorm));
VelLine.angle:=KP_TARGET*(ErroThetaLine+(1/TI_TARGET)*IntegralThetaLine);
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelLine);
end;
// Função de Seguir um Círculo
procedure FollowCircle(Robot: LongInt; VNom: Double; FlagSentido: Integer;
Circulo: TState2D; Angulo: Double);
begin
VNom := Abs(VNom);
// Cálculo do ângulo que o Robô faz com a Horizontal no Centro do Círculo
ThetaLinha := ATan2((RobotPos.y-Circulo.y),(RobotPos.x-Circulo.x));
// Normatização do ângulo que o Robô faz com a Horizontal no Centro do
Círculo
if(ThetaLinha > Rad(90))then
begin
ThetaLinhaNorm := Rad(180) - ThetaLinha;
end
else if(ThetaLinha < Rad(-90))then
begin
ThetaLinhaNorm := -Rad(180) - ThetaLinha;
end
else
begin
ThetaLinhaNorm := ThetaLinha;
end;
// Verifica a Flag de Sentido
if((FlagSentido <> 1) and (FlagSentido <> -1))then
begin
FlagSentido := 1;
end;
// Geração dos Sinais de Controle no Círculo
if((ThetaLinha>=Rad(0))and(ThetaLinha<Rad(90)))then // Robô no 1º
Quadrante
begin
SinalX := -FlagSentido;
SinalY := FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := -FlagSentido;
end;
end
else if((ThetaLinha>=Rad(90))and(ThetaLinha<Rad(180)))then // Robô no 2º
Quadrante
begin
SinalX := -FlagSentido;
SinalY := -FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
83
SinalYPID := -FlagSentido;
end;
end
else if((ThetaLinha<Rad(-90))and(ThetaLinha>=Rad(-180)))then // Robô no
3º Quadrante
begin
SinalX := FlagSentido;
SinalY := -FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := -FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := FlagSentido;
end;
end
else if((ThetaLinha<Rad(0))and(ThetaLinha>=Rad(-90)))then // Robô no 4º
Quadrante
begin
SinalX := FlagSentido;
SinalY := FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := -FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := FlagSentido;
end;
end;
// PID de Controle de Círculo
ErroCircle:=Circulo.angle-Sqrt((RobotPos.y-Circulo.y)*(RobotPos.y-
Circulo.y)+(RobotPos.x-Circulo.x)*(RobotPos.x-Circulo.x));
ErroThetaCircle := Angulo-RobotPos.angle;
DerivadaCircle := (ErroCircle-ErroACircle)/TEMPO_DE_AMOSTRA;
IntegralCircle := IntegralCircle+TEMPO_DE_AMOSTRA*ErroCircle;
ErroACircle := ErroCircle;
VelPIDCircle :=
KP_CIRCLE*(ErroCircle+(1/TI_CIRCLE)*IntegralCircle+TD_CIRCLE*DerivadaCircle
);
IntegralThetaCircle :=
IntegralThetaCircle+TEMPO_DE_AMOSTRA*ErroThetaCircle;
// Controle de Saturação do PID
if(VelPIDCircle > VNom)then
begin
VelPIDCircle := VNom;
IntegralCircle := IntegralCircle-TEMPO_DE_AMOSTRA*ErroCircle;
end;
if(VelPIDCircle < -VNom)then
begin
VelPIDCircle := -VNom;
IntegralCircle := IntegralCircle-TEMPO_DE_AMOSTRA*ErroCircle;
end;
if(VelCircle.angle > VNom)then //Saturação no Controlador de W
begin
VelCircle.angle := VNom;
IntegralThetaCircle := IntegralThetaCircle-
TEMPO_DE_AMOSTRA*ErroThetaCircle;
end;
if(VelCircle.angle<-VNom)then
begin
84
VelCircle.angle := -VNom;
IntegralThetaCircle := IntegralThetaCircle-
TEMPO_DE_AMOSTRA*ErroThetaCircle;
end;
// Cálculo das Componentes de Velocidade
VelCircle.x :=
SinalXPID*Abs(VelPIDCircle)*Cos(Abs(ThetaLinhaNorm))+SinalX*VNom*Sin(Abs(Th
etaLinhaNorm));
VelCircle.y :=
SinalYPID*Abs(VelPIDCircle)*Sin(Abs(ThetaLinhaNorm))+SinalY*VNom*Cos(Abs(Th
etaLinhaNorm));
VelCircle.angle :=
KP_TARGET*(ErroThetaCircle+(1/TI_TARGET)*IntegralThetaCircle);
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelCircle);
end;
// Atualiza os dados de posição e orientação
procedure AtualizaDados;
var parcelax, parcelay: double;
begin
// Cálculo do Deslocamento Linear das Rodas
DTheta.x := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_0)/ENCODER_PPR;
DTheta.y := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_1)/ENCODER_PPR;
DTheta.angle := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_2)/ENCODER_PPR;
// Cálculo da Velocidade Linear das Rodas
Vr.x := (DTheta.x/TEMPO_DE_AMOSTRA)*RAIO;
Vr.y := (DTheta.y/TEMPO_DE_AMOSTRA)*RAIO;
Vr.angle := (DTheta.angle/TEMPO_DE_AMOSTRA)*RAIO;
// Cálculo de Posicionamento do Robô
RobotPos := GetRobotPos2D(ID_ROBOT_1);
end;
// Função Geral de Controle do Programa
procedure Control;
begin
AtualizaDados;
AddTrailNode(ID_ROBOT_1, RobotPos.x, RobotPos.y, 0.001);
// GotoTarget(ID_ROBOT_1, 0.8, Alvo);
// FollowLine(ID_ROBOT_1, 0.5, 1, Ponto2, Ponto1, Rad(90));
FollowCircle(ID_ROBOT_1, 0.5, -1, Alvo, Rad(45));
end;
// Função de Inicialização do Programa
procedure Initialize;
begin
SetRobotPos(ID_ROBOT_1, 0, 0, 0, 0);
RobotPos.x := 0;
RobotPos.y := 0;
RobotPos.angle := 0;
VrRef.x := 0;
VrRef.y := 0;
VrRef.angle := 0;
Vr.x := 0;
Vr.y := 0;
Vr.angle := 0;
DTheta.x := 0;
DTheta.y := 0;
DTheta.angle := 0;
Erro.x := 0;
Erro.y := 0;
Erro.angle := 0;
ErroA.x := 0;
ErroA.y := 0;
ErroA.angle := 0;
Derivada.x := 0;
Derivada.y := 0;
Derivada.angle := 0;
Integral.x := 0;
Integral.y := 0;
Integral.angle := 0;
Deslocamento.x := 0;
Deslocamento.y := 0;
85
Deslocamento.angle := 0;
U.x := 0;
U.y := 0;
U.angle := 0;
VRef.x := 0;
VRef.y := 0;
VRef.angle := 0;
VelGlobal.x := 1;
VelGlobal.y := 0;
VelGlobal.angle := 0;
Alvo.x := 0;
Alvo.y := 0;
Alvo.angle := 1;
ErroTarget.x := 0;
ErroTarget.y := 0;
ErroTarget.angle := 0;
IntegralTarget.x := 0;
IntegralTarget.y := 0;
IntegralTarget.angle := 0;
Ponto1.x := 0;
Ponto1.y := 0;
Ponto1.angle := 0;
Ponto2.x := 2;
Ponto2.y := 1;
Ponto2.angle := 0;
VelLine.x := 0;
VelLine.y := 0;
VelLine.angle := 0;
ErroLine := 0;
ErroALine := 0;
DerivadaLine := 0;
IntegralLine := 0;
VelPIDLine := 0;
a := 0;
b := 0;
c := 0;
ThetaLinha := 0;
ThetaLinhaNorm := 0;
SinalX := 0;
SinalY := 0;
SinalXPID := 0;
SinalYPID := 0;
VelCircle.x := 0;
VelCircle.y := 0;
VelCircle.angle := 0;
ErroThetaCircle := 0;
ErroCircle := 0;
ErroACircle := 0;
DerivadaCircle := 0;
IntegralCircle := 0;
IntegralThetaCircle := 0;
VelPIDCircle := 0;
end;
Alexandre Costa Mota
e-mail: alexandremota.eng@gmail.com