Faculdade de Tecnologia e Ciências Curso de Engenharia Mecatrônica

Alexandre Costa Mota

Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais

Salvador 2010

Alexandre Costa Mota

Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais

Monografia apresentada ao Curso de Graduação em Engenharia Mecatrônica da Faculdade de Tecnologia e Ciências (FTC) como requisito para obtenção do título de Bacharel em Engenharia Mecatrônica.

Orientador: Prof. Antonio Carlos Caldeira

Pimenta

Salvador 2010

Alexandre Costa Mota

Controle Discreto de Velocidade e Trajetória para Robôs Omni-direcionais

Monografia aprovada como requisito parcial para obtenção do título de Bacharel em Engenharia Mecatrônica, da Faculdade de Tecnologia e Ciências (FTC). Salvador, ___ de __________ de 2010.

Paschoal Molinari Neto._______________________________________________ Mestre em Engenharia Eletrônica e Computação – Instituto Tecnológico de Aeronáutica - (ITA). Coordenador do curso de Engenharia Mecatrônica: Faculdade de Tecnologia e

Ciências - (FTC). Antonio Carlos Caldeira Pimenta._______________________________________

Mestre em Engenharia Elétrica – UFBA. CREA: BA36350. Orientador: Faculdade de Tecnologia e Ciências (FTC - Salvador).

Cauby de Souza Filho.________________________________________________

Mestre em Desenvolvimento Humano e Responsabilidade Social – FVC-CEPPEV. UCP-RJ. CREA: 37570; CONFEAG: 200511821-4. Professor da Disciplina: Faculdade de Tecnologia e Ciências (FTC – Salvador).

A minha família por acreditar e investir em mim.

AGRADECIMENTOS

Agradeço a Deus e Meishu-Sama por me conceder a oportunidade de chegar

até este momento.

A meu Pai ****** Mota, pessoa que amo e que alegra minha vida tentando

torná-la a melhor possível e me mostrando os caminhos.

Agradeço à minha Mãe ***** Mota que me educou com carinho, amor,

atenção e outras qualidades infinitas ao meu pensamento.

À minha irmã ****** Mota que caminha ao meu lado para um futuro brilhante.

Aos meus tios e avós que confiaram sempre nos meus passos.

Aos meus sinceros amigos que conquistei durante todo esse tempo, sempre

presentes para comemorar nas vitórias e amparar nas derrotas.

Ao meu orientador por aceitar e acreditar neste trabalho. Sua dedicação no

auxilio do desenvolvimento deste trabalho foi de grande importância.

Aos meus colegas de faculdade por esses longos cinco anos juntos, de

grandes momentos inesquecíveis, que ficarão guardados na memória por toda

minha vida.

“Quando os homens abrirem as portas dos seus corações, desaparecerão as trevas

que envolvem este mundo.”

Autor: Mokiti Okada. Fonte: O Pão Nosso de Cada Dia: O Alimento Espiritual do Cotidiano.

SUMÁRIO

Página

1 INTRODUÇÃO ............................................................................................... 14

1.1 DELIMITAÇÃO DO TEMA .............................................................................. 14

1.2 PROBLEMA DE PESQUISA ........................................................................... 15

1.3 HIPÓTESE ...................................................................................................... 15

1.4 OBJETIVOS .................................................................................................... 16

1.4.1 Objetivo Geral ............................................................................................... 16

1.4.2 Objetivos Específicos................................................................................... 16

1.5 JUSTIFICATIVA .............................................................................................. 16

1.6 METODOLOGIA ............................................................................................. 17

1.7 ESTRUTURA DO TRABALHO ....................................................................... 17

2 A ROBÓTICA MÓVEL ................................................................................... 19

2.1 PARADIGMAS EM ROBÓTICA MÓVEL ........................................................ 20

2.2 LOCOMOÇÃO COM RODAS ......................................................................... 22

2.3 LOCALIZAÇÃO ............................................................................................... 25

3 MODELAGEM DE ROBÔS OMNI-DIRECIONAIS ......................................... 29

3.1 MODELO CINEMÁTICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS 30

3.2 MODELO DINÂMICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS .... 32

4 CARACTERÍSTICAS DA ELETRÔNICA EMBARCADA ............................... 35

4.1 O MICROCONTROLADOR E SUAS CARACTERÍSTICAS ............................ 35

4.1.1 Interrupções no PIC ...................................................................................... 37

4.2 SENSORES E ATUADORES ......................................................................... 38

4.3 AQUISIÇÃO DOS DADOS POR RADIOFREQUÊNCIA ................................. 40

5 ESTIMAÇÃO DE POSIÇÃO E ORIENTAÇÃO .............................................. 42

5.1 O MÉTODO DEAD-RECKONING................................................................... 42

6 CONTROLE PID DISCRETO ......................................................................... 45

6.1 O MÉTODO ZIEGLER-NICHOLS EM MALHA FECHADA ............................. 46

7 VARIÁVEIS .................................................................................................... 47

8 METODOLOGIA ............................................................................................. 48

8.1 DESCRIÇÃO DA ELETRÔNICA EMBARCADA ............................................. 49

8.2 A PLATAFORMA DE SIMULAÇÃO SIMTWO ................................................ 51

8.2.1 O Robô Omni-direcional no SimTwo ........................................................... 53

8.3 O CONTROLADOR DE VELOCIDADE PROPOSTO ..................................... 55

8.4 O CONTROLADOR DE TRAJETÓRIA PROPOSTO ...................................... 57

8.4.1 Descrição da Função GoToTarget............................................................... 57

8.4.2 Descrição da Função FollowLine ................................................................ 58

8.4.3 Descrição da Função FollowCircle.............................................................. 60

9 RESULTADOS OBTIDOS .............................................................................. 62

10 CONCLUSÃO ................................................................................................. 68

REFERÊNCIAS ......................................................................................................... 69

APÊNDICE A – ESQUEMA ELETRÔNICO DO ROBÔ ............................................ 72

APÊNDICE B – CÓDIGO DE MODELAGEM DO ROBÔ ......................................... 73

APÊNDICE C – CÓDIGO DE CONTROLE DE VELOCIDADE ................................ 74

APÊNDICE D – CÓDIGO COMPLETO DA SIMULAÇÃO ........................................ 77

LISTA DE FIGURAS

Página

Figura 1 – Relação entre paradigmas primitivos nos paradigmas: a) Hierárquico, b) Reativo e c)

Híbrido. .........................................................................................................................21

Figura 2 – Esquema de controle para um robô móvel. ....................................................................22

Figura 3 – Sistema de Posicionamento Global. ...............................................................................27

Figura 4 – Sistema Vetorial para um robô omni-direcional de três rodas. ........................................30

Figura 5 – Diagrama geral de um microcontrolador. ........................................................................35

Figura 6 – Diagrama de pinos e encapsulamento PDIP dos PICs a)18F4550 b)16F628A. ..............37

Figura 7 – Diagrama de blocos típico de controle de um motor DC. ................................................39

Figura 8 – Esquema de funcionamento de um encoder de quadratura. ...........................................39

Figura 9 – Módulo de comunicação por radiofreqüência XBee-Pro. ................................................40

Figura 10 – Diagrama de Fluxo de Dados de um sistema com interface UART. ................................41

Figura 11 – Esquema de movimento do robô para uma trajetória curvilínea. .....................................43

Figura 12 – Sistema de controle em malha fechada para um motor. .................................................45

Figura 13 – Sistema de comunicação da eletrônica embarcada. .......................................................50

Figura 14 – Ambiente 3D e demais janelas do Simulador SimTwo. ...................................................52

Figura 15 – Robô omni-direcional no ambiente de simulação SimTwo. .............................................53

Figura 16 – Rotina de controle padrão do SimTwo............................................................................54

Figura 17 – Fluxograma de validação de controle de velocidade. .....................................................56

Figura 18 – Demonstração da função GoToTarget. ..........................................................................57

Figura 19 – Erro de distância do FollowLine. ....................................................................................59

Figura 20 – Demonstração da função FollowLine. ............................................................................60

Figura 21 – Erro de distância do FollowCircle. ..................................................................................61

Figura 22 – Demonstração da função FollowCircle. ..........................................................................61

Figura 23 – Gráfico velocidade x tempo da função VelControl. .........................................................62

Figura 24 – Gráfico velocidade angular x tempo da função VelControl. .............................................63

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

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

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

Figura 28 – Simulação 3D da função GoToTarget com Dead-Reckoning. .........................................65

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

Figura 30 – Simulação 3D da função FollowLine. .............................................................................66

Figura 31 – Gráfico posição x tempo da função FollowCircle. ...........................................................66

Figura 32 – Simulação 3D da função FollowCircle. ...........................................................................67

LISTA DE QUADROS

Página Quadro 1 – Comparativo entre paradigmas primitivos da robótica.....................................................20

Quadro 2 – Modelos de Tração com rodas (adaptado). ....................................................................23

Quadro 3 – Comparativo das séries 12F, 16F e 18F de microcontroladores PIC. ..............................36

Quadro 4 – Dados de especificação do módulo XBee-Pro. ...............................................................40

Quadro 5 – Sintonia de um controlador PID pelo método Ziegler-Nichols..........................................46

Quadro 6 – Características do robô omni-direcional no SimTwo. ......................................................53

Quadro 7 – Constantes dos Controladores. ......................................................................................67

LISTA DE ABREVIATURA E SIGLAS

DC Direct current

EEPROM Electrically-Erasable Programmable Read-Only Memory

GLSCENE Biblioteca 3D em OpenGL para Delphi

IEEE Institute of Electrical and Electronics Engineers

ISP In-System Programming

ODE Open Dynamics Engine

PDIP Plastic Dual In-line Package

PID Proportional–integral–derivative

PWM Pulse-width modulation

RAM Random Access Memory

RF Rádio freqüência

RISC Reduced Instruction Set Computer

ROM Read Only Memory

RS-232 Padrão para troca serial de dados binários

SFR Special Function Registers

SYNEDIT Editor de controle avançado multi-linha

UART Universal asynchronous receiver/transmitter

USART Universal Synchronous Asynchronous Receiver Transmitter

USB Universal Serial Bus

XML Extensible Markup Language

LISTA DE SÍMBOLOS

dc [m] Distância entre o ponto central do robô e as rodas

t [s] Tempo

n [s] Tempo discreto

v0, v1, v2 [m/s] Velocidade linear das rodas

0,1,2 [rad/s] Velocidade angular das rodas

f0, f1, f2 [N] Força de tração nas rodas

T0, T1, T2 [N·m] Torque de tração nas rodas

v, vn [m/s] Velocidade linear do robô

[rad/s] Velocidade angular do robô

Fv, Fvn [N] Força de tração no robô segundo as direções v e vn

T [N·m] Torque de rotação no robô segundo o seu eixo de rotação

x, y [m] Variáveis de posicionamento global

[rad] Posição angular do robô

vx, vy [m/s] Velocidade linear do robô segundo os referenciais x e y

R [m] Raio das rodas

M [kg] Massa do robô

J [kg·m2] Momento de inércia do robô

FBv, FBvn [N] Força de atrito dinâmico no robô segundo as direções v e vn

TB [N·m] Torque de atrito dinâmico no robô segundo o seu eixo de rotação

FCv, FCvn [N] Força de atrito estático no robô segundo as direções v e vn

TC [N·m] Torque de atrito estático no robô segundo o seu eixo de rotação

Bv,Bvn [N/(m/s)] Coeficiente de atrito dinâmico segundo as direções v e vn

B [N·m/(rad/s)] Coeficiente de atrito dinâmico segundo o eixo de rotação do robô

Cv,Cvn [N] Coeficiente de atrito estático segundo as direções v e vn

C [N·m] Coeficiente de atrito estático segundo o eixo de rotação do robô

l Fator de relação de transmissão

Kt [N·m/A] Constante de binário dos motores

i0, i1, i2 [A] Corrente nos motores

Np[pulsos] Numero de pulsos medidos de um encoder incremental

Res [pulsos/rad] Resolução de um encoder incremental

ta [s] Constante de intervalo de tempo entre n e n-1, é o tempo de amostra dos dados

di, dn [s] Deslocamento do robô na direção do vetor de velocidade linear v e vn

Kp, Ki, Kd Constantes de proporcionalidade, integração e derivação respectivamente

E Variável para cálculo de erro

Kcr, Pcr Constantes de proporcionalidade crítico e constante de tempo crítico

a, b, c Constantes fundamentais da reta

RESUMO

A maioria dos robôs não holonômicos não podem andar em uma direção perpendicular

às suas rodas motrizes, como por exemplo: um robô de tração diferencial pode andar para frente, para trás, em uma curva, ou girar ao redor de algo, mas não pode andar de lado, porem os robôs omni-direcionais são capazes de se movimentar em qualquer direção em um plano. Esse documento busca propor um sistema de controle de movimento embarcado para um robô holonômico envolvendo velocidades e trajetos simples como andar em linha, em circulo ou ir até um ponto-alvo sem restrições. Esse documento também descreve o sistema eletrônico embarcado e os próprios controles de velocidades e trajetória discretos desse sistema. Para validar os controladores são realizados simulações num software de cálculos matemáticos e um software de simulação de robôs móveis. No final do trabalho o robô tem a capacidade de descrever trajetórias de ações básicas como andar em linha ou em círculo com velocidades controladas.

Palavras-Chave: Controle, trajetória, PID, sistema, autônomo, robótica, móvel, robôs holonômicos.

ABSTRACT Most nonholonomic robots cannot move in a perpendicular direction to their drive wheels,

for example, a differential drive robot can move forward, backward, on a curve or turn around something, but cannot move aside, but the omni-directional robots are able to move in any direction in a plane. This paper aims to propose a embedded motion control system for a holonomic robot involving speeds and simple paths as walking in line, in circle or go to a target without restrictions. This document also describes the embedded electronics system and the discrete controls of speed and trajectory of this robot. To validate the controllers are made simulations in mathematical software and a simulation software for mobile robots. At the end of the work the robot has the ability to describe trajectories of basic actions like move in line or in circle with controlled speeds.

