Você está na página 1de 87

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 Sidney 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 Telma Mota que me educou com carinho, amor,
atenção e outras qualidades infinitas ao meu pensamento.
À minha irmã Bianca 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 RODAS30


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ô
2
J [kg·m ] 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
Arrangement Description
Wheels
One steering wheel in the front, one traction wheel in the rear

02
Two-wheel differential drive with the center of mass (COM)
below the axle

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
03
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

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
04 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):
 cos  sin  0  
    sin  cos  0 · 

 0 0 1 
(4)
A partir da cinemática inversa é possível determinar as velocidades vx e
vy, tal como pode ser observado na seguinte equação (OLIVEIRA, 2007, p. 40):
  cos   sin  0 
 

   sin  cos  0 ·  
 0 0 1 

(5)
A relação entre as velocidades das rodas v0, v1 e v2, com as velocidades
do robô v, vn e ω é descrita pela equação abaixo (OLIVEIRA, 2007, p. 40):
# #
  !sin " 3 % cos " 3 % & ) 
(
   0 1 & ( .  
  # # ( 
 sin " 3 % cos " 3 % & '
(6)
Sendo R a distancia do centro da roda j=[0,1,2] até sua extremidade, se
tem:
+  + · ,

(7)
Então, a equação de cinemática inversa do robô omni-direcional de três
rodas sendo 60º o ângulo de inclinação das rodas é dada por:

!√3 1 & )
  
1 2 2 (
    0 (
1 & ·  
, (
  √3 1 ( 
 2  &'
2
(8)
Segundo Oliveira (2007, p. 41), se pode obter a equação de cinemática
direta a partir da equação de cinemática inversa, resultando em:
32

1 1
! # 0 # )
2 · sin " % 2 · sin " % (
3 3
 1 1 1 (  
# # # (
 
    2 · cos " % / 1 cos " % / 1 2 · cos " % / 1 ( ·  
 3 3 3  
π (
1 cos " % 1 (
3
# # # (
 2 ·  · cos " % / 1  · cos " % / 1 2 ·  · cos " % / 1'
3 3 3
(9)
Substituindo a equação (7) na equação (9), se obtêm:

!√3 0
√3 )
 3 3 (  
1 2 1 (
   ,  ·   
 3 3 3 ((  
1 1 1 (
3. & 3. & 3. & '
(10)
Os controladores de trajetória utilizam o sistema de coordenadas global,
ou seja, um sistema de coordenadas do meio em que o robô se encontra. Para se
localizar, o robô precisa saber a relação de sua posição atual com o ponto de
referência no sistema global, por isso, utilizam-se as equações (4) e (5) para esse
fim.
As equações (8) e (10) relacionam as velocidades angular das rodas nos
seus respectivos sistemas de coordenadas com as velocidades do robô no sistema
de coordenadas global, representado pela figura (4). Essas relações são importantes
para o controle de posicionamento do robô visto que a odometria é o principal meio
de obtenção de re-alimentação, obtendo valores relativos discretos de posição das
rodas.

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


1  2 34   354   364 

(11)
 
1  2 34   354   364 

(12)
 
7  2 8  859   869 

(13)
As equações acima sugerem que a aceleração do robô segundo as
direções v, vn e o seu eixo de rotação é igual ao somatório das forças do robô,
subtraída das forças de atrito aplicadas nessas mesmas direções ou eixo de
rotação. As forças de atrito presentes no robô são, neste caso, designadas de atrito
viscoso ou dinâmico e atrito de Coulomb ou estático. Os atritos viscosos (FBv, FBvn,
TBω), apresentam-se como uma relação linear entre a força aplicada e a velocidade,
enquanto que os atritos de Coulomb (FCv, FCvn, TCω), apresentam uma amplitude
constante e sinal de modo a contrariar o movimento (OLIVEIRA, 2007, p. 44).
As equações para as forças de atrito viscoso são:
354   :4 · 
(14)
354   :4 · ;
(15)
359   :9 · 
(16)
As equações para as forças de atrito de Coulomb são:
364   <4 · =>?;
(17)
364   <4 · =>?; 
(18)
869   <9 · =>?; 
(19)
Atrito viscoso é uma força que surge quando um determinado corpo
apresenta movimento relativo, opondo-se a esse movimento (OLIVEIRA, 2007, p.
45). E atrito de Coulomb é presente quando o corpo se encontra na iminência do
movimento, ou seja, no princípio da atuação das forças aplicadas. Contudo a
34

explicação mais simples diz que é a força que o corpo terá de fazer para vencer o
seu estado de repouso (OLIVEIRA, 2007, p. 46).
Sendo 60º a inclinação das rodas, as relações entre as forças de tração e
torque do robô com as forças de tração nas rodas são descritas pelas seguintes
equações (OLIVEIRA, 2007, p. 47):
#
2 34   @   @  · sin " %
3
(20)
#
2 3   @  / @  / @  · cos " %
3
(21)
2 8  @  / @  / @  · 

(22)
A força de tração em cada roda é calculada através do torque nas rodas
j=[0,1,2] que, por sua vez, é determinado usando a corrente consumida pelo motor,
tal como é descrito nas seguintes equações (OLIVEIRA, 2007, p. 47):
8+ 
@+  
,
(23)
8+   A · BC · >+ 

(24)
Como mostrado, a partir das equações (23) e (24) calcula-se a força
aplicada em cada roda a partir de uma entrada de corrente elétrica, essas forças são
inseridas nas equações (20), (21) e (22) para se obter as forças geradas pelo robô
no sistema de coordenadas local. Com as equações (14) a (19) calcula-se as forças
de atrito estático e dinâmico nas rodas. Por fim, as equações de força são inseridas
na equação de dinâmica geral relacionado forças e velocidades utilizadas pelos
controladores.
35

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


Mid-range cost and
Low cost and small size High performance
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
Up to 2×8-bit and 3×16-bit
8-bit and 16-bit timer 2×8-bit and 1×16-bit timers timers
Up to 4 analogue inputs Up to 8 analogue inputs Up to 16 analogue inputs
In-circuit programming and In-circuit programming and In-circuit programming and
debugging debugging debugging
37

Serial communication ports, Serial communication ports,


parallel slave port parallel slave port
1/2 pulse width modulation Up to 14 pulse width modulation
outputs, capture & compare outputs, capture and compare
inputs 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
250mA (@3.3 V) (150mA for international variant) RPSMA
Transmit Current (typical)
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
OUR-XBEEPRO
15.247)
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:
FG
,DE 
2·#
(25)
FG ;  FG ;  1
∆ + ; 
,DE
(26)
Assumindo que em um intervalo de tempo ta de amostra entre os
instantes n-1 e n pode-se obter a variação da posição e orientação do robô num
instante n tornando discreta a equação de cinemática direta obtendo a equação
abaixo:

!√3 0
√3 )
I ; 3 3 ( ∆  ;
1 2 1 (
  ;   , ·  · ∆  ;
∆ ; 3 3 3 (( ∆  ;
1 1 1 (
3. & 3. & 3. & '
(27)
A figura abaixo mostra a contribuição das velocidades v e ω para vn = 0
sendo que o robô descreve um movimento curvilíneo:

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

; / 1  ; / I · cos ;   · sin ;


(28)
; / 1  ; / I · sin ; /  · cos ;
(29)
Com ω ≠ 0:
JI · sin∆ ; /  · cosK∆ ;L  1M ∆ ;
; / 1  ; / · cos ; / 
∆ ; 2
JI · 1  cosK∆ ;L /  · sin∆ ;M ∆ ;
 · sin ; / 
∆ ; 2
(30)
JI · sin∆ ; /  · cosK∆ ;L  1M ∆ ;
; / 1  ; / · sin ; / 
∆ ; 2
JI · 1  cosK∆ ;L /  · sin∆ ;M ∆ ;
/ · cos ; / 
∆ ; 2
(31)
; / 1  ; / ∆ ;
(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
C
N
<  BG · N / BI O N ·  / BP ·



(33)
O primeiro termo é o termo proporcional, em que Kp é a constante de
proporcionalidade, o segundo termo é o termo integrador onde Ki é a constante de
integração e o terceiro termo é o derivativo em que Kd é a constante de derivação.
Para o controle digital pode-se tornar todos os termos discretos obtendo a
equação abaixo (MICROCHIP TECHNOLOGY INC., 2004b, p. 3):

Q N;  N;  1
<;  B · N; / · 2 N; / 8P · 
8I Q


(34)
A sintonia de um controlador PID baseia-se em determinar as constantes
de proporcionalidade, integração e derivação. Para tal existem métodos
experimentais diversificados conforma o modelo do sistema. A seguir é descrito o
método Ziegler-Nichols de Malha Fechada.

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
0,5 · B&T ∞
1
P 0
0,45 · B&T ·W
1,2 &T
PI 0
PID 0,6 · B&T 0,5 · W&T 0,125 · W&T
Fonte – Nunes e Lopes, 2001, p. 2.
47

7 VARIÁVEIS

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 4A
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:
N+ ;  +  + ;

(35)
Encontrado o erro de velocidade, calcula-se a saída do controlador
conforme a equação (34). As constantes K, Ti e Td são obtidas experimentalmente
com a ajuda do método de sintonização Ziegler-Nichols mostrado no Capítulo 6.
Para saber o valor da velocidade atual de cada eixo utiliza-se a equação (7), sendo
que a velocidade angular instantânea das rodas j=[0,1,2] é calculada conforme a
equação (3), em que sua forma discreta no tempo é mostrada na equação abaixo:
56

∆ + ;
+ ; 
8Q
(36)
Para validar o controle de velocidade que depende da modelagem
cinemática do robô, o fluxograma abaixo mostra a seqüência de comandos do
SimTwo que são escritas no editor de código em Pascal Script detalhando as
funções Initialize e Control:

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:
Y· /Z· /[
N; 
√Y / Z 
(37)
Depois de calculado o erro do sistema calcula-se a saída do controlador
conforme a equação (34) normalizada a um valor de velocidade resultante. Tal
velocidade é então distribuída em duas componentes e somadas duas a duas com
mais outras componentes que representam a velocidade nominal de entrada da
função. As equações abaixo mostram o somatório vetorial das velocidades:
  \]^ · sin _I`Q  / abc · cos _I`Q 
(38)
59


 \]^ · cos _I`Q  / abc · sin _I`Q 

(39)
A variável θLinha é o ângulo que a reta faz com a horizontal e é descrita
pela equação abaixo:
  
_I`Q  tanf  
  
(40)
Como a reta pode assumir diversas configurações no plano utiliza-se a
função computacional arco-tangente2 que abrange todos os quadrantes do espaço
cartesiano. A figura abaixo mostra o erro de distância do robô á reta:

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:


N;  ,Y>g  h  &  /   & 
(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


obteve o funcionamento correto das
comunicações envolvidas e do funcionamento dos motores, com a ajuda de
terminais virtuais para comunicação serial pode-se
pode se visualizar o tráfego de caracteres
entre os microcontroladores, porém
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.
encoders
O controle de velocidade na simulação com SimTwo satisfaz sem
nenhuma dúvida o funcionamento do mesmo.
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


elocidade x tempo
empo da função VelControl.
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


elocidade angular x tempo
empo da função VelControl.
VelControl
Fonte – Autor, 2010.

O método Dead-Reckoning
Dead Reckoning descrito no Capítulo 5 foi implementado no
SimTwo a partir da função GoToTarget pois não se fazia necessário a utilização
desta para o controle de velocidade. O próprio simulador é capaz de fornecer a
posição e orientação do robô, assim pode-se
pode se calibrar os controladores PID de
posição com essa
ess função,
função o que garantiu precisão do controle. O gráfico abaixo
mostra um ensaio da função GoToTarget para uma posição inicial do robô (0, 0, 0º)
e posição final (0.5, 0.5, 45º) e velocidade nominal 0.5 m/s sem a utilização
utilização do
método Dead-Reckoning
Reckoning
Reckoning:

Figura 25 – Gráfico posição x tempo da função GoToTarget sem Dead-Reckoning


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
mostra a mesma simulação em 3D:

Figura 26 – Simulação 3D da função GoToTarget sem Dead-Reckoning


Reckoning.
Fonte – Autor, 2010.

O mesmo ensaio, porém com utilização do método Dead Reckoning pode


Dead-Reckoning
ser visto no gráfico abaixo:

Figura 27 – Gráfico posição x tempo da função GoToTarget com Dead-Reckoning


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


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
Dead-Reckoning
FollowCircle. No ensaio abaixo pode-se
pode se ver o funcionamento do FollowLine em que
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
orientaç 30º:
º:

Figura 29 – Gráfico posição x tempo da função FollowLine.


FollowLine
Fonte – Autor, 2010.

O ensaio
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.


FollowLine
Fonte – Autor, 2010.

A função FollowCircle pode ser vista no ensaio abaixo. Nesse ensaio o


robô encontra-
encont -se
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.


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.an
gle;
// Cálculo do Erro de Velocidade das Rodas
Erro.x := VrRef.x -Vr.x;
Erro.y := VrRef.y -Vr.y;
Erro.angle := VrRef.angle-Vr.angle;
// Cálculo da Derivada do Erro de Velocidade das Rodas
Derivada.x := (Erro.x -ErroA.x) /TEMPO_DE_AMOSTRA;
Derivada.y := (Erro.y -ErroA.y) /TEMPO_DE_AMOSTRA;
Derivada.angle := (Erro.angle-ErroA.angle)/TEMPO_DE_AMOSTRA;
// Cálculo da Integral do Erro de Velocidade das Rodas
Integral.x := Integral.x +TEMPO_DE_AMOSTRA*Erro.x;
Integral.y := Integral.y +TEMPO_DE_AMOSTRA*Erro.y;
Integral.angle := Integral.angle+TEMPO_DE_AMOSTRA*Erro.angle;
// Cálculo do Erro Anterior de Velocidade das Rodas
ErroA.x := Erro.x;
ErroA.y := Erro.y;
ErroA.angle := Erro.angle;
// PIDs de Tensão das Rodas
U.x := KP*(Erro.x +(1/TI)*Integral.x +TD*Derivada.x);
U.y := KP*(Erro.y +(1/TI)*Integral.y +TD*Derivada.y);
U.angle := KP*(Erro.angle+(1/TI)*Integral.angle+TD*Derivada.angle);
// Controle de Saturação dos PIDs
if(U.x>TENSAO_MAX)then //Saturação no Controlador da Roda 0
begin
75

U.x := TENSAO_MAX;
Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x;
end;
if(U.x<-TENSAO_MAX)then
begin
U.x := -TENSAO_MAX;
Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x;
end;
if(U.y>TENSAO_MAX)then //Saturação no Controlador da Roda 1
begin
U.y := TENSAO_MAX;
Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y;
end;
if(U.y<-TENSAO_MAX)then
begin
U.y := -TENSAO_MAX;
Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y;
end;
if(U.angle>TENSAO_MAX)then //Saturação no Controlador da Roda 2
begin
U.angle := TENSAO_MAX;
Integral.angle:=Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
if(U.angle<-TENSAO_MAX)then
begin
U.angle := -TENSAO_MAX;
Integral.angle:=Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle;
end;
// Insere o Valor Controlado de Tensão nas Rodas
SetAxisVoltageRef(Robot, ID_RODA_0, U.x);
SetAxisVoltageRef(Robot, ID_RODA_1, U.y);
SetAxisVoltageRef(Robot, ID_RODA_2, U.angle);
end;
// Atualiza os dados de posição e orientação
procedure AtualizaDados;
begin
// Cálculo do Deslocamento Linear das Rodas
DTheta.x := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_0)/ENCODER_PPR;
DTheta.y := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_1)/ENCODER_PPR;
DTheta.angle := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_2)/ENCODER_PPR;
// Cálculo da Velocidade Linear das Rodas
Vr.x := (DTheta.x/TEMPO_DE_AMOSTRA)*RAIO;
Vr.y := (DTheta.y/TEMPO_DE_AMOSTRA)*RAIO;
Vr.angle := (DTheta.angle/TEMPO_DE_AMOSTRA)*RAIO;
// Cálculo de Posicionamento do Robô
RobotPos := GetRobotPos2D(ID_ROBOT_1);
end;
// This procedure is called periodicaly (default: 40 ms)
procedure Control;
begin
AtualizaDados;
AddTrailNode(ID_ROBOT_1, RobotPos.x, RobotPos.y, 0.001);
VelControl(ID_ROBOT_1, VelGlobal);
end;
// This procedure is called once when the script is started
procedure Initialize;
begin
SetRobotPos(ID_ROBOT_1, 0, 0, 0, 0);
RobotPos.x := 0;
RobotPos.y := 0;
RobotPos.angle := 0;
VrRef.x := 0;
VrRef.y := 0;
VrRef.angle := 0;
Vr.x := 0;
Vr.y := 0;
Vr.angle := 0;
DTheta.x := 0;
DTheta.y := 0;
DTheta.angle := 0;
76

Erro.x := 0;
Erro.y := 0;
Erro.angle := 0;
ErroA.x := 0;
ErroA.y := 0;
ErroA.angle := 0;
Derivada.x := 0;
Derivada.y := 0;
Derivada.angle := 0;
Integral.x := 0;
Integral.y := 0;
Integral.angle := 0;
U.x := 0;
U.y := 0;
U.angle := 0;
VRef.x := 0;
VRef.y := 0;
VRef.angle := 0;
VelGlobal.x := 0.1;
VelGlobal.y := 0.1;
VelGlobal.angle := 0.1;
end;
77

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.an
gle;
// Cálculo do Erro de Velocidade das Rodas
Erro.x := VrRef.x -Vr.x;
Erro.y := VrRef.y -Vr.y;
Erro.angle := VrRef.angle-Vr.angle;
// Cálculo da Derivada do Erro de Velocidade das Rodas
Derivada.x := (Erro.x -ErroA.x) /TEMPO_DE_AMOSTRA;
Derivada.y := (Erro.y -ErroA.y) /TEMPO_DE_AMOSTRA;
Derivada.angle := (Erro.angle-ErroA.angle)/TEMPO_DE_AMOSTRA;
// Cálculo da Integral do Erro de Velocidade das Rodas
Integral.x := Integral.x +TEMPO_DE_AMOSTRA*Erro.x;
Integral.y := Integral.y +TEMPO_DE_AMOSTRA*Erro.y;
Integral.angle := Integral.angle+TEMPO_DE_AMOSTRA*Erro.angle;
// Cálculo do Erro Anterior de Velocidade das Rodas
ErroA.x := Erro.x;
ErroA.y := Erro.y;
ErroA.angle := Erro.angle;
// PIDs de Tensão das Rodas
U.x := KP*(Erro.x +(1/TI)*Integral.x +TD*Derivada.x);
U.y := KP*(Erro.y +(1/TI)*Integral.y +TD*Derivada.y);
U.angle := KP*(Erro.angle+(1/TI)*Integral.angle+TD*Derivada.angle);
// Controle de Saturação dos PIDs
if(U.x>TENSAO_MAX)then //Saturação no Controlador da Roda 0
begin
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.angl
e;
// PIs de Velocidade Global
VelTarget.x:=KP_TARGET*(ErroTarget.x+(1/TI_TARGET)*IntegralTarget.x);
VelTarget.y:=KP_TARGET*(ErroTarget.y+(1/TI_TARGET)*IntegralTarget.y);
VelTarget.angle:=KP_TARGET*(ErroTarget.angle+(1/TI_TARGET)*IntegralTarget.a
ngle);
// Controle de Saturação dos PIs
if(VelTarget.x > VNom)then //Saturação no Controlador de Vx
begin
VelTarget.x:=VNom;
IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x;
end;
if(VelTarget.x < -VNom)then
begin
VelTarget.x:=-VNom;
IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x;
end;
if(VelTarget.y > VNom)then //Saturação no Controlador de Vy
begin
VelTarget.y:=VNom;
IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y;
end;
if(VelTarget.y < -VNom)then
begin
VelTarget.y:=-VNom;
IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y;
end;
if(VelTarget.angle > VNom)then //Saturação no Controlador de W
begin
VelTarget.angle:=VNom;
IntegralTarget.angle:=IntegralTarget.angle-
TEMPO_DE_AMOSTRA*ErroTarget.angle;
end;
if(VelTarget.angle<-VNom)then
begin
VelTarget.angle:=-VNom;
IntegralTarget.angle:=IntegralTarget.angle-
TEMPO_DE_AMOSTRA*ErroTarget.angle;
end;
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelTarget);
end;
// Função de Seguir uma Linha
80

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*C
os(Abs(ThetaLinhaNorm));
VelLine.y:=SinalYPID*Abs(VelPIDLine)*Cos(Abs(ThetaLinhaNorm))+SinalY*VNom*S
in(Abs(ThetaLinhaNorm));
VelLine.angle:=KP_TARGET*(ErroThetaLine+(1/TI_TARGET)*IntegralThetaLine);
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelLine);
end;
// Função de Seguir um Círculo
procedure FollowCircle(Robot: LongInt; VNom: Double; FlagSentido: Integer;
Circulo: TState2D; Angulo: Double);
begin
VNom := Abs(VNom);
// Cálculo do ângulo que o Robô faz com a Horizontal no Centro do Círculo
ThetaLinha := ATan2((RobotPos.y-Circulo.y),(RobotPos.x-Circulo.x));
// Normatização do ângulo que o Robô faz com a Horizontal no Centro do
Círculo
if(ThetaLinha > Rad(90))then
begin
ThetaLinhaNorm := Rad(180) - ThetaLinha;
end
else if(ThetaLinha < Rad(-90))then
begin
ThetaLinhaNorm := -Rad(180) - ThetaLinha;
end
else
begin
ThetaLinhaNorm := ThetaLinha;
end;
// Verifica a Flag de Sentido
if((FlagSentido <> 1) and (FlagSentido <> -1))then
begin
FlagSentido := 1;
end;
// Geração dos Sinais de Controle no Círculo
if((ThetaLinha>=Rad(0))and(ThetaLinha<Rad(90)))then // Robô no 1º
Quadrante
begin
SinalX := -FlagSentido;
SinalY := FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := -FlagSentido;
end;
end
else if((ThetaLinha>=Rad(90))and(ThetaLinha<Rad(180)))then // Robô no 2º
Quadrante
begin
SinalX := -FlagSentido;
SinalY := -FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
83

SinalYPID := -FlagSentido;
end;
end
else if((ThetaLinha<Rad(-90))and(ThetaLinha>=Rad(-180)))then // Robô no
3º Quadrante
begin
SinalX := FlagSentido;
SinalY := -FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := -FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := FlagSentido;
end;
end
else if((ThetaLinha<Rad(0))and(ThetaLinha>=Rad(-90)))then // Robô no 4º
Quadrante
begin
SinalX := FlagSentido;
SinalY := FlagSentido;
if(((ErroCircle>=0)and(FlagSentido>=0))or((ErroCircle<0)and(FlagSentido
< 0)))then
begin
SinalXPID := FlagSentido;
SinalYPID := -FlagSentido;
end;
if(((ErroCircle<0)and(FlagSentido>=0))or((ErroCircle>=0)and(FlagSentido
< 0)))then
begin
SinalXPID := -FlagSentido;
SinalYPID := FlagSentido;
end;
end;
// PID de Controle de Círculo
ErroCircle:=Circulo.angle-Sqrt((RobotPos.y-Circulo.y)*(RobotPos.y-
Circulo.y)+(RobotPos.x-Circulo.x)*(RobotPos.x-Circulo.x));
ErroThetaCircle := Angulo-RobotPos.angle;
DerivadaCircle := (ErroCircle-ErroACircle)/TEMPO_DE_AMOSTRA;
IntegralCircle := IntegralCircle+TEMPO_DE_AMOSTRA*ErroCircle;
ErroACircle := ErroCircle;
VelPIDCircle :=
KP_CIRCLE*(ErroCircle+(1/TI_CIRCLE)*IntegralCircle+TD_CIRCLE*DerivadaCircle
);
IntegralThetaCircle :=
IntegralThetaCircle+TEMPO_DE_AMOSTRA*ErroThetaCircle;
// Controle de Saturação do PID
if(VelPIDCircle > VNom)then
begin
VelPIDCircle := VNom;
IntegralCircle := IntegralCircle-TEMPO_DE_AMOSTRA*ErroCircle;
end;
if(VelPIDCircle < -VNom)then
begin
VelPIDCircle := -VNom;
IntegralCircle := IntegralCircle-TEMPO_DE_AMOSTRA*ErroCircle;
end;
if(VelCircle.angle > VNom)then //Saturação no Controlador de W
begin
VelCircle.angle := VNom;
IntegralThetaCircle := IntegralThetaCircle-
TEMPO_DE_AMOSTRA*ErroThetaCircle;
end;
if(VelCircle.angle<-VNom)then
begin
84

VelCircle.angle := -VNom;
IntegralThetaCircle := IntegralThetaCircle-
TEMPO_DE_AMOSTRA*ErroThetaCircle;
end;
// Cálculo das Componentes de Velocidade
VelCircle.x :=
SinalXPID*Abs(VelPIDCircle)*Cos(Abs(ThetaLinhaNorm))+SinalX*VNom*Sin(Abs(Th
etaLinhaNorm));
VelCircle.y :=
SinalYPID*Abs(VelPIDCircle)*Sin(Abs(ThetaLinhaNorm))+SinalY*VNom*Cos(Abs(Th
etaLinhaNorm));
VelCircle.angle :=
KP_TARGET*(ErroThetaCircle+(1/TI_TARGET)*IntegralThetaCircle);
// Insere o Valor Controlado de Velocidade Global
VelControl(Robot, VelCircle);
end;
// Atualiza os dados de posição e orientação
procedure AtualizaDados;
var parcelax, parcelay: double;
begin
// Cálculo do Deslocamento Linear das Rodas
DTheta.x := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_0)/ENCODER_PPR;
DTheta.y := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_1)/ENCODER_PPR;
DTheta.angle := Rad(360)*GetAxisOdo(ID_ROBOT_1, ID_RODA_2)/ENCODER_PPR;
// Cálculo da Velocidade Linear das Rodas
Vr.x := (DTheta.x/TEMPO_DE_AMOSTRA)*RAIO;
Vr.y := (DTheta.y/TEMPO_DE_AMOSTRA)*RAIO;
Vr.angle := (DTheta.angle/TEMPO_DE_AMOSTRA)*RAIO;
// Cálculo de Posicionamento do Robô
RobotPos := GetRobotPos2D(ID_ROBOT_1);
end;
// Função Geral de Controle do Programa
procedure Control;
begin
AtualizaDados;
AddTrailNode(ID_ROBOT_1, RobotPos.x, RobotPos.y, 0.001);
// GotoTarget(ID_ROBOT_1, 0.8, Alvo);
// FollowLine(ID_ROBOT_1, 0.5, 1, Ponto2, Ponto1, Rad(90));
FollowCircle(ID_ROBOT_1, 0.5, -1, Alvo, Rad(45));
end;
// Função de Inicialização do Programa
procedure Initialize;
begin
SetRobotPos(ID_ROBOT_1, 0, 0, 0, 0);
RobotPos.x := 0;
RobotPos.y := 0;
RobotPos.angle := 0;
VrRef.x := 0;
VrRef.y := 0;
VrRef.angle := 0;
Vr.x := 0;
Vr.y := 0;
Vr.angle := 0;
DTheta.x := 0;
DTheta.y := 0;
DTheta.angle := 0;
Erro.x := 0;
Erro.y := 0;
Erro.angle := 0;
ErroA.x := 0;
ErroA.y := 0;
ErroA.angle := 0;
Derivada.x := 0;
Derivada.y := 0;
Derivada.angle := 0;
Integral.x := 0;
Integral.y := 0;
Integral.angle := 0;
Deslocamento.x := 0;
Deslocamento.y := 0;
85

Deslocamento.angle := 0;
U.x := 0;
U.y := 0;
U.angle := 0;
VRef.x := 0;
VRef.y := 0;
VRef.angle := 0;
VelGlobal.x := 1;
VelGlobal.y := 0;
VelGlobal.angle := 0;
Alvo.x := 0;
Alvo.y := 0;
Alvo.angle := 1;
ErroTarget.x := 0;
ErroTarget.y := 0;
ErroTarget.angle := 0;
IntegralTarget.x := 0;
IntegralTarget.y := 0;
IntegralTarget.angle := 0;
Ponto1.x := 0;
Ponto1.y := 0;
Ponto1.angle := 0;
Ponto2.x := 2;
Ponto2.y := 1;
Ponto2.angle := 0;
VelLine.x := 0;
VelLine.y := 0;
VelLine.angle := 0;
ErroLine := 0;
ErroALine := 0;
DerivadaLine := 0;
IntegralLine := 0;
VelPIDLine := 0;
a := 0;
b := 0;
c := 0;
ThetaLinha := 0;
ThetaLinhaNorm := 0;
SinalX := 0;
SinalY := 0;
SinalXPID := 0;
SinalYPID := 0;
VelCircle.x := 0;
VelCircle.y := 0;
VelCircle.angle := 0;
ErroThetaCircle := 0;
ErroCircle := 0;
ErroACircle := 0;
DerivadaCircle := 0;
IntegralCircle := 0;
IntegralThetaCircle := 0;
VelPIDCircle := 0;
end;
Alexandre Costa Mota
e-mail: alexandremota.eng@gmail.com

Você também pode gostar