Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais - Alexandre Costa Mota
-
Upload
alexandre-costa-mota -
Category
Documents
-
view
460 -
download
0
description
Transcript of Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais - Alexandre Costa Mota
Faculdade de Tecnologia e Ciências Curso de Engenharia Mecatrônica
Alexandre Costa Mota
Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais
Salvador 2010
Alexandre Costa Mota
Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais
Monografia apresentada ao Curso de Graduação em Engenharia Mecatrônica da Faculdade de Tecnologia e Ciências (FTC) como requisito para obtenção do título de Bacharel em Engenharia Mecatrônica.
Orientador: Prof. Antonio Carlos Caldeira
Pimenta
Salvador 2010
Alexandre Costa Mota
Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais
Monografia aprovada como requisito parcial para obtenção do título de Bacharel em Engenharia Mecatrônica, da Faculdade de Tecnologia e Ciências (FTC). Salvador, ___ de __________ de 2010.
Paschoal Molinari Neto._______________________________________________ Mestre em Engenharia Eletrônica e Computação – Instituto Tecnológico de Aeronáutica - (ITA). Coordenador do curso de Engenharia Mecatrônica: Faculdade de Tecnologia e
Ciências - (FTC). Antonio Carlos Caldeira Pimenta._______________________________________
Mestre em Engenharia Elétrica – UFBA. CREA: BA36350. Orientador: Faculdade de Tecnologia e Ciências (FTC - Salvador).
Cauby de Souza Filho.________________________________________________
Mestre em Desenvolvimento Humano e Responsabilidade Social – FVC-CEPPEV. UCP-RJ. CREA: 37570; CONFEAG: 200511821-4. Professor da Disciplina: Faculdade de Tecnologia e Ciências (FTC – Salvador).
A minha família por acreditar e investir em mim.
AGRADECIMENTOS
Agradeço a Deus e Meishu-Sama por me conceder a oportunidade de chegar
até este momento.
A meu Pai ****** Mota, pessoa que amo e que alegra minha vida tentando
torná-la a melhor possível e me mostrando os caminhos.
Agradeço à minha Mãe ***** Mota que me educou com carinho, amor,
atenção e outras qualidades infinitas ao meu pensamento.
À minha irmã ****** Mota que caminha ao meu lado para um futuro brilhante.
Aos meus tios e avós que confiaram sempre nos meus passos.
Aos meus sinceros amigos que conquistei durante todo esse tempo, sempre
presentes para comemorar nas vitórias e amparar nas derrotas.
Ao meu orientador por aceitar e acreditar neste trabalho. Sua dedicação no
auxilio do desenvolvimento deste trabalho foi de grande importância.
Aos meus colegas de faculdade por esses longos cinco anos juntos, de
grandes momentos inesquecíveis, que ficarão guardados na memória por toda
minha vida.
“Quando os homens abrirem as portas dos seus corações, desaparecerão as trevas
que envolvem este mundo.”
Autor: Mokiti Okada. Fonte: O Pão Nosso de Cada Dia: O Alimento Espiritual do Cotidiano.
SUMÁRIO
Página
1 INTRODUÇÃO ............................................................................................... 14
1.1 DELIMITAÇÃO DO TEMA .............................................................................. 14
1.2 PROBLEMA DE PESQUISA ........................................................................... 15
1.3 HIPÓTESE ...................................................................................................... 15
1.4 OBJETIVOS .................................................................................................... 16
1.4.1 Objetivo Geral ............................................................................................... 16
1.4.2 Objetivos Específicos................................................................................... 16
1.5 JUSTIFICATIVA .............................................................................................. 16
1.6 METODOLOGIA ............................................................................................. 17
1.7 ESTRUTURA DO TRABALHO ....................................................................... 17
2 A ROBÓTICA MÓVEL ................................................................................... 19
2.1 PARADIGMAS EM ROBÓTICA MÓVEL ........................................................ 20
2.2 LOCOMOÇÃO COM RODAS ......................................................................... 22
2.3 LOCALIZAÇÃO ............................................................................................... 25
3 MODELAGEM DE ROBÔS OMNI-DIRECIONAIS ......................................... 29
3.1 MODELO CINEMÁTICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS 30
3.2 MODELO DINÂMICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS .... 32
4 CARACTERÍSTICAS DA ELETRÔNICA EMBARCADA ............................... 35
4.1 O MICROCONTROLADOR E SUAS CARACTERÍSTICAS ............................ 35
4.1.1 Interrupções no PIC ...................................................................................... 37
4.2 SENSORES E ATUADORES ......................................................................... 38
4.3 AQUISIÇÃO DOS DADOS POR RADIOFREQUÊNCIA ................................. 40
5 ESTIMAÇÃO DE POSIÇÃO E ORIENTAÇÃO .............................................. 42
5.1 O MÉTODO DEAD-RECKONING................................................................... 42
6 CONTROLE PID DISCRETO ......................................................................... 45
6.1 O MÉTODO ZIEGLER-NICHOLS EM MALHA FECHADA ............................. 46
7 VARIÁVEIS .................................................................................................... 47
8 METODOLOGIA ............................................................................................. 48
8.1 DESCRIÇÃO DA ELETRÔNICA EMBARCADA ............................................. 49
8.2 A PLATAFORMA DE SIMULAÇÃO SIMTWO ................................................ 51
8.2.1 O Robô Omni-direcional no SimTwo ........................................................... 53
8.3 O CONTROLADOR DE VELOCIDADE PROPOSTO ..................................... 55
8.4 O CONTROLADOR DE TRAJETÓRIA PROPOSTO ...................................... 57
8.4.1 Descrição da Função GoToTarget............................................................... 57
8.4.2 Descrição da Função FollowLine ................................................................ 58
8.4.3 Descrição da Função FollowCircle.............................................................. 60
9 RESULTADOS OBTIDOS .............................................................................. 62
10 CONCLUSÃO ................................................................................................. 68
REFERÊNCIAS ......................................................................................................... 69
APÊNDICE A – ESQUEMA ELETRÔNICO DO ROBÔ ............................................ 72
APÊNDICE B – CÓDIGO DE MODELAGEM DO ROBÔ ......................................... 73
APÊNDICE C – CÓDIGO DE CONTROLE DE VELOCIDADE ................................ 74
APÊNDICE D – CÓDIGO COMPLETO DA SIMULAÇÃO ........................................ 77
LISTA DE FIGURAS
Página
Figura 1 – Relação entre paradigmas primitivos nos paradigmas: a) Hierárquico, b) Reativo e c)
Híbrido. .........................................................................................................................21
Figura 2 – Esquema de controle para um robô móvel. ....................................................................22
Figura 3 – Sistema de Posicionamento Global. ...............................................................................27
Figura 4 – Sistema Vetorial para um robô omni-direcional de três rodas. ........................................30
Figura 5 – Diagrama geral de um microcontrolador. ........................................................................35
Figura 6 – Diagrama de pinos e encapsulamento PDIP dos PICs a)18F4550 b)16F628A. ..............37
Figura 7 – Diagrama de blocos típico de controle de um motor DC. ................................................39
Figura 8 – Esquema de funcionamento de um encoder de quadratura. ...........................................39
Figura 9 – Módulo de comunicação por radiofreqüência XBee-Pro. ................................................40
Figura 10 – Diagrama de Fluxo de Dados de um sistema com interface UART. ................................41
Figura 11 – Esquema de movimento do robô para uma trajetória curvilínea. .....................................43
Figura 12 – Sistema de controle em malha fechada para um motor. .................................................45
Figura 13 – Sistema de comunicação da eletrônica embarcada. .......................................................50
Figura 14 – Ambiente 3D e demais janelas do Simulador SimTwo. ...................................................52
Figura 15 – Robô omni-direcional no ambiente de simulação SimTwo. .............................................53
Figura 16 – Rotina de controle padrão do SimTwo............................................................................54
Figura 17 – Fluxograma de validação de controle de velocidade. .....................................................56
Figura 18 – Demonstração da função GoToTarget. ..........................................................................57
Figura 19 – Erro de distância do FollowLine. ....................................................................................59
Figura 20 – Demonstração da função FollowLine. ............................................................................60
Figura 21 – Erro de distância do FollowCircle. ..................................................................................61
Figura 22 – Demonstração da função FollowCircle. ..........................................................................61
Figura 23 – Gráfico velocidade x tempo da função VelControl. .........................................................62
Figura 24 – Gráfico velocidade angular x tempo da função VelControl. .............................................63
Figura 25 – Gráfico posição x tempo da função GoToTarget sem Dead-Reckoning. .........................63
Figura 26 – Simulação 3D da função GoToTarget sem Dead-Reckoning. .........................................64
Figura 27 – Gráfico posição x tempo da função GoToTarget com Dead-Reckoning. .........................64
Figura 28 – Simulação 3D da função GoToTarget com Dead-Reckoning. .........................................65
Figura 29 – Gráfico posição x tempo da função FollowLine. ..............................................................65
Figura 30 – Simulação 3D da função FollowLine. .............................................................................66
Figura 31 – Gráfico posição x tempo da função FollowCircle. ...........................................................66
Figura 32 – Simulação 3D da função FollowCircle. ...........................................................................67
LISTA DE QUADROS
Página Quadro 1 – Comparativo entre paradigmas primitivos da robótica.....................................................20
Quadro 2 – Modelos de Tração com rodas (adaptado). ....................................................................23
Quadro 3 – Comparativo das séries 12F, 16F e 18F de microcontroladores PIC. ..............................36
Quadro 4 – Dados de especificação do módulo XBee-Pro. ...............................................................40
Quadro 5 – Sintonia de um controlador PID pelo método Ziegler-Nichols..........................................46
Quadro 6 – Características do robô omni-direcional no SimTwo. ......................................................53
Quadro 7 – Constantes dos Controladores. ......................................................................................67
LISTA DE ABREVIATURA E SIGLAS
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
dc [m] Distância entre o ponto central do robô e as rodas
t [s] Tempo
n [s] Tempo discreto
v0, v1, v2 [m/s] Velocidade linear das rodas
0,1,2 [rad/s] Velocidade angular das rodas
f0, f1, f2 [N] Força de tração nas rodas
T0, T1, T2 [N·m] Torque de tração nas rodas
v, vn [m/s] Velocidade linear do robô
[rad/s] Velocidade angular do robô
Fv, Fvn [N] Força de tração no robô segundo as direções v e vn
T [N·m] Torque de rotação no robô segundo o seu eixo de rotação
x, y [m] Variáveis de posicionamento global
[rad] Posição angular do robô
vx, vy [m/s] Velocidade linear do robô segundo os referenciais x e y
R [m] Raio das rodas
M [kg] Massa do robô
J [kg·m2] Momento de inércia do robô
FBv, FBvn [N] Força de atrito dinâmico no robô segundo as direções v e vn
TB [N·m] Torque de atrito dinâmico no robô segundo o seu eixo de rotação
FCv, FCvn [N] Força de atrito estático no robô segundo as direções v e vn
TC [N·m] Torque de atrito estático no robô segundo o seu eixo de rotação
Bv,Bvn [N/(m/s)] Coeficiente de atrito dinâmico segundo as direções v e vn
B [N·m/(rad/s)] Coeficiente de atrito dinâmico segundo o eixo de rotação do robô
Cv,Cvn [N] Coeficiente de atrito estático segundo as direções v e vn
C [N·m] Coeficiente de atrito estático segundo o eixo de rotação do robô
l Fator de relação de transmissão
Kt [N·m/A] Constante de binário dos motores
i0, i1, i2 [A] Corrente nos motores
Np[pulsos] Numero de pulsos medidos de um encoder incremental
Res [pulsos/rad] Resolução de um encoder incremental
ta [s] Constante de intervalo de tempo entre n e n-1, é o tempo de amostra dos dados
di, dn [s] Deslocamento do robô na direção do vetor de velocidade linear v e vn
Kp, Ki, Kd Constantes de proporcionalidade, integração e derivação respectivamente
E Variável para cálculo de erro
Kcr, Pcr Constantes de proporcionalidade crítico e constante de tempo crítico
a, b, c Constantes fundamentais da reta
RESUMO
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
Robôs móveis autônomos têm despertado um crescente interesse em
diversos tipos de aplicações, tornando-se uma área de intensas pesquisas
científicas relacionadas com o projeto de novas arquiteturas de locomoção e com o
desenvolvimento de novos algoritmos para controle e navegação (CONCEIÇÃO,
2007, p. viii).
Robôs móveis podem realizar diversas tarefas como: entregar
automaticamente as peças entre os postos de montagem em uma fábrica seguindo
guias posicionadas no espaço; transportar alimentos e medicamentos por um
hospital, acompanhando a posição das luzes no teto que são especificas para este;
servir como um veículo automatizado para limpeza especializada, onde podem
aproveitar o padrão geométrico dos corredores para facilitar sua localização e as
funções de navegação; entre outras funcionalidades.
Segundo Braunl (2006, p. 113), um robô "holonômico" ou omni-direcional
é capaz de se movimentar em qualquer direção. 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. Por esse motivo, robôs móveis com tração omni-direcional têm
sido aplicados largamente em robótica móvel.
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 até mesmo ir até um ponto-alvo sem restrições.
1.1 DELIMITAÇÃO DO TEMA
Sistema de controle de velocidade e trajetória embarcado para um robô
omni-direcional de três rodas.
15
1.2 PROBLEMA DE PESQUISA
Segundo Siegwart & Nourbakhsh (2004, p. 17), a robótica móvel aborda
várias questões como: características de estabilidade do robô, características de
contato do robô com o ambiente e características do ambiente. Essas questões
influenciam diretamente em tomadas de decisão de robôs autônomos em diversas
tarefas como, por exemplo: mover-se a uma velocidade desejada ou seguir uma
trajetória específica. Tarefas básicas como essas influenciam em tarefas mais
complexas como mover-se em um labirinto ou conduzir uma bola de futebol pelo
campo.
Vistos essas características de ambiente como terreno irregular, ou
questões de estabilidade como centro de gravidade do robô, essa pesquisa tem
como foco o seguinte problema: como controlar a trajetória de um robô omni-
direcional de três rodas apartir de velocidades e trajetos pré-definidos?
1.3 HIPÓTESE
Entende-se que para controlar velocidade e trajetória de um robô omni-
direcional pode-se utilizar do modelo cinemático e dinâmico do mesmo.
A eletrônica embarcada do robô possui um controle individual por motor e
um controlador geral para trajetória. Cada controle é discreto no tempo em
microcontroladores que trocam informações entre si.
O controle de velocidade se baseia em um sistema em malha fechada a
partir da velocidade individual das rodas do robô. Essas medidas de velocidade são
introduzidas na modelagem do robô para posteriormente calcular o erro em relação
à velocidade desejada e assim controlar o sistema.
Em relação ao controle de trajetória, dados de referência de posição no
espaço de trabalho e tipo de trajetória a ser descrita são introduzidos para que o
controlador possa ajustar seu posicionamento gerando dados de setpoint para os
controladores de velocidade ao longo do tempo. Esses dados são calculados em
16
malha fechada com realimentação provida da odometria inserida nas equações de
modelagem.
1.4 OBJETIVOS
1.4.1 Objetivo Geral
Descrever uma eletrônica embarcada para um robô omni-direcional de
três rodas com controladores de velocidade e trajetória.
1.4.2 Objetivos Específicos
a) Descrever o sistema eletrônico embarcado do robô omni-direcional;
b) Descrever um controlador de velocidade discreto para o robô omni-
direcional;
c) Descrever um controlador de trajetória discreto para o robô omni-
direcional;
1.5 JUSTIFICATIVA
Para resolver diversos problemas de locomoção, o engenheiro de robótica
deve entender de cinemática, dinâmica e teoria de controle. Localização e
navegação demandam conhecimentos de algoritmos de computador, teoria da
informação, inteligência artificial e teoria da probabilidade (SIEGWART e
NOURBAKHSH, 2004, p. 8). A partir de estudos como esses a robótica móvel
abrange novas areas de conhecimento constantemente.
17
Robôs omni-direcionais estão sendo largamente utilizados para realizar
tarefas de transporte, limpesa, montagem e muitas outras que necessitam de
conhecimentos específiocos por parte do engenheiro de robótica que os projeta. Em
tarefas como essas, controlar a velocidade e a trajetória do robô são duas
características fundamentais para a autonomia desse robô.
Controlar velocidade é uma necessidade direta para realizar ações
básicas como seguir por uma linha, andar em circulo, etc e essas ações básicas são
necessidades diretas para ações de tarefas como no caso de um robô jogador de
futebol ir até a bola, drible, etc e assim por diante passando por ações de papeis até
ações táticas.
1.6 METODOLOGIA
A metodologia utilizada para este trabalho foi com documentação indireta
através da pesquisa bibliográfica em livros, monografias, teses, entre outros. Essa
pesquisa envolve vários assuntos multidisciplinares como métodos de controle,
inteligência artificial, eletrônica embarcada, sistemas de visão artificial, etc.
Durante a fase de pesquisa foi observado uma grande disponibilidade de
material bibliográfico sobre o tema que é bastante abrangente e envolve diversos
assuntos complementares.
Outra parte desse documento aplica-se a pesquisa experimental, em que
realizadas simulações para validar o sistema proposto passa-se a realizar
experimentos reais que põem em prática o objetivo do trabalho.
1.7 ESTRUTURA DO TRABALHO
Aqui estão apresentados os capítulos abordados neste trabalho, onde são
evidenciados os seus conteúdos:
a) Capítulo 1: aborda uma visão geral do trabalho, descrevendo os
objetivos a serem alcançados com sua conclusão;
18
b) Capítulo 2: descreve alguns termos importantes, apresenta os
principais obstáculos e tipos de locomoção existentes em robótica
móvel;
c) Capítulo 3: descreve a modelagem de um robô omni-direcional;
d) Capítulo 4: descreve as necessidades eletrônicas para um robô móvel
omni-direcional;
e) Capítulo 5: descreve o método para estimar a posição e orientação de
um robô omni-direcional;
f) Capítulo 6: apresenta o sistema de controle PID discreto que é utilizado
no robô omni-direcional;
g) Capítulo 7: mostra as variáveis envolvidas no trabalho;
h) Capítulo 8: apresenta a metodologia de trabalho;
i) Capítulo 9: apresenta os resultados obtidos ao final deste trabalho;
j) Capítulo 10: apresenta a conclusão ao final deste trabalho.
19
2 A ROBÓTICA MÓVEL
Em uma nova etapa de evolução dos sistemas robóticos, diversos
pesquisadores têm concentrado grandes esforços na construção de robôs móveis,
introduzindo nessas máquinas as capacidades de mobilidade e autonomia para
reagir adequadamente em um ambiente, o que abre um vasto campo de novas
aplicações e conseqüentemente muitos desafios (SCATENA, 2008, p. 12).
Segundo Scatena (2008, p. 12-13) os robôs móveis podem ser
classificados conforme sua independência a usuários externos da seguinte forma:
a) Robôs móveis controlados: esses robôs são capazes de se movimentar
através da interação com usuários externos. Nessa categoria de robôs,
o ser humano toma as decisões de navegação, tática, etc.
b) Robôs móveis autônomos: esses robôs são capazes de se movimentar
em ambientes abertos ou fechados, sem a intervenção de um usuário
externo. Esse tipo de robô pode tomar decisões de navegação com
total independência.
Segundo Franco (2007, p. 10) um robô móvel autônomo necessita de
mecanismos de locomoção que o possibilite mover-se pelo seu ambiente de maneira
autônoma, ou seja, sem a necessidade de ser controlado pelo homem. Existe uma
grande variedade de movimentos que podem ser executados por um robô e a
escolha de um sistema apropriado de locomoção é um aspecto importante para o
projeto. A maioria dos mecanismos de locomoção usados nos robôs móveis é
inspirada em modelos biológicos. Existe porem uma categoria que não segue este
modelo, os robôs com rodas, também conhecidos como veículos, extremamente
eficientes em superfícies planas.
Segundo Conceição (2007, p. 1) um robô móvel é uma combinação de
vários componentes físicos (hardware) e computacionais (software). Conforme esses
componentes, um robô móvel pode ser considerado como um conjunto de
subsistemas de:
a) Locomoção: capacidade para se movimentar em um ambiente.
b) Sensoriamento: capacidade para medir suas características e do
ambiente.
20
c) Racionalização: capacidade para mapear as medidas em ações.
d) Comunicação: capacidade para comunicar com o exterior.
2.1 PARADIGMAS EM ROBÓTICA MÓVEL
Segundo Murphy (2000, p. 2) atualmente existe três paradigmas para a
organização de inteligência nos robôs: hierárquico, reativo e um híbrido de
hierárquico com reativo. Esses paradigmas são descritos pela relação entre três
outros paradigmas primitivos da robótica: Sensoriamento, Planejamento e Ação.
É uma função de Sensoriamento recolher informações a partir do meio
em que se localiza robô. A partir das informações vindas dos sensores e/ou
conhecimentos em base de dados sobre o ambiente, a função de Planejamento
produz uma ou mais tarefas para o robô possa executar, como: ir até o fundo do
corredor, vire à esquerda, prossiga 3 metros e pare. Funções que produzem
comandos diretos para os atuadores como: girar motor em 90º, no sentido horário,
com uma velocidade de giro de 200 r.p.m. dispõem das funções de Ação. No quadro
abaixo se pode ver o comparativo de entradas e saídas dos paradigmas primitivos:
Quadro 1 – Comparativo entre paradigmas primitivos da robótica.
Robot Primitives Input Output
Sense Sensor data Sensed information
Plan Information (sensed and / or cognitive) Directives
Act Sensed information or directives Actuator commands Fonte – Murphy, 2000, p. 6.
O paradigma Hierárquico ou Deliberativo é o paradigma mais antigo. Sob
ele, o robô opera baseado em planejamento. Isto foi baseado em uma visão
introspectiva de como as pessoas pensam, por exemplo: O robô localiza uma porta,
mapeia o ambiente e traça um percurso em torno das cadeiras até chegar à porta.
Sob esse paradigma, os sentidos do robô no ambiente norteiam em planejar a
próxima ação, portanto a necessidade de Sensoriamento, Planejamento e Ação
(MURPHY, 2000, p. 6).
21
O paradigma Reativo foi uma reação ao paradigma hierárquico. O
Paradigma Reativo foi possível graças a duas tendências. Uma delas foi um
movimento popular entre os pesquisadores de Inteligência Artificial para investigar a
biologia e psicologia cognitiva, a fim de examinar exemplares vivos de inteligência.
Outro motivo foi a rápida diminuição do custo de hardware de computador acoplado
com o aumento do poder de computação. Esse paradigma é bastante utilizado para
imitar comportamento de insetos em robôs. Em suma, esse paradigma traduz uma
reação a um estímulo (MURPHY, 2000, p. 8).
O Paradigma Reativo serve de base para o paradigma Deliberativo /
Reativo. O Paradigma híbrido surgiu na década de 1990 e continua a ser a área
atual de pesquisa. Sob o paradigma híbrido, o robô primeiramente delibera a melhor
forma de decompor uma tarefa em subtarefas (também chamado planejamento), e
em seguida, delibera os comportamentos adequados para realizar cada subtarefa.
Os comportamentos de execução são realizados conforme o Paradigma reativo. Em
suma, o planejamento é feito em uma etapa, depois de sensoriamento e atuação
são feitas em conjunto (MURPHY, 2000, p. 9). A figura abaixo mostra os paradigmas
em função de paradigmas primitivos.
Figura 1 – Relação entre paradigmas primitivos nos paradigmas: a) Hierárquico, b) Reativo e c) Híbrido. Fonte – Murphy, 2000, p. 7.
22
Analogamente à arquitetura Hierárquica pode-se comparar a figura abaixo
com a figura 1-a) em que Localização e Planejamento de trajetória fazem parte do
paradigma de Planejamento, o bloco de Percepção faz parte do paradigma
Sensoriamento e o bloco Controle de Movimento faz parte do paradigma Ação:
Figura 2 – Esquema de controle para um robô móvel. Fonte – Siegwart & Nourbakhsh, 2004, p. 10.
Como visto anteriormente, Ação, Sensoriamento e Planejamento são
fundamentais para a robótica. O foco desse documento está na implementação de
controladores para um robô com controle Hierárquico, em que se toma como
paradigmas de Ação o controle da velocidade motores do robô, Sensoriamento a
leitura da velocidade angular das rodas, mas o Planejamento apesar de ser um
paradigma levado em conta, esse documento não trata de especificar essa
abordagem por possuir grande escala de pesquisa e foco diferenciado.
2.2 LOCOMOÇÃO COM RODAS
Um robô móvel precisa de mecanismos de locomoção que lhe permitem
deslocar sem limitações durante todo o seu ambiente. Mas há uma grande
23
variedade de formas possíveis de se mover e por isso a seleção do modelo de
locomoção de um robô é um aspecto importante do projeto do robô móvel
(SIEGWART e NOURBAKHSH, 2004, p. 13).
A roda foi de longe o mecanismo de locomoção mais popular em robótica
móvel. Ela pode alcançar bons ganhos de eficiência fora a facilidade de
implementação relativamente simples. Além disso, equilíbrio não é um problema de
robô com rodas, pois estes são quase sempre projetados de modo que todas as
rodas estão em contato com o solo. Assim, três rodas são suficientes para garantir
um equilíbrio estável. Quando mais de três rodas são usadas, um sistema de
suspensão é necessário para permitir que todas as rodas mantenham contato com o
solo quando o robô se encontra em terreno irregular (SIEGWART e NOURBAKHSH,
2004, p. 30-31). O quadro abaixo mostra as principais configurações de tração em
robôs com rodas e suas descrições.
Quadro 2 – Modelos de Tração com rodas (adaptado).
Number of
Wheels Arrangement Description
02
One steering wheel in the front, one traction wheel in the rear
Two-wheel differential drive with the center of mass (COM) below the axle
03
Two-wheel centered differential drive with a third point of contact
Two independently driven wheels in the rear/front, 1 unpowered omni-directional wheel in the front/rear
Two free wheels in rear, 1 steered traction wheel in front
Three motorized Swedish or spherical wheels arranged in a triangle; Omni-directional movement is possible
04
Two motorized wheels in the rear, 2 steered wheels in the front; steering has to be different for the 2 wheels to avoid
slipping/skidding.
Two motorized and steered wheels in the front, 2 free wheels in the rear; steering has to be different for the 2 wheels to
avoid slipping/skidding.
Four steered and motorized wheels
24
Four Omni-directional wheels
Two-wheel differential drive with 2 additional points of contact
Icons for the each wheel type are as follows
unpowered Omni-directional wheel (spherical, castor, Swedish)
motorized Swedish wheel (Stanford wheel)
unpowered standard Wheel
motorized standard Wheel
steered standard Wheel
connected wheels
Fonte – Siegwart & Nourbakhsh, 2004, p. 34-36.
A geometria, quantidade e disposição das rodas sobre a base do robô são
responsáveis pelo seu grau de mobilidade, pela sua capacidade de manobra e pela
sua estabilidade (FRANCO, 2007, p. 11).
São necessárias, pelo menos, duas rodas para garantir a estabilidade de
um robô. Robôs com duas rodas motores fixas, chamados de robôs diferenciais, têm
uma estabilidade garantida caso o seu centro de massa esteja abaixo do eixo das
rodas. A garantia da estabilidade de um robô com três rodas é satisfeita quando o
centro de massa do robô está localizado dentro do triangulo formado por elas
(FRANCO, 2007, p. 11).
A manobrabilidade é a capacidade do veículo de mudar de direção em
seu movimento. Esta capacidade depende do grau de liberdade das rodas e da sua
disposição sobre a base do robô. Alguns robôs possuem a habilidade para mover-se
em qualquer direção sobre um plano (x, y) sem a necessidade de mudar a sua
orientação em relação ao eixo vertical ao plano (FRANCO, 2007, p. 12) como, por
exemplo, robôs omni-direcionais.
A mobilidade é a capacidade que um robô tem de mover-se no ambiente.
As restrições básicas que limitam a mobilidade são regras que cada roda deve
satisfazer, sendo estas as restrições de deslizamento lateral (FRANCO, 2007, p. 12).
Os robôs omni-direcionais usam rodas que possuem rolamentos sobre a
sua superfície de contato. Estes rolamentos diminuem o atrito de deslizamento
lateral da roda fazendo com que esta, passe a ter um grau de liberdade a mais. Um
25
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
Localização e navegação são as duas tarefas mais importantes para
robôs móveis. O robô precisa saber onde ele está e precisa ser capaz de fazer um
planejamento de como chegar a um destino. Claro que estes dois problemas não
são isolados uns dos outros, mas intimamente ligados. Se um robô não sabe a sua
posição exata no início de uma trajetória planejada, vai ter problemas em chegar ao
destino (BRAUNL, 2006, p. 197).
Segundo Borenstein, Everett & Feng (1996, p. 10), os problemas gerais
de navegação de robôs móveis são resumidos por três perguntas: "Onde estou?",
"Onde eu estou indo?" e "Como devo chegar lá?". Talvez o resultado mais
importante do levantamento do vasto corpo de literatura sobre o posicionamento do
robô móvel é que até hoje não há nenhuma solução realmente elegante para o
problema. As muitas soluções parciais podem ser grosseiramente classificadas em
dois grupos: medidas de posição relativa e absoluta. Devido à falta de um único bom
método, os desenvolvedores de Automated Guided Vehicles e robôs móveis
costumam combinar dois métodos, um de cada categoria. Localização Relativa pode
ser dividida em Odometria e Navegação por Inércia e Localização Absoluta se divide
em Balizas Ativas, Marcos Artificiais, Marcos Naturais e Correspondência por
Mapas.
Segundo Borenstein, Everett & Feng (1996, p. 10), a odometria é um
método que usa encoders para medir a rotação das rodas e/ou orientação da
direção. A odometria tem a vantagem de que é totalmente auto-suficiente e é capaz
de fornecer ao veículo uma estimativa da sua posição. A desvantagem de utilizar
odometria é que o erro de posição cresce sem limites, a não ser que uma referência
independente seja utilizada periodicamente para reduzir esse erro.
Em odometria pode-se destacar como erros sistemáticos:
a) Diâmetros desiguais das rodas.
26
b) A média dos diâmetros das rodas real difere de rodas de diâmetro
nominal.
c) A distância entre eixos real difere da distância entre eixos nominal.
d) Desalinhamento das rodas.
e) Resolução finita dos encoders.
f) Taxa de amostragem finita dos decodificadores.
Como erros não sistemáticos se podem destacar:
a) Trajetos sobre pisos irregulares.
b) Localização inesperada de objetos no piso.
c) Derrapagem das rodas devido a: superfícies diferentes, sobre
aceleração, forças externas, forças internas, nenhum ponto de
contato das rodas com o chão.
Navegação por inércia é um método que usa giroscópios e acelerômetros
para medir a velocidade de rotação e aceleração. Essas medidas necessitam de
integração uma vez ou duas vezes para se obter a posição. Sistemas de navegação
inercial também são auto-suficientes como a odometria. Por outro lado, os dados do
sensor inercial extrapolam com o tempo devido à necessidade de integrar os dados
da taxa de rendimento de posição, qualquer pequeno erro aumenta sem limite
constante após a integração. Sensores inerciais são, portanto, impróprios para o
posicionamento preciso por um longo período de tempo (BORENSTEIN, EVERETT
e FENG, 1996, p. 10).
A localização por balizas ativas é um método que calcula a posição
absoluta do robô a partir da medição da direção de incidência de três balizas ou
mais por transmissão de dados. Os transmissores, geralmente usando luz ou rádio
freqüência, devem estar localizados em locais conhecidos no meio ambiente
(BORENSTEIN, EVERETT e FENG, 1996, p. 10). Esse tipo de localização absoluta
utiliza métodos matemáticos como Trilateração (baseada na medida das distâncias
ou diferenças de distâncias) e Triangulação (baseada na medida dos ângulos). A
figura abaixo mostra um exemplo de um sistema de balizas ativas, o GPS.
27
Figura 3 – Sistema de Posicionamento Global. Fonte – Braunl, 2006, p. 198.
Marcos artificiais são colocados em locais conhecidos no meio ambiente
para esse método de localização. A vantagem de utilizar marcos artificiais é que eles
podem ser projetados para a detecção do ideal, mesmo sob condições ambientais
adversas. Tal como acontece com balizas ativas, três ou mais pontos de referência
devem ser visíveis para permitir a estimativa de posição. Outra vantagem é que os
erros de posição são limitados, mas a detecção de pontos de referência externos e
posição em tempo real podem não ser sempre possíveis. Marcos artificiais podem
ser definidos como um conjunto de formas ou áreas geométricas. Informações
adicionais como a distância, podem ser derivadas da medição das propriedades
geométricas do marco, mas esta abordagem é computacionalmente pesada e não
muito precisa (BORENSTEIN, EVERETT e FENG, 1996, p. 10).
No reconhecimento de Marcos Naturais, os marcos são características
distintas no meio ambiente. Não há necessidade de preparação do ambiente, mas o
ambiente deve ser conhecido antecipadamente. A confiabilidade deste método não é
tão elevado como o método de marcos artificiais (BORENSTEIN, EVERETT e
FENG, 1996, p. 11).
No método de correspondência por mapas as informações adquiridas a
partir de sensores a bordo do robô são comparadas a um mapa ou modelo do meio
ambiente. A localização baseada em mapas muitas vezes inclui a melhoria do mapa
global baseado nas novas observações sensoriais em um ambiente dinâmico e a
integração de mapas locais para o mapa global para cobrir as áreas ainda não
28
exploradas. Os mapas utilizados na navegação incluem dois tipos principais: mapas
geométricos e mapas topológicos. Mapas geométricos representam o ambiente em
um sistema global de coordenadas, enquanto mapas topológicos representam o
ambiente como uma rede de nós e arcos (BORENSTEIN, EVERETT e FENG, 1996,
p. 11).
Dentre os métodos de localização abordados, a odometria está
diretamente ligada com esse documento por diversos fatores incluindo viabilidade
econômica, facilidade de implementação física, etc.
29
3 MODELAGEM DE ROBÔS OMNI-DIRECIONAIS
Aspectos de caráter mecânico, elétrico e a disposição física das partes
que compõem um robô são características relevantes na escolha e na
implementação de seus controladores. A capacidade de um robô se locomover e as
equações que regem o seu movimento são influenciadas pela sua geometria e pela
sua dinâmica (FRANCO, 2007, p. 43).
De modo a aumentar o desempenho dinâmico de robôs, muitos têm sido
os estudos de modelos dinâmicos e cinemáticos. As estruturas em estudo são
baseadas em sistemas dinâmicos lineares e não lineares e a determinação dos seus
parâmetros tem sido alvo de contínua investigação. Uma vez construído o modelo
dinâmico é necessário fazer a estimação dos seus parâmetros. O método mais
comum para identificação de parâmetros de robôs baseia-se em modelos lineares
(mínimos quadrados, variáveis Instrumentais e toda uma série de variantes iterativas
que aplicam mínimos quadrados repetidamente sobre dados pré-filtrados de modo a
diminuir o ruído) (OLIVEIRA, 2007, p. 36).
Naturalmente, para sistemas não lineares os métodos disponíveis não
são genéricos e o problema agrava-se se houver dificuldade em obter medidas com
níveis de ruído suficientemente baixo. Torna-se, portanto interessante uma
abordagem focada nos modelos mais adequados para os robôs móveis omni-
direcionais (OLIVEIRA, 2007, p. 36).
Os modelos para robôs acionados por rodas podem ser divididos em
quatro tipos: modelo cinemático de postura, modelo cinemático de configuração,
modelo dinâmico de postura e modelo dinâmico de configuração. Os modelos
cinemáticos descrevem o robô em função da velocidade e orientação das rodas,
enquanto os modelos dinâmicos descrevem o robô em função das forças
generalizadas aplicadas pelos atuadores, por exemplo, torques nas rodas (LAGES,
1998, p. 9).
Os modelos de postura consideram como estado apenas posição e
orientação do robô, enquanto os modelos de configuração consideram além da
postura outras variáveis internas como, por exemplo, deslocamento angular das
rodas. Do ponto de vista de controle da posição e orientação espacial do robô,
30
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.
3.1 MODELO CINEMÁTICO DO ROBÔ OMNI-DIRECIONAL 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).
Figura 4 – Sistema Vetorial para um robô omni-direcional de três rodas. Fonte – Oliveira, 2007, p. 37.
A cinemática apenas tem que ver com as relações geométricas que
regem o sistema físico. Sabendo a posição do robô (x, y, ) ao longo do tempo, é
possível calcular as velocidades (vx, vy, ) a partir da derivada, tal como apresentado
nas equações abaixo (OLIVEIRA, 2007, p. 39):
(1)
(2)
31
(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):
(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):
(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):
(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:
(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
(9)
Substituindo a equação (7) na equação (9), se obtêm:
(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.
3.2 MODELO DINÂMICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS
A dinâmica do robô é o estudo do movimento relacionado com as forças
presentes no sistema físico, incluindo a energia e as velocidades. As equações da
dinâmica segundo os sistemas de eixos apresentados na figura (4) são descritas nas
equações abaixo (OLIVEIRA, 2007, p. 43):
33
(11)
(12)
(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:
(14)
(15)
(16)
As equações para as forças de atrito de Coulomb são:
(17)
(18)
(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):
(20)
(21)
(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):
(23)
(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
4 CARACTERÍSTICAS DA ELETRÔNICA EMBARCADA
A eletrônica embarcada utilizada nesse sistema é capaz de controlar as
velocidades dos motores do robô com feedback. Para tal, alguns componentes são
mostrados a seguir para um melhor entendimento do sistema.
4.1 O MICROCONTROLADOR E SUAS CARACTERÍSTICAS
Para entender a arquitetura de um chip complexo como um
microcontrolador, o diagrama abaixo mostra um modelo geral de funcionamento do
mesmo. Isso permite entender a operação geral do chip sem ter que analisar seu
circuito, que é muito complexo (BATES, 2004, p. 61).
Figura 5 – Diagrama geral de um microcontrolador. Fonte – Bates, 2004, p. 62.
O diagrama de blocos acima mostra o esquema geral de um
microcontrolador que pode ser considerado em duas partes, a seção de execução
do programa e a seção de processamento de registradores. Esta divisão reflete uma
arquitetura em que o programa e os dados são acessados separadamente,
aumentando a velocidade de execução do programa e é conhecida como arquitetura
Harvard (BATES, 2004, p. 61).
36
Todos os microcontroladores PIC utilizam a mesma arquitetura básica e
um conjunto de instruções para fornecer um caminho de evolução do produto, desde
simples programas desenvolvidos para a série "12", até as mais complexas
aplicações para dispositivos da série "18". As características comuns da arquitetura
do PIC são (BATES, 2004, p. 237):
a) Arquitetura Harvard;
b) Conjunto de instruções RISC;
c) Memória flash ROM de programa com ISP;
d) Bloco de memória RAM, incluindo SFRs;
e) EEPROM – Memória de dados não-volátil;
f) Registrador de trabalho único;
g) Pilha não gravável;
h) Power-up e watchdog timers;
i) Múltiplas fontes de interrupção;
j) Hardware timers;
k) Função sleep;
l) Programação serial in-circuit.
Os principais grupos de microcontroladores PIC são apresentados no
quadro abaixo. Eles são divididos em três grupos, com um número de prefixo
diferente: a série "12" possui PICs em miniatura de 8 pinos, o grupo "16" pode ser
descrito como a série padrão e os dispositivos da série "18" são PICs de alto
desempenho (BATES, 2004, p. 237).
Quadro 3 – Comparativo das séries 12F, 16F e 18F de microcontroladores PIC.
12FXXX 16FXXX 18FXXX
Low cost and small size Mid-range cost and
performance High performance
8-pin packages 14–40-pin packages 18–80-pin packages
6 I/O pins 12–33 I/O pins 13–68 I/O pins
33/35×12/14-bit instructions 35×14-bit instructions 58×16-bit instructions
1 k word program memory 1–8 k word program memory 2–64 k word program memory
20MHz clock 20MHz clock 40MHz clock
4MHz internal oscillator 4/8MHz internal oscillator 8/10MHz internal oscillator
8-bit and 16-bit timer 2×8-bit and 1×16-bit timers Up to 2×8-bit and 3×16-bit timers
Up to 4 analogue inputs Up to 8 analogue inputs Up to 16 analogue inputs
In-circuit programming and debugging
In-circuit programming and debugging
In-circuit programming and debugging
37
Serial communication ports, parallel slave port
Serial communication ports, parallel slave port
1/2 pulse width modulation outputs, capture & compare inputs
Up to 14 pulse width modulation outputs, capture and compare inputs
CAN communication interface
2–64 k program memory
Fonte – Bates, 2004, p. 238.
As figuras abaixo mostram um paralelo entre o PIC 16F628A e o PIC
18F4550:
Figura 6 – Diagrama de pinos e encapsulamento PDIP dos PICs a)18F4550 b)16F628A. Fonte – Microchip Technology Inc., a) 2006, p. 2 b) 2004, p. 2.
Apesar dos numerosos recursos que os microcontroladores PIC possuem,
os próximos tópicos abordam as principais funções utilizadas nesses dispositivos
para esse documento.
4.1.1 Interrupções no PIC
As interrupções no PIC são de extrema importância em diversas
aplicações, entre elas podem-se destacar interrupções de timer, interrupção por
mudança de estado e interrupção de USART.
As interrupções de timer acontecem sempre que um dos contadores de
tempo interno estoura (SOUZA, 2005, p. 32).
38
A interrupção por mudança de estado é gerada por um sinal externo
ligado a uma porta específica do PIC caso ela esteja configurada como entrada.
Esse tipo de interrupção pode ser utilizado, por exemplo, para criar um sincronismo
com a rede de 60hz (SOUZA, 2005, p. 32).
A maioria dos PICs possui um sistema completo para comunicação serial
do tipo USART. Esse sistema, além de facilitar todo processamento para a entrada e
saída de dados seriais, possui duas interrupções para informar o programa quando
um dado é recebido e quando a transmissão de outro dado já foi terminada (SOUZA,
2005, p. 33).
4.2 SENSORES E ATUADORES
Devido a sua inércia, atrito estático e as suas características
eletromagnéticas, um motor DC não começa a girar até que haja uma corrente
elétrica razoavelmente grande em seus terminais. Isso faz com que sua resposta
seja não-linear, o que significa que a velocidade não é diretamente proporcional à
corrente ou tensão fornecida (BATES, 2004, p. 213).
A velocidade de um motor não pode ser prevista com precisão para
qualquer dado atual, porque a carga sobre seu eixo irá afetá-la. Portanto, se a
velocidade ou a posição de um motor DC está sendo controlada, são necessários
sensores para medir as variáveis de saída e um sistema de controle para o
acionamento do motor (BATES, 2004, p. 213).
A velocidade de um motor DC é controlada pela corrente na sua
armadura, que interage com o campo magnético produzido por enrolamentos do
campo (ou ímãs permanentes) para produzir torque. Um sistema de controle
analógico permite o controle permanente sobre a corrente do motor, e um conversor
digital para analógico pode ser usado na saída se o feedback e o controle é digital.
No entanto, a interface de controle pode ser simplificada se um sinal PWM é
utilizado (BATES, 2004, p. 214).
Um feedback digital pode ser obtido a partir de um sensor que detecta a
rotação de um eixo. Uma maneira de fazer isso é utilizar um disco perfurado ou
setorizado ligado ao eixo e um sensor óptico para detectar as ranhuras ou furos no
39
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:
Figura 7 – Diagrama de blocos típico de controle de um motor DC. Fonte – Microchip Technology Inc., 2003 p. 1.
O sistema de feedback da figura acima é detalhado na figura abaixo com
representação de um encoder de quadratura:
Figura 8 – Esquema de funcionamento de um encoder de quadratura. Fonte – Microchip Technology Inc., 2003 p. 14.
40
Um encoder de quadratura pode fornecer dados para velocidade, direção
e posição relativa. Esse encoder de quadratura é tipicamente fixado dentro do
conjunto do motor e fornece lógicas de sinais que podem ser inseridas ao
microcontrolador. A relação de fase entre o canal A e B é usado para determinar o
sentido de rotação do motor.
4.3 AQUISIÇÃO DOS DADOS POR RADIOFREQUÊNCIA
Para analisar os dados de velocidade das rodas e do robô no espaço de
trabalho, a troca de informação entre o robô e um computador se da por
radiofreqüência. O dispositivo utilizado para transmitir essas informações pode ser
visto na figura abaixo e descrito a seguir:
Figura 9 – Módulo de comunicação por radiofreqüência XBee-Pro. Fonte – http://www.digi.com/images/products/prd-xbeepromoduleseries1_lg.jpg.
O módulo RF XBee-Pro foi projetado para atender o padrão de
comunicação IEEE 802.15.4 e satisfazer necessidades como baixo custo e baixa
potência para redes de sensores sem fio. Os módulos consomem pouca energia e
proporcionam um tráfego confiável de dados entre dispositivos (DIGI
INTERNATIONAL INC., 2009, p. 4). O quadro abaixo mostra alguns dados de
especificação desse módulo:
Quadro 4 – Dados de especificação do módulo XBee-Pro.
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
RF Data Rate 250,000 bps
Serial Interface Data Rate 1200 bps - 250 kbps (non-standard baud rates also supported)
Receiver Sensitivity -100 dBm (1% packet error rate)
Power Requirements
Supply Voltage 2.8 – 3.4 V
Transmit Current (typical) 250mA (@3.3 V) (150mA for international variant) RPSMA module only: 340mA (@3.3 V) (180mA for international variant)
Idle / Receive Current (typical) 55mA (@ 3.3 V)
Power-down Current < 10 μA
General
Operating Frequency ISM 2.4 GHz
Dimensions 0.960” x 1.297” (2.438cm x 3.294cm)
Operating Temperature -40 to 85º C (industrial)
Antenna Options Integrated Whip, Chip or U.FL Connector, RPSMA Connector
Networking & Security
Supported Network Topologies
Number of Channels 12 Direct Sequence Channels
Addressing Options PAN ID, Channel and Addresses
Agency Approvals
United States (FCC Part 15.247)
OUR-XBEEPRO
Industry Canada (IC) 4214A XBEEPRO
Europe (CE) ETSI (Max. 10 dBm transmit power output)*
Japan R201WW08215111 (Max. 10 dBm transmit power output)*
Austraila C-Tick
Fonte – Digi International Inc., 2009, p. 5.
Através de sua porta serial, o módulo XBee-Pro pode se comunicar com
qualquer lógica e tensão UART compatível, ou através de um tradutor de nível a
qualquer dispositivo serial como por exemplo uma interface RS-232 com um
computador (DIGI INTERNATIONAL INC., 2009, p. 10). Na figura abaixo se pode ver
um exemplo de comunicação entre microcontroladores através de dois módulos
XBee-Pro:
Figura 10 – Diagrama de Fluxo de Dados de um sistema com interface UART. Fonte – Digi International Inc., 2009, p. 10.
42
5 ESTIMAÇÃO DE POSIÇÃO E ORIENTAÇÃO
Hoje, várias técnicas estão disponíveis para a obtenção da posição e da
orientação de robôs móveis utilizando encoders, visão computacional, sonar e
scanner a laser (LAGES, 1998, p. 110).
A técnica Dead-Reckoning é um procedimento matemático simples para
determinar a localização atual de um robô, pelo seu deslocamento em relação a uma
posição anterior (BORENSTEIN, EVERETT e FENG, 1996, p. 13).
É conhecido que para trajetórias longas, o método de dead-reckoning não
é adequado para estimação da posição de robôs móveis, pois pela própria natureza
do método, os erros vão sendo acumulados ao integrarem-se os deslocamentos
para obtenção da posição e orientação. Os erros presentes nesse método podem
ser sistemáticos e não sistemáticos (LAGES, 1998, p. 110).
Os erros sistemáticos são causados basicamente por incertezas nos
parâmetros geométricos utilizados para calcular a posição e orientação do robô. As
principais causas são: a diferença entre o diâmetro nominal médio e o diâmetro real
das rodas, a diferença entre os diâmetros médios das rodas e a diferença entre
rodas nominal e real (LAGES, 1998, p. 110).
Erros não sistemáticos ocorrem devido ao escorregamento das rodas,
imperfeições no solo e fenômenos similares (LAGES, 1998, p. 110).
O método de dead-reckoning é parte principal para a realização do
controle de velocidade e trajetória para o robô omni-direcional e é descrita a seguir.
5.1 O MÉTODO DEAD-RECKONING
Uma das técnicas mais simples para estimação de posição e orientação
consiste em contar-se a quantidade de pulsos a partir dos encoders de cada roda
em certo intervalo de tempo (LAGES, 1998, p. 115). Assumindo que a resolução dos
encoders a uma volta completa de uma das rodas pode-se obter a variação da
43
posição angular de cada eixo j = [0,1,2] no tempo discreto como mostra as equações
abaixo:
(25)
(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:
(27)
A figura abaixo mostra a contribuição das velocidades v e para vn = 0
sendo que o robô descreve um movimento curvilíneo:
Figura 11 – Esquema de movimento do robô para uma trajetória curvilínea. Fonte – Moreira 2008, p. 17.
A aproximação discreta da posição do robô pode ser descrita pelas
equações abaixo (MOREIRA, 2008, p. 18):
Com = 0:
44
(28)
(29)
Com ≠ 0:
(30)
(31)
(32)
Com as equações descritas anteriormente o robô consegue ter dados de
posição e orientação necessários para descrever trajetórias no espaço de trabalho e
assim realizar tarefas diversas.
45
6 CONTROLE PID DISCRETO
Há vários elementos dentro de um sistema de controle e para esse
documento os termos abaixo descrevem esses principais elementos envolvidos:
a) Planta – Os motores do robô.
b) Sensores – Os encoders incrementais acoplados no eixo dos motores.
c) Set point – O valor de velocidade que o eixo da roda deve atingir.
d) Sinal de erro – É a diferença entre a resposta da planta e da resposta
desejada (valor nominal).
e) Distúrbios – São as entradas indesejadas para a planta como
irregularidades no eixo, imprecisão nos sensores, etc.
f) Controlador – O controlador é responsável por diversas tarefas e é o
elo que une todos os elementos físicos e não físicos. Ele mede o sinal
de saída dos sensores da, processa o sinal e retira um erro baseado na
medição do sinal e o valor nominal. Uma vez que os dados do sensor
foram coletados e processados, o resultado é usado para encontrar
valores PID, que então é enviado para uma central de correção de
erros. A taxa em que tudo isso acontece depende do poder de
processamento do microcontrolador (MICROCHIP TECHNOLOGY
INC., 2004a, p. 1). A figura abaixo mostra um sistema de exemplo para
um controle de um motor:
Figura 12 – Sistema de controle em malha fechada para um motor. Fonte – Microchip Technology Inc., 2004a p. 3.
Na aplicação do controlador PID existem três termos que são baseados
na medição de erro mostrados na equação abaixo (MICROCHIP TECHNOLOGY
INC., 2004b, p. 3):
46
(33)
O primeiro termo é o termo proporcional, em que Kp é a constante de
proporcionalidade, o segundo termo é o termo integrador onde K i é 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):
(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.
6.1 O MÉTODO ZIEGLER-NICHOLS EM MALHA FECHADA
Com o método de Sintonização de Ziegler-Nichols de Malha Fechada
pretende-se obter no máximo 25% de overshoot e baseia-se experimentalmente em
(NUNES e LOPES, 2001, p. 2):
a) Determinar K crítico, considerando-se apenas como ganho
proporcional, isto é fazendo Td igual a zero e Ti igual a infinito.
b) Determinar a freqüência de oscilação e, portanto, determinar o período
crítico.
A seguir apresenta-se o quadro de sintonização:
Quadro 5 – Sintonia de um controlador PID pelo método Ziegler-Nichols.
Controlador K Ti Td
P 0
PI 0
PID
Fonte – Nunes e Lopes, 2001, p. 2.
47
7 VARIÁVEIS
Variáveis Primárias Variáveis Secundárias
1. Eletrônica embarcada a) Alimentação do circuito;
b) Encoders incrementais;
c) Módulo de radiofreqüência;
d) Sistema de controle de potência dos motores.
2. Controle de velocidade a) Controle PID;
b) Modelo cinemático do robô;
c) Tensão nos motores;
d) Velocidades das rodas.
3. Controle de trajetória a) Controle PID;
b) Deslocamento das rodas;
c) Modelo matemático da função de controle;
d) Orientação do robô no espaço;
e) Posição do robô no espaço.
4. Simulação do robô a) Modelo do robô;
b) Programação do robô.
48
8 METODOLOGIA
Através de pesquisas bibliográficas em livros, monografias, teses,
pesquisas experimentais e simulações, esse trabalho foi realizado conforme a
seqüência abaixo:
a) Estudo da Robótica Móvel em diversos aspectos como inteligência
computacional, sistemas de locomoção e sistemas de localização.
Esses aspectos foram de extrema importância para o desenvolvimento
desse projeto porque após esse estudo pode-se direcionar a pesquisa
para os assuntos correntes e obter conhecimento global do assunto.
b) Estudo do modelo cinemático e dinâmico do robô omni-direcional de
três rodas. Tal estudo deve-se ao fato de que o controle de velocidade
e trajetória do mesmo depende diretamente das variáveis cinemáticas
e/ou dinâmicas do sistema.
c) Estudo dos dispositivos eletrônicos para o robô: como mostrado na
modelagem do robô, este dispõe de motores elétricos para seu
funcionamento além de sensores e dispositivos de comunicação por
radiofreqüência fazendo necessário o estudo dos mesmos.
d) Estudo de métodos matemáticos para auto-localização: tendo
estudado os dispositivos de sensoriamento, fez-se necessário um
estudo analítico de posição e orientação do robô para obter-se um
modelo matemático de re-alimentação dessas variáveis.
e) Estudou-se o método de controle Proporcional Integral Derivativo com
métodos de sintonização para ser implementado na eletrônica
embarcada.
f) Descreveu-se a eletrônica embarcada no robô visando aspectos de
inteligência computacional, sensoriamento, acionamento dos motores,
alimentação do circuito e comunicação por radiofreqüência.
g) Descreveu-se o controlador de velocidade discreto em malha fechada
do robô baseado na cinemática do mesmo.
49
h) Realizaram-se simulações do comportamento do robô num ambiente
virtual com o software SimTwo Versão 0.9 para validação do modelo
cinemático e dos controles de velocidade das rodas.
i) Descreveram-se os controladores de trajetória em três funções de
movimento: GoToTarget, FollowLine e FollowCircle.
j) Realizaram-se simulações do comportamento do robô num ambiente
virtual com o software SimTwo Versão 0.9 para validação dos
controladores de trajetória propostos.
Os tópicos mencionados nas letras “a” até “e” foram descritos do capítulo
2 ao capítulo 6, os tópicos “f” até “j” são descritos a seguir.
8.1 DESCRIÇÃO DA ELETRÔNICA EMBARCADA
A eletrônica embarcada proposta nesse trabalho possui as seguintes
características:
a) Tensão de entrada contínua 12V.
b) Regulagem de tensão contínua interna de 5V para alimentação dos
microcontroladores e demais dispositivos.
c) Regulagem de tensão contínua interna de 3.3V para alimentação do
módulo transceptor Xbee.
d) Seis saídas com tensão variável de 0V a 12V para alimentação dos
motores (duas para cada motor com inversão de sentido).
e) Três circuitos em ponte para inversão de sentido dos motores.
f) Seis entradas digitais para feedback dos sinais dos encoders (duas
para cada encoder).
O circuito possui um microcontrolador central PIC18F4550 que gerencia a
trajetória do robô. Tal microcontrolador se comunica com mais 3 outros
microcontroladores PIC16F628A através de uma comunicação USART com
mecanismo de acesso ao meio (conjunto de regras que determinam quem pode
transmitir) tipo Token-Ring em que um dispositivo só transmite após receber uma
informação ficam dispostos com topologia em anel.
50
Os três microcontroladores periféricos têm por função receber o sinal de
controle de velocidade dos motores e controlá-los individualmente. O controle dos
motores é em malha fechada e possuí como sensores encoders incrementais.
O microcontrolador central também é capaz de transmitir dados por
radiofreqüência através de um módulo transceptor Xbee conectado a uma porta
serial virtual USART criada via software, pois este PIC só possui uma única porta
serial incorporada no seu hardware. Com esse recurso o engenheiro pode visualizar
os dados processados no microcontrolador podendo analisá-los e assim realizar
melhorias.
Para facilitar a gravação e atualização do firmware no PIC18F4550 pode-
se programá-lo em modo BootLoader que nada mais é do que um firmware que
permite a gravação de um outro firmware “por cima” via comunicação serial.
A figura abaixo mostra as comunicações envolvidas na eletrônica
embarcada:
Figura 13 – Sistema de comunicação da eletrônica embarcada. Fonte – Autor, 2010.
O circuito de comunicação à esquerda da figura acima representa uma
placa confeccionada industrialmente e amplamente encontrada no mercado para
comunicação USB com o XBEE. O esquema eletrônico restante do robô encontra-se
detalhado no Apêndice A.
51
8.2 A PLATAFORMA DE SIMULAÇÃO SIMTWO
A plataforma de simulação SimTwo foi desenvolvida pelo Prof. Dr. Paulo
Costa da Faculdade de Engenharia da Universidade do Porto. Esta plataforma
evidencia um cuidado especial no realismo, especialmente na dinâmica e na
modelagem. Estes incluem características não lineares, que se revelam importantes
no comportamento dos objetos reais. O simulador permite também a visualização 3D
em tempo real da simulação, o que se revela útil para supervisão da simulação
(COSTA, 2010).
O SimTwo é um sistema de simulação realista onde é possível
implementar vários tipos de robôs como:
a) Robôs móveis com diferentes configurações como: robôs diferenciais
ou com rodas omnidirecionais;
b) Manipuladores;
c) Quadrúpedes;
d) Humanóides;
e) Qualquer tipo de robô terrestre que possa ser descrito com uma
mistura de articulações rotativas e rodas clássicas/omnidirecionais;
f) Veículos mais leves que o ar com ou sem hélices para propulsão.
O realismo da dinâmica implementada no SimTwo é conseguida
decompondo o robô num sistema de corpos rígidos e motores elétricos. A mecânica
associada aos corpos é simulada numericamente considerando as suas
propriedades físicas: forma, massa e momentos de inércia, atritos das superfícies e
elasticidade. Certas articulações típicas: eixo, cardã e calha são definidas
explicitamente e podem ter associado um sistema de acionamento e sensores
(COSTA, 2010).
O sistema de acionamento pode ser constituído por um motor DC,
opcionalmente com caixa redutora e um controlador. O controlador pode ser de
vários tipos: um PID aplicado ao sinal de posição ou de velocidade ou uma
realimentação de estado (COSTA, 2010).
O modelo do motor DC contém vários elementos não lineares tais como
saturação da tensão aplicada, limite de corrente e atrito de Coulomb (COSTA, 2010).
52
Além do sistema de controle de baixo nível, o SimTwo oferece a
possibilidade de fornecer os sinais de referência para esses controladores através
de um controlador de mais alto nível implementado pelo utilizador.
A figura abaixo mostra as janelas do SimTwo Versão 0.9.3.8 Compilado
em 19/05/2010:
Figura 14 – Ambiente 3D e demais janelas do Simulador SimTwo. Fonte – Autor, 2010.
O SimTwo recorre a várias bibliotecas de código aberto que são:
a) GLScene – biblioteca para a visualização 3D;
b) ODE – simulação de corpos rígidos;
c) Pascal Script – permite a programação do sistema de controle;
d) SynEdit – implementa o editor de scripts;
e) OmniXml – facilita o uso de arquivos de configuração em formato
XML;
f) RxLib – conjunto variado de componentes (Persistência, etc.).
53
8.2.1 O Robô Omni-direcional no SimTwo
A janela do SimTwo de nome “XML Scene Edit” é onde se realiza a
modelagem da simulação 3D. Em linguagem de programação XML pode-se modelar
robôs, estruturas fixas, corpos livres e desenhos no chão do ambiente de simulação.
Todos os corpos inclusive os robôs são derivados de estruturas geométricas simples
como cilindros, paralelepípedos, esferas combinadas.
A figura abaixo mostra o robô omni-direcional no ambiente de simulação
do SimTwo, o desenho no seu topo indica com uma seta a orientação em que este
se encontra:
Figura 15 – Robô omni-direcional no ambiente de simulação SimTwo. Fonte – Autor, 2010.
O quadro abaixo descreve as características do robô omni-direcional que
são incorporadas na simulação do SimTwo:
Quadro 6 – Características do robô omni-direcional no SimTwo.
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
Distância ao centro 0,1m
Ângulo Roda 0 60°
Ângulo Roda 1 180°
Ângulo Roda 2 -60°
Atrito estático no eixo 10-3
N.m
Atrito dinâmico no eixo 10-5
N.m/rad/s
Motor redutor
Tensão Máxima 12 V
Corrente Máxima 4 A
Rel. de Transmissão 6:1
Resistência Interna 0,3 Ω
Torque/Corrente 0,024 N.m/A
Fonte – Autor, 2010.
Ainda que sejam permitidos vários robôs na mesma simulação, o
programa de controle é único e é capaz de controlar a todos simultaneamente. Cada
robô possui um número de identificação, iniciado em zero, e atribuído pela ordem
em que é colocado no arquivo scene.xml. Por sua vez, cada robô pode possuir
vários eixos controlados e vários corpos (COSTA, 2010). O código que modela o
robô omni-direcional pode ser visto no Apêndice B.
Na janela de controle do SimTwo pode-se programar robôs,
comunicações, interações com a janela matricial, etc. Essa janela é um editor de
código para linguagem Pascal Script que tem opções de visualização de variáveis,
opções de salvar e carregar arquivos, entre outros. O fluxograma abaixo mostra o
procedimento padrão que deve existir no código para o funcionamento correto do
controle:
Figura 16 – Rotina de controle padrão do SimTwo. Fonte – Autor, 2010.
55
8.3 O CONTROLADOR DE VELOCIDADE PROPOSTO
O controle de velocidade do robô baseia-se em inserir uma velocidade
desejada do espaço de trabalho do mesmo. Como descrito no Capítulo 3, o robô
omni-direcional é capaz de se locomover com velocidades (vx, vy, w) independentes
uma da outra, ou seja, o robô pode locomover-se apenas na direção x e não
locomover-se na direção y nem na orientação θ.
Para inserir uma dada velocidade global no robô deve-se inserir uma
tensão específica em cada uma das rodas e é para determinar essas tensões que se
necessita da modelagem cinemática do mesmo. Para o controle dessa velocidade, a
re-alimentação de velocidade das rodas também é necessária.
Dada uma velocidade desejada (vx, vy, w) e a orientação atual θ do robô,
o microcontrolador central calcula o vetor (v, vn, w) do robô pela equação (4). Após
calculado, o vetor (v, vn, w) é inserido na equação (6) para se obter a velocidade
desejada das rodas do robô (v0, v1, v2). Essas velocidades são enviadas para os
microcontroladores periféricos que ficam responsáveis de inseri-las nas rodas.
Quando os microcontroladores periféricos recebem os valores de
velocidade desejado das rodas utiliza-se um algoritmo de controle PID discreto para
garantir que os eixos adquiram a tal velocidade. Esse algoritmo age com sucessivas
interações de cálculos que dependem diretamente do erro em que a velocidade do
eixo se encontra no momento da interação.
O erro de velocidade instantânea em cada eixo j=[0,1,2] é calculado
subtraindo a velocidade desejada pela velocidade instantânea como mostra a
equação abaixo:
(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
(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:
Figura 17 – Fluxograma de validação de controle de velocidade. Fonte – Autor, 2010.
A função Control é “chamada” pelo programa num período de 40 ms, esse
período é perfeitamente ajustável na janela de configuração do programa. Por
padrão pede-se ajustar esse período acima de 10 ms para o funcionamento
adequado do software.
A função “AtualizaDados” é executada para atualizar as variáveis de
velocidade e posição instantânea a cada interação do programa. A função
“VelControl” recebe os dados de identificação do robô a ser controlado e a
velocidade de referência que o mesmo deve possuir, depois compara com a
velocidade instantânea e realiza o controle de potência dos motores.
O código do controle de velocidade encontra-se no Apêndice C.
57
8.4 O CONTROLADOR DE TRAJETÓRIA PROPOSTO
O controle de trajetória do robô baseia-se em três funções que movimento
do robô no espaço global, essas funções são GoToTarget, FollowLine e
FollowCircle. Essas funções são descritas a seguir.
8.4.1 Descrição da Função GoToTarget
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:
Figura 18 – Demonstração da função GoToTarget. Fonte – Autor, 2010.
A figura acima mostra a trajetória do robô com a função GoToTarget,
observa-se que o percurso que o robô descreve não é de devida importância para
essa função. Sendo que a posição do robô é descrita em (x, y, ) este sai em (0, 0,
0º) e chega até o alvo (2, 1, 45º).
58
O controle proposto da referida função baseia-se em um algoritmo PID
discreto como descrito no Capítulo 6. Para cada variável (x, y, ) calcula-se o erro
relativo ao ponto alvo subtraindo a coordenada (x, y, ) do alvo à coordenada atual
do robô. Depois de calculados os erros do sistema calculam-se a saída do
controlador conforme a equação (34). Tal saída é normalizada para valores de
velocidade (vx, vy, w) que são inseridas no controlador de velocidade e as constantes
K, Ti e Td são obtidas experimentalmente com a ajuda do método de sintonização
Ziegler-Nichols.
8.4.2 Descrição da Função FollowLine
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:
(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:
(38)
59
(39)
A variável Linha é o ângulo que a reta faz com a horizontal e é descrita
pela equação abaixo:
(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:
Figura 19 – Erro de distância do FollowLine. Fonte – Autor, 2010.
Para a orientação calcula-se o erro subtraindo a orientação desejada
pela orientação atual do robô. Depois de calculado o erro do de orientação calcula-
se a saída do controlador conforme a equação (34). Tal saída é normalizada para
um valore de velocidade angular w que é inserido no controlador de velocidade.
A figura abaixo demonstra uma trajetória em uma reta para uma
orientação de 90º e os pontos (0, 0) e (2, 1) da reta com o robô na posição (0, 0, 0º):
60
Figura 20 – Demonstração da função FollowLine. Fonte – Autor, 2010.
8.4.3 Descrição da Função FollowCircle
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
Figura 21 – Erro de distância do FollowCircle. Fonte – Autor, 2010.
A equação abaixo mostra o cálculo do erro para o FollowCircle:
(41)
A figura abaixo demonstra a função FollowCircle em que o robô sai de (0,
0, 0º) e segue por um círculo de raio 1 e centro (0, 0):
Figura 22 – Demonstração da função FollowCircle. Fonte – Autor, 2010.
Observa-se que na figura acima o robô aproxima-se da curva em sentido
horário permanece seguindo a curva com orientação de 45º como desejado. O
código completo da simulação pode ser visto no Apêndice D.
62
9 RESULTADOS OBTIDOS
Na simulação da eletrônica obteve-se o funcionamento correto das
comunicações envolvidas e do funcionamento dos motores, com a ajuda de
terminais virtuais para comunicação serial pode-se visualizar o tráfego de caracteres
entre os microcontroladores, porém com um grave atraso relativo ao processamento
computacional da simulação. O controle de motores é perfeitamente aceitável visto a
disponibilidade de simulação dos encoders.
O controle de velocidade na simulação com SimTwo satisfaz sem
nenhuma dúvida o funcionamento do mesmo. A figura abaixo mostra o gráfico das
velocidades vx e vy pelo tempo com um set point de (0.1, 0.1, 0) sendo (vx, vy, w):
Figura 23 – Gráfico velocidade x tempo da função VelControl. Fonte – Autor, 2010.
O gráfico abaixo mostra o mesmo ensaio realizado anteriormente, porém
com as velocidades angulares das rodas:
63
Figura 24 – Gráfico velocidade angular x tempo da função VelControl. Fonte – Autor, 2010.
O método 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 calibrar os controladores PID de
posição com essa 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 do
método Dead-Reckoning:
Figura 25 – Gráfico posição x tempo da função GoToTarget sem Dead-Reckoning. Fonte – Autor, 2010.
64
No gráfico acima se observa que a orientação do robô está medida em
radianos. A figura abaixo mostra a mesma simulação em 3D:
Figura 26 – Simulação 3D da função GoToTarget sem Dead-Reckoning. Fonte – Autor, 2010.
O mesmo ensaio, porém com utilização do método Dead-Reckoning pode
ser visto no gráfico abaixo:
Figura 27 – Gráfico posição x tempo da função GoToTarget com Dead-Reckoning. Fonte – Autor, 2010.
Na figura abaixo se pode ver o ensaio acima na simulação 3D:
65
Figura 28 – Simulação 3D da função GoToTarget com Dead-Reckoning. Fonte – Autor, 2010.
No caso da função FollowLine não se obteve sucesso no uso do método
Dead-Reckoning e por isso, esse método foi descartado também para a função
FollowCircle. No ensaio abaixo pode-se ver o funcionamento do FollowLine em que
o robô parte do ponto (0, 0, 0º) e segue pro uma reta que passa pelos pontos (0, 0) e
(2, 1) com velocidade nominal 0.5 m/s e orientação 30º:
Figura 29 – Gráfico posição x tempo da função FollowLine. Fonte – Autor, 2010.
O ensaio acima pode ser visto no modo de simulação 3D conforme a
figura abaixo:
66
Figura 30 – Simulação 3D da função FollowLine. Fonte – Autor, 2010.
A função FollowCircle pode ser vista no ensaio abaixo. Nesse ensaio o
robô encontra-se inicialmente na posição (0, 0, 0º) e seque por um círculo de centro
(0, 0, 0º) e raio 1 m com velocidade nominal 0.5 m/s e orientação 90º:
Figura 31 – Gráfico posição x tempo da função FollowCircle. Fonte – Autor, 2010.
O ensaio acima pode ser visto na simulação 3D pela figura abaixo:
67
Figura 32 – Simulação 3D da função FollowCircle. Fonte – Autor, 2010.
O quadro abaixo mostra o valor das constantes dos controladores obtidas
experimentalmente com a ajuda do método de sintonização Ziegler-Nichols:
Quadro 7 – Constantes dos Controladores.
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
Fonte – Autor, 2010.
68
10 CONCLUSÃO
A eletrônica embarcada obteve êxito no que diz respeito a comunicação
entre os microcontroladores e por radiofreqüência, controle dos motores e
alimentação do circuito. Contudo, a simulação não prevê o funcionamento em tempo
real do controle pois o software é incapaz de realizar esse tipo de procedimento.
Sendo assim, somente um ensaio real é capaz de informar a capacidade dos
microcontroladores quando processarem os algoritmos de cinemática e de controle
propostos.
O algoritmo de controle de velocidade obteve sucesso e pode ser
implementado perfeitamente em um robô real. A simulação para esse controle se
aproxima muito do modelo real provando a capacidade desse controle.
Durante a simulação pode-se perceber que o método Dead-Reckoning
não pode ser utilizado para trajetórias grandes e assim não se conseguiu atingir uma
realimentação de posição apenas com esse método.
Os algoritmos de controle de trajetória foram todos satisfatórios e,
portanto podem ser implementados em escala real, no entanto, os mesmos foram
calibrados com pouca aproximação do modelo real porque a simulação não insere
erros na re-alimentação de posição, portanto, conseguiu-se um modelo de controle
ideal que depende exclusivamente no quão preciso é a re-alimentação de posição.
69
REFERÊNCIAS
BATES, Martin. PIC Microcontrollers: An Introduction to Microelectronics. 2ª ed. Elsevier, 2004. 372p. BORENSTEIN, Johann; EVERETT, H. R.; FENG, Liqiang. Where am I? Sensors and Methods for Mobile Robot Positioning. 1996. 281f. Oak Ridge National Lab
(ORNL) D&D Program and United States Department of Energy, University of Michigan, Michigan. BRAUNL, Thomas. Embedded Robotics: Mobile Robot Design and Applications with Embedded Systems. 2ª ed. Berlin: Springer, 2006. 458p.
CONCEIÇÃO, André Gustavo Scolari. Controlo e Cooperação de Robôs Móveis Autónomos Omnidireccionais. 2007. 155f. Tese (Doutorado) – Engenharia
Eletrotécnica e de Computadores, Universidade do Porto, Porto. 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. DIGI INTERNATIONAL INC. XBee®/XBee-PRO® RF Modules: Product Manual v1.xEx - 802.15.4 Protocol, 2009. Disponível em: <http://ftp1.digi.com/support/documentation/90000982_B.pdf>. Acesso em: 15 de Novembro 2009. FRANCO, Alexandre da Costa e Silva. Geração e Controle de Trajetória de Robôs Móveis Omnidirecionais. 2007. 142f. Tese (Mestrado) - Escola Politécnica,
Universidade Federal da Bahia, Salvador. LAGES, Walter Fetter. Controle e Estimação de Posição e Orientação de Robôs Móveis. 1998. 159f. Tese (Doutorado) - Centro Técnico Aeroespacial, Instituto Tecnológico de Aeronáutica, São José dos Campos. MICROCHIP TECHNOLOGY INC. Motor Control Sensor Feedback Circuits, 2003. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00894a.pdf>. Acesso em: 15 de Novembro 2009.
70
MICROCHIP TECHNOLOGY INC. PIC16F627A/628A/648A Data Sheet: Flash-
Based 8-Bit CMOS Microcontrollers with nanoWatt Technology, 2004. Disponível em: <http://ww1.microchip.com/downloads/en/DeviceDoc/40044F.pdf>. Acesso em: 11 de Novembro 2009. MICROCHIP TECHNOLOGY INC. Implementing a PID Controller Using a PIC18 MCU, 2004a. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00937a.pdf>. Acesso em: 29 de Novembro 2009. MICROCHIP TECHNOLOGY INC. Software PID Control of an Inverted Pendulum Using the PIC16F684, 2004b. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00964A.pdf>. Acesso em: 28 de Novembro 2009. MICROCHIP TECHNOLOGY INC. PIC18F2455/2550/4455/4550 Data Sheet:
28/40/44-Pin, High-Performance, Enhanced Flash, USB Microcontrollers with nanoWatt Technology, 2006. Disponível em: <http://ww1.microchip.com/downloads/en/devicedoc/39632c.pdf>. Acesso em: 11 de Novembro 2009. MOREIRA, António Paulo Gomes Mendes. Sistemas Robóticos Autónomos. 2008. 84f. Notas de aula (Mestrado) – Engenharia Eletrotécnica e de Computadores, Universidade do Porto, Porto. MURPHY, Robin R. Introduction to AI Robotics. 1ª Ed. Massachusetts: MIT Press,
2000. 466p.
NUNES, Urbano Carreira; LOPES, Ana Cristina. 3ª Aula de Controlo Inteligente: Controlo PID Discreto, 2001. Disponível em: <http://orion.ipt.pt/~anacris/ci_1/pdf/aula3.pdf>. Acesso em: 29 de Novembro 2009.
OLIVEIRA, Hélder Filipe Pinto de. Análise do Desempenho e da Dinâmica de Robôs Omnidireccionais de Três e Quatro Rodas. 2007. 156f. Tese (Mestrado) –
Engenharia Eletrotécnica e de Computadores, Universidade do Porto, Porto. SCATENA, Jean Miler. Ambiente de Desenvolvimento de Aplicações para Robôs Móveis. 2008. 131f. Tese (Doutorado) - Escola de Engenharia Elétrica de São Carlos, Universidade de São Paulo, São Carlos.
71
SIEGWART, Roland; NOURBAKHSH, Illah R. Introduction to Autonomous Mobile Robots. 1ª Ed. Massachusetts, EUA: MIT Press, 2004. 321p.
SOUZA, David José de. Desbravando o PIC: Ampliado e Atualizado para PIC 16F628A. 9ª ed. São Paulo: Érica, 2005. 267p.
72
APÊNDICE A – ESQUEMA ELETRÔNICO DO ROBÔ
73
APÊNDICE B – CÓDIGO DE MODELAGEM DO ROBÔ
<?xml version="1.0" ?> <robot> <kind value='Robot'/> <defines> <const name='Raio' value='0.09' /> <const name='Altura' value='0.125' /> <const name='Densidade' value='500' /> <const name='Raio_Roda' value='0.04' /> <const name='Largura_Roda' value='0.025' /> <const name='Distancia_Ao_Centro' value='0.1' /> <const name='Densidade_Roda' value='3000' /> <const name='Encoder_PPR' value='1000' /> <const name='Caixa_de_Reducao' value='6' /> <const name='Angulo_Roda_0' value='60' /> <const name='Angulo_Roda_1' value='180' /> <const name='Angulo_Roda_2' value='-60' /> </defines> <solids> <cylinder> <ID value='Corpo'/> <mass value='Densidade*3.1415*(Raio*Raio)*Altura'/> <size x='Raio' y='0' z='Altura'/> <pos x='0' y='0' z='0.01+Altura/2'/> <rot_deg x='0' y='0' z='0'/> <color_rgb r='128' g='0' b='0'/> <texture name='MatBallTriangle' scale='6'/> </cylinder> </solids> <wheels> <default> <omni/> <tyre mass='Densidade_Roda*3.1415*(Raio_Roda*Raio_Roda)*Largura_Roda' radius='Raio_Roda' width='Largura_Roda' centerdist='Distancia_Ao_Centro'/> <surface mu='1' mu2='0.001'/> <axis angle='0'/> <motor ri='0.3' ki='2.4e-2' vmax='12' imax='4' active='1'/> <gear ratio='Caixa_de_Reducao'/> <friction bv='1e-5' fc='1e-3' coulomblimit='1e-2'/> <encoder ppr='Encoder_PPR' mean='0' stdev='0'/> <controller mode='pidspeed' kp='0' ki='0' kd='0' kf='0' active='0' period='10'/> <color_rgb r='128' g='0' b='128'/> </default> <wheel> <axis angle='Angulo_Roda_0'/> </wheel> <wheel> <axis angle='Angulo_Roda_1'/> </wheel> <wheel> <axis angle='Angulo_Roda_2'/> </wheel> </wheels> </robot>
74
APÊNDICE C – CÓDIGO DE CONTROLE DE VELOCIDADE
// 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.angle; // 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
APÊNDICE D – CÓDIGO COMPLETO DA SIMULAÇÃO
// 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
var a, b, c, ThetaLinha, ThetaLinhaNorm: Double; //Variáveis para Geração da Reta var SinalX, SinalY, SinalXPID, SinalYPID: Double; //Sinais de Controle das Componentes de Velocidade // Variáveis de Controle de Círculo var VelCircle: TState2D;//Velocidade Resultante no Espaço para Controle de Círculo (Vx, Vy, W) var VelPIDCircle: Double; //Velocidade no Espaço para Controle de Círculo (Vx, Vy, W) var ErroThetaCircle: Double; //Erro de Angulação do Robô var ErroCircle: Double; //Erro de Distância ao Círculo var ErroACircle: Double; //Erro de Distância ao Círculo var DerivadaCircle: Double; //Derivada do Erro de Distância ao Círculo var IntegralCircle: Double; //Integral do Erro de Distância ao Círculo var IntegralThetaCircle: Double; //Integral do Erro de Angulação do Robô // 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.angle; // 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 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
79
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.angle; // 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.angle); // 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
procedure FollowLine(Robot: LongInt; VNom: Double; FlagSentido: Integer; PontoA, PontoB: TState2D; Angulo: Double); begin VNom := Abs(VNom); // Cálculo do ângulo que a Reta faz com a Horizontal ThetaLinha := ATan2((PontoB.y-PontoA.y),(PontoB.x-PontoA.x)); // Normatização do ângulo que a Reta faz com a Horizontal 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 da Reta e dos Sinais de Controle if((PontoB.y-PontoA.y) = 0)then // Linha Horizontal begin a := 0; b := -1; c := PontoA.y; if(c <> 0)then begin SinalX := FlagSentido*(PontoA.y)/Abs(PontoA.y); SinalY := (PontoA.y)/Abs(PontoA.y); end else begin SinalX := FlagSentido; SinalY := 0; end; if(ErroLine >= 0)then begin SinalXPID := 1; SinalYPID := 1; end else begin SinalXPID := -1; SinalyPID := -1; end; end else if((PontoB.x-PontoA.x) = 0)then // Linha Vertical begin a := -1; b := 0; c := PontoA.x; if(c <> 0)then begin SinalX := (PontoA.x)/Abs(PontoA.x); SinalY := FlagSentido*(PontoA.x)/Abs(PontoA.x); end else begin SinalX := 0; SinalY := FlagSentido; end; if(ErroLine >= 0)then begin SinalXPID := 1; SinalYPID := 1;
81
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*Cos(Abs(ThetaLinhaNorm)); VelLine.y:=SinalYPID*Abs(VelPIDLine)*Cos(Abs(ThetaLinhaNorm))+SinalY*VNom*Sin(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(ThetaLinhaNorm)); VelCircle.y := SinalYPID*Abs(VelPIDCircle)*Sin(Abs(ThetaLinhaNorm))+SinalY*VNom*Cos(Abs(ThetaLinhaNorm)); 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: [email protected]