Key-Words: Control, trajectory, PID, autonomous, system, mobile, robotic, holonomic robots.

14

1 INTRODUÇÃO

Robôs móveis autônomos têm despertado um crescente interesse em

diversos tipos de aplicações, tornando-se uma área de intensas pesquisas

científicas relacionadas com o projeto de novas arquiteturas de locomoção e com o

desenvolvimento de novos algoritmos para controle e navegação (CONCEIÇÃO,

2007, p. viii).

Robôs móveis podem realizar diversas tarefas como: entregar

automaticamente as peças entre os postos de montagem em uma fábrica seguindo

guias posicionadas no espaço; transportar alimentos e medicamentos por um

hospital, acompanhando a posição das luzes no teto que são especificas para este;

servir como um veículo automatizado para limpeza especializada, onde podem

aproveitar o padrão geométrico dos corredores para facilitar sua localização e as

funções de navegação; entre outras funcionalidades.

Segundo Braunl (2006, p. 113), um robô "holonômico" ou omni-direcional

é capaz de se movimentar em qualquer direção. A maioria dos robôs não

holonômicos não podem andar em uma direção perpendicular às suas rodas

motrizes, como por exemplo: um robô de tração diferencial pode andar para frente,

para trás, em uma curva, ou girar ao redor de algo, mas não pode andar de lado,

porem os robôs omni-direcionais são capazes de se movimentar em qualquer

direção em um plano. Por esse motivo, robôs móveis com tração omni-direcional têm

sido aplicados largamente em robótica móvel.

Esse documento busca propor um sistema de controle de movimento

embarcado para um robô holonômico envolvendo velocidades e trajetos simples

como andar em linha, em circulo ou até mesmo ir até um ponto-alvo sem restrições.

1.1 DELIMITAÇÃO DO TEMA

Sistema de controle de velocidade e trajetória embarcado para um robô

omni-direcional de três rodas.

15

1.2 PROBLEMA DE PESQUISA

Segundo Siegwart & Nourbakhsh (2004, p. 17), a robótica móvel aborda

várias questões como: características de estabilidade do robô, características de

contato do robô com o ambiente e características do ambiente. Essas questões

influenciam diretamente em tomadas de decisão de robôs autônomos em diversas

tarefas como, por exemplo: mover-se a uma velocidade desejada ou seguir uma

trajetória específica. Tarefas básicas como essas influenciam em tarefas mais

complexas como mover-se em um labirinto ou conduzir uma bola de futebol pelo

campo.

Vistos essas características de ambiente como terreno irregular, ou

questões de estabilidade como centro de gravidade do robô, essa pesquisa tem

como foco o seguinte problema: como controlar a trajetória de um robô omni-

direcional de três rodas apartir de velocidades e trajetos pré-definidos?

1.3 HIPÓTESE

Entende-se que para controlar velocidade e trajetória de um robô omni-

direcional pode-se utilizar do modelo cinemático e dinâmico do mesmo.

A eletrônica embarcada do robô possui um controle individual por motor e

um controlador geral para trajetória. Cada controle é discreto no tempo em

microcontroladores que trocam informações entre si.

O controle de velocidade se baseia em um sistema em malha fechada a

partir da velocidade individual das rodas do robô. Essas medidas de velocidade são

introduzidas na modelagem do robô para posteriormente calcular o erro em relação

à velocidade desejada e assim controlar o sistema.

Em relação ao controle de trajetória, dados de referência de posição no

espaço de trabalho e tipo de trajetória a ser descrita são introduzidos para que o

controlador possa ajustar seu posicionamento gerando dados de setpoint para os

controladores de velocidade ao longo do tempo. Esses dados são calculados em

16

malha fechada com realimentação provida da odometria inserida nas equações de

modelagem.

1.4 OBJETIVOS

1.4.1 Objetivo Geral

Descrever uma eletrônica embarcada para um robô omni-direcional de

três rodas com controladores de velocidade e trajetória.

1.4.2 Objetivos Específicos

a) Descrever o sistema eletrônico embarcado do robô omni-direcional;

b) Descrever um controlador de velocidade discreto para o robô omni-

direcional;

c) Descrever um controlador de trajetória discreto para o robô omni-

direcional;

1.5 JUSTIFICATIVA

Para resolver diversos problemas de locomoção, o engenheiro de robótica

deve entender de cinemática, dinâmica e teoria de controle. Localização e

navegação demandam conhecimentos de algoritmos de computador, teoria da

informação, inteligência artificial e teoria da probabilidade (SIEGWART e

NOURBAKHSH, 2004, p. 8). A partir de estudos como esses a robótica móvel

abrange novas areas de conhecimento constantemente.

17

Robôs omni-direcionais estão sendo largamente utilizados para realizar

tarefas de transporte, limpesa, montagem e muitas outras que necessitam de

conhecimentos específiocos por parte do engenheiro de robótica que os projeta. Em

tarefas como essas, controlar a velocidade e a trajetória do robô são duas

características fundamentais para a autonomia desse robô.

Controlar velocidade é uma necessidade direta para realizar ações

básicas como seguir por uma linha, andar em circulo, etc e essas ações básicas são

necessidades diretas para ações de tarefas como no caso de um robô jogador de

futebol ir até a bola, drible, etc e assim por diante passando por ações de papeis até

ações táticas.

1.6 METODOLOGIA

A metodologia utilizada para este trabalho foi com documentação indireta

através da pesquisa bibliográfica em livros, monografias, teses, entre outros. Essa

pesquisa envolve vários assuntos multidisciplinares como métodos de controle,

inteligência artificial, eletrônica embarcada, sistemas de visão artificial, etc.

Durante a fase de pesquisa foi observado uma grande disponibilidade de

material bibliográfico sobre o tema que é bastante abrangente e envolve diversos

assuntos complementares.

Outra parte desse documento aplica-se a pesquisa experimental, em que

realizadas simulações para validar o sistema proposto passa-se a realizar

experimentos reais que põem em prática o objetivo do trabalho.

1.7 ESTRUTURA DO TRABALHO

Aqui estão apresentados os capítulos abordados neste trabalho, onde são

evidenciados os seus conteúdos:

a) Capítulo 1: aborda uma visão geral do trabalho, descrevendo os

objetivos a serem alcançados com sua conclusão;

18

b) Capítulo 2: descreve alguns termos importantes, apresenta os

principais obstáculos e tipos de locomoção existentes em robótica

móvel;

c) Capítulo 3: descreve a modelagem de um robô omni-direcional;

d) Capítulo 4: descreve as necessidades eletrônicas para um robô móvel

omni-direcional;

e) Capítulo 5: descreve o método para estimar a posição e orientação de

um robô omni-direcional;

f) Capítulo 6: apresenta o sistema de controle PID discreto que é utilizado

no robô omni-direcional;

g) Capítulo 7: mostra as variáveis envolvidas no trabalho;

h) Capítulo 8: apresenta a metodologia de trabalho;

i) Capítulo 9: apresenta os resultados obtidos ao final deste trabalho;

j) Capítulo 10: apresenta a conclusão ao final deste trabalho.

19

2 A ROBÓTICA MÓVEL

Em uma nova etapa de evolução dos sistemas robóticos, diversos

pesquisadores têm concentrado grandes esforços na construção de robôs móveis,

introduzindo nessas máquinas as capacidades de mobilidade e autonomia para

reagir adequadamente em um ambiente, o que abre um vasto campo de novas

aplicações e conseqüentemente muitos desafios (SCATENA, 2008, p. 12).

Segundo Scatena (2008, p. 12-13) os robôs móveis podem ser

classificados conforme sua independência a usuários externos da seguinte forma:

a) Robôs móveis controlados: esses robôs são capazes de se movimentar

através da interação com usuários externos. Nessa categoria de robôs,

o ser humano toma as decisões de navegação, tática, etc.

b) Robôs móveis autônomos: esses robôs são capazes de se movimentar

em ambientes abertos ou fechados, sem a intervenção de um usuário

externo. Esse tipo de robô pode tomar decisões de navegação com

total independência.

Segundo Franco (2007, p. 10) um robô móvel autônomo necessita de

mecanismos de locomoção que o possibilite mover-se pelo seu ambiente de maneira

autônoma, ou seja, sem a necessidade de ser controlado pelo homem. Existe uma

grande variedade de movimentos que podem ser executados por um robô e a

escolha de um sistema apropriado de locomoção é um aspecto importante para o

projeto. A maioria dos mecanismos de locomoção usados nos robôs móveis é

inspirada em modelos biológicos. Existe porem uma categoria que não segue este

modelo, os robôs com rodas, também conhecidos como veículos, extremamente

eficientes em superfícies planas.

Segundo Conceição (2007, p. 1) um robô móvel é uma combinação de

vários componentes físicos (hardware) e computacionais (software). Conforme esses

componentes, um robô móvel pode ser considerado como um conjunto de

subsistemas de:

a) Locomoção: capacidade para se movimentar em um ambiente.

b) Sensoriamento: capacidade para medir suas características e do

ambiente.

20

c) Racionalização: capacidade para mapear as medidas em ações.

d) Comunicação: capacidade para comunicar com o exterior.

2.1 PARADIGMAS EM ROBÓTICA MÓVEL

Segundo Murphy (2000, p. 2) atualmente existe três paradigmas para a

organização de inteligência nos robôs: hierárquico, reativo e um híbrido de

hierárquico com reativo. Esses paradigmas são descritos pela relação entre três

outros paradigmas primitivos da robótica: Sensoriamento, Planejamento e Ação.

É uma função de Sensoriamento recolher informações a partir do meio

em que se localiza robô. A partir das informações vindas dos sensores e/ou

conhecimentos em base de dados sobre o ambiente, a função de Planejamento

produz uma ou mais tarefas para o robô possa executar, como: ir até o fundo do

corredor, vire à esquerda, prossiga 3 metros e pare. Funções que produzem

comandos diretos para os atuadores como: girar motor em 90º, no sentido horário,

com uma velocidade de giro de 200 r.p.m. dispõem das funções de Ação. No quadro

abaixo se pode ver o comparativo de entradas e saídas dos paradigmas primitivos:

Quadro 1 – Comparativo entre paradigmas primitivos da robótica.

Robot Primitives Input Output

Sense Sensor data Sensed information

Plan Information (sensed and / or cognitive) Directives

Act Sensed information or directives Actuator commands Fonte – Murphy, 2000, p. 6.

O paradigma Hierárquico ou Deliberativo é o paradigma mais antigo. Sob

ele, o robô opera baseado em planejamento. Isto foi baseado em uma visão

introspectiva de como as pessoas pensam, por exemplo: O robô localiza uma porta,

mapeia o ambiente e traça um percurso em torno das cadeiras até chegar à porta.

Sob esse paradigma, os sentidos do robô no ambiente norteiam em planejar a

próxima ação, portanto a necessidade de Sensoriamento, Planejamento e Ação

(MURPHY, 2000, p. 6).

21

O paradigma Reativo foi uma reação ao paradigma hierárquico. O

Paradigma Reativo foi possível graças a duas tendências. Uma delas foi um

movimento popular entre os pesquisadores de Inteligência Artificial para investigar a

biologia e psicologia cognitiva, a fim de examinar exemplares vivos de inteligência.

Outro motivo foi a rápida diminuição do custo de hardware de computador acoplado

com o aumento do poder de computação. Esse paradigma é bastante utilizado para

imitar comportamento de insetos em robôs. Em suma, esse paradigma traduz uma

reação a um estímulo (MURPHY, 2000, p. 8).

O Paradigma Reativo serve de base para o paradigma Deliberativo /

Reativo. O Paradigma híbrido surgiu na década de 1990 e continua a ser a área

atual de pesquisa. Sob o paradigma híbrido, o robô primeiramente delibera a melhor

forma de decompor uma tarefa em subtarefas (também chamado planejamento), e

em seguida, delibera os comportamentos adequados para realizar cada subtarefa.

Os comportamentos de execução são realizados conforme o Paradigma reativo. Em

suma, o planejamento é feito em uma etapa, depois de sensoriamento e atuação

são feitas em conjunto (MURPHY, 2000, p. 9). A figura abaixo mostra os paradigmas

em função de paradigmas primitivos.

Figura 1 – Relação entre paradigmas primitivos nos paradigmas: a) Hierárquico, b) Reativo e c) Híbrido. Fonte – Murphy, 2000, p. 7.

22

Analogamente à arquitetura Hierárquica pode-se comparar a figura abaixo

com a figura 1-a) em que Localização e Planejamento de trajetória fazem parte do

paradigma de Planejamento, o bloco de Percepção faz parte do paradigma

Sensoriamento e o bloco Controle de Movimento faz parte do paradigma Ação:

Figura 2 – Esquema de controle para um robô móvel. Fonte – Siegwart & Nourbakhsh, 2004, p. 10.

Como visto anteriormente, Ação, Sensoriamento e Planejamento são

fundamentais para a robótica. O foco desse documento está na implementação de

controladores para um robô com controle Hierárquico, em que se toma como

paradigmas de Ação o controle da velocidade motores do robô, Sensoriamento a

leitura da velocidade angular das rodas, mas o Planejamento apesar de ser um

paradigma levado em conta, esse documento não trata de especificar essa

abordagem por possuir grande escala de pesquisa e foco diferenciado.

2.2 LOCOMOÇÃO COM RODAS

Um robô móvel precisa de mecanismos de locomoção que lhe permitem

deslocar sem limitações durante todo o seu ambiente. Mas há uma grande

23

variedade de formas possíveis de se mover e por isso a seleção do modelo de

locomoção de um robô é um aspecto importante do projeto do robô móvel

(SIEGWART e NOURBAKHSH, 2004, p. 13).

A roda foi de longe o mecanismo de locomoção mais popular em robótica

móvel. Ela pode alcançar bons ganhos de eficiência fora a facilidade de

implementação relativamente simples. Além disso, equilíbrio não é um problema de

robô com rodas, pois estes são quase sempre projetados de modo que todas as

rodas estão em contato com o solo. Assim, três rodas são suficientes para garantir

um equilíbrio estável. Quando mais de três rodas são usadas, um sistema de

suspensão é necessário para permitir que todas as rodas mantenham contato com o

solo quando o robô se encontra em terreno irregular (SIEGWART e NOURBAKHSH,

2004, p. 30-31). O quadro abaixo mostra as principais configurações de tração em

robôs com rodas e suas descrições.

Quadro 2 – Modelos de Tração com rodas (adaptado).

Number of

Wheels Arrangement Description

02

One steering wheel in the front, one traction wheel in the rear

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

03

Two-wheel centered differential drive with a third point of contact

Two independently driven wheels in the rear/front, 1 unpowered omni-directional wheel in the front/rear

Two free wheels in rear, 1 steered traction wheel in front

Three motorized Swedish or spherical wheels arranged in a triangle; Omni-directional movement is possible

04

Two motorized wheels in the rear, 2 steered wheels in the front; steering has to be different for the 2 wheels to avoid

slipping/skidding.

Two motorized and steered wheels in the front, 2 free wheels in the rear; steering has to be different for the 2 wheels to

avoid slipping/skidding.

Four steered and motorized wheels

24

Four Omni-directional wheels

Two-wheel differential drive with 2 additional points of contact

Icons for the each wheel type are as follows

unpowered Omni-directional wheel (spherical, castor, Swedish)

motorized Swedish wheel (Stanford wheel)

unpowered standard Wheel

motorized standard Wheel

steered standard Wheel

connected wheels

Fonte – Siegwart & Nourbakhsh, 2004, p. 34-36.

A geometria, quantidade e disposição das rodas sobre a base do robô são

responsáveis pelo seu grau de mobilidade, pela sua capacidade de manobra e pela

sua estabilidade (FRANCO, 2007, p. 11).

São necessárias, pelo menos, duas rodas para garantir a estabilidade de

um robô. Robôs com duas rodas motores fixas, chamados de robôs diferenciais, têm

uma estabilidade garantida caso o seu centro de massa esteja abaixo do eixo das

rodas. A garantia da estabilidade de um robô com três rodas é satisfeita quando o

centro de massa do robô está localizado dentro do triangulo formado por elas

(FRANCO, 2007, p. 11).

A manobrabilidade é a capacidade do veículo de mudar de direção em

seu movimento. Esta capacidade depende do grau de liberdade das rodas e da sua

disposição sobre a base do robô. Alguns robôs possuem a habilidade para mover-se

em qualquer direção sobre um plano (x, y) sem a necessidade de mudar a sua

orientação em relação ao eixo vertical ao plano (FRANCO, 2007, p. 12) como, por

exemplo, robôs omni-direcionais.

A mobilidade é a capacidade que um robô tem de mover-se no ambiente.

As restrições básicas que limitam a mobilidade são regras que cada roda deve

satisfazer, sendo estas as restrições de deslizamento lateral (FRANCO, 2007, p. 12).

Os robôs omni-direcionais usam rodas que possuem rolamentos sobre a

sua superfície de contato. Estes rolamentos diminuem o atrito de deslizamento

lateral da roda fazendo com que esta, passe a ter um grau de liberdade a mais. Um

25

robô com pelo menos três rodas deste tipo pode se deslocar em qualquer direção,

independentemente da sua orientação (FRANCO, 2007, p. 12).

2.3 LOCALIZAÇÃO

Localização e navegação são as duas tarefas mais importantes para

robôs móveis. O robô precisa saber onde ele está e precisa ser capaz de fazer um

planejamento de como chegar a um destino. Claro que estes dois problemas não

são isolados uns dos outros, mas intimamente ligados. Se um robô não sabe a sua

posição exata no início de uma trajetória planejada, vai ter problemas em chegar ao

destino (BRAUNL, 2006, p. 197).

Segundo Borenstein, Everett & Feng (1996, p. 10), os problemas gerais

de navegação de robôs móveis são resumidos por três perguntas: "Onde estou?",

"Onde eu estou indo?" e "Como devo chegar lá?". Talvez o resultado mais

importante do levantamento do vasto corpo de literatura sobre o posicionamento do

robô móvel é que até hoje não há nenhuma solução realmente elegante para o

problema. As muitas soluções parciais podem ser grosseiramente classificadas em

dois grupos: medidas de posição relativa e absoluta. Devido à falta de um único bom

método, os desenvolvedores de Automated Guided Vehicles e robôs móveis

costumam combinar dois métodos, um de cada categoria. Localização Relativa pode

ser dividida em Odometria e Navegação por Inércia e Localização Absoluta se divide

em Balizas Ativas, Marcos Artificiais, Marcos Naturais e Correspondência por

Mapas.

Segundo Borenstein, Everett & Feng (1996, p. 10), a odometria é um

método que usa encoders para medir a rotação das rodas e/ou orientação da

direção. A odometria tem a vantagem de que é totalmente auto-suficiente e é capaz

de fornecer ao veículo uma estimativa da sua posição. A desvantagem de utilizar

odometria é que o erro de posição cresce sem limites, a não ser que uma referência

independente seja utilizada periodicamente para reduzir esse erro.

Em odometria pode-se destacar como erros sistemáticos:

a) Diâmetros desiguais das rodas.

26

b) A média dos diâmetros das rodas real difere de rodas de diâmetro

nominal.

c) A distância entre eixos real difere da distância entre eixos nominal.

d) Desalinhamento das rodas.

e) Resolução finita dos encoders.

f) Taxa de amostragem finita dos decodificadores.

Como erros não sistemáticos se podem destacar:

a) Trajetos sobre pisos irregulares.

b) Localização inesperada de objetos no piso.

c) Derrapagem das rodas devido a: superfícies diferentes, sobre

aceleração, forças externas, forças internas, nenhum ponto de

contato das rodas com o chão.

Navegação por inércia é um método que usa giroscópios e acelerômetros

para medir a velocidade de rotação e aceleração. Essas medidas necessitam de

integração uma vez ou duas vezes para se obter a posição. Sistemas de navegação

inercial também são auto-suficientes como a odometria. Por outro lado, os dados do

sensor inercial extrapolam com o tempo devido à necessidade de integrar os dados

da taxa de rendimento de posição, qualquer pequeno erro aumenta sem limite

constante após a integração. Sensores inerciais são, portanto, impróprios para o

posicionamento preciso por um longo período de tempo (BORENSTEIN, EVERETT

e FENG, 1996, p. 10).

A localização por balizas ativas é um método que calcula a posição

absoluta do robô a partir da medição da direção de incidência de três balizas ou

mais por transmissão de dados. Os transmissores, geralmente usando luz ou rádio

freqüência, devem estar localizados em locais conhecidos no meio ambiente

(BORENSTEIN, EVERETT e FENG, 1996, p. 10). Esse tipo de localização absoluta

utiliza métodos matemáticos como Trilateração (baseada na medida das distâncias

ou diferenças de distâncias) e Triangulação (baseada na medida dos ângulos). A

figura abaixo mostra um exemplo de um sistema de balizas ativas, o GPS.

27

Figura 3 – Sistema de Posicionamento Global. Fonte – Braunl, 2006, p. 198.

Marcos artificiais são colocados em locais conhecidos no meio ambiente

para esse método de localização. A vantagem de utilizar marcos artificiais é que eles

podem ser projetados para a detecção do ideal, mesmo sob condições ambientais

adversas. Tal como acontece com balizas ativas, três ou mais pontos de referência

devem ser visíveis para permitir a estimativa de posição. Outra vantagem é que os

erros de posição são limitados, mas a detecção de pontos de referência externos e

posição em tempo real podem não ser sempre possíveis. Marcos artificiais podem

ser definidos como um conjunto de formas ou áreas geométricas. Informações

adicionais como a distância, podem ser derivadas da medição das propriedades

geométricas do marco, mas esta abordagem é computacionalmente pesada e não

muito precisa (BORENSTEIN, EVERETT e FENG, 1996, p. 10).

No reconhecimento de Marcos Naturais, os marcos são características

distintas no meio ambiente. Não há necessidade de preparação do ambiente, mas o

ambiente deve ser conhecido antecipadamente. A confiabilidade deste método não é

tão elevado como o método de marcos artificiais (BORENSTEIN, EVERETT e

FENG, 1996, p. 11).

No método de correspondência por mapas as informações adquiridas a

partir de sensores a bordo do robô são comparadas a um mapa ou modelo do meio

ambiente. A localização baseada em mapas muitas vezes inclui a melhoria do mapa

global baseado nas novas observações sensoriais em um ambiente dinâmico e a

integração de mapas locais para o mapa global para cobrir as áreas ainda não

28

exploradas. Os mapas utilizados na navegação incluem dois tipos principais: mapas

geométricos e mapas topológicos. Mapas geométricos representam o ambiente em

um sistema global de coordenadas, enquanto mapas topológicos representam o

ambiente como uma rede de nós e arcos (BORENSTEIN, EVERETT e FENG, 1996,

p. 11).

Dentre os métodos de localização abordados, a odometria está

diretamente ligada com esse documento por diversos fatores incluindo viabilidade

econômica, facilidade de implementação física, etc.

29

3 MODELAGEM DE ROBÔS OMNI-DIRECIONAIS

Aspectos de caráter mecânico, elétrico e a disposição física das partes

que compõem um robô são características relevantes na escolha e na

implementação de seus controladores. A capacidade de um robô se locomover e as

equações que regem o seu movimento são influenciadas pela sua geometria e pela

sua dinâmica (FRANCO, 2007, p. 43).

De modo a aumentar o desempenho dinâmico de robôs, muitos têm sido

os estudos de modelos dinâmicos e cinemáticos. As estruturas em estudo são

baseadas em sistemas dinâmicos lineares e não lineares e a determinação dos seus

parâmetros tem sido alvo de contínua investigação. Uma vez construído o modelo

dinâmico é necessário fazer a estimação dos seus parâmetros. O método mais

comum para identificação de parâmetros de robôs baseia-se em modelos lineares

(mínimos quadrados, variáveis Instrumentais e toda uma série de variantes iterativas

que aplicam mínimos quadrados repetidamente sobre dados pré-filtrados de modo a

diminuir o ruído) (OLIVEIRA, 2007, p. 36).

Naturalmente, para sistemas não lineares os métodos disponíveis não

são genéricos e o problema agrava-se se houver dificuldade em obter medidas com

níveis de ruído suficientemente baixo. Torna-se, portanto interessante uma

abordagem focada nos modelos mais adequados para os robôs móveis omni-

direcionais (OLIVEIRA, 2007, p. 36).

Os modelos para robôs acionados por rodas podem ser divididos em

quatro tipos: modelo cinemático de postura, modelo cinemático de configuração,

modelo dinâmico de postura e modelo dinâmico de configuração. Os modelos

cinemáticos descrevem o robô em função da velocidade e orientação das rodas,

enquanto os modelos dinâmicos descrevem o robô em função das forças

generalizadas aplicadas pelos atuadores, por exemplo, torques nas rodas (LAGES,

1998, p. 9).

Os modelos de postura consideram como estado apenas posição e

orientação do robô, enquanto os modelos de configuração consideram além da

postura outras variáveis internas como, por exemplo, deslocamento angular das

rodas. Do ponto de vista de controle da posição e orientação espacial do robô,

30

apenas os modelos de postura são necessários (LAGES, 1998, p. 10) por esse

motivo são esses os modelos utilizados nesse documento.

Os próximos tópicos abordam as equações de cinemática e dinâmica

para robôs omni-direcionais de três rodas.

3.1 MODELO CINEMÁTICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS

A figura abaixo serve como base para a maioria das fórmulas presentes

posteriormente abordando características vetoriais presentes num robô omni-

direcional de três rodas (ver lista de símbolos).

Figura 4 – Sistema Vetorial para um robô omni-direcional de três rodas. Fonte – Oliveira, 2007, p. 37.

A cinemática apenas tem que ver com as relações geométricas que

regem o sistema físico. Sabendo a posição do robô (x, y, ) ao longo do tempo, é

possível calcular as velocidades (vx, vy, ) a partir da derivada, tal como apresentado

nas equações abaixo (OLIVEIRA, 2007, p. 39):

(1)

(2)

31

(3)

A partir das velocidades lineares vx e vy é possível calcular as velocidades

lineares v e vn, pela equação abaixo (OLIVEIRA, 2007, p. 40):

(4)

A partir da cinemática inversa é possível determinar as velocidades vx e

vy, tal como pode ser observado na seguinte equação (OLIVEIRA, 2007, p. 40):

(5)

A relação entre as velocidades das rodas v0, v1 e v2, com as velocidades

do robô v, vn e é descrita pela equação abaixo (OLIVEIRA, 2007, p. 40):

(6)

Sendo R a distancia do centro da roda j=[0,1,2] até sua extremidade, se

tem:

(7)

Então, a equação de cinemática inversa do robô omni-direcional de três

rodas sendo 60º o ângulo de inclinação das rodas é dada por:

(8)

Segundo Oliveira (2007, p. 41), se pode obter a equação de cinemática

direta a partir da equação de cinemática inversa, resultando em:

32

(9)

Substituindo a equação (7) na equação (9), se obtêm:

(10)

Os controladores de trajetória utilizam o sistema de coordenadas global,

ou seja, um sistema de coordenadas do meio em que o robô se encontra. Para se

localizar, o robô precisa saber a relação de sua posição atual com o ponto de

referência no sistema global, por isso, utilizam-se as equações (4) e (5) para esse

fim.

As equações (8) e (10) relacionam as velocidades angular das rodas nos

seus respectivos sistemas de coordenadas com as velocidades do robô no sistema

de coordenadas global, representado pela figura (4). Essas relações são importantes

para o controle de posicionamento do robô visto que a odometria é o principal meio

de obtenção de re-alimentação, obtendo valores relativos discretos de posição das

rodas.

3.2 MODELO DINÂMICO DO ROBÔ OMNI-DIRECIONAL DE TRÊS RODAS

A dinâmica do robô é o estudo do movimento relacionado com as forças

presentes no sistema físico, incluindo a energia e as velocidades. As equações da

dinâmica segundo os sistemas de eixos apresentados na figura (4) são descritas nas

equações abaixo (OLIVEIRA, 2007, p. 43):

33

(11)

(12)

(13)

As equações acima sugerem que a aceleração do robô segundo as

direções v, vn e o seu eixo de rotação é igual ao somatório das forças do robô,

subtraída das forças de atrito aplicadas nessas mesmas direções ou eixo de

rotação. As forças de atrito presentes no robô são, neste caso, designadas de atrito

viscoso ou dinâmico e atrito de Coulomb ou estático. Os atritos viscosos (FBv, FBvn,

TB), apresentam-se como uma relação linear entre a força aplicada e a velocidade,

enquanto que os atritos de Coulomb (FCv, FCvn, TC), apresentam uma amplitude

constante e sinal de modo a contrariar o movimento (OLIVEIRA, 2007, p. 44).

As equações para as forças de atrito viscoso são:

(14)

(15)

(16)

As equações para as forças de atrito de Coulomb são:

(17)

(18)

(19)

Atrito viscoso é uma força que surge quando um determinado corpo

apresenta movimento relativo, opondo-se a esse movimento (OLIVEIRA, 2007, p.

45). E atrito de Coulomb é presente quando o corpo se encontra na iminência do

movimento, ou seja, no princípio da atuação das forças aplicadas. Contudo a

34

explicação mais simples diz que é a força que o corpo terá de fazer para vencer o

seu estado de repouso (OLIVEIRA, 2007, p. 46).

Sendo 60º a inclinação das rodas, as relações entre as forças de tração e

torque do robô com as forças de tração nas rodas são descritas pelas seguintes

equações (OLIVEIRA, 2007, p. 47):

(20)

(21)

(22)

A força de tração em cada roda é calculada através do torque nas rodas

j=[0,1,2] que, por sua vez, é determinado usando a corrente consumida pelo motor,

tal como é descrito nas seguintes equações (OLIVEIRA, 2007, p. 47):

(23)

(24)

Como mostrado, a partir das equações (23) e (24) calcula-se a força

aplicada em cada roda a partir de uma entrada de corrente elétrica, essas forças são

inseridas nas equações (20), (21) e (22) para se obter as forças geradas pelo robô

no sistema de coordenadas local. Com as equações (14) a (19) calcula-se as forças

de atrito estático e dinâmico nas rodas. Por fim, as equações de força são inseridas

na equação de dinâmica geral relacionado forças e velocidades utilizadas pelos

controladores.

35

4 CARACTERÍSTICAS DA ELETRÔNICA EMBARCADA

A eletrônica embarcada utilizada nesse sistema é capaz de controlar as

velocidades dos motores do robô com feedback. Para tal, alguns componentes são

mostrados a seguir para um melhor entendimento do sistema.

4.1 O MICROCONTROLADOR E SUAS CARACTERÍSTICAS

Para entender a arquitetura de um chip complexo como um

microcontrolador, o diagrama abaixo mostra um modelo geral de funcionamento do

mesmo. Isso permite entender a operação geral do chip sem ter que analisar seu

circuito, que é muito complexo (BATES, 2004, p. 61).

Figura 5 – Diagrama geral de um microcontrolador. Fonte – Bates, 2004, p. 62.

O diagrama de blocos acima mostra o esquema geral de um

microcontrolador que pode ser considerado em duas partes, a seção de execução

do programa e a seção de processamento de registradores. Esta divisão reflete uma

arquitetura em que o programa e os dados são acessados separadamente,

aumentando a velocidade de execução do programa e é conhecida como arquitetura

Harvard (BATES, 2004, p. 61).

36

Todos os microcontroladores PIC utilizam a mesma arquitetura básica e

um conjunto de instruções para fornecer um caminho de evolução do produto, desde

simples programas desenvolvidos para a série "12", até as mais complexas

aplicações para dispositivos da série "18". As características comuns da arquitetura

do PIC são (BATES, 2004, p. 237):

a) Arquitetura Harvard;

b) Conjunto de instruções RISC;

c) Memória flash ROM de programa com ISP;

d) Bloco de memória RAM, incluindo SFRs;

e) EEPROM – Memória de dados não-volátil;

f) Registrador de trabalho único;

g) Pilha não gravável;

h) Power-up e watchdog timers;

i) Múltiplas fontes de interrupção;

j) Hardware timers;

k) Função sleep;

l) Programação serial in-circuit.

Os principais grupos de microcontroladores PIC são apresentados no

quadro abaixo. Eles são divididos em três grupos, com um número de prefixo

diferente: a série "12" possui PICs em miniatura de 8 pinos, o grupo "16" pode ser

descrito como a série padrão e os dispositivos da série "18" são PICs de alto

desempenho (BATES, 2004, p. 237).

Quadro 3 – Comparativo das séries 12F, 16F e 18F de microcontroladores PIC.

12FXXX 16FXXX 18FXXX

Low cost and small size Mid-range cost and

performance High performance

8-pin packages 14–40-pin packages 18–80-pin packages

6 I/O pins 12–33 I/O pins 13–68 I/O pins

33/35×12/14-bit instructions 35×14-bit instructions 58×16-bit instructions

1 k word program memory 1–8 k word program memory 2–64 k word program memory

20MHz clock 20MHz clock 40MHz clock

4MHz internal oscillator 4/8MHz internal oscillator 8/10MHz internal oscillator

8-bit and 16-bit timer 2×8-bit and 1×16-bit timers Up to 2×8-bit and 3×16-bit timers

Up to 4 analogue inputs Up to 8 analogue inputs Up to 16 analogue inputs

In-circuit programming and debugging

In-circuit programming and debugging

In-circuit programming and debugging

37

Serial communication ports, parallel slave port

Serial communication ports, parallel slave port

1/2 pulse width modulation outputs, capture & compare inputs

Up to 14 pulse width modulation outputs, capture and compare inputs

CAN communication interface

2–64 k program memory

Fonte – Bates, 2004, p. 238.

As figuras abaixo mostram um paralelo entre o PIC 16F628A e o PIC

18F4550:

Figura 6 – Diagrama de pinos e encapsulamento PDIP dos PICs a)18F4550 b)16F628A. Fonte – Microchip Technology Inc., a) 2006, p. 2 b) 2004, p. 2.

Apesar dos numerosos recursos que os microcontroladores PIC possuem,

os próximos tópicos abordam as principais funções utilizadas nesses dispositivos

para esse documento.

4.1.1 Interrupções no PIC

As interrupções no PIC são de extrema importância em diversas

aplicações, entre elas podem-se destacar interrupções de timer, interrupção por

mudança de estado e interrupção de USART.

As interrupções de timer acontecem sempre que um dos contadores de

tempo interno estoura (SOUZA, 2005, p. 32).

38

A interrupção por mudança de estado é gerada por um sinal externo

ligado a uma porta específica do PIC caso ela esteja configurada como entrada.

Esse tipo de interrupção pode ser utilizado, por exemplo, para criar um sincronismo

com a rede de 60hz (SOUZA, 2005, p. 32).

A maioria dos PICs possui um sistema completo para comunicação serial

do tipo USART. Esse sistema, além de facilitar todo processamento para a entrada e

saída de dados seriais, possui duas interrupções para informar o programa quando

um dado é recebido e quando a transmissão de outro dado já foi terminada (SOUZA,

2005, p. 33).

4.2 SENSORES E ATUADORES

Devido a sua inércia, atrito estático e as suas características

eletromagnéticas, um motor DC não começa a girar até que haja uma corrente

elétrica razoavelmente grande em seus terminais. Isso faz com que sua resposta

seja não-linear, o que significa que a velocidade não é diretamente proporcional à

corrente ou tensão fornecida (BATES, 2004, p. 213).

A velocidade de um motor não pode ser prevista com precisão para

qualquer dado atual, porque a carga sobre seu eixo irá afetá-la. Portanto, se a

velocidade ou a posição de um motor DC está sendo controlada, são necessários

sensores para medir as variáveis de saída e um sistema de controle para o

acionamento do motor (BATES, 2004, p. 213).

A velocidade de um motor DC é controlada pela corrente na sua

armadura, que interage com o campo magnético produzido por enrolamentos do

campo (ou ímãs permanentes) para produzir torque. Um sistema de controle

analógico permite o controle permanente sobre a corrente do motor, e um conversor

digital para analógico pode ser usado na saída se o feedback e o controle é digital.

No entanto, a interface de controle pode ser simplificada se um sinal PWM é

utilizado (BATES, 2004, p. 214).

Um feedback digital pode ser obtido a partir de um sensor que detecta a

rotação de um eixo. Uma maneira de fazer isso é utilizar um disco perfurado ou

setorizado ligado ao eixo e um sensor óptico para detectar as ranhuras ou furos no

39

disco. A posição do eixo pode ser detectada por contagem de pulsos e a velocidade

pelo incremento desses pulsos variando no tempo (BATES, 2004, p. 214).

O sinal de feedback pode ser inserido em um microcontrolador que

monitora essa entrada pulsante e varia sua saída para controlar a velocidade e/ou

posição do motor (BATES, 2004, p. 214). A figura abaixo mostra esse sistema em

diagrama de blocos:

Figura 7 – Diagrama de blocos típico de controle de um motor DC. Fonte – Microchip Technology Inc., 2003 p. 1.

O sistema de feedback da figura acima é detalhado na figura abaixo com

representação de um encoder de quadratura:

Figura 8 – Esquema de funcionamento de um encoder de quadratura. Fonte – Microchip Technology Inc., 2003 p. 14.

40

Um encoder de quadratura pode fornecer dados para velocidade, direção

e posição relativa. Esse encoder de quadratura é tipicamente fixado dentro do

conjunto do motor e fornece lógicas de sinais que podem ser inseridas ao

microcontrolador. A relação de fase entre o canal A e B é usado para determinar o

sentido de rotação do motor.

4.3 AQUISIÇÃO DOS DADOS POR RADIOFREQUÊNCIA

Para analisar os dados de velocidade das rodas e do robô no espaço de

trabalho, a troca de informação entre o robô e um computador se da por

radiofreqüência. O dispositivo utilizado para transmitir essas informações pode ser

visto na figura abaixo e descrito a seguir:

Figura 9 – Módulo de comunicação por radiofreqüência XBee-Pro. Fonte – http://www.digi.com/images/products/prd-xbeepromoduleseries1_lg.jpg.

O módulo RF XBee-Pro foi projetado para atender o padrão de

comunicação IEEE 802.15.4 e satisfazer necessidades como baixo custo e baixa

potência para redes de sensores sem fio. Os módulos consomem pouca energia e

proporcionam um tráfego confiável de dados entre dispositivos (DIGI

INTERNATIONAL INC., 2009, p. 4). O quadro abaixo mostra alguns dados de

especificação desse módulo:

Quadro 4 – Dados de especificação do módulo XBee-Pro.

Specification XBee-Pro

Performance

Indoor/Urban Range Up to 300 ft. (90 m), up to 200 ft (60 m) International variant

Outdoor RF line-of-sight Range Up to 1 mile (1600 m), up to 2500 ft (750 m) international variant

Transmit Power Output 63mW (18dBm)* 10mW (10 dBm) for International variant

41

RF Data Rate 250,000 bps

Serial Interface Data Rate 1200 bps - 250 kbps (non-standard baud rates also supported)

Receiver Sensitivity -100 dBm (1% packet error rate)

Power Requirements

Supply Voltage 2.8 – 3.4 V

Transmit Current (typical) 250mA (@3.3 V) (150mA for international variant) RPSMA module only: 340mA (@3.3 V) (180mA for international variant)

Idle / Receive Current (typical) 55mA (@ 3.3 V)

Power-down Current < 10 μA

General

Operating Frequency ISM 2.4 GHz

Dimensions 0.960” x 1.297” (2.438cm x 3.294cm)

Operating Temperature -40 to 85º C (industrial)

Antenna Options Integrated Whip, Chip or U.FL Connector, RPSMA Connector

Networking & Security

Supported Network Topologies

Number of Channels 12 Direct Sequence Channels

Addressing Options PAN ID, Channel and Addresses

Agency Approvals

United States (FCC Part 15.247)

OUR-XBEEPRO

Industry Canada (IC) 4214A XBEEPRO

Europe (CE) ETSI (Max. 10 dBm transmit power output)*

Japan R201WW08215111 (Max. 10 dBm transmit power output)*

Austraila C-Tick

Fonte – Digi International Inc., 2009, p. 5.

Através de sua porta serial, o módulo XBee-Pro pode se comunicar com

qualquer lógica e tensão UART compatível, ou através de um tradutor de nível a

qualquer dispositivo serial como por exemplo uma interface RS-232 com um

computador (DIGI INTERNATIONAL INC., 2009, p. 10). Na figura abaixo se pode ver

um exemplo de comunicação entre microcontroladores através de dois módulos

XBee-Pro:

Figura 10 – Diagrama de Fluxo de Dados de um sistema com interface UART. Fonte – Digi International Inc., 2009, p. 10.

42

5 ESTIMAÇÃO DE POSIÇÃO E ORIENTAÇÃO

Hoje, várias técnicas estão disponíveis para a obtenção da posição e da

orientação de robôs móveis utilizando encoders, visão computacional, sonar e

scanner a laser (LAGES, 1998, p. 110).

A técnica Dead-Reckoning é um procedimento matemático simples para

determinar a localização atual de um robô, pelo seu deslocamento em relação a uma

posição anterior (BORENSTEIN, EVERETT e FENG, 1996, p. 13).

É conhecido que para trajetórias longas, o método de dead-reckoning não

é adequado para estimação da posição de robôs móveis, pois pela própria natureza

do método, os erros vão sendo acumulados ao integrarem-se os deslocamentos

para obtenção da posição e orientação. Os erros presentes nesse método podem

ser sistemáticos e não sistemáticos (LAGES, 1998, p. 110).

Os erros sistemáticos são causados basicamente por incertezas nos

parâmetros geométricos utilizados para calcular a posição e orientação do robô. As

principais causas são: a diferença entre o diâmetro nominal médio e o diâmetro real

das rodas, a diferença entre os diâmetros médios das rodas e a diferença entre

rodas nominal e real (LAGES, 1998, p. 110).

Erros não sistemáticos ocorrem devido ao escorregamento das rodas,

imperfeições no solo e fenômenos similares (LAGES, 1998, p. 110).

O método de dead-reckoning é parte principal para a realização do

controle de velocidade e trajetória para o robô omni-direcional e é descrita a seguir.

5.1 O MÉTODO DEAD-RECKONING

Uma das técnicas mais simples para estimação de posição e orientação

consiste em contar-se a quantidade de pulsos a partir dos encoders de cada roda

em certo intervalo de tempo (LAGES, 1998, p. 115). Assumindo que a resolução dos

encoders a uma volta completa de uma das rodas pode-se obter a variação da

43

posição angular de cada eixo j = [0,1,2] no tempo discreto como mostra as equações

abaixo:

(25)

(26)

Assumindo que em um intervalo de tempo ta de amostra entre os

instantes n-1 e n pode-se obter a variação da posição e orientação do robô num

instante n tornando discreta a equação de cinemática direta obtendo a equação

abaixo:

(27)

A figura abaixo mostra a contribuição das velocidades v e para vn = 0

sendo que o robô descreve um movimento curvilíneo:

Figura 11 – Esquema de movimento do robô para uma trajetória curvilínea. Fonte – Moreira 2008, p. 17.

A aproximação discreta da posição do robô pode ser descrita pelas

equações abaixo (MOREIRA, 2008, p. 18):

Com = 0:

44

(28)

(29)

Com ≠ 0:

(30)

(31)

(32)

Com as equações descritas anteriormente o robô consegue ter dados de

posição e orientação necessários para descrever trajetórias no espaço de trabalho e

assim realizar tarefas diversas.

45

6 CONTROLE PID DISCRETO

Há vários elementos dentro de um sistema de controle e para esse

documento os termos abaixo descrevem esses principais elementos envolvidos:

a) Planta – Os motores do robô.

b) Sensores – Os encoders incrementais acoplados no eixo dos motores.

c) Set point – O valor de velocidade que o eixo da roda deve atingir.

d) Sinal de erro – É a diferença entre a resposta da planta e da resposta

desejada (valor nominal).

e) Distúrbios – São as entradas indesejadas para a planta como

irregularidades no eixo, imprecisão nos sensores, etc.

f) Controlador – O controlador é responsável por diversas tarefas e é o

elo que une todos os elementos físicos e não físicos. Ele mede o sinal

de saída dos sensores da, processa o sinal e retira um erro baseado na

medição do sinal e o valor nominal. Uma vez que os dados do sensor

foram coletados e processados, o resultado é usado para encontrar

valores PID, que então é enviado para uma central de correção de

erros. A taxa em que tudo isso acontece depende do poder de

processamento do microcontrolador (MICROCHIP TECHNOLOGY

INC., 2004a, p. 1). A figura abaixo mostra um sistema de exemplo para

um controle de um motor:

Figura 12 – Sistema de controle em malha fechada para um motor. Fonte – Microchip Technology Inc., 2004a p. 3.

Na aplicação do controlador PID existem três termos que são baseados

na medição de erro mostrados na equação abaixo (MICROCHIP TECHNOLOGY

INC., 2004b, p. 3):

46

(33)

O primeiro termo é o termo proporcional, em que Kp é a constante de

proporcionalidade, o segundo termo é o termo integrador onde K i é a constante de

integração e o terceiro termo é o derivativo em que Kd é a constante de derivação.

Para o controle digital pode-se tornar todos os termos discretos obtendo a

equação abaixo (MICROCHIP TECHNOLOGY INC., 2004b, p. 3):

(34)

A sintonia de um controlador PID baseia-se em determinar as constantes

de proporcionalidade, integração e derivação. Para tal existem métodos

experimentais diversificados conforma o modelo do sistema. A seguir é descrito o

método Ziegler-Nichols de Malha Fechada.

6.1 O MÉTODO ZIEGLER-NICHOLS EM MALHA FECHADA

Com o método de Sintonização de Ziegler-Nichols de Malha Fechada

pretende-se obter no máximo 25% de overshoot e baseia-se experimentalmente em

(NUNES e LOPES, 2001, p. 2):

a) Determinar K crítico, considerando-se apenas como ganho

proporcional, isto é fazendo Td igual a zero e Ti igual a infinito.

b) Determinar a freqüência de oscilação e, portanto, determinar o período

crítico.

A seguir apresenta-se o quadro de sintonização:

Quadro 5 – Sintonia de um controlador PID pelo método Ziegler-Nichols.

Controlador K Ti Td

P 0

PI 0

PID

Fonte – Nunes e Lopes, 2001, p. 2.

47

7 VARIÁVEIS

Variáveis Primárias Variáveis Secundárias

1. Eletrônica embarcada a) Alimentação do circuito;

b) Encoders incrementais;

c) Módulo de radiofreqüência;

d) Sistema de controle de potência dos motores.

2. Controle de velocidade a) Controle PID;

b) Modelo cinemático do robô;

c) Tensão nos motores;

d) Velocidades das rodas.

3. Controle de trajetória a) Controle PID;

b) Deslocamento das rodas;

c) Modelo matemático da função de controle;

d) Orientação do robô no espaço;

e) Posição do robô no espaço.

4. Simulação do robô a) Modelo do robô;

b) Programação do robô.

48

8 METODOLOGIA

Através de pesquisas bibliográficas em livros, monografias, teses,

pesquisas experimentais e simulações, esse trabalho foi realizado conforme a

seqüência abaixo:

a) Estudo da Robótica Móvel em diversos aspectos como inteligência

computacional, sistemas de locomoção e sistemas de localização.

Esses aspectos foram de extrema importância para o desenvolvimento

desse projeto porque após esse estudo pode-se direcionar a pesquisa

para os assuntos correntes e obter conhecimento global do assunto.

b) Estudo do modelo cinemático e dinâmico do robô omni-direcional de

três rodas. Tal estudo deve-se ao fato de que o controle de velocidade

e trajetória do mesmo depende diretamente das variáveis cinemáticas

e/ou dinâmicas do sistema.

c) Estudo dos dispositivos eletrônicos para o robô: como mostrado na

modelagem do robô, este dispõe de motores elétricos para seu

funcionamento além de sensores e dispositivos de comunicação por

radiofreqüência fazendo necessário o estudo dos mesmos.

d) Estudo de métodos matemáticos para auto-localização: tendo

estudado os dispositivos de sensoriamento, fez-se necessário um

estudo analítico de posição e orientação do robô para obter-se um

modelo matemático de re-alimentação dessas variáveis.

e) Estudou-se o método de controle Proporcional Integral Derivativo com

métodos de sintonização para ser implementado na eletrônica

embarcada.

f) Descreveu-se a eletrônica embarcada no robô visando aspectos de

inteligência computacional, sensoriamento, acionamento dos motores,

alimentação do circuito e comunicação por radiofreqüência.

g) Descreveu-se o controlador de velocidade discreto em malha fechada

do robô baseado na cinemática do mesmo.

49

h) Realizaram-se simulações do comportamento do robô num ambiente

virtual com o software SimTwo Versão 0.9 para validação do modelo

cinemático e dos controles de velocidade das rodas.

i) Descreveram-se os controladores de trajetória em três funções de

movimento: GoToTarget, FollowLine e FollowCircle.

j) Realizaram-se simulações do comportamento do robô num ambiente

virtual com o software SimTwo Versão 0.9 para validação dos

controladores de trajetória propostos.

Os tópicos mencionados nas letras “a” até “e” foram descritos do capítulo

2 ao capítulo 6, os tópicos “f” até “j” são descritos a seguir.

8.1 DESCRIÇÃO DA ELETRÔNICA EMBARCADA

A eletrônica embarcada proposta nesse trabalho possui as seguintes

características:

a) Tensão de entrada contínua 12V.

b) Regulagem de tensão contínua interna de 5V para alimentação dos

microcontroladores e demais dispositivos.

c) Regulagem de tensão contínua interna de 3.3V para alimentação do

módulo transceptor Xbee.

d) Seis saídas com tensão variável de 0V a 12V para alimentação dos

motores (duas para cada motor com inversão de sentido).

e) Três circuitos em ponte para inversão de sentido dos motores.

f) Seis entradas digitais para feedback dos sinais dos encoders (duas

para cada encoder).

O circuito possui um microcontrolador central PIC18F4550 que gerencia a

trajetória do robô. Tal microcontrolador se comunica com mais 3 outros

microcontroladores PIC16F628A através de uma comunicação USART com

mecanismo de acesso ao meio (conjunto de regras que determinam quem pode

transmitir) tipo Token-Ring em que um dispositivo só transmite após receber uma

informação ficam dispostos com topologia em anel.

50

Os três microcontroladores periféricos têm por função receber o sinal de

controle de velocidade dos motores e controlá-los individualmente. O controle dos

motores é em malha fechada e possuí como sensores encoders incrementais.

O microcontrolador central também é capaz de transmitir dados por

radiofreqüência através de um módulo transceptor Xbee conectado a uma porta

serial virtual USART criada via software, pois este PIC só possui uma única porta

serial incorporada no seu hardware. Com esse recurso o engenheiro pode visualizar

os dados processados no microcontrolador podendo analisá-los e assim realizar

melhorias.

Para facilitar a gravação e atualização do firmware no PIC18F4550 pode-

se programá-lo em modo BootLoader que nada mais é do que um firmware que

permite a gravação de um outro firmware “por cima” via comunicação serial.

A figura abaixo mostra as comunicações envolvidas na eletrônica

embarcada:

Figura 13 – Sistema de comunicação da eletrônica embarcada. Fonte – Autor, 2010.

O circuito de comunicação à esquerda da figura acima representa uma

placa confeccionada industrialmente e amplamente encontrada no mercado para

comunicação USB com o XBEE. O esquema eletrônico restante do robô encontra-se

detalhado no Apêndice A.

51

8.2 A PLATAFORMA DE SIMULAÇÃO SIMTWO

A plataforma de simulação SimTwo foi desenvolvida pelo Prof. Dr. Paulo

Costa da Faculdade de Engenharia da Universidade do Porto. Esta plataforma

evidencia um cuidado especial no realismo, especialmente na dinâmica e na

modelagem. Estes incluem características não lineares, que se revelam importantes

no comportamento dos objetos reais. O simulador permite também a visualização 3D

em tempo real da simulação, o que se revela útil para supervisão da simulação

(COSTA, 2010).

O SimTwo é um sistema de simulação realista onde é possível

implementar vários tipos de robôs como:

a) Robôs móveis com diferentes configurações como: robôs diferenciais

ou com rodas omnidirecionais;

b) Manipuladores;

c) Quadrúpedes;

d) Humanóides;

e) Qualquer tipo de robô terrestre que possa ser descrito com uma

mistura de articulações rotativas e rodas clássicas/omnidirecionais;

f) Veículos mais leves que o ar com ou sem hélices para propulsão.

O realismo da dinâmica implementada no SimTwo é conseguida

decompondo o robô num sistema de corpos rígidos e motores elétricos. A mecânica

associada aos corpos é simulada numericamente considerando as suas

propriedades físicas: forma, massa e momentos de inércia, atritos das superfícies e

elasticidade. Certas articulações típicas: eixo, cardã e calha são definidas

explicitamente e podem ter associado um sistema de acionamento e sensores

(COSTA, 2010).

O sistema de acionamento pode ser constituído por um motor DC,

opcionalmente com caixa redutora e um controlador. O controlador pode ser de

vários tipos: um PID aplicado ao sinal de posição ou de velocidade ou uma

realimentação de estado (COSTA, 2010).

O modelo do motor DC contém vários elementos não lineares tais como

saturação da tensão aplicada, limite de corrente e atrito de Coulomb (COSTA, 2010).

52

Além do sistema de controle de baixo nível, o SimTwo oferece a

possibilidade de fornecer os sinais de referência para esses controladores através

de um controlador de mais alto nível implementado pelo utilizador.

A figura abaixo mostra as janelas do SimTwo Versão 0.9.3.8 Compilado

em 19/05/2010:

Figura 14 – Ambiente 3D e demais janelas do Simulador SimTwo. Fonte – Autor, 2010.

O SimTwo recorre a várias bibliotecas de código aberto que são:

a) GLScene – biblioteca para a visualização 3D;

b) ODE – simulação de corpos rígidos;

c) Pascal Script – permite a programação do sistema de controle;

d) SynEdit – implementa o editor de scripts;

e) OmniXml – facilita o uso de arquivos de configuração em formato

XML;

f) RxLib – conjunto variado de componentes (Persistência, etc.).

53

8.2.1 O Robô Omni-direcional no SimTwo

A janela do SimTwo de nome “XML Scene Edit” é onde se realiza a

modelagem da simulação 3D. Em linguagem de programação XML pode-se modelar

robôs, estruturas fixas, corpos livres e desenhos no chão do ambiente de simulação.

Todos os corpos inclusive os robôs são derivados de estruturas geométricas simples

como cilindros, paralelepípedos, esferas combinadas.

A figura abaixo mostra o robô omni-direcional no ambiente de simulação

do SimTwo, o desenho no seu topo indica com uma seta a orientação em que este

se encontra:

Figura 15 – Robô omni-direcional no ambiente de simulação SimTwo. Fonte – Autor, 2010.

O quadro abaixo descreve as características do robô omni-direcional que

são incorporadas na simulação do SimTwo:

Quadro 6 – Características do robô omni-direcional no SimTwo.

Robô

Raio 0,09 m

Altura 0,125 m

Massa 1,59 Kg

Encoder

Resolução 1000 pulsos/revolução

Média de Ruído 0

Desvio padrão do Ruído 0

Rodas

Raio 0,04 m

Largura 0,025 m

Massa 0,377 Kg

54

Distância ao centro 0,1m

Ângulo Roda 0 60°

Ângulo Roda 1 180°

Ângulo Roda 2 -60°

Atrito estático no eixo 10-3

N.m

Atrito dinâmico no eixo 10-5

N.m/rad/s

Motor redutor

Tensão Máxima 12 V

Corrente Máxima 4 A

Rel. de Transmissão 6:1

Resistência Interna 0,3 Ω

Torque/Corrente 0,024 N.m/A

Fonte – Autor, 2010.

Ainda que sejam permitidos vários robôs na mesma simulação, o

programa de controle é único e é capaz de controlar a todos simultaneamente. Cada

robô possui um número de identificação, iniciado em zero, e atribuído pela ordem

em que é colocado no arquivo scene.xml. Por sua vez, cada robô pode possuir

vários eixos controlados e vários corpos (COSTA, 2010). O código que modela o

robô omni-direcional pode ser visto no Apêndice B.

Na janela de controle do SimTwo pode-se programar robôs,

comunicações, interações com a janela matricial, etc. Essa janela é um editor de

código para linguagem Pascal Script que tem opções de visualização de variáveis,

opções de salvar e carregar arquivos, entre outros. O fluxograma abaixo mostra o

procedimento padrão que deve existir no código para o funcionamento correto do

controle:

Figura 16 – Rotina de controle padrão do SimTwo. Fonte – Autor, 2010.

55

8.3 O CONTROLADOR DE VELOCIDADE PROPOSTO

O controle de velocidade do robô baseia-se em inserir uma velocidade

desejada do espaço de trabalho do mesmo. Como descrito no Capítulo 3, o robô

omni-direcional é capaz de se locomover com velocidades (vx, vy, w) independentes

uma da outra, ou seja, o robô pode locomover-se apenas na direção x e não

locomover-se na direção y nem na orientação θ.

Para inserir uma dada velocidade global no robô deve-se inserir uma

tensão específica em cada uma das rodas e é para determinar essas tensões que se

necessita da modelagem cinemática do mesmo. Para o controle dessa velocidade, a

re-alimentação de velocidade das rodas também é necessária.

Dada uma velocidade desejada (vx, vy, w) e a orientação atual θ do robô,

o microcontrolador central calcula o vetor (v, vn, w) do robô pela equação (4). Após

calculado, o vetor (v, vn, w) é inserido na equação (6) para se obter a velocidade

desejada das rodas do robô (v0, v1, v2). Essas velocidades são enviadas para os

microcontroladores periféricos que ficam responsáveis de inseri-las nas rodas.

Quando os microcontroladores periféricos recebem os valores de

velocidade desejado das rodas utiliza-se um algoritmo de controle PID discreto para

garantir que os eixos adquiram a tal velocidade. Esse algoritmo age com sucessivas

interações de cálculos que dependem diretamente do erro em que a velocidade do

eixo se encontra no momento da interação.

O erro de velocidade instantânea em cada eixo j=[0,1,2] é calculado

subtraindo a velocidade desejada pela velocidade instantânea como mostra a

equação abaixo:

(35)

Encontrado o erro de velocidade, calcula-se a saída do controlador

conforme a equação (34). As constantes K, Ti e Td são obtidas experimentalmente

com a ajuda do método de sintonização Ziegler-Nichols mostrado no Capítulo 6.

Para saber o valor da velocidade atual de cada eixo utiliza-se a equação (7), sendo

que a velocidade angular instantânea das rodas j=[0,1,2] é calculada conforme a

equação (3), em que sua forma discreta no tempo é mostrada na equação abaixo:

56

(36)

Para validar o controle de velocidade que depende da modelagem

cinemática do robô, o fluxograma abaixo mostra a seqüência de comandos do

SimTwo que são escritas no editor de código em Pascal Script detalhando as

funções Initialize e Control:

Figura 17 – Fluxograma de validação de controle de velocidade. Fonte – Autor, 2010.

A função Control é “chamada” pelo programa num período de 40 ms, esse

período é perfeitamente ajustável na janela de configuração do programa. Por

padrão pede-se ajustar esse período acima de 10 ms para o funcionamento

adequado do software.

A função “AtualizaDados” é executada para atualizar as variáveis de

velocidade e posição instantânea a cada interação do programa. A função

“VelControl” recebe os dados de identificação do robô a ser controlado e a

velocidade de referência que o mesmo deve possuir, depois compara com a

velocidade instantânea e realiza o controle de potência dos motores.

O código do controle de velocidade encontra-se no Apêndice C.

57

8.4 O CONTROLADOR DE TRAJETÓRIA PROPOSTO

O controle de trajetória do robô baseia-se em três funções que movimento

do robô no espaço global, essas funções são GoToTarget, FollowLine e

FollowCircle. Essas funções são descritas a seguir.

8.4.1 Descrição da Função GoToTarget

Do inglês “ir até o alvo”, essa função faz com que o robô se locomova de

onde estiver a uma dada velocidade nominal até um ponto dado (x, y) e permaneça

em uma dada orientação θ. Para isso parte-se do princípio que o robô sabe sua

posição e orientação atual, portanto é proposto um algoritmo que realiza essa

função. A figura abaixo mostra o exemplo de como funciona esse algoritmo:

Figura 18 – Demonstração da função GoToTarget. Fonte – Autor, 2010.

A figura acima mostra a trajetória do robô com a função GoToTarget,

observa-se que o percurso que o robô descreve não é de devida importância para

essa função. Sendo que a posição do robô é descrita em (x, y, ) este sai em (0, 0,

0º) e chega até o alvo (2, 1, 45º).

58

O controle proposto da referida função baseia-se em um algoritmo PID

discreto como descrito no Capítulo 6. Para cada variável (x, y, ) calcula-se o erro

relativo ao ponto alvo subtraindo a coordenada (x, y, ) do alvo à coordenada atual

do robô. Depois de calculados os erros do sistema calculam-se a saída do

controlador conforme a equação (34). Tal saída é normalizada para valores de

velocidade (vx, vy, w) que são inseridas no controlador de velocidade e as constantes

K, Ti e Td são obtidas experimentalmente com a ajuda do método de sintonização

Ziegler-Nichols.

8.4.2 Descrição da Função FollowLine

Do inglês “seguir em uma linha”, essa função faz com que o robô siga por

uma dada reta imaginária que corta dois pontos (x1, y1), (x2, y2) dados no espaço

global com uma dada velocidade nominal. Parte-se do princípio que o robô sabe sua

localização atual e descreve um percurso para se aproximar da reta ao tempo que

segue por ela. Esta função ainda permite que uma dada orientação seja fixada no

robô ao tempo que esse descreve seu movimento.

Dados os dois pontos quaisquer, calculam-se as constantes fundamentais

a, b e c da reta que passa por estes. O controle proposto da referida função baseia-

se em um algoritmo PID discreto como descrito no Capítulo 6. O cálculo do erro para

esse controlador está na distância entre o ponto de localização do robô e a reta

calculada anteriormente. Esse cálculo pode ser visto na equação abaixo:

(37)

Depois de calculado o erro do sistema calcula-se a saída do controlador

conforme a equação (34) normalizada a um valor de velocidade resultante. Tal

velocidade é então distribuída em duas componentes e somadas duas a duas com

mais outras componentes que representam a velocidade nominal de entrada da

função. As equações abaixo mostram o somatório vetorial das velocidades:

(38)

59

(39)

A variável Linha é o ângulo que a reta faz com a horizontal e é descrita

pela equação abaixo:

(40)

Como a reta pode assumir diversas configurações no plano utiliza-se a

função computacional arco-tangente2 que abrange todos os quadrantes do espaço

cartesiano. A figura abaixo mostra o erro de distância do robô á reta:

Figura 19 – Erro de distância do FollowLine. Fonte – Autor, 2010.

Para a orientação calcula-se o erro subtraindo a orientação desejada

pela orientação atual do robô. Depois de calculado o erro do de orientação calcula-

se a saída do controlador conforme a equação (34). Tal saída é normalizada para

um valore de velocidade angular w que é inserido no controlador de velocidade.

A figura abaixo demonstra uma trajetória em uma reta para uma

orientação de 90º e os pontos (0, 0) e (2, 1) da reta com o robô na posição (0, 0, 0º):

60

Figura 20 – Demonstração da função FollowLine. Fonte – Autor, 2010.

8.4.3 Descrição da Função FollowCircle

Do inglês “seguir em círculo”, essa função faz com que o robô siga por um

círculo imaginário com centro em (xc, yc) e raio dado no espaço global com uma

dada velocidade nominal. Parte-se do princípio que o robô sabe sua localização

atual e descreve um percurso para se aproximar do círculo ao tempo que segue por

ele. Esta função ainda permite que uma dada orientação seja fixada no robô ao

tempo que esse descreve seu movimento.

O controlador da orientação segue o mesmo princípio descrito

anteriormente para a função FollowLine que também é igual a função GoToTarget.

O controle proposto da referida função baseia-se em um algoritmo PID

discreto como descrito no Capítulo 6. O cálculo do erro para esse controle está na

subtração do raio informado a função pela distancia do robô à reta ao centro da

curva. Tal reta é perpendicular ao raio do círculo como mostra a figura abaixo:

61

Figura 21 – Erro de distância do FollowCircle. Fonte – Autor, 2010.

A equação abaixo mostra o cálculo do erro para o FollowCircle:

(41)

A figura abaixo demonstra a função FollowCircle em que o robô sai de (0,

0, 0º) e segue por um círculo de raio 1 e centro (0, 0):

Figura 22 – Demonstração da função FollowCircle. Fonte – Autor, 2010.

Observa-se que na figura acima o robô aproxima-se da curva em sentido

horário permanece seguindo a curva com orientação de 45º como desejado. O

código completo da simulação pode ser visto no Apêndice D.

62

9 RESULTADOS OBTIDOS

Na simulação da eletrônica obteve-se o funcionamento correto das

comunicações envolvidas e do funcionamento dos motores, com a ajuda de

terminais virtuais para comunicação serial pode-se visualizar o tráfego de caracteres

entre os microcontroladores, porém com um grave atraso relativo ao processamento

computacional da simulação. O controle de motores é perfeitamente aceitável visto a

disponibilidade de simulação dos encoders.

O controle de velocidade na simulação com SimTwo satisfaz sem

nenhuma dúvida o funcionamento do mesmo. A figura abaixo mostra o gráfico das

velocidades vx e vy pelo tempo com um set point de (0.1, 0.1, 0) sendo (vx, vy, w):

Figura 23 – Gráfico velocidade x tempo da função VelControl. Fonte – Autor, 2010.

O gráfico abaixo mostra o mesmo ensaio realizado anteriormente, porém

com as velocidades angulares das rodas:

63

Figura 24 – Gráfico velocidade angular x tempo da função VelControl. Fonte – Autor, 2010.

O método Dead-Reckoning descrito no Capítulo 5 foi implementado no

SimTwo a partir da função GoToTarget pois não se fazia necessário a utilização

desta para o controle de velocidade. O próprio simulador é capaz de fornecer a

posição e orientação do robô, assim pode-se calibrar os controladores PID de

posição com essa função, o que garantiu precisão do controle. O gráfico abaixo

mostra um ensaio da função GoToTarget para uma posição inicial do robô (0, 0, 0º)

e posição final (0.5, 0.5, 45º) e velocidade nominal 0.5 m/s sem a utilização do

método Dead-Reckoning:

Figura 25 – Gráfico posição x tempo da função GoToTarget sem Dead-Reckoning. Fonte – Autor, 2010.

64

No gráfico acima se observa que a orientação do robô está medida em

radianos. A figura abaixo mostra a mesma simulação em 3D:

Figura 26 – Simulação 3D da função GoToTarget sem Dead-Reckoning. Fonte – Autor, 2010.

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

ser visto no gráfico abaixo:

Figura 27 – Gráfico posição x tempo da função GoToTarget com Dead-Reckoning. Fonte – Autor, 2010.

Na figura abaixo se pode ver o ensaio acima na simulação 3D:

65

Figura 28 – Simulação 3D da função GoToTarget com Dead-Reckoning. Fonte – Autor, 2010.

No caso da função FollowLine não se obteve sucesso no uso do método

Dead-Reckoning e por isso, esse método foi descartado também para a função

FollowCircle. No ensaio abaixo pode-se ver o funcionamento do FollowLine em que

o robô parte do ponto (0, 0, 0º) e segue pro uma reta que passa pelos pontos (0, 0) e

(2, 1) com velocidade nominal 0.5 m/s e orientação 30º:

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

O ensaio acima pode ser visto no modo de simulação 3D conforme a

figura abaixo:

66

Figura 30 – Simulação 3D da função FollowLine. Fonte – Autor, 2010.

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

robô encontra-se inicialmente na posição (0, 0, 0º) e seque por um círculo de centro

(0, 0, 0º) e raio 1 m com velocidade nominal 0.5 m/s e orientação 90º:

Figura 31 – Gráfico posição x tempo da função FollowCircle. Fonte – Autor, 2010.

O ensaio acima pode ser visto na simulação 3D pela figura abaixo:

67

Figura 32 – Simulação 3D da função FollowCircle. Fonte – Autor, 2010.

O quadro abaixo mostra o valor das constantes dos controladores obtidas

experimentalmente com a ajuda do método de sintonização Ziegler-Nichols:

Quadro 7 – Constantes dos Controladores.

Constantes PID Kp Ti Td

Controlador de Velocidade 4,5 0,25 0,006 Controlador GoToTarget 1 2 0 Controlador FollowLine 6 2 0,05 Controlador FollowCircle 10 10000 0,0001

Fonte – Autor, 2010.

68

10 CONCLUSÃO

A eletrônica embarcada obteve êxito no que diz respeito a comunicação

entre os microcontroladores e por radiofreqüência, controle dos motores e

alimentação do circuito. Contudo, a simulação não prevê o funcionamento em tempo

real do controle pois o software é incapaz de realizar esse tipo de procedimento.

Sendo assim, somente um ensaio real é capaz de informar a capacidade dos

microcontroladores quando processarem os algoritmos de cinemática e de controle

propostos.

O algoritmo de controle de velocidade obteve sucesso e pode ser

implementado perfeitamente em um robô real. A simulação para esse controle se

aproxima muito do modelo real provando a capacidade desse controle.

Durante a simulação pode-se perceber que o método Dead-Reckoning

não pode ser utilizado para trajetórias grandes e assim não se conseguiu atingir uma

realimentação de posição apenas com esse método.

Os algoritmos de controle de trajetória foram todos satisfatórios e,

portanto podem ser implementados em escala real, no entanto, os mesmos foram

calibrados com pouca aproximação do modelo real porque a simulação não insere

erros na re-alimentação de posição, portanto, conseguiu-se um modelo de controle

ideal que depende exclusivamente no quão preciso é a re-alimentação de posição.

69

REFERÊNCIAS

BATES, Martin. PIC Microcontrollers: An Introduction to Microelectronics. 2ª ed. Elsevier, 2004. 372p. BORENSTEIN, Johann; EVERETT, H. R.; FENG, Liqiang. Where am I? Sensors and Methods for Mobile Robot Positioning. 1996. 281f. Oak Ridge National Lab

(ORNL) D&D Program and United States Department of Energy, University of Michigan, Michigan. BRAUNL, Thomas. Embedded Robotics: Mobile Robot Design and Applications with Embedded Systems. 2ª ed. Berlin: Springer, 2006. 458p.

CONCEIÇÃO, André Gustavo Scolari. Controlo e Cooperação de Robôs Móveis Autónomos Omnidireccionais. 2007. 155f. Tese (Doutorado) – Engenharia

Eletrotécnica e de Computadores, Universidade do Porto, Porto. COSTA, Paulo José Cerqueira Gomes da. SimTwo. 2010. Disponível em:

<http://paginas.fe.up.pt/~paco/wiki/index.php?n=Main.SimTwo>. Acesso em: 10 de Junho 2010. DIGI INTERNATIONAL INC. XBee®/XBee-PRO® RF Modules: Product Manual v1.xEx - 802.15.4 Protocol, 2009. Disponível em: <http://ftp1.digi.com/support/documentation/90000982_B.pdf>. Acesso em: 15 de Novembro 2009. FRANCO, Alexandre da Costa e Silva. Geração e Controle de Trajetória de Robôs Móveis Omnidirecionais. 2007. 142f. Tese (Mestrado) - Escola Politécnica,

Universidade Federal da Bahia, Salvador. LAGES, Walter Fetter. Controle e Estimação de Posição e Orientação de Robôs Móveis. 1998. 159f. Tese (Doutorado) - Centro Técnico Aeroespacial, Instituto Tecnológico de Aeronáutica, São José dos Campos. MICROCHIP TECHNOLOGY INC. Motor Control Sensor Feedback Circuits, 2003. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00894a.pdf>. Acesso em: 15 de Novembro 2009.

70

MICROCHIP TECHNOLOGY INC. PIC16F627A/628A/648A Data Sheet: Flash-

Based 8-Bit CMOS Microcontrollers with nanoWatt Technology, 2004. Disponível em: <http://ww1.microchip.com/downloads/en/DeviceDoc/40044F.pdf>. Acesso em: 11 de Novembro 2009. MICROCHIP TECHNOLOGY INC. Implementing a PID Controller Using a PIC18 MCU, 2004a. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00937a.pdf>. Acesso em: 29 de Novembro 2009. MICROCHIP TECHNOLOGY INC. Software PID Control of an Inverted Pendulum Using the PIC16F684, 2004b. Disponível em: <http://ww1.microchip.com/downloads/en/AppNotes/00964A.pdf>. Acesso em: 28 de Novembro 2009. MICROCHIP TECHNOLOGY INC. PIC18F2455/2550/4455/4550 Data Sheet:

28/40/44-Pin, High-Performance, Enhanced Flash, USB Microcontrollers with nanoWatt Technology, 2006. Disponível em: <http://ww1.microchip.com/downloads/en/devicedoc/39632c.pdf>. Acesso em: 11 de Novembro 2009. MOREIRA, António Paulo Gomes Mendes. Sistemas Robóticos Autónomos. 2008. 84f. Notas de aula (Mestrado) – Engenharia Eletrotécnica e de Computadores, Universidade do Porto, Porto. MURPHY, Robin R. Introduction to AI Robotics. 1ª Ed. Massachusetts: MIT Press,

2000. 466p.

NUNES, Urbano Carreira; LOPES, Ana Cristina. 3ª Aula de Controlo Inteligente: Controlo PID Discreto, 2001. Disponível em: <http://orion.ipt.pt/~anacris/ci_1/pdf/aula3.pdf>. Acesso em: 29 de Novembro 2009.

OLIVEIRA, Hélder Filipe Pinto de. Análise do Desempenho e da Dinâmica de Robôs Omnidireccionais de Três e Quatro Rodas. 2007. 156f. Tese (Mestrado) –

Engenharia Eletrotécnica e de Computadores, Universidade do Porto, Porto. SCATENA, Jean Miler. Ambiente de Desenvolvimento de Aplicações para Robôs Móveis. 2008. 131f. Tese (Doutorado) - Escola de Engenharia Elétrica de São Carlos, Universidade de São Paulo, São Carlos.

71

SIEGWART, Roland; NOURBAKHSH, Illah R. Introduction to Autonomous Mobile Robots. 1ª Ed. Massachusetts, EUA: MIT Press, 2004. 321p.

SOUZA, David José de. Desbravando o PIC: Ampliado e Atualizado para PIC 16F628A. 9ª ed. São Paulo: Érica, 2005. 267p.

72

APÊNDICE A – ESQUEMA ELETRÔNICO DO ROBÔ

73

APÊNDICE B – CÓDIGO DE MODELAGEM DO ROBÔ

<?xml version="1.0" ?> <robot> <kind value='Robot'/> <defines> <const name='Raio' value='0.09' /> <const name='Altura' value='0.125' /> <const name='Densidade' value='500' /> <const name='Raio_Roda' value='0.04' /> <const name='Largura_Roda' value='0.025' /> <const name='Distancia_Ao_Centro' value='0.1' /> <const name='Densidade_Roda' value='3000' /> <const name='Encoder_PPR' value='1000' /> <const name='Caixa_de_Reducao' value='6' /> <const name='Angulo_Roda_0' value='60' /> <const name='Angulo_Roda_1' value='180' /> <const name='Angulo_Roda_2' value='-60' /> </defines> <solids> <cylinder> <ID value='Corpo'/> <mass value='Densidade*3.1415*(Raio*Raio)*Altura'/> <size x='Raio' y='0' z='Altura'/> <pos x='0' y='0' z='0.01+Altura/2'/> <rot_deg x='0' y='0' z='0'/> <color_rgb r='128' g='0' b='0'/> <texture name='MatBallTriangle' scale='6'/> </cylinder> </solids> <wheels> <default> <omni/> <tyre mass='Densidade_Roda*3.1415*(Raio_Roda*Raio_Roda)*Largura_Roda' radius='Raio_Roda' width='Largura_Roda' centerdist='Distancia_Ao_Centro'/> <surface mu='1' mu2='0.001'/> <axis angle='0'/> <motor ri='0.3' ki='2.4e-2' vmax='12' imax='4' active='1'/> <gear ratio='Caixa_de_Reducao'/> <friction bv='1e-5' fc='1e-3' coulomblimit='1e-2'/> <encoder ppr='Encoder_PPR' mean='0' stdev='0'/> <controller mode='pidspeed' kp='0' ki='0' kd='0' kf='0' active='0' period='10'/> <color_rgb r='128' g='0' b='128'/> </default> <wheel> <axis angle='Angulo_Roda_0'/> </wheel> <wheel> <axis angle='Angulo_Roda_1'/> </wheel> <wheel> <axis angle='Angulo_Roda_2'/> </wheel> </wheels> </robot>

74

APÊNDICE C – CÓDIGO DE CONTROLE DE VELOCIDADE

// Constantes do Robô const TENSAO_MAX = 12; const DIST_AO_CENTRO = 0.1; const TEMPO_DE_AMOSTRA = 0.04; const RAIO = 0.04; const ENCODER_PPR = 1000; const ID_ROBOT_1 = 0; const ID_RODA_0 = 0; const ID_RODA_1 = 1; const ID_RODA_2 = 2; // Constantes de Controle de Velocidade const KP = 4.5; const TI = 0.25; const TD = 0.006; // Variáveis Globais do Robô var RobotPos: TState2D; //Coordenadas de Posição do Robô (X, Y, Theta) // Varáveis de Controle de Velocidade var VelGlobal: TState2D; //Velocidade no Espaço (Vx, Vy, W) var VRef: TState2D; //Velocidade no Robô (V, Vn, W) var VrRef: TState2D; //Velocidade de Referência das Rodas (V0, V1, V2) var Vr: TState2D; //Velocidade Atual das Rodas (V0, V1, V2) var DTheta: TState2D; //Deslocamento Angular das Rodas (DTheta0, DTheta1, DTheta2) var Erro: TState2D; //Erro de Velocidade das Rodas (ErroV0, ErroV1, ErroV2) var ErroA: TState2D; //Erro de Velocidade das Rodas (ErroAnteriorV0, ErroAnteriorV1, ErroAnteriorV2) var Derivada: TState2D; //Derivada do Erro de Velocidade das Rodas (DerivadaV0, DerivadaV1, DerivadaV2) var Integral: TState2D; //Integral do Erro de Velocidade das Rodas (IntegralV0, IntegralV1, IntegralV2) var U: TState2D; //Tensão nas Rodas (U0, U1, U2) // Função de Controle da Velocidade do Robô procedure VelControl(Robot: LongInt; VGlobalRef: TState2D); begin // Matriz de Rotação da Velocidade Global para a Velocidade Local VRef.x:=Cos(RobotPos.angle)*VGlobalRef.x+Sin(RobotPos.angle)*VGlobalRef.y; VRef.y:=Sin(RobotPos.angle)*VGlobalRef.x+Cos(RobotPos.angle)*VGlobalRef.y; VRef.angle:=VGlobalRef.angle; // Matriz Cinemática da Velocidade Local para a Velocidade Linear das Rodas VrRef.x:=Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.angle; VrRef.y:=(0)*VRef.x-(1)*VRef.y+DIST_AO_CENTRO*VRef.angle; VrRef.angle:=Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.angle; // Cálculo do Erro de Velocidade das Rodas Erro.x := VrRef.x -Vr.x; Erro.y := VrRef.y -Vr.y; Erro.angle := VrRef.angle-Vr.angle; // Cálculo da Derivada do Erro de Velocidade das Rodas Derivada.x := (Erro.x -ErroA.x) /TEMPO_DE_AMOSTRA; Derivada.y := (Erro.y -ErroA.y) /TEMPO_DE_AMOSTRA; Derivada.angle := (Erro.angle-ErroA.angle)/TEMPO_DE_AMOSTRA; // Cálculo da Integral do Erro de Velocidade das Rodas Integral.x := Integral.x +TEMPO_DE_AMOSTRA*Erro.x; Integral.y := Integral.y +TEMPO_DE_AMOSTRA*Erro.y; Integral.angle := Integral.angle+TEMPO_DE_AMOSTRA*Erro.angle; // Cálculo do Erro Anterior de Velocidade das Rodas ErroA.x := Erro.x; ErroA.y := Erro.y; ErroA.angle := Erro.angle; // PIDs de Tensão das Rodas U.x := KP*(Erro.x +(1/TI)*Integral.x +TD*Derivada.x); U.y := KP*(Erro.y +(1/TI)*Integral.y +TD*Derivada.y); U.angle := KP*(Erro.angle+(1/TI)*Integral.angle+TD*Derivada.angle); // Controle de Saturação dos PIDs if(U.x>TENSAO_MAX)then //Saturação no Controlador da Roda 0 begin

75

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

76

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

77

APÊNDICE D – CÓDIGO COMPLETO DA SIMULAÇÃO

// Constantes do Robô const TENSAO_MAX = 12; const DIST_AO_CENTRO = 0.1; const TEMPO_DE_AMOSTRA = 0.04; const RAIO = 0.04; const ENCODER_PPR = 1000; const ID_ROBOT_1 = 0; const ID_RODA_0 = 0; const ID_RODA_1 = 1; const ID_RODA_2 = 2; // Constantes de Controle de Velocidade const KP = 4.5; const TI = 0.25; const TD = 0.006; // Constantes de Controle de Posição const KP_TARGET = 1; const TI_TARGET = 2; // Constantes de Controle de Linha const KP_LINE = 6; const TI_LINE = 2; const TD_LINE = 0.05; // Constantes de Controle de Círculo const KP_CIRCLE = 10; const TI_CIRCLE = 10000; const TD_CIRCLE = 0.0001; // Variáveis Globais do Robô var RobotPos: TState2D; //Coordenadas de Posição do Robô (X, Y, Theta) var Deslocamento: TState2D; //Deslocamento de Posição do Robô (DX, DY, DTheta) // Varáveis de Controle de Velocidade var VelGlobal: TState2D; //Velocidade no Espaço (Vx, Vy, W) var VRef: TState2D; //Velocidade no Robô (V, Vn, W) var VrRef: TState2D; //Velocidade de Referência das Rodas (V0, V1, V2) var Vr: TState2D; //Velocidade Atual das Rodas (V0, V1, V2) var DTheta: TState2D; //Deslocamento Angular das Rodas (DTheta0, DTheta1, DTheta2) var Erro: TState2D; //Erro de Velocidade das Rodas (ErroV0, ErroV1, ErroV2) var ErroA: TState2D; //Erro de Velocidade das Rodas (ErroAnteriorV0, ErroAnteriorV1, ErroAnteriorV2) var Derivada: TState2D; //Derivada do Erro de Velocidade das Rodas (DerivadaV0, DerivadaV1, DerivadaV2) var Integral: TState2D; //Integral do Erro de Velocidade das Rodas (IntegralV0, IntegralV1, IntegralV2) var U: TState2D; //Tensão nas Rodas (U0, U1, U2) // var iáveis de Controle de Posição var VelTarget: TState2D; //Velocidade no Espaço para Controle de Posição (Vx, Vy, W) var ErroTarget: TState2D; //Erro ao Ponto-Alvo (ErroX, ErroY, ErroTheta) var IntegralTarget: TState2D; //Integral do Erro de Posição (IntegralX, IntegralY, IntegralTheta) var Alvo: TState2D; //Coordenada Absoluta do Pont-Alvo (X, Y, Theta) // Variáveis de Controle de Linha var VelLine: TState2D; //Velocidade Resultante no Espaço para Controle de Linha (Vx, Vy, W) var VelPIDLine: Double; //Velocidade no Espaço para Controle de Linha (Vx, Vy, W) var ErroThetaLine: Double; //Erro de Angulação do Robô var ErroLine: Double; //Erro de Distância à Linha var ErroALine: Double; //Erro de Distância à Linha var DerivadaLine: Double; //Derivada do Erro de Distância à Linha var IntegralLine: Double; //Integral do Erro de Distância à Linha var IntegralThetaLine: Double; //Integral do Erro de Angulação do Robô var Ponto1: TState2D; //Coordenada Absoluta de um Ponto Qualquer (X, Y, Theta) var Ponto2: TState2D; //Coordenada Absoluta de um Ponto Qualquer (X, Y, Theta)

78

var a, b, c, ThetaLinha, ThetaLinhaNorm: Double; //Variáveis para Geração da Reta var SinalX, SinalY, SinalXPID, SinalYPID: Double; //Sinais de Controle das Componentes de Velocidade // Variáveis de Controle de Círculo var VelCircle: TState2D;//Velocidade Resultante no Espaço para Controle de Círculo (Vx, Vy, W) var VelPIDCircle: Double; //Velocidade no Espaço para Controle de Círculo (Vx, Vy, W) var ErroThetaCircle: Double; //Erro de Angulação do Robô var ErroCircle: Double; //Erro de Distância ao Círculo var ErroACircle: Double; //Erro de Distância ao Círculo var DerivadaCircle: Double; //Derivada do Erro de Distância ao Círculo var IntegralCircle: Double; //Integral do Erro de Distância ao Círculo var IntegralThetaCircle: Double; //Integral do Erro de Angulação do Robô // Função de Controle da Velocidade do Robô procedure VelControl(Robot: LongInt; VGlobalRef: TState2D); begin // Matriz de Rotação da Velocidade Global para a Velocidade Local VRef.x:=Cos(RobotPos.angle)*VGlobalRef.x+Sin(RobotPos.angle)*VGlobalRef.y; VRef.y:=-Sin(RobotPos.angle)*VGlobalRef.x+Cos(RobotPos.angle)*VGlobalRef.y; VRef.angle := VGlobalRef.angle; // Matriz Cinemática da Velocidade Local para a Velocidade Linear das Rodas VrRef.x:=-Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.angle; VrRef.y:=(0)*VRef.x-(1)*VRef.y+DIST_AO_CENTRO*VRef.angle; VrRef.angle:=Sin(Rad(60))*VRef.x+Cos(Rad(60))*VRef.y+DIST_AO_CENTRO*VRef.angle; // Cálculo do Erro de Velocidade das Rodas Erro.x := VrRef.x -Vr.x; Erro.y := VrRef.y -Vr.y; Erro.angle := VrRef.angle-Vr.angle; // Cálculo da Derivada do Erro de Velocidade das Rodas Derivada.x := (Erro.x -ErroA.x) /TEMPO_DE_AMOSTRA; Derivada.y := (Erro.y -ErroA.y) /TEMPO_DE_AMOSTRA; Derivada.angle := (Erro.angle-ErroA.angle)/TEMPO_DE_AMOSTRA; // Cálculo da Integral do Erro de Velocidade das Rodas Integral.x := Integral.x +TEMPO_DE_AMOSTRA*Erro.x; Integral.y := Integral.y +TEMPO_DE_AMOSTRA*Erro.y; Integral.angle := Integral.angle+TEMPO_DE_AMOSTRA*Erro.angle; // Cálculo do Erro Anterior de Velocidade das Rodas ErroA.x := Erro.x; ErroA.y := Erro.y; ErroA.angle := Erro.angle; // PIDs de Tensão das Rodas U.x := KP*(Erro.x +(1/TI)*Integral.x +TD*Derivada.x); U.y := KP*(Erro.y +(1/TI)*Integral.y +TD*Derivada.y); U.angle := KP*(Erro.angle+(1/TI)*Integral.angle+TD*Derivada.angle); // Controle de Saturação dos PIDs if(U.x>TENSAO_MAX)then //Saturação no Controlador da Roda 0 begin U.x := TENSAO_MAX; Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x; end; if(U.x<-TENSAO_MAX)then begin U.x := -TENSAO_MAX; Integral.x:=Integral.x-TEMPO_DE_AMOSTRA*Erro.x; end; if(U.y>TENSAO_MAX)then //Saturação no Controlador da Roda 1 begin U.y := TENSAO_MAX; Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y; end; if(U.y<-TENSAO_MAX)then begin U.y := -TENSAO_MAX; Integral.y:=Integral.y-TEMPO_DE_AMOSTRA*Erro.y; end; if(U.angle>TENSAO_MAX)then //Saturação no Controlador da Roda 2

79

begin U.angle := TENSAO_MAX; Integral.angle := Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle; end; if(U.angle<-TENSAO_MAX)then begin U.angle := -TENSAO_MAX; Integral.angle := Integral.angle-TEMPO_DE_AMOSTRA*Erro.angle; end; // Insere o Valor Controlado de Tensão nas Rodas SetAxisVoltageRef(Robot, ID_RODA_0, U.x); SetAxisVoltageRef(Robot, ID_RODA_1, U.y); SetAxisVoltageRef(Robot, ID_RODA_2, U.angle); end; // Função de Controle de Posição do Robô procedure GotoTarget(Robot: LongInt; VNom: Double; Target: TState2D); begin VNom := Abs(VNom); // Cálculo do Erro de Posição do Robô ErroTarget.x := Target.x-RobotPos.x; ErroTarget.y := Target.y-RobotPos.y; ErroTarget.angle := Target.angle-RobotPos.angle; // Cálculo da Integral do Erro de Posição do Robô IntegralTarget.x:=IntegralTarget.x+TEMPO_DE_AMOSTRA*ErroTarget.x; IntegralTarget.y:=IntegralTarget.y+TEMPO_DE_AMOSTRA*ErroTarget.y; IntegralTarget.angle:=IntegralTarget.angle+TEMPO_DE_AMOSTRA*ErroTarget.angle; // PIs de Velocidade Global VelTarget.x:=KP_TARGET*(ErroTarget.x+(1/TI_TARGET)*IntegralTarget.x); VelTarget.y:=KP_TARGET*(ErroTarget.y+(1/TI_TARGET)*IntegralTarget.y); VelTarget.angle:=KP_TARGET*(ErroTarget.angle+(1/TI_TARGET)*IntegralTarget.angle); // Controle de Saturação dos PIs if(VelTarget.x > VNom)then //Saturação no Controlador de Vx begin VelTarget.x:=VNom; IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x; end; if(VelTarget.x < -VNom)then begin VelTarget.x:=-VNom; IntegralTarget.x:=IntegralTarget.x-TEMPO_DE_AMOSTRA*ErroTarget.x; end; if(VelTarget.y > VNom)then //Saturação no Controlador de Vy begin VelTarget.y:=VNom; IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y; end; if(VelTarget.y < -VNom)then begin VelTarget.y:=-VNom; IntegralTarget.y:=IntegralTarget.y-TEMPO_DE_AMOSTRA*ErroTarget.y; end; if(VelTarget.angle > VNom)then //Saturação no Controlador de W begin VelTarget.angle:=VNom; IntegralTarget.angle:=IntegralTarget.angle-TEMPO_DE_AMOSTRA*ErroTarget.angle; end; if(VelTarget.angle<-VNom)then begin VelTarget.angle:=-VNom; IntegralTarget.angle:=IntegralTarget.angle-TEMPO_DE_AMOSTRA*ErroTarget.angle; end; // Insere o Valor Controlado de Velocidade Global VelControl(Robot, VelTarget); end; // Função de Seguir uma Linha

80

procedure FollowLine(Robot: LongInt; VNom: Double; FlagSentido: Integer; PontoA, PontoB: TState2D; Angulo: Double); begin VNom := Abs(VNom); // Cálculo do ângulo que a Reta faz com a Horizontal ThetaLinha := ATan2((PontoB.y-PontoA.y),(PontoB.x-PontoA.x)); // Normatização do ângulo que a Reta faz com a Horizontal if(ThetaLinha > Rad(90))then begin ThetaLinhaNorm := Rad(180) - ThetaLinha; end else if(ThetaLinha < Rad(-90))then begin ThetaLinhaNorm := -Rad(180) - ThetaLinha; end else begin ThetaLinhaNorm := ThetaLinha; end; // Verifica a Flag de Sentido if((FlagSentido <> 1) and (FlagSentido <> -1))then begin FlagSentido := 1; end; // Geração da Reta e dos Sinais de Controle if((PontoB.y-PontoA.y) = 0)then // Linha Horizontal begin a := 0; b := -1; c := PontoA.y; if(c <> 0)then begin SinalX := FlagSentido*(PontoA.y)/Abs(PontoA.y); SinalY := (PontoA.y)/Abs(PontoA.y); end else begin SinalX := FlagSentido; SinalY := 0; end; if(ErroLine >= 0)then begin SinalXPID := 1; SinalYPID := 1; end else begin SinalXPID := -1; SinalyPID := -1; end; end else if((PontoB.x-PontoA.x) = 0)then // Linha Vertical begin a := -1; b := 0; c := PontoA.x; if(c <> 0)then begin SinalX := (PontoA.x)/Abs(PontoA.x); SinalY := FlagSentido*(PontoA.x)/Abs(PontoA.x); end else begin SinalX := 0; SinalY := FlagSentido; end; if(ErroLine >= 0)then begin SinalXPID := 1; SinalYPID := 1;

81

end else begin SinalXPID := -1; SinalyPID := -1; end; end else // Linha Inclinada begin a := Tan(ThetaLinha); b := -1; c := -a*PontoA.x-b*PontoA.y; if((a >= 0)and(ErroLine >= 0))then begin SinalX := FlagSentido; SinalY := FlagSentido; SinalXPID := -1; SinalyPID := 1; end else if((a >= 0)and(ErroLine < 0))then begin SinalX := FlagSentido; SinalY := FlagSentido; SinalXPID := 1; SinalyPID := 0; end else if((a < 0)and(ErroLine < 0))then begin SinalX := FlagSentido; SinalY := -FlagSentido; SinalXPID := -1; SinalyPID := -1; end else if((a < 0)and(ErroLine >= 0))then begin SinalX := FlagSentido; SinalY := -FlagSentido; SinalXPID := 1; SinalyPID := 0; end; end; // PID de Controle de Linha ErroLine := (a*RobotPos.x+b*RobotPos.y+c)/Sqrt(a*a+b*b); ErroThetaLine := Angulo-RobotPos.angle; DerivadaLine := (ErroLine-ErroALine)/TEMPO_DE_AMOSTRA; IntegralLine := IntegralLine+TEMPO_DE_AMOSTRA*ErroLine; IntegralThetaLine := IntegralThetaLine+TEMPO_DE_AMOSTRA*ErroThetaLine; ErroALine := ErroLine; VelPIDLine := KP_LINE*(ErroLine+(1/TI_LINE)*IntegralLine+TD_LINE*DerivadaLine); // Controle de Saturação do PID if(VelPIDLine>VNom)then begin VelPIDLine := VNom; IntegralLine := IntegralLine-TEMPO_DE_AMOSTRA*ErroLine; end; if(VelPIDLine<-VNom)then begin VelPIDLine := -VNom; IntegralLine := IntegralLine-TEMPO_DE_AMOSTRA*ErroLine; if(VelLine.angle > VNom)then //Saturação no Controlador de W begin VelLine.angle := VNom; IntegralThetaLine := IntegralThetaLine-TEMPO_DE_AMOSTRA*ErroThetaLine; end; if(VelLine.angle<-VNom)then begin VelLine.angle := -VNom; IntegralThetaLine := IntegralThetaLine-TEMPO_DE_AMOSTRA*ErroThetaLine; end;

82

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

83

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

84

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

85

Deslocamento.angle := 0; U.x := 0; U.y := 0; U.angle := 0; VRef.x := 0; VRef.y := 0; VRef.angle := 0; VelGlobal.x := 1; VelGlobal.y := 0; VelGlobal.angle := 0; Alvo.x := 0; Alvo.y := 0; Alvo.angle := 1; ErroTarget.x := 0; ErroTarget.y := 0; ErroTarget.angle := 0; IntegralTarget.x := 0; IntegralTarget.y := 0; IntegralTarget.angle := 0; Ponto1.x := 0; Ponto1.y := 0; Ponto1.angle := 0; Ponto2.x := 2; Ponto2.y := 1; Ponto2.angle := 0; VelLine.x := 0; VelLine.y := 0; VelLine.angle := 0; ErroLine := 0; ErroALine := 0; DerivadaLine := 0; IntegralLine := 0; VelPIDLine := 0; a := 0; b := 0; c := 0; ThetaLinha := 0; ThetaLinhaNorm := 0; SinalX := 0; SinalY := 0; SinalXPID := 0; SinalYPID := 0; VelCircle.x := 0; VelCircle.y := 0; VelCircle.angle := 0; ErroThetaCircle := 0; ErroCircle := 0; ErroACircle := 0; DerivadaCircle := 0; IntegralCircle := 0; IntegralThetaCircle := 0; VelPIDCircle := 0; end;

Alexandre Costa Mota e-mail: alexandremota.eng@gmail.com