Daniel Alves Barbosa de Oliveira Vaz

Planejamento de movimento cinemtico-dinmico

para robs mveis com rodas deslizantes
Dissertao apresentada Escola de Enge-
nharia de So Carlos da Universidade de So
Paulo para obteno do Ttulo de Mestre em
Cincias pelo Programa de Ps-Graduao de
Engenharia Eltrica.
rea de Concentrao:
Sistemas Dinmicos
Orientador:
Prof. Dr. Valdir Grassi Junior
So Carlos
2011
Trata-se da verso corrigida da dissertao. A verso original se encontra disponvel na EESC/USP que aloja
o Programa de Ps-Graduao de Engenharia Eltrica.
AUTORIZO A REPRODUO E DIVULGAO TOTAL OU PARCIAL DESTE
TRABALHO, POR QUALQUER MEIO CONVENCIONAL OU ELETRNICO, PARA
FINS DE ESTUDO E PESQUISA, DESDE QUE CITADA A FONTE.
Ficha catalogrca preparada pela Seo de
Tratamento
da Informao do Servio de Biblioteca EESC/USP
Vaz, Daniel Alves Barbosa de Oliveira
Planejamento de movimento cinemtico-dinmico para robs
mveis com rodas deslizantes/ Daniel Alves Barbosa de Oliveira
Vaz ; orientador Valdir Grassi Junior. So Carlos, 2011.
66 p.
Dissertao (Mestrado - Programa de Ps-Graduao em En-
genharia Eltrica e rea de Concentrao em Sistemas Dinmi-
cos) Escola de Engenharia de So Carlos Universidade de So
Paulo, 2011.
1.Robs mveis. 2. Planejamento de trajetria. 3. Acom-
panhamento de trajetria. I. Planejamento de movimento
cinemtico-dinmico para robs mveis com rodas deslizantes.

Aos meus pais Dlcio Vaz de Oliveira Machado e Zlia Barbosa de Oliveira com amor e
gratido.
Agradecimentos
A Deus por ter me dado fora, pacincia e inteligncia, virtudes indispensveis para a con-
cluso deste trabalho.
A todos meus familiares que sempre me apoiaram ao longo da realizao deste trabalho.
Ao Prof Dr. Valdir Grassi Junior pela oportunidade, orientao, pacincia, sabedoria e pelo
tempo dedicado a este trabalho.
Aos meus amigos da Igreja Presbiteriana de So Carlos, obrigado pelo carinho e hospitali-
dade.
Aos meus amigos de repblica, Igor e Srgio, pela amizade e o convvio ao longo destes
anos.
Aos meus amigos que me acolheram em sua residncia: Miguel e Giovana.
Aos meus amigos da ps-graduao: Tatiana, Amanda, Aline, Moussa, Roberto, Raphael,
Fabolo, Joo, Samuel, Andr e Gildson pela amizade, companheirismo e colaboraes durante
a realizao das disciplinas e deste trabalho.
Ao Prof Dr. Lus Fernando Costa Alberto pela dedicao, carisma, didtica e sabedoria;
virtudes transmitidas ao longo do curso de Sistemas No-Lineares.
Aos demais professores por compartilharem a sabedoria.
Ao corpo de funcionrios do Departamento de Engenharia Eltrica da Escola de Engenharia
de So Carlos.
A Coordenao de Aperfeioamento de Pessoal de Nvel Superior (CAPES) pela concesso
da bolsa de mestrado.
Algumas pessoas acham que foco signica dizer sim para a coisa em que voc ir se focar.
Mas no nada disso. Signica dizer no s centenas de outras boas idias que existem.
(Steve Jobs)
Resumo
O planejamento de movimento um dos problemas fundamentais em navegao autnoma
para robs mveis. Uma vez planejado o caminho, o rob executa o acompanhamento da tra-
jetria, frequentemente, com o auxlio de um controlador em malha fechada. Este controlador
tem o objetivo de minimizar os erros de acompanhamento, a m de que a trajetria executada se
aproxime da trajetria planejada. Entretanto a maioria dos planejadores de movimento no le-
vam em considerao o modelo dinmico do rob, dicultando assim o trabalho do controlador
que executa o acompanhamento da trajetria. Incluindo as restries cinemticas e dinmicas
do modelo do rob, o custo computacional durante a fase de planejamento de trajetria ser
mais alto. Isto ocorre pois so necessrias mais variveis para representar o espao de estados
do rob. No entanto ao levar em considerao tais restries durante a fase de planejamento, as
trajetrias geradas sero factveis de serem acompanhadas. Os planejadores probabilsticos de
movimento podem ser usados para minimizar o impacto do alto custo computacional, devido
ao aumento de variveis que representam o espao de estados. Tais planejadores tambm so
chamados de planejadores de movimento baseados em amostragem. A busca por um caminho
livre de colises entre dois estados feito de maneira aleatria. Caso exista uma soluo, a
probabilidade do algoritmo encontr-la tende para 1 quanto do tempo de busca tende a innito,
isto , quanto mais tempo o algoritmo possui para realizar a busca ser mais provvel que ele
encontre a soluo. Neste trabalho proposto um planejador de movimentos baseado em amos-
tragem que leva em considerao os aspectos cinemticos e dinmicos do rob. Alm disto esta
abordagem de planejamento desenvolvida permite conhecer e levar em considerao os efei-
tos do controlador que faz o acompanhamento da trajetria, ainda na fase de planejamento de
movimento. As trajetrias planejadas foram executadas no rob Pioneer 3AT. Foram levanta-
dos os dados relacionados ao desempenho do algoritmo em termos de custo computacional. E
na sequncia so apresentados os resultados experimentais tanto na parte de planejamento de
trajetrias quanto na fase de acompanhamento.
Abstract
Motion planning is one of the fundamental problems in autonomous navigation for mobile
robots. Once the path is planned, the robot performs the trajectory tracking, often with the
aid of a closed loop controller. This controller is designed to minimize tracking errors, in
order that tracked trajectory get closer to planned path. However, the most motion planners
do not take into account the dynamic model of the robot, thus hindering the work of closed
loop controller. When including the kinematic constraints and dynamic model of the robot, the
computational cost during the planning phase trajectory will be increased. This is because more
variables are needed to represent the state space of the robot. But when taking into account these
constraints during the planning phase, the trajectories generated are feasible to be followed. The
probabilistic motion planners can be used to minimize the impact of high computational cost
due to the increase of variables that represent the state space. These planners are also called
sampling based motion planners. The search for a collision-free path between two states is
done randomly. If a solution exists, the probability of the algorithm to nd it tends to one
while the search time tends to innity, that is, the longer time the algorithm has to perform the
search will be more likely to nd the solution. This paper proposes a sampling based motion
planner that takes into account the kinematic and dynamic aspects of the robot. Furthermore this
approach allows one to know and take into account the effects of the controller that perform the
trajectory tracking, still in the motion planning phase. The planned trajectories were performed
on the robot Pioneer 3AT. Data related to the computational cost of the algorithm were analyzed.
Following the experimental results are presented both in the planning of trajectories and in the
tracking phase.
Lista de Figuras
1.1 Rob enviado a marte e braos robticos industriais . . . . . . . . . . . . . . . 17
1.2 Rob Pioneer 3AT P3-AT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.1 (a) Espao de trabalho, (b) espao de conguraes, (c) rob representado como
um ponto. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2 Expanso de uma RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.1 Geometria do rob mvel. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.2 Foras de trao e resistivas do rob mvel. . . . . . . . . . . . . . . . . . . . 34
4.1 Abordagem de Planejamento Cinemtico-Dinmico de Movimento baseado em
Acompanhamento Exemplo de expanso. . . . . . . . . . . . . . . . . . . . 41
4.2 Diagrama de blocos da fase de expanso . . . . . . . . . . . . . . . . . . . . . 41
4.3 Diagrama de blocos da fase de simulao de acompanhamento . . . . . . . . . 41
5.1 Diagrama simplicado de classes Software rrt-motion-planner . . . . . . . . 45
5.2 Diagrama da classe RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
5.3 Diagrama de classes RobotGeometryModel, EnvModel e World . . . . . . . 49
5.4 Diagrama de classes RobotModel . . . . . . . . . . . . . . . . . . . . . . . 50
5.5 Diagrama de classes - Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.6 Diagrama de classes - ProxyRobot e ProxyPosition . . . . . . . . . . . . . . . 54
6.1 Passagem estreita - Dinmico . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6.2 Passagem estreita - Baseado em acompanhamento . . . . . . . . . . . . . . . . 58
6.3 Estacionamento paralelo - Dinmico . . . . . . . . . . . . . . . . . . . . . . . 59
6.4 Estacionamento paralelo - Baseado em acompanhamento . . . . . . . . . . . . 59
6.5 Passagem estreita - Resultado experimental . . . . . . . . . . . . . . . . . . . 60
6.6 Estacionamento paralelo - Resultado experimental . . . . . . . . . . . . . . . . 60
6.7 Passagem estreita - Resultado experimental . . . . . . . . . . . . . . . . . . . 61
6.8 Passagem estreita - Resultado experimental . . . . . . . . . . . . . . . . . . . 61
Lista de Tabelas
6.1 Tabela de desempenho - Passagem estreita . . . . . . . . . . . . . . . . . . . . 56
6.2 Tabela de desemepenho - Estacionamento paralelo . . . . . . . . . . . . . . . 58
6.3 Parmetros do Pioneer 3-AT. . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
Sumrio
1 Introduo 17
2 Planejamento de movimento 23
2.1 Espao de conguraes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.2 Planejadores de movimento baseado em amostragem . . . . . . . . . . . . . . 25
2.3 Planejamento Cinemtico-Dinmico . . . . . . . . . . . . . . . . . . . . . . . 26
2.4 Rapidly-exploring Random Tree . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.4.1 Restries diferenciais no espao de estados . . . . . . . . . . . . . . . 26
2.4.2 Algoritmo RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.4.3 Planejadores baseados em RRT . . . . . . . . . . . . . . . . . . . . . 28
3 Modelagem do rob mvel 31
3.1 Modelo cinemtico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2 Modelo dinmico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.3 Seguimento de trajetria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4 Planejamento de movimento cinemtico-dinmico baseado em acompanhamento 39
4.1 Metodologia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5 Implementao 43
5.1 Ferramentas e bibliotecas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.1.1 Player/Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.1.2 PQP Proximity Query Package . . . . . . . . . . . . . . . . . . . . . 44
5.1.3 LEMON . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.2 Arquitetura do software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.2.1 Classe RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.2.2 Classe StateSampler . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5.2.3 Classes DistanceMeter e BiasedMeterP3AT . . . . . . . . . . . . . . . 47
5.2.4 Classes TrackingControl, ProportionalControl e Pioneer3ATState . . . 48
5.2.5 Classes RobotGeometryModel, EnvModel e World . . . . . . . . . . . 48
5.2.6 Classe RobotModel . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.3 Gerao de torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5.4 Acompanhamento de trajetria . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.4.1 Implementao do acompanhamento de trajetria para o Player/Stage . 54
6 Resultados 55
6.1 Planejamento de movimento . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
6.2 Acompanhamento das trajetrias planejadas . . . . . . . . . . . . . . . . . . . 59
6.2.1 Planejador de movimento cinemtico-dinmico . . . . . . . . . . . . . 59
6.2.2 Planejador de movimento cinemtico-dinmico baseado em acompa-
nhamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
7 Concluso 63
Referncias Bibliogrcas 65
17
1 Introduo
Os robs ou sistemas robticos foram concebidos com o propsito de auxiliar e at mesmo
substituir a ao humana em diversos tipos de tarefas. habitual observar a atuao de robs em
tarefas repetitivas, ou mesmo perigosas que trazem riscos a vida. Como exemplo de tarefas que
hoje so realizadas por robs pode-se citar: linha de montageme pintura de veculos, explorao
submarina, explorao espacial, explorao e manuteno em ambientes inspitos, combate a
incndios, desarmamento de bombas, dentre outras.
Figura 1.1: Rob enviado a marte e braos robticos industriais
O planejamento de movimento faz-se necessrio para a realizao de tarefas de locomoo
do rob e tem como objetivo determinar quais aes o rob deve executar de forma que alcance
seu objetivo nal, evitando colises ao longo da trajetria. No processo de planejamento de
movimento utiliza-se informaes tais como o mapa do ambiente e o modelo do rob. O modo
como o rob se locomove e suas restries de movimento, so fatores importantes na etapa de
planejamento de movimento.
Quanto a forma de locomoo, os robs podem ser classicados em omnidirecionais sendo
capazes de se locomover em qualquer direo, e no holonmicos que possuem determinadas
restries ao se movimentar. Os veculos de rodas esterantes, incapazes de se movimentar
lateralmente, so um exemplo de modelo no holonmico. Devido a essa restrio so neces-
18
srias algumas manobras at que se atinja um determinado estado nal, como por exemplo na
manobra de estacionamento paralelo ou baliza.
importante ressaltar que o planejamento de movimento no se restringe apenas a rea de
robtica. Os mtodos de planejamento de movimento podem ser utilizados na rea de animao
de personagens digitais, planejamento cirrgico, desenho assistido por computador CAD,
desenvolvimento de novos medicamentos, etc.
A maioria dos planejadores bsicos de movimento ignoram completamente a dinmica do
modelo do rob, levando em considerao somente a cinemtica. Isto pode resultar em ca-
minhos incapazes de serem executados pelo rob, devido a limitao das foras e torques que
acionam o rob real. A soluo clssica em pesquisas robticas tem sido desacoplar o problema
geral em duas partes: resolver o planejamento bsico de movimento na busca de uma trajetria
e logo em seguida projetar um controlador que satisfaa a dinmica do modelo e siga o caminho
planejado (LAVALLE; KUFFNER, 2001). Donald et al. (1993) segue outra vertente e prope o
termo planejamento cinemtico-dinmico de movimento, cujo objetivo incluir na fase de pla-
nejamento de movimento, as restries impostas pelo modelo dinmico do rob, possibilitando
a gerao de caminhos factveis e facilitando o seguimento da trajetria planejada.
O Rapidly-exploring Random Tree, RRT, um mtodo de planejamento de movimento
baseado em amostragem e foi proposto com a inteno de resolver problemas de planejamento
cinemtico-dinmico (LAVALLE; KUFFNER, 2001). Quando leva-se em conta a dinmica
do rob o nmero de graus de liberdade aumenta, isto , o nmero mmino de parmetros
que representam completamente o estado do rob cresce. Os mtodos de planejamento de
movimento baseados em amostragem so apropriados quando o nmero de graus de liberdade
grande, uma vez que estes mtodos amostram estados aleatrios e aplicam as aes de controle
no modelo do sistema robtico para propagar o movimento do rob realizando testes de coliso
ao longo desta propagao. Este o funcionamento bsico do RRT que realiza duas tarefas
simultaneamente: expande uma rvore aleatria que possui tima capacidade de explorao do
ambiente e guia o crescimento dessa rvore em direo ao estado nal desejado.
H vrias modicaes no mtodo bsico de RRT que podem ser aplicadas para alcanar
um melhor desempenho na busca de solues. Abaixo so destacadas algumas extenses feitas
ao mtodo bsico que permitem ser aplicadas em problemas de planejamento de movimento
do tipo cinemtico-dinmico, tais como: melhorar a qualidade da soluo de forma incremen-
tal, trabalhar com planejamento de movimento em tempo real e trabalhar em ambientes com
passagens estreitas dentre outras.
Embora o algoritmo bsico de RRT mostre-se efetivo na gerao de trajetria factveis,
19
ele no fornece controle sobre a qualidade das solues produzidas. Com o m de atender
os requisitos de tempo real e aumentar a qualidade da trajetrias geradas, uma nova extenso
para o RRT foi introduzido em (FERGUSON; STENTZ, 2006). Esta nova extenso capaz
de produzir rapidamente uma soluo inicial e aprimor-la enquanto houver tempo ocioso de
processamento. O aperfeioamento incremental, isto , a qualidade das solues subsequentes
melhorada com base nas informaes das trajetrias geradas anteriormente.
Em problemas de planejamento de movimento que envolve corredores estreitos, o algo-
ritmo bsico RRT apresenta diculdades em encontrar uma trajetria. A abordagem chamada
Obstacle-based RRT (RODRIGUEZ et al., 2006) usa a informao sobre a forma dos obstcu-
los para guiar o crescimento da rvore. O planejador de movimentos resultante desta mudana
mais rpido e consegue resolver um problema com corredores estreitos em um nmero menor
de iteraes.
No campo experimental h uma grande preocupao com a capacidade dos mtodos de pla-
nejamento de movimento de atender os requisitos de tempo real. O Execution extended RRT
(ERRT) (BRUCE; VELOSO, 2002) intercala planejamento de movimento e execuo, utili-
zando uma fase de simulao e ento aplicando os comandos em um rob real. Este mtodo
introduz duas novas extenses encontradas em trabalhos anteriores sobre RRT que so: way-
point cache e busca penalizada por custo adaptativo. Juntos eles aumentam a qualidade dos
caminhos obtidos e a ecincia no replanejamento de trajetrias. O mtodo ERRT foi aplicado
com sucesso na RoboCup (BRUCE; VELOSO, 2002).
Percebeu-se que poucos trabalhos de planejamento de movimento foram realizados para a
classe de robs mveis com rodas deslizantes. Essa classe de robs utiliza o deslizamento das
rodas, que causa o aparecimento de uma velocidade lateral, para mudar a orientao do rob.
Alm disso grande parte dos trabalhos de planejamento cinemtico-dinmico de movimento
focam em resultados de simulao, no apresentando resultados experimentais de seguimento
das trajetrias planejadas.
Duas abordagens so apresentadas neste trabalho. Uma delas o algoritmo bsico de RRT
para planejamento de movimento cinemtico-dinmico de um rob mvel de rodas deslizantes
utilizando um modelo que possui como entradas de controle os torques aplicados nos motores
de roda do rob mvel (VAZ; INOUE; GRASSI, 2010). A outra uma nova abordagem de
planejamento cinemtico-dinmico, proposta neste trabalho, que aqui denominamos de Plane-
jamento cinemtico-dinmico de movimento baseado em acompanhamento. Esta proposta pode
ser considerada uma extenso de planejadores de movimentos baseados em RRT. O diferencial
desta extenso aplicar o mesmo controlador usado na execuo da trajetria ainda na fase de
20
planejamento de movimento.
Portanto, as principais contribuies deste trabalho foram: aplicar o planejamento cinemtico-
dinmico baseado em RRT para a classe de robs mveis com rodas deslizantes; e propor uma
nova extenso para o mtodo bsico de planejamento cinematico-dinmico baseado em RRT,
aqui denomidada de planejamento de movimento basedo em acompanhamento.
Neste trabalho as trajetrias obtidas pelo planejador de movimentos so simuladas utilizando-
se os softwares Player/Stage (GERKEY; VAUGHAN; HOWARD, 2003) e ento so feitos
experimentos reais com o rob Pioneer-3AT visto na Figura 1.2, que um rob mvel no
holonmico com rodas deslizantes. Os resultados deste trabalho concentram-se em comparar
os resultados de um planejador de movimentos cinemtico-dinmico bsico (VAZ; INOUE;
GRASSI, 2010), com um planejador de movimentos cinemtico-dinmico baseado em acom-
panhamento apresentado no Captulo 4. O modelo cinemtico-dinmico utilizado apresentado
por Caracciolo, Luca e Iannitti (1999), que mostra o seguimento de trajetria para um rob com
sistema de direo semelhante.
Figura 1.2: Rob Pioneer 3AT P3-AT
O texto divido da seguinte forma: o Captulo 2 apresenta conceitos fundamentais sobre
planejamento de movimento, discute as abordagens de planejamento baseado em amostragem
e planejamento cinemtico-dinmico, alm de apresentar de maneira detalhada o mtodo de
planejamento de movimento RRT. Os modelos cinemtico e dinmico do rob Pioneer 3-AT,
utilizados pelo planejador so apresentados no Captulo 3, que tambm prope um controlador
proporcional usado no planejamento de movimento e no acompanhamento das trajetrias plane-
jadas. A abordagem proposta nessa dissertao que leva em considerao o mesmo controlador
21
em fases distintas, planejamento e acompanhamento, apresentada no Captulo 4. O Captulo 5
mostra a implementao do planejamento baseado em acompanhemento usando como base o
algoritmo RRT. Os resultados experimentais so mostrados no Captulo 6, as concluses so
apresentadas no Captulo 7.
22
23
2 Planejamento de movimento
O objetivo central do planejamento de movimento levar o rob de um estado atual a
um estado nal desejado, evitando colises com os obstculos presentes no ambiente durante
o percurso. Esta uma tarefa desaadora e considerada um dos problemas fundamentais da
robtica autnoma (CHOSET et al., 2005).
Esse captulo apresenta a denio dos conceitos bsicos utilizados no planejamento de
movimento, em seguida apresentado a abordagem de planejamento baseado em amostra-
gem, planejamento cinemtico-dinmico, e discutido com maior profundidade o algoritmo
do Rapidly-exploring Random Tree, planejador de movimentos utilizado ao longo desse traba-
lho.
2.1 Espao de conguraes
O espao de conguraes um dos conceitos mais importantes no planejamento de mo-
vimento. Antes de denir o conceito de congurao e espao de conguraes, interessante
comentar sobre o espao de trabalho do rob. O espao de trabalho, W, o espao fsico por
onde o rob se desloca. No caso de robs mveis que se locomovem no plano, o espao de
trabalho o R
2
.
Seja F
W
o sistema de coordenadas inercial e F
R
o sistema de coordenadas xo no corpo
do rob. A congurao, q, de um rob a representao completa da posio de todos os
pontos do rob, relativa ao sistema de coordenadas xo do ambiente, F
W
. O nmero mnimo
de parmetros que especicam completamente a congurao de um rob denido como
sendo o nmero de graus de liberdade do rob e tambm representa a dimenso do espao de
conguraes (CHOSET et al., 2005).
O espao de conguraes C o conjunto de todas as possveis conguraes que podem
ser assumidas pelo rob. Dentro do contexto de espao de conguraes, pode-se denir dois
conjuntos que auxiliam na formulao de problemas de planejamento de movimento: o espao
24
de conguraes ocupado por obstculos, C
obs
, e o espao de conguraes livre, C
free
.
A Figura 2.1, apresentada em Choset et al. (2005), mostra a diferena entre espao de
trabalho e espao de conguraes para um dos casos mais simples de se obter C
obs
, caso em
que se considera um rob mvel de geometria circular se movendo no plano. O estado do rob
representado pela posio do centro geomtrico. Deslizando o rob pelo contorno do obstculo,
a posio do centro geomtrico dene uma linha, Figura 2.1 (b), que representa a fronteira de
coliso entre rob e obstculo. Aps essa operao o rob pode ser representado por um ponto.
Realizando essa mesma operao para todos os obstculos do ambiente, tem-se a representao
completa de C
obs
e consequentemente, C
free
.
Figura 2.1: (a) Espao de trabalho, (b) espao de conguraes, (c) rob representado como
um ponto.
O objetivo do planejamento de movimento encontrar um caminho em C
free
que leve o
rob de uma congurao inicial q
init
a uma congurao nal q
goal
. Alguns mtodos de plane-
jamento de movimento so ditos completos. Isto signica que se existe uma soluo, o mtodo
consegue encontr-la em tempo nito ou ento simplesmente retorna uma mensagem relatando
que falhou na busca do caminho (CHOSET et al., 2005). Os mtodos completos fazem a cons-
truo explcita do espao de conguraes. Como dito anteriormente, um dos casos mais
simples de construo explcita do espao de conguraes para um rob de geometria circu-
lar movendo-se no plano. O custo computacional utilizado na construo explcita do espao de
conguraes de robs com geometria retangular e cuja orientao levada em considerao
caro. Para problemas de planejamento de movimento cinemtico-dinmico, isto , quando a
dinmica do rob levada em considerao, alm da postura do rob tambm so levados em
considerao durante a etapa de planejamento as velocidades linear e angular, resultando em
um estado (x, y, , , ) tornando a soluo do problema de planejamento de movimento ainda
mais custosa.
25
2.2 Planejadores de movimento baseado em amostragem
Para problemas de planejamento de movimento cuja dimenso de C grande, a busca por
solues completas se torna um problema computacional custoso de ser tratado. Tendo isso em
mente, os planejadores de movimento baseados em amostragem evitam a construo explcita
de C
obs
e ao invs disso realizam uma busca por caminhos livres no espao de conguraes uti-
lizando tcnicas de amostragem. Essa busca possibilitada pelo uso de detectores de colises
que permitem distinguir se a amostra encontra-se em C
free
ou no. Esta abordagem de planeja-
mento de movimento tem possibilitado a soluo de problemas em que o nmero de graus de
liberdade bastante grande (CHOSET et al., 2005; LAVALLE, 2006).
Contudo por trabalharem com uma amostragem do espao de conguraes, estes mtodos
no podem ser considerados mtodos completos de planejamento. Ao invs disso, utiliza-se
um conceito mais fraco que este. Considera-se que os mtodos de planejamento de movimento
baseados em amostragem so probabilisticamente completos. Ou seja, quanto maior o nmero
de amostras a probabilidade do mtodo encontrar uma soluo existente tende a 1. Um dado de
grande importncia mas de difcil estimao a taxa de convergncia do algoritmo baseado em
amostragem (LAVALLE, 2006).
Os mtodos baseados em amostragem podem ser classicados em mtodos de busca nica
(single query) ou busca mltipla (multiple query). Dado o nico par (q
init
, q
goal
), que representa
respectivamente a congurao inicial e a congurao nal desejada, o algoritmo de busca
nica retorna uma soluo em tempo nito, ou no havendo ou no encontrando uma soluo,
reporta falha na busca. J os mtodos de busca mltipla investem um esforo na explorao
do espao de conguraes para contruirem uma estrutura de dados que armazena informaes
sobre o ambiente. Essa estrutura ento usada para tornar eciente a busca de solues para
diferentes pares (q
init
, q
goal
) (CHOSET et al., 2005).
A maioria dos planejadores bsicos de movimento ignoram completamente a dinmica do
rob, levando em considerao somente a cinemtica. Isto pode resultar em caminhos incapazes
de serem executados pelo rob real, devido a limitao das foras e torques que acionam o
rob (LAVALLE; KUFFNER, 2001). A abordagem de planejamento cinemtico-dinmico de
movimento prope uma soluo alternativa que leva em considerao tais restries.
26
2.3 Planejamento Cinemtico-Dinmico
O planejamento cinemtico-dinmico de movimento de robs mveis tem como objetivo
planejar caminhos factveis, isto , que obedeam as restries impostas pelo modelo dinmico
do rob, alm de evitar colises com os obstculos presentes no ambiente. Uma vez que o mo-
delo dinmico do rob utilizado na fase de planejamento, as trajetrias planejada e executada
se assemelham, quanto maior for a delidade entre modelo e o rob real.
Quando a dinmica levada em considerao, a dimenso do espao de estados que repre-
senta o rob cresce, uma vez que necessrio representar no apenas a posio e orientao,
mas tambm as velocidades ao longo do caminho planejado. Como discutido na Seo ante-
rior, os planejadores de movimento baseados em amostragem so ecientes na resoluo de
problemas em que a dimenso do espao de estados grande.
Em LaValle e Kuffner (2001) proposto um mtodo de planejamento de movimento ba-
seado em amostragem, classicado como um algoritmo de busca nica para planejamento
cinemtico-dinmico de movimento, que utiliza como base a construo de rvores aleat-
rias que exploram o espao de estados rapidamente. Esse algoritmo denominado Rapidly-
exploring Random Tree.
2.4 Rapidly-exploring Random Tree
O mtodo de planejamento de movimento baseado em RRT, foi desenvolvido com o in-
tuito de solucionar problemas de planejamento cinemtico-dinmico em um espao de estados
que tenha restries diferenciais de primeira ordem. O conceito de espao de conguraes
utilizado nos algoritmos clssicos de planejamento substitudo pelo conceito de espao de
estados para o planejamento cinemtico-dinmico. A seguinte analogia apresentada em La-
Valle e Kuffner (2001): seja, q C, uma congurao do rob no espao de conguraes e
representando o espao de estados por X, o estado do rob, x X, denido como x = (q, q).
2.4.1 Restries diferenciais no espao de estados
Utilizando mecnica Lagrangiana, a dinmica do modelo pode ser representada por um
conjunto de equaes diferenciais de segunda ordem, da forma h( q, q, q) = 0. O conjunto de
equaes diferenciais de segunda ordem, pode ser representado no espao de estados por um
conjunto de m equaes implcitas, de primeira ordem, da forma g( x, x) = 0, m < 2n, sendo n
27
a dimenso do espao de conguraes, C. Sobre determinadas condies, o teorema da funo
implcita pode ser aplicado para permitir que as restries diferenciais de primeira ordem sejam
expressas como
x = f(x, u) (2.1)
tal que u U, e U representa o conjunto das entradas, ou controles, permitido (LAVALLE;
KUFFNER, 2001).
2.4.2 Algoritmo RRT
O algoritmo RRT, descrito pelos Algoritmos 1 e 2, utiliza o conjunto de equaes diferen-
ciais de primeira ordem, representada pela equao 2.1, para estimar o estado futuro do rob
atravs de integrao numrica. Os dados de entrada do algoritmo RRT so: estado inicial do
rob, x
init
, estado nal desejado x
goal
ou regio nal desejada r
goal
, nmero mximo de tentati-
vas de expanso, modelo cinemtico-dinmico do rob e o mapa do ambiente. De posse dessas
informaes criado um grafo que ir representar a rvore de expanses.
Algoritmo 1 Construo - RRT
Entrada:
x
init
: estado inicial.
n: nmero de tentativas de expanso da rvore.
r
goal
: regio nal desejada.
Sada:
Uma rvore T = (V, E) que tem como raiz x
init
e possui n ns.
1: V q
0
2: E
3: for i = 1 at n do
4: x
rand
estado no espao de estados livre
5: x
chosen
expande RRT (Algoritmo 2)
6: if regio_do_objetivo_foi_alcanada(x
chosen
) then
7: break
8: end if
9: end for
10: return T
O grafo inicializado com um nico n que representa o estado inicial do rob. Um estado
x
rand
amostrado do conjunto de todos os possveis estados, X. realizada uma busca no
grafo, pelo n mais prximo de x
rand
, uma vez encontrado, esse n denominado x
near
. Consi-
derando x
near
como o estado atual do rob, um conjunto de entradas de controle permitidas so
aplicadas no modelo cinemtico-dinmico para calcular as possveis expanses desse estado,
28
Algoritmo 2 Expanso - RRT
Entrada:
T = (V, E): uma RRT
x: um estado em que a rvore T dever crescer rumo a ele.
Sada:
Um novo estado x
chosen
em direo a x, ou , caso falha.
1: x
near
vizinho mais proximo de x na rvore T
2: x
chosen

3: d
min

4: for u in U do
5: x
temp
EstimaNovoEstado(x
aux
, u)
6: if RoboEmPosioSegura(x
temp
) then
7: d (x, x
temp
)
8: if d < d
min
then
9: d
min
d
10: x
chosen
x
temp
11: end if
12: end if
13: end for
14: V V {x
chosen
}
15: E E {(x
near
, x
chosen
)}
16: return x
chosen
isto , estima-se os possveis estados futuros do rob. Durante o processo de expanso veri-
cado se o rob encontra-se a uma distncia segura dos obstculos. Essa vericao realizada
para todos os estados intermedirios entre o estado atual e o estado estimado futuro. Dentre
os possveis estados futuros estimados a partir do n x
near
, o estado, x
chosen
, que se encontrar
mais prximo de x
rand
escolhido para ser inserido no grafo, que tambm armazena a aresta
(x
near
, x
chosen
).
O processo repetido at que o nmero mximo de tentativas de expanses seja excedido
ou que o rob alcance o objetivo nal, x
goal
ou r
goal
. A gura 2.2 mostra esse processo, sendo
que as linhas pontilhadas representam as possibilidades de expanso e a linha destacada em
vermelho representa a expanso escolhida de acordo com uma mtrica .
2.4.3 Planejadores baseados em RRT
O algoritmo bsico do RRT, discutido acima, tem como objetivo promover a explorao
rpida do espao de estados enquanto verica se o objetivo nal foi alcanado. Dessa maneira
o algoritmo no possui uma heurstica que busque encontrar um caminho para x
goal
, isto , o
objetivo nal alcanado aleatoriamente devido a boa capacidade de explorao do espao de
estados intrnseca ao algoritmo RRT (LAVALLE; KUFFNER, 2000).
29
Figura 2.2: Expanso de uma RRT
Para que o objetivo seja alcanado mais rapidamente, a varivel q
rand
no mais amostrada
aleatoriamente de uma distribuio uniforme. Ao invs disso uma heurstica de planejamento
utilizada em conjunto com o algoritmo bsico do RRT, guia a amostragem e atribui varivel
q
rand
o estado nal desejado com uma certa probabilidade p. Com probabilidade (1 p)
retornado um estado aleatrio, amostrado de uma distribuio uniforme.
Para pequenos valores de p, tal como 0.05, a rvore converge mais rapidamente para o
objetivo do que quando utilizado apenas uma amostragem aleatria uniforme. Se o privilgio
a q
goal
for muito grande o algoritmo RRT acabar funcionando como um planejador baseado
em campos potenciais, o que poderia ocasionar o aprisionamento do rob em mnimos locais
(LAVALLE; KUFFNER, 2000; CHOSET et al., 2005).
Existem outras tcnicas de planejamento de movimento baseados em RRT. LaValle e Kuff-
ner (2000) prope tambm o privilgio de uma regio ao redor de x
goal
e apresentam a possibi-
lidade de criar duas rvores que exploram o espao de estados, sendo que a raiz de uma rvore
seria o estado inicial e a outra teria como raiz o estado nal desejado. Dessa maneira as duas
rvores seriam alternadamente expandidas at que elas se cruzem.
O RRT tambm pode ser utilizado como planejador local. Em Nourani-vatani et al. (2006)
foi desenvolvido um cortador de gramas automatizado que usa um planejador global para ex-
plorar completamente o ambiente. Quando o planejador global no consegue encontrar um
caminho livre de obstculos, o mtodo RRT utilizado para tentar buscar uma trajetria fact-
30
vel. Em Vallejo, Jones e Amato (2001) o planejamento global verica se as regies do espao
de conguraes que englobam o estado inicial e nal do rob so interconectadas, isto , se
existe um caminho que ligue o estado inicial ao estado nal. Caso exista o algortimo RRT uti-
lizado para gerar uma trajetria factvel, baseado nos waypoints calculados pelo planejamento
global.
31
3 Modelagem do rob mvel
Nesse captulo, so desenvolvidos os modelos cinemtico e dinmico para o rob Pioneer
3-AT. Este rob bastante conhecido e empregado em pesquisas na rea de robtica mvel,
podendo ser modelado como um veculo de rodas deslizantes. dotado de quatro rodas atuadas
por dois motores, sendo que um motor responsvel por acionar as rodas do lado direito e
outro motor aciona as rodas do lado esquerdo. A mudana de orientao do rob feita ao
se aplicar torques com valores distintos para cada motor. A modelagem aqui apresentada est
baseada em (CARACCIOLO; LUCA; IANNITTI, 1999; KOZLOWSKI; PAZDERSKI, 2004).
Estes modelos so utilizados pelo mtodo de planejamento de movimento, no Algoritmo 2
Linha 5, para calcular o estado futuro do rob, dado o estado atual e as entradas de controle. O
captulo ainda discute a estratgia de controle utilizada para realizar o seguimento das trajetrias
planejadas.
3.1 Modelo cinemtico
A geometria do rob mostrada na Figura 3.1, sendo que (X, Y ) o sistema de coordena-
das inercial, (X
0
, Y
0
) o sistema de coordenadas xo no corpo tendo como origem o centro de
gravidade do rob CG. A coordenada do centro de gravidade do rob no sistema de coorde-
nadas inerciais (x, y), e o ngulo entre o eixo X
0
e o eixo X. As coordenadas do centro
instantneo de rotao (CIR) so (x
0
CIR
, y
0
CIR
). O parmetro a corresponde distncia entre o
centro de gravidade e o eixo de simetria da roda atuada frontal paralelo ao eixo Y
0
, b a distncia
entre centro de gravidade e o eixo de simetria paralelo ao eixo Y
0
da roda atuada traseira, c a
metade da distncia entre as rodas ao longo do eixo Y
0
e r o raio das rodas.
No sistema de coordenada do corpo a velocidade linear do rob dada por =

x
0
y
0 0

T
,
sendo
x
0 e
y
0 as componentes de no eixo X
0
e Y
0
, e a velocidade angular dada por
= [0 0 ], sendo =

. As velocidades lineares e angulares de cada roda atuada so dadas
por
i
e
i
, sendo i = 1, 2, . . . , 4 a i-sima roda atuada.
32
Figura 3.1: Geometria do rob mvel.
As velocidades absolutas x, y e

no sistema de coordenadas inercial so dadas por

x
y

cos sin 0
sin cos 0
0 0 1

x
0

y
0

= R()

x
0

y
0

.
(3.1)
Diferenciando a equao acima em relao ao tempo tem-se

x
y

= R()

x
0

y
0

y
0 +

x
0

= R()

a
x
0
a
y
0

.
(3.2)
Diferentemente da maioria dos robs mveis, a velocidade lateral do RMRD geralmente
diferente de zero, isso vem da estrutura mecnica do RMRD que faz o deslizamento lateral
necessrio para que o veculo mude de orientao, e quando a velocidade do rob
y
0 = 0 no
h deslocamento lateral. A velocidade angular e a velocidade lateral
y
0 ambas desaparecem
quando o rob movimenta-se em linha reta, e o CIR vai para innito ao longo do eixo Y
0
. J
33
em uma trajetria curva o CIR desloca x
0
CIR
ao longo do eixo X
0
. Se x
0
CIR
ultrapassar a base
das rodas do rob, o veculo desliza drasticamente com perda de estabilidade de movimento.
Assim, para completar o modelo cinemtico do RMRD, a seguinte restrio no-holonmica
foi introduzida em (CARACCIOLO; LUCA; IANNITTI, 1999)

y
0 = x
0
CIR

, com x
0
CIR
(b, a). (3.3)
No entanto, de (3.1) obtm-se

y
0 = sin x + cos y. (3.4)
Seja q = [x y ]
T
, o estado do rob, substituindo a equao (3.4) em (3.3) e escrevendo
na forma Pfafan, tem-se
h
sin cos x
0
CIR
i

x
y

= A(q) q = 0
A partir da Figura 3.1, as velocidades q podem ser expressas como:
q = S(q), (3.5)
onde:
S(q) =

cos x
0
CIR
sin
sin x
0
CIR
cos
0 1

,
=
"

x
0

#
.
S(q) uma matriz de posto completo, cujas colunas esto no espao nulo de A(q), ou seja
S
T
(q)A
T
(q) = 0
interessante observar que como a dim() = 2 < dim(q) = 3, a equao (3.5) descreve
a cinemtica de um rob subatuado, que um sistema no-holonmico por causa da restrio
(3.3).
34
3.2 Modelo dinmico
A Figura 3.2 mostra as foras de trao F
x
0
i
que so sujeitas a foras resistivas longitudinais
R
x
0
i
. O Pioneer 3-AT possui dois motores, cada um responsvel por acionar as rodas de um
dos lados do rob. Ento assume-se que a atuao das rodas igual em cada lado, ou seja,
F
x
0
1
= F
x
0
3
e F
x
0
2
= F
x
0
4
, diminuindo assim o deslizamento longitudinal. As foras laterais
R
y
0
i
que atuam sobre as rodas so consequncias do deslizamento lateral. O momento resistivo
M
r
em volta do centro de massa, que se ope ao momento M induzido pelas foras R
y
0
i
e
R
x
0
i
.
Figura 3.2: Foras de trao e resistivas do rob mvel.
Para um veculo, como mostrado na Figura 3.2, de massa m e de inrcia I no centro de
massa, as equaes de movimento no sistema de coordenada do corpo so
ma
x
0 = 2F
x
0
1
+ 2F
x
0
2
R
x
0 ,
ma
y
0 = R
y
0 ,
I

= 2c(F
x
0
1
F
x
0
2
) M
r
.
(3.6)
Para representar as foras longitudinais R
x
0 , as foras laterais R
y
0 , e o momento resistivo
M
r
, deve-se considerar mg como a carga gravitacional, sendo m a massa do rob e g a acele-
rao da gravidade, que dividida sobre as rodas do rob, e introduzir o modelo Coulomb de
atrito para o contato das rodas com a superfcie. Desse modo, desde que o rob tenha uma linha
35
mediana longitudinal de simetria, obtm-se
R
z
0
1
= R
z
0
2
=
b
2(a + b)
mg,
R
z
0
3
= R
z
0
4
=
a
2(a + b)
mg.
Em velocidades baixas, a carga lateral transferida devido as foras centrifugas sobre per-
cursos curvados pode ser desconsiderada. No caso de superfcies duras, pode-se assumir que o
contato entre as rodas e o solo retangular e que a carga vertical produz uma presso unifor-
memente distribuda. Assim R
x
0
i
= f
r
R
z
0
i
sgn(
x
0
i
) sendo que: f
r
o coeciente resistivo de
rolagem independente da velocidade e sgn(.) a funo sinal. A fora resistiva longitudinal total

R
x
0 =
4
X
i=1
R
x
0
i
=
f
r
mg
2
(sgn(
x
0
1
) + sgn(
x
0
2
)) ,
Introduzindo o coeciente de atrito lateral , temos que R
y
0
i
= R
z
0
i
sgn(
y
0
i
). Portanto a
fora lateral total dada por
R
y
0 =
4
X
i=1
R
y
0
i
=
mg
a + b

b sgn(
y
0
1
) + a sgn(
y
0
3
)

,
enquanto o momento resistivo ser
M
r
=
abmg
a + b

sgn(
y
0
1
) sgn(
y
0
3
)

+
f
r
cmg
2
(sgn(
x
0
1
) sgn(
x
0
2
)) .
Segundo Caracciolo, Luca e Iannitti (1999), utilizando-se notao matricial, o modelo dinmico
reescrito no sistema de coordenadas inerciais
M q + C(q, q) = E(q), (3.7)
36
sendo
M =

m 0 0
0 m 0
0 0 I

,
C(q, q) =

cos R
x
0 sin R
y
0
sin R
x
0 + cos R
y
0
M
r

,
E(q) =

cos
r
cos
r
sin
r
sin
r
c
r

c
r

,
=
"

L

R
#
=
"

1
+
3

2
+
4
#
,
i
= rF
x
0
i
.
Incluindo a restrio no-holonmica no modelo dinmico (3.7), como introduzido em
(CARACCIOLO; LUCA; IANNITTI, 1999), tem-se
M q + C(q, q) = E(q) + A
T
(q), (3.8)
sendo o vetor de multiplicadores de Lagrange correspondente a equao (3.3). Para estimar as
aceleraes a partir do torque aplicado, devemos diferenciar a equao (3.5), substituir em (3.8)
e eliminar , ento teremos o seguinte resultado (CARACCIOLO; LUCA; IANNITTI, 1999):
=

S
T
MS

1
S
T

E M

S C

(3.9)
O planejador de movimentos apresentado no Captulo 2 utiliza as equaes (3.5) e (3.9)
para incluir respectivamente as restries cinemticas e dinmicas na fase de planejamento.
3.3 Seguimento de trajetria
Uma vez que a trajetria planejada leva em considerao o modelo cinemtico-dinmico
do rob, ela ser factvel. Os parmetros do modelo matemtico, que representa um sistema
fsico, devem ser ajustados para que os resultados obtidos a partir do modelo sejam vlidos
para o sistema fsico real. Levando em considerao que os parmetros do modelo representam
bem o rob P3-AT e que o planejamento cinemtico-dinmico baseado em RRT gera caminhos
possveis de serem realizados, as trajetrias planejada e executada sero bastante semelhantes.
Portanto o erro de deslocamento do rob, isto , o erro entre posio de referncia e posio
atual do rob, ao longo da trajetria tende a ser pequeno e por isso propomos uma estratgia de
37
controle simples para realizar o seguimento do caminho planejado.
Seja o estado de referncia q
r
= [ x
r
y
r

r
]
T
, o estado atual do rob q
c
= [ x
c
y
c
]
T
e o estado que representa o erro q
e
= q
r
q
c
. Da equao (3.5), o seguinte controlador propor-
cional foi desenvolvido:

e
= S
+
q
e
"

e
w
e
#
=

cos sin 0

x
CIR
sin
x
2
CIR
+1
x
CIR
cos
x
2
CIR
+1
1
x
2
icr
+1

x
r
x
c
y
r
y
c

(3.10)
sendo que S
+
a pseudo-inversa da matriz S e
e
= [
e

e
]
T
. A partir das velocidades de
erro apresentadas acima, temos a seguinte lei de controle:

track
=
r
+ k

e
w
track
= w
r
+ k

w
e
(3.11)
sendo que k

, k

so constantes e
r
,
r
as velocidades de referncia, linear e angular respecti-
vamente. Os resultados do seguimento de trajetria so mostrados no Captulo 6.
38
39
4 Planejamento de movimento
cinemtico-dinmico baseado em
acompanhamento
Os planejadores de movimento bsicos desconsideram completamente a dinmica do rob,
levando em conta apenas a cinemtica. Ao fazer isto os caminhos gerados podem ser impratic-
veis, devido a limitaes de fora e torque do rob real. Grande parte das pesquisas conduzidas
em robtica na rea de planejamento de movimento, separam o problema de planejamento em
duas fases. Na primeira fase utilizam um planejador de movimentos bsico e na segunda fase
projetam um controlador com o objetivo de acompanhar o caminho planejado obedecendo as
restries dinmicas do modelo do rob.
Aabordagemaqui proposta denominada Planejamento de Movimento Cinemtico-Dinmico
Baseado em Acompanhamento aplica o controlador que ser utilizado no acompanhamento da
trajetria planejada na fase de planejamento de movimento. Esta abordagem implementada a
partir do planejador de movimento cinemtico-dinmico bsico, apresentado no captulo 2, que
possui como algoritmo base o RRT.
4.1 Metodologia
O comportamento do algoritmo RRT bsico inteiramente aproveitado nesta abordagem
de planejamento de movimento. Isto signica que todas as caractersticas inerentes ao mtodo
RRT, tais como: habilidade de explorao do ambiente e aleatoriedade nas trajetrias planeja-
das, so compartilhadas pela abordagem proposta.
Aseguir descreveremos, emdetalhes, o mtodo de planejamento de movimento cinemtico-
dinmico baseado em acompanhamento. A fase inicial do algoritmo implica em denir o
estado inicial e o estado nal desejado do rob. O estado do rob denido pelo vetor
x = (x, y, , , ) sendo (x, y, ) a posio e orientao do rob e (, ) as velocidades li-
40
near e angular. Logo em seguida dene-se um nmero mximo de tentativas para atingir o
estado nal desejado. O processo de buscar uma trajetria entre o estado inicial e o estado nal
semelhante ao proposto no Captulo 2 Seo 2.4. Um estado aleatrio x
rand
amostrado, e
feita uma busca na rvore para encontrar o estado mais prximo de x
rand
, a este estado dado
o nome de x
near
. A partir de x
near
, que j se encontra na rvore, so aplicadas as entradas de
controle permitidas gerando um conjunto de possveis estados futuros. Ao contrrio do algo-
ritmo bsico do RRT que apenas escolheria, dentre os possveis estados futuros, o estado que
mais se aproxima de x
rand
, agora h uma nova fase que chamamos de fase de simulao de
acompanhamento.
Seja q
near
o estado a partir do qual feita a expanso para os estados futuros estimados,
q
estimated
o estado futuro estimado e u
control
a entrada de controle que transforma q
near
em
q
estimated
. A fase de acompanhamento tem como objetivo utilizar os dados acima apresentados
como entrada de um controlador e estimar a inuncia do controle na execuo da trajetria.
Desta maneira saberemos ainda na fase de planejamento de movimento o que acontecer ao
utilizarmos o controle na fase de execuo da trajetria planejada. Teremos o seguinte resultado:
q
tracked
= control_law(q
near
, q
estimated
, u
control
) (4.1)
Sendo q
tracked
o estado acompanhado esperado ao aplicarmos o controlador no rob real.
Aps aplicarmos a Equao 4.1 a todos os estados estimados (q
estimated
) teremos um conjunto
de estados acompanhados. Dentre os estados acompanhados, o que mais se aproxima do estado
aleatrio q
rand
escolhido e adicionado na rvore.
Na Figura 4.1 tem-se um exemplo de expanso da rvore RRT utilizando a abordagem
proposta. As linhas tracejadas pretas indicam as possveis trajetrias estimadas ao aplicar as
entradas de controle tendo como estado inicial x
near
. As linhas pontilhadas mostram os estados
acompanhados, isto , mostram as trajetrias obtidas aplicando-se a Equao 4.1. E a linha
pontilhada verde mostra a trajetria nal escolhida.
O diagrama de blocos mostrado na Figura 4.2 sumariza o processo de expanso usado para
obter os possveis estados futuros. As entradas de controle aplicadas no modelo dinmico so
um conjunto de aceleraes linear e angular. Desta maneira para cada par de acelerao ( , )
calculado o torque que o motor deve alcanar. Caso o torque calculado esteja dentro dos limites
fsicos do motor do rob, a entrada de controle validada. O modelo cinemtico propagado
atravs de integrao numrica e obtemos os possveis estados futuros.
De posse dos possveis estados futuros, o diagrama de blocos da Figura 4.3 realiza a abor-
41
dagem proposta, alimentando a lei de controle 3.11 conforme descrito pela equao 4.1. Depois
que todos os estados futuros estimados passarem pela fase de simulao de acompanhamento,
o estado acompanhado que mais se aproximar de x
rand
ser adicionado na rvore.
Figura 4.1: Abordagem de Planejamento Cinemtico-Dinmico de Movimento baseado em
Acompanhamento Exemplo de expanso.
Figura 4.2: Diagrama de blocos da fase de expanso
Figura 4.3: Diagrama de blocos da fase de simulao de acompanhamento
42
43
5 Implementao
O mtodo de planejamento de movimento baseado em RRT, descrito nos algoritmos apre-
sentados, foi implementado utilizando-se a linguagem de programao C++. O planejador de
movimentos desenvolvido nesse trabalho foi chamado de rrt-motion-planner e para ofe-
recer reusabilidade do cdigo em projetos futuros o software foi implementado segundo o pa-
radigma de programao orientada a objetos. O cdigo fonte desenvolvido ao longo desse tra-
balho est disponibilizado online e pode ser acessado atravs do endereo: http://code.
google.com/p/rrt-motion-planner/.
Foram tambm desenvolvidos alguns softwares auxiliares que cuidam da obteno de va-
lores de torques vlidos para o rob, execuo e acompanhamento da trajetria planejada, vi-
sualizao das trajetrias planejada e realizada pelo rob P3-AT. As ferramentas e bibliotecas
utilizadas na implementao do rrt-motion-planner e a discusso mais detalhada da
funcionalidade das classes que compe o planejador de movimentos so descritos nas sees
seguintes.
5.1 Ferramentas e bibliotecas
5.1.1 Player/Stage
As trajetrias planejadas pelo software rrt-motion-planner so executadas tanto em
ambiente computacional, atravs de simulaes, quanto no rob P3-AT. O conjunto de aplicati-
vos Player/Stage foi utilizado nesse trabalho para realizar ambas tarefas.
O software Player permite, atravs de uma camada de abstrao de hardware, que o usu-
rio acesse diferentes tipos de sensores, instalados em plataformas robticas distintas, sem se
preocupar em reescrever um novo cdigo de controle para cada rob diferente. Um arquivo
de congurao utilizado na inicializao do Player, informa quais dispostivos esto presentes
no rob e fornecem os respectivos drivers para cada dispostivo. Dessa maneira um mesmo c-
digo de controle pode ser utilizado para controlar diversos tipos de robs, bastando para isso
44
modicar apenas o arquivo de congurao que informa ao player o rob utilizado.
O Stage um simulador que possibilita o teste do cdigo, antes de aplic-lo em um rob
real. Portanto, a unio dessas duas caractersticas, quais sejam, reusabilidade de cdigo e ambi-
ente de simulao, fazem da sute de aplicativos Player/Stage uma poderosa ferramenta na rea
de robtica (GERKEY; VAUGHAN; HOWARD, 2003).
5.1.2 PQP Proximity Query Package
Para garantir que o rob mantenha-se a uma distncia segura dos obstculos presentes no
ambiente e no entre em coliso, o software rrt-motion-planner utiliza a biblioteca Pro-
ximity Query Package, PQP (LARSEN et al., 2000). Essa biblioteca utiliza modelos geom-
tricos compostos por um conjunto de tringulos que permitem representar vrias formas de
obstculos e robs, alm de realizar trs tipos de consultas de proximidade entre os modelos:
distncia de tolerncia, deteco de coliso e distncia mnima. Este trabalho utiliza a veri-
cao de distncia de tolerncia para evitar coliso entre os modelos geomtricos do rob e
obstculos.
5.1.3 LEMON
Como visto na Seo 2.4, o algoritmo de planejamento RRT utiliza uma estrutura de dados
para armazenar a rvore de expanses e o caminho planejado. A biblioteca LEMON (Egervry
Research Group on Combinatorial Optimization, 2010) implementa vrias classes que permi-
tem utilizar diferentes abordagens de grafos. Alm disso a biblioteca tambm possui alguns
algoritmos de busca em grafos, de grande utilidade para o software implementado. Essa bibli-
oteca implementada em C++, que torna fcil a sua utilizao pelo software rrt-motion-
planner, justicando assim o motivo de sua escolha.
5.2 Arquitetura do software
O rrt-motion-planner foi desenvolvido com o propsito de lidar com qualquer
classe de rob. Uma vez que se conhea o modelo do rob, representado por equaes di-
ferenciais, possvel estender o planejamento de movimento para qualquer modelo robtico.
Dessa maneira o paradigma de programao orientada a objetos bastante ecaz, devido a
funcionalidades como herana e polimorsmo. O diagrama de classes para o software rrt-
motion-planner mostrado na Figura 5.1.
45
Figura 5.1: Diagrama simplicado de classes Software rrt-motion-planner
As seguintes classes bases so denidas: RobotModel que descreve o modelo matemtico
do rob (cinemtico, dinmico, baseado em acompanhamento) e RobotGeometryModel
que descreve as caractersticas fsicas do rob. A partir dessas classes podem ser criadas classes
lhas, permitindo assim descrever diversos modelos de robs. A classe EnvModel cuida da
representao do mapa do ambiente e a classe World dos testes de vericao de distncia de
tolerncia entre o modelo geomtrico do rob e mapa do ambiente. A RRT a principal classe
do software pois trata diretamente do planejamento de movimento baseado no algoritmo RRT.
5.2.1 Classe RRT
Esta classe implementa o mtodo de planejamento de movimento baseado nos algoritmos
bsicos de RRT apresentados no Captulo 2. A classe RRT, juntamente com seus atributos e
mtodos, pode ser vista na Figura 5.2.
Para criar uma instncia dessa classe necessrio denir os seguintes parmetros: estado
inicial, estado nal desejado e nmero mximo de tentativas de expanso. Os algoritmos 1
e 2 utilizam uma estrutura de dados para armazenar as expanses em forma de grafo. Cada
n do grafo um estado que o rob pode assumir e cada aresta do grafo guarda as seguintes
informaes: estado atual, ao de controle, tempo de aplicao da ao de controle e o estado
futuro estimado. A biblioteca LEMON cuida de toda a parte relacionada a grafos, desde a
insero de novos ns at a busca pelo caminho mais curto entre dois ns do grafo.
Como apresentado no Captulo 3, as coordenadas (x, y) representam o centro de gravidade
do rob. A partir dessas coordenadas e do ngulo de orientao, , a classe VehicleGeome-
46
Figura 5.2: Diagrama da classe RRT
tryModel calcula os vrtices de dois tringulos, que juntos formam um retngulo que modela
o corpo do rob. Os testes realizados pela biblioteca PQP, tem o objetivo de evitar a coliso
com os obstculos presentes no ambiente, e dessa forma estabelecer uma distncia segura de
tolerncia, que deve ser respeitada ao longo do planejamento de movimento.
A classe StateSampler amostra o espao de estados e retorna como resultado um estado
aleatrio x
rand
. A partir da posio atual do rob e tendo como objetivo local o estado x
rand
,
o mtodo CheckPathNoCollision substitui a Linha 5 do Algoritmo 2. Esse mtodo faz
a integrao numrica das equaes diferenciais representadas do modelo do rob e verica
para cada etapa da integrao se o estado estimado encontra-se a uma distncia segura dos
obstculos, ao invs de realizar isso apenas no nal da integrao numrica. Como vrias
entradas de controle so utilizadas nas tentativas de expanso da rvore, utiliza-se uma mtrica
para escolher o estado futuro estimado que mais aproxima-se de x
rand
. A mtrica utilizada no
Algoritmo 2, implementado pela classe BiasedMeterP3AT linha 7.
Para evitar duplicidade de ns no grafo, aps o estado x
chosen
ser escolhido de acordo com
a mtrica acima apresentada, realizada uma busca por x
chosen
no grafo. Essa busca feita pelo
mtodo CheckDuplicatedNode que retorna o ndice do n caso ele j esteja adicionado no
grafo ou ento adiciona o n. O grafo tambm armazena uma aresta que contm, alm de x
near
e x
chosen
, a entrada de controle e o tempo total de integrao.
Aps esgotar o nmero mximo de tentativas de expanso ou caso o destino seja alcanado,
realizada uma busca no grafo pelo caminho mais curto entre o estado inicial e o estado nal
desejado (ou o estado que mais se aproxima do objetivo), realizado pelo mtodo PathToClo-
sestGoalState. Esse caminho ento exportado para um arquivo de log, atravs do mtodo
47
ExportPath para que seja possvel a execuo da trajetria no ambiente de simulao ou pelo
rob P3-AT.
5.2.2 Classe StateSampler
No algoritmo 1 linha 4 necessrio amostrar um estado aleatrio que guia o crescimento
da rvore. Esta classe fornece diferentes tipos de mtodos de amostragem do espao de esta-
dos. Desta maneira para problemas distintos pode-se escolher o mtodo de amostragem mais
apropriado. Os mtodos de amostragem fornecidos so: distribuio gaussiana ou normal, dis-
tribuio uniforme e amostragem tendenciosa. Este ltimo mtodo de amostragem retorna o
estado tendencioso desejado com uma probabilidade p e com probabilidade 1 p retorna um
estado tomado de uma distribuio uniforme.
5.2.3 Classes DistanceMeter e BiasedMeterP3AT
Aps o estado aleatrio ser escolhido necessrio encontrar o estado mais prximo j
inserido na rvore de expanses. Para realizar estas medidas de distncia foi criada a classe
DistanceMeter, que tambm a classe base da classe lha BiasedMeterP3AT.
A classe base DistanceMeter fornece 3 mtodos virtuais: NearestNodeMetric
que utilizado para determinar qual n da rvore encontra-se mais prximo do estado aleatrio.
Determinando o n da rvore mais prximo do estado aleatrio, so feitas expanses a partir
deste n, aplicando-se as possveis entradas de controle. Para determinar qual dos ns expan-
didos encontra-se mais prximo do estado aleatrio utilizado o mtodo DistanceWeight.
Aps encontrar qual n expandido encontra-se mais prximo do estado aleatrio, este n in-
serido na rvore e ento o mtodo GoalStateAchieved determina se o n que acabou de
ser inserido est prximo o bastante do estado nal desejado, encerrando ou no a expanso da
rvore.
A classe lha BiasedMeterP3AT sobrescreve os trs mtodos descritos acima, permi-
tindo que o planejador de movimentos se comporte de uma maneira adequada ao rob Pioneer-
3AT. O estado que representa o rob : (x, y, , v, ). No mtodo NearestNodeMetric o
n mais prximo escolhido com base na distncia euclideana, uma certa ponderao no ngulo
de orientao e na diferena entre as velocidades do estado aleatrio e n da rvore. Caso o
estado aleatrio esteja dentro de um crculo de raio r cujo centro seja o estado nal desejado,
as velocidades (v, ) do estado aleatrio so alteradas para o valor das velocidades do estado
nal desejado, isto feito para aumentar a probabilidade do estado nal ser atingido com as
48
velocidades pr-determinadas.
O mtodo DistanceWeight utilizado para obter uma medida entre estado a ser atin-
gido e estado expandido atravs da aplicao de uma entrada de controle. Esta medida leva
considerao a distncia euclidiana entre os dois estados, a diferena do ngulo de orientao
e a diferena da velocidade linear.
5.2.4 Classes TrackingControl, ProportionalControl e Pioneer3ATState
A classe TrackingControl uma classe virtual que possui dois mtodos que devem ser
reimplementados pelas classes lhas, que so: run e InitializeControllerWeights.
Esta classe tem a funo de simular o acompanhamento da trajetria na etapa de planejamento,
realizando desta maneira o que chamamos de Planejamento Baseado emAcompanhamento, dis-
cutido anteriormente. O mtodo run tem o comportamento do algoritmo descrito no Captulo
4.
A classe ProportionalControl uma classe lha da classe TrackingControl.
Esta classe implementa um controlador Propocional e a classe Pioneer3ATState utili-
zada para clculos de erros de velocidade e acelerao, que sero utilizados no controlador
proporcional.
5.2.5 Classes RobotGeometryModel, EnvModel e World
Essas trs classes trabalham em conjunto para representar geometricamente o rob e am-
biente, alm de realizar testes de distncia de tolerncia entre os modelos RobotGeometry-
Model e EnvModel com o intuito de evitar colises ao longo do caminho que ser planejado.
O relacionamento dessas trs classes representando no diagrama de classes da Figura 5.3.
A funo da classe RobotGeometryModel representar geometricamente um rob. A
classe lha, VehicleGeometryModel representa um rob que assemelha-se a um veculo,
isto , um rob cujo corpo pode ser representado por um retngulo. Essa classe recebe infor-
maes de largura, comprimento, distncia entre os eixos do veculo, a coordenada (x, y) que
informa a posio do centro de rotao do veculo e o angulo de orientao . A partir dessas
informaes o mtodo SetVerticesPosition faz os clculos, tanto de rotao quanto de
translao, para gerar os quatro vrtices que representam o corpo do veculo na forma de um
retngulo. importante ressaltar que se houver necessidade de utilizar um rob cujo corpo no
se assemelhe a um retngulo, basta criar uma nova classe lha, da classe pai RobotGeome-
tryModel.
49
Figura 5.3: Diagrama de classes RobotGeometryModel, EnvModel e World
A classe EnvModel responsvel por armazenar as informaes relacionadas ao mapa
do ambiente. Essa classe trabalha em conjunto com a biblioteca PQP. Os obstculos presentes
no ambiente, so armazenados em um arquivo de texto. Cada linha desse arquivo contm trs
pontos, no espao cartesiano tridimensional, que formam um tringulo. A partir desse arquivo,
a classe EnvModel constri modelos compatveis com a biblioteca PQP.
O mtodo GetPosition da classe RobotGeometryModel utilizado para obter in-
formaes a respeito da posio do corpo do rob e construir um modelo do tipo PQP_Model.
Aclasse World implementa o mtodo IsVehicleInSafePosition que faz o teste de dis-
tncia de tolerncia entre o modelo do veculo e o mapa do ambiente representado pelo atributo
obstacles pertencente a classe EnvModel.
5.2.6 Classe RobotModel
A classe base RobotModel responsvel por guardar as informaes do modelo do rob,
tais como: dimenso do espao de estados, entradas de controle permitidas, equaes diferenci-
ais que representam o rob etc. Alm disso, implementa apenas mtodos virtuais, isto , todos
os mtodos por ela implementados devem ser redenidos em uma classe lha. A Figura 5.4
mostra o diagrama de classes detalhado para a classe RobotModel.
50
Figura 5.4: Diagrama de classes RobotModel
No software desenvolvido foram implementadas duas classes de robs, derivadas da classe
base RobotModel. A classe CarLikeModel representa o modelo cinemtico de um ve-
culo com rodas esterantes, utilizando como espao de estados o vetor (x, y, , , ). A classe
SkidSteerDynamicModel representa o rob Pioneer 3-AT e leva em considerao a din-
mica do modelo, o espao de estados representado pelos parmetros (x, y, , , , , ).
A classe SkidSteerTrackingBased representa o modelo matemtico do rob de ro-
das deslizantes Pioneer 3AT e agrega tambm um controlador do tipo TrackingControl.
Desta forma diferentes tipos de controle podem ser utilizados, bastando criar classes lhas
da classe base TrackingControl. Esta classe utilizada na abordagem de planejamento
proposta no Captulo 4 - Planejamento de movimento cinemtico-dinmico baseado em acom-
panhamento.
O modelo do sistema robtico representado por um conjunto de equaes diferenciais
ordinrias. No Algoritmo 2 linha 5, o mtodo EstimateNewState estima o estado futuro
do rob a partir do estado atual do rob, da entrada de controle e das equaes diferenciais (3.9)
e (3.5).
O mtodo EstimateNewState usa a equao (3.9) para estimar as aceleraes linear
e angular, a partir do torque aplicado . Por integrao numrica encontra-se . De posse do
estado atual q
n
, do vetor velocidade e do tempo de integrao t, estima-se o estado futuro
51
do rob, q
n+1
, utilizando-se a equao (3.5). O processo de integrao numrica utilizado
o mtodo Runge-Kutta de quarta ordem. A equao (5.1) descreve o mtodo de integrao
numrica para a equao (3.5).
x = h(x, , t), x(t
o
) = x
n
,
x
n+1
= x
n
+
t
6
(k
1
+ 2k
2
+ 2k
3
+ k
4
) ,
t
n+1
= t
n
+t.
(5.1)
sendo que:
k
1
= h(x
n
, , t
n
),
k
2
= h

x
n
+
t
2
k
1
, , t
n
+
t
2

,
k
3
= h

x
n
+
t
2
k
2
, , t
n
+
t
2

,
k
4
= h(x
n
+tk
3
, , t
n
+t) .
Conhecendo as equaes diferenciais que regem o modelo, ao aplicar uma entrada de con-
trole consegue-se estimar o estado futuro do rob. Apenas os estados futuros estimados que no
se encontram em coliso, podem ser utilizados na expanso da rvore. Como dito no Captulo 2
as entradas de controle aplicadas ao modelo devem ser entradas vlidas e que no desobedeam
s restries impostas pelo modelo dinmico. A prxima Seo apresenta um software auxiliar
criado para gerar uma tabela de torques que armazena quais possveis valores de torques podem
ser aplicados ao modelo levando em considerao a velocidade atual do rob.
5.3 Gerao de torques
O algoritmo de planejamento espera apenas entradas de controle permitidas, assim h uma
ferramenta responsvel por gerar uma tabela de torques, a partir de dados como torque mnimo e
mximo, valor absoluto da acelerao mxima permitida, valor absoluto da velocidade mxima
permitida e valor atual da velocidade do rob.
vlido ressaltar que a discretizao dos valores de torques causa um impacto direto no
desempenho do algoritmo de planejamento de movimento. O Algoritmo 2 dependente das
entradas de controle do modelo utilizado para representar o rob. Desta maneira ao discretizar
os valores de torque a serem aplicados no modelo, limita-se os tipos de movimentos que o rob
poder executar. Tendo isto em mente chega-se a concluso que quanto maior o nmero de
entradas de controle, maior ser a diversidade de possveis curvaturas a serem realizadas pelo
rob. No entanto, a discretizao faz-se necessria pois quanto maior o nmero de entradas de
controle mais custoso computacionalmente ser o clculo de todas as curvaturas possveis.
52
Trs tabelas distintas so geradas. Os valores de torques da primeira tabela so calculados
para atuar no movimento linear do rob, isto , faz com que o rob acelere ou freie, sem inter-
ferir no ngulo de orientao. As outras duas tabelas so responsveis por armazenar valores
de torque que auxiliam o rob a realizar curvas para a direita ou para esquerda. Os valores uti-
lizados nestas tabelas foram escolhidos de maneira a maximizar a relao custo-benefcio entre
nmero de entradas de controle e custo computacional do algoritmo.
O Algoritmo 3 descreve a gerao de torques que fazem o rob acelerar ou freiar (primeira
tabela). Para gerar torques que provoquem o movimento de rotao do rob basta substituir
a Linha 4 e atribuir valores diferentes para
left
e
right
, alm de fornecer velocidade angular
mxima, acelerao angular mxima e velocidade angular atual do rob, como parmetros de
entrada para o algoritmo.
Algoritmo 3 Gerao de torques vlidos
Entrada:
_lin_abs
max
, acc_lin_abs
max
,
min
,
max
Sada:
Tabela de valores de torques vlidos.
1: for = _lin_abs_max; <= _lin_abs_max; = + _step do
2: x (0, 0, 0, , 0)
3: for torque =
min
; torque <=
max
; torque = torque + torque_step do
4:
left

right
torque
5: input_control (
left
,
right
)
6: x
new
EstimaNovoEstado(x, input_control)
7: acc
inst
= fabs(x
new
. x.)
8: if acc
inst
<= acc_lin_abs
max
then
9: Insere o valor do torque na tabela, relacionada a velocidade linear quantizada
atual
10: end if
11: end for
12: end for
Como visto no Algoritmo 3 Linha 1 no possvel representar de forma contnua a velo-
cidade do rob, portanto os torques permitidos so gerados apenas para valores quantizados de
velocidade. Isto signica que no momento da expanso da rvore, a velocidade atual do rob
ser arredondada para o valor mais prximo das velocidades quantizadas.
Para uma dada velocidade quanticada , quinze valores de torques so escolhidos, sendo
que so tomados cinco valores por tabela de acordo com os seguintes critrios:
Tabela 1: dois valores de torques que freiam o rob, dois valores de torques que acelerem
o rob e um valor de torque que no altere a velocidade linear.
53
Tabela 2 e 3: os cinco valores, para cada tabela, so tomados em ordem decrescente
de acelerao angular, isto , os cinco torques que promovem as maiores aceleraes
angulares sero escolhidos.
Realizando-se essa escolha para as n velocidades quanticadas possveis, criado um ar-
quivo de texto que guarda esses valores de torques. Desta forma, para cada velocidade quanti-
cada h quinze entradas de controle que podem ser aplicadas para estimar os estados futuros.
Os mtodos GenerateInputs e GetValidInputs da classe RobotModel tem res-
pectivamente as seguintes funcionalidades:
Ler o arquivo de texto e armazenar todas as entradas da tabela no vetor inputs.
Selecionar os elementos do vetor inputs de acordo com o estado atual do rob, isto
, apenas as entradas de controle permitidas levando em considerao o estado atual do
rob.
5.4 Acompanhamento de trajetria
Aps o trmino da execuo do algoritmo de planejamento de movimento, gerado um
arquivo de log que contm as aes de controle aplicadas no rob ao longo do percurso. A
classe Tracking responsvel por enviar esses comandos ao software Player. O diagrama
de classes simplicado da Figura 5.5 mostra o relacionamento entre as classes utilizadas pelo
software de acompanhamento de trajetria.
Figura 5.5: Diagrama de classes - Tracking
O arquivo de congurao escolhido para inicializar o Player, permite que o usurio alterne
entre o modo de simulao, que utiliza o Stage, e a execuo da trajetria no rob real. Fo-
54
ram implementados dois mtodos responsveis por realizar o acompanhamento de trajetria:
PathTracking e OpenLoopControl. O primeiro implementa as equaes (3.10) e (3.11)
do controlador proporcional apresentadas na Seo 3.3. J o segundo mtodo aplica as entradas
de controle em malha aberta.
5.4.1 Implementao do acompanhamento de trajetria para o Player/Stage
O Player fornece diversas classes que possibilitam acessar os mais diversos tipos de dis-
positivos comumente encontrados em robs. No entanto uma boa prtica de programao
encapsular classes fornecidas por uma biblioteca, de maneira que se algum mtodo ou atri-
buto da biblioteca for alterado, no seja necessrio alterar todo o cdigo, bastando assim alterar
apenas a classe encapsuladora. Duas classes foram implementadas para encapsular a comuni-
cao com o Player: ProxyRobot e ProxyPosition, mostradas no diagrama de classes
da Figura 5.6.
Figura 5.6: Diagrama de classes - ProxyRobot e ProxyPosition
Aclasse ProxyPosition derivada da classe fornecida pelo Player Position2dProxy.
O mtodo GetPose retorna a posio atual do rob a partir da leitura do odmetro, em uma
estrutura do tipo (x, y, ). Os mtodos SetMotorStatus e SetOdomPos so responsveis
por ligar o motor e ajustar a posio atual do odmetro, respectivamente. O mtodo SetNewS-
peed envia para o rob a nova velocidade (linear e angular). A classe ProxyRobot cuida da
conexo com o Player, encapsulando a classe ClientProxy, fornecida pelo Player, e tem
como atributo uma instncia da classe ProxyPosition. Assim a classe Tracking atravs
de uma instncia da classe ProxyRobot consegue acesso aos comandos de controle do rob.
55
6 Resultados
A seguir so mostrados os resultados obtidos durante o decorrer deste trabalho. O captulo
est dividido em duas sees. A seo 6.1 trata apenas da fase de planejamento de movimento.
Nesta seo sero apresentados o planejamento de movimento cinemtico-dinmico bsico e o
planejamento de movimento cinemtico-dinmico baseado em acompanhamento.
A seo 6.2 apresenta os resultados experimentais de acompanhamento das trajetrias pla-
nejadas na seo anterior. Os resultados experimentais foram levantados a partir da leitura dos
sensores embarcados no rob Pioneer 3AT.
Como dito no Captulo 2, o algoritmo RRT, devido sua natureza aleatria, tem como ca-
racterstica uma boa capacidade de explorao do ambiente. Para que o algoritmo alcance o
destino mais rapidamente, ao invs de sempre amostrar aleatoriamente um estado para o qual
a rvore dever crescer, o objetivo nal desejado escolhido com uma certa propabilidade p.
Esta polarizao do crescimento da rvore faz com que o objetivo seja alcanado com menores
iteraes mas tambm pode prender a expanso da rvore em um mnimo local. Nos problemas
de planejamento mostrados adiante, foi escolhido uma probabilidade de p = 0.15. Isto 85%
das tentativas de expanso da rvore sero aleatrias e 15% delas tero como valor o estado
nal desejado.
importante ressaltar que o planejador de movimento cinemtico-dinmico bsico utiliza
uma mtrica diferente da utilizada pelo planejador de movimento baseado em acompnhamento.
A subseo 5.2.3 descreve em detalhes como so escolhidos os estados expandidos que sero
adicionados rvore RRT para ambos os planejadores de movimento.
6.1 Planejamento de movimento
Foram escolhidos dois problemas de planejamento de movimento. No primeiro cenrio o
rob deve obrigatoriamente atravessar uma passagem estreita para atingir o estado nal dese-
jado. No outro cenrio o rob dever realizar um estacionamento paralelo, ou baliza, entre dois
56
obstculos. O objetivo realizar a comparao entre as solues geradas pelo planjeador de
movimento cinemtico-dinmico bsico e pelo planejador de movimento baseado em acompa-
nhamento.
Para o primeiro problema de planejamento, as passagens tem a largura de 1 m e a distncia
de tolerncia adotada foi de 15 cm, ento as passagens tem uma largura til de 70 cm, sendo
que a largura do rob P3-AT de 50 cm.
De posse das seguintes informaes: mapa do ambiente, estado inicial do rob, estado nal
desejado, nmero mximo de tentativas de expanso da rvore e distncia segura dos obstculos,
o mesmo problema de planejamento de movimento foi executado 100 vezes. O nmero mximo
de tentativas de expanso para cada planejamento max_trial = 13000. Um arquivo de log foi
gerado para cada planejamento. Como o mtodo RRT explora a caracterstica de aleatoriedade
na escolha de x
rand
para expandir a rvore, diferentes respostas para o mesmo problema de
planejamento de movimento podem ser encontradas.
Todo planejamento de movimento dito bem sucedido se o planejador de movimento con-
segue encontrar uma trajetria que leva o rob do estado inicial para o estado nal sem esgotar
o nmero mximo de tentativas de expanso.
O tempo mdio de um planejamento bem sucedido o tempo gasto para o algoritmo RRT
encontrar uma soluo que leva o rob do estado inicial ao estado nal sem colises. Caso no
haja soluo (planejamento mal sucedido) o tempo representar o quanto foi gasto em segundos
para esgotar o nmero mximo de iteraes do algoritmo.
Abaixo na tabela 6.1 temos uma comparao dos tempos gastos durante o planejamento
das trajetrias. A tabela compara os valores obtidos utilizando o planejador de movimento
cinemtico-dinmico bsico e a abordagem proposta neste trabalho, o planejador de movimento
cinemtico-dinmico baseado em acompanhamento.
Tabela 6.1: Tabela de desempenho - Passagem estreita
Bsico Baseado em acompanhamento
Planejamentos bem sucedidos 82 73
Tempo mdio 2.44 s 9.23 s
Nmero mdio de ns inseridos 1008 4166
Planejamentos mal sucedidos 18 27
Tempo mdio 7.5 s 26.25 s
Tempo total 335 s 1383 s
Pode-se observar que o planejador de movimento cinemtico-dinmico bsico teve maior
desempenho, resolvendo 82 das 100 fases de planejamento de movimento. Isto deve-se ao fato
57
dos dois planejadores utilizarem mtricas diferentes durante a expanso da rvore. O planeja-
dor de movimento cinemtico-dinmico baseado em acompanhamento considera que o estado
nal desejado foi alcanado quando a posio e orientao esto dentro de um certo raio de
tolerncia e o erro entre a velocidade com que deseja-se alcanar o objetivo e a velocidade
nal planejada encontra-se dentro de um limite. Por outro lado o planejador de movimento
cinemtico-dinmico bsico leva em considerao apenas a posio e orientao como parme-
tros para decidir se o estado nal foi alcanado.
Para permitir que o usurio escolha qual das trajetrias planejadas ser a mais apropriada
para o problema de planejamento, os arquivos de log gerados para as diferentes trajetrias so
ordenados seguindo cinco critrios: distncia entre o estado nal alcanado pela expanso da
RRT e x
goal
, o tempo gasto de x
init
at x
goal
, a distncia percorrida, o tempo que o rob se
locomove com velocidade negativa e uma medida da mudana de orientao do rob ao longo
da trajetria.
As guras 6.1 e 6.2 mostram o resultado do planejamento de movimento para um mesmo
problema de passagem estreita, utilizando as abordagens de planejamento dinmico e baseado
em acompanhamento. O critrio de seleo utilizado para a escolha das solues foi a distncia
nal do estado nal desejado.
Figura 6.1: Passagem estreita - Dinmico
O segundo problema de planejamento que consiste em realizar um estacionamento paralelo
ou baliza, a distncia livre entre os obstculos de 1,20 m. Levando em considerao a mesma
distncia segura mantida dos obstculos, o rob realiza o estacionamento em um espao til de
90 cm. O nmero mximo de tentativas de expanso para cada planejamento max_trial =
20000. Tambm foram realizadas 100 tentativas de planejamento de movimento.
58
Figura 6.2: Passagem estreita - Baseado em acompanhamento
Abaixo na tabela 6.2 temos uma comparao dos tempos gastos durante o planejamento
das trajetrias. A tabela compara os valores obtidos utilizando o planejador de movimento
cinemtico-dinmico bsico e a abordagem proposta neste trabalho baseada em acompanha-
mento.
Tabela 6.2: Tabela de desemepenho - Estacionamento paralelo
Bsico Baseado em acompanhamento
Planejamentos bem sucedidos 37 51
Tempo mdio 3.72 s 12.16 s
Nmero mdio de ns 1384 4618
Tempo total 605 s 1849 s
Dentre os 100 planejamentos realizados para solucionar o problema de estacionamento pa-
ralelo, escolheu-se uma soluo para cada abordagem. As solues escolhidas foram as que
levaram o menor tempo entre o estado inicial e o objetivo e podem ser vistas nas guras 6.3 e
6.4.
Com base nas estatsticas de ambos problemas pode-se concluir que o planejamento de
movimento baseado em acompanhamento mais custoso em termos computacionais na ordem
de 5 vezes. Isto deve-se ao fato de que o nmero de ns adicionados em um planejamento bem
sucedido baseado em amostragem est prximo a 5 vezes o nmero de ns adicionados para um
planejamento de movimento cinemtico-dinmico bsico.
O nmero de ns presentes na rvore tem impacto no algoritmo 2, onde h a necessidade
de descobrir qual n que est presente na rvore encontra-se mais prximo do estado aleatrio
amostrado. Desta maneira quanto maior o nmero de ns que a rvore possuir mais iteraes
59
Figura 6.3: Estacionamento paralelo - Dinmico
Figura 6.4: Estacionamento paralelo - Baseado em acompanhamento
sero feitas na busca de x
near
. Alm disso ao tentar adicionar um novo n e uma nova aresta na
rvore, realizada uma vericao a m de evitar ns e arestas duplicadas, ento todos os ns
e aretas da rvore so visitados para garantir que no haja ns/arestas repetidos.
6.2 Acompanhamento das trajetrias planejadas
6.2.1 Planejador de movimento cinemtico-dinmico
Foram tomadas duas solues aleatrias geradas pelo planejador de movimento que leva
em considerao a dinmica do rob. As guras 6.5 e 6.6 mostram a execuo das trajetrias
executadas pelo rob Pioneer 3AT.
60
Para a trajetria representada pela gura 6.5 os erros mximos ao longo da execuo so:
|x
e
| = 25 cm, |y
e
| = 23 cm e |
e
| = 17 graus. Os erros entre o estado nal desejado (objetivo)
e o estado nal alcanado so: x
e
= 2.4 cm, y
e
= 6.2 cm e
e
= 2 graus.
Figura 6.5: Passagem estreita - Resultado experimental
Figura 6.6: Estacionamento paralelo - Resultado experimental
Para a trajetria representada pela gura 6.6 os erros mximos ao longo da execuo so:
|x
e
| = 12 cm, |y
e
| = 18 cm e |
e
| = 9.3 graus. O erro entre o estado nal desejado (objetivo)
e o estado nal alcanado so: x
e
= 10.4 cm, y
e
= 10.2 cm e
e
= 9 graus.
61
6.2.2 Planejador de movimento cinemtico-dinmico baseado em acom-
panhamento
Os testes experimentais tambm foram realizados para as trajetrias planejadas utilizando a
abordagem de planejamento de movimento baseado em acompanhamento. A gura 6.7 mostra
a execuo da trajetria para o problema de passagem estreita e a gura 6.8 mostra os erros de
acompanhamento ao longo da trajetria.
Figura 6.7: Passagem estreita - Resultado experimental
Figura 6.8: Passagem estreita - Resultado experimental
Os erros mximos ao longo do acompanhamento so: |x
e
| = 16 cm, |y
e
| = 17 cm e |
e
| =
17 graus. Os erros entre o estado nal desejado e o estado nal alcanado so: x
e
= 8.2 cm,
y
e
= 2.9 cm e |
e
| = 3 graus.
62
Os parmetros utilizados na fase de planejamento de movimento, que representam o rob
Pioneer 3-AT da tabela 6.3, foram extrados de (KRISHNAMURTHY, 2008).
Tabela 6.3: Parmetros do Pioneer 3-AT.
Parmetro Valor Unidade
I 0.413 Kgm
2
m 40 Kg
f
r
0.043
0.506
x
CIR
0.008 m
a 0.138 m
b 0.122 m
c 0.1975 m
r 0.4 m
63
7 Concluso
Esse trabalho apresenta a implementao de um planejador cinemtico-dinmico de mo-
vimentos, baseado em RRT. Os resultados experimentais apresentados utilizam o rob mvel
de rodas deslizantes Pioneer 3-AT. Os experimentos mostraram que o planejador cinemtico-
dinmico de movimentos, rrt-motion-planner, pode ser usado para solucionar diferentes
tipos de problemas de planejamento, tais como estacionamento paralelo e realizar manobras por
passagens estreitas.
O software implementado pode ser estendido para qualquer classe de rob, bastando conhe-
cer o conjunto de equaes diferenciais ordinrias que regem a transio de estados do modelo
do rob e as caractersticas fsicas do rob.
A abordagem proposta neste trabalho, denominada Planejamento de Movimento Baseado
em Acompanhamento tem como caracterstica central utilizar o mesmo controlador tanto no
planejamento do movimento quanto no acompanhamento das trajetrias planejadas. Esta abor-
dagem permite levar em considerao o efeito da malha de controle ainda na fase de planeja-
mento.
Os resultados experimentais tem o propsito de fechar o ciclo que envolve as fases de
planejamento de movimento e acompanhamento de trajetria. Foram conduzidos duas clas-
ses de experimentos: a primeira classe de experimentos utiliza um planejador de movimentos
cinemtico-dinmico baseado em RRT, cujas entradas de controle para o modelo dinmico, que
representa o rob, so os torques aplicados nas rodas do rob.
Asegunda classe de experimentos utiliza umplanejador de movimentos cinemtico-dinmico
baseado em acompanhamento, cujas entradas de controle para o modelo dinmico so as ace-
leraes, linear e angular, a serem aplicadas no rob. Na segunda classe de experimentos h
uma certa vantagem em termos de entradas de controle, pois o conceito de acelerao mais
comumente utilizado no dia a dia.
Durante a fase de planejamento o planejador de movimentos baseado em acompanhamento
consumiu maiores recursos de memria e tempo de processamento. Embora o planejamento
64
baseado em acompanhamento seja mais lento, as solues por ele alcanadas possuem algumas
caractristicas que auxiliam na tarefa de execuo da trajetria planejada. H a possibilidade
de estabelecer uma velocidade nal desejada que o rob deve alcanar o objetivo e pode-se
denir uma velocidade mdia desejada ao longo da trajetria. Todas as transies de estados
so validadas atravs do modelo dinmico do rob.
Omodelo usado pelo planejador de movimentos baseado emacompanhamento possui como
entradas as aceleraes, linear e angular, aplicadas ao rob real, parmetros que podem ser le-
vantados com maior facilidade do que a gerao de tabelas de torque anteriormente utilizadas
pelo planejador dinmico. A principal vantagem do planejamento baseado em acompanha-
mento que os efeitos das aes de controle aplicados na etapa de acompanhamento da trajet-
ria so levados em considerao ao se planejar os movimentos.
Para o problema de passagem estreita o planejador de movimento cinemtico-dinmico
teve melhor desempenho. Isto deve-se caracterstica da mtrica utilizada pelo planejador
de movimento cinemtico-dinmico bsico. Como tal planejamento de movimento no leva em
considerao a velocidade desejada no estado nal, considerado que o estado nal foi atingido
quando qualquer estado que esteja dentro de um raio de tolerancia levando em conta apenas
a posio e orientao do rob alcanado. J o planejamento de movimento cinemtico-
dinmico tenta alcanar o estado nal atingindo a velocidade desejada, neste caso igual a zero.
Assim mais uma restrio imposta para garantir que o estado nal desejado foi alcanado.
Para a soluo do problema de estacionamento paralelo o planejamento de movimento ba-
seado em acompanhamento mostrou um melhor desempenho se comparado ao planejamento de
movimento cinemtico-dinmico bsico. Pode-se inferir que isto deve-se ao fato de que a ve-
locidade no estado nal desejado levado em considerao quando utilizado o planejamento
baseado em acompanhamento. Desta maneira ao aproximar-se do destino apenas estados cuja
velocidade esteja prxima de zero so utilizados para expandir a rvore. Isto garante que o
rob no entrar com uma velocidade demasiadamente grande na vaga de estacionamento, o
que poderia causar uma coliso com os obstculos.
Ambos experimentos foram realizados com o rob P3-AT. Os erros entre estado nal pla-
nejado e estado nal acompanhado mostraram-se com o mesmo desempenho para as trajetrias
geradas por ambos planejadores de movimento. No entanto os caminhos obtidos com o plane-
jador de movimentos baseado em acompanhamento mostraram-se mais suaves.
65
Referncias Bibliogrcas
BRUCE, J.; VELOSO, M. Real-time randomized path planning for robot navigation. In:
Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference on. [S.l.: s.n.], 2002.
v. 3, p. 2383 2388 vol.3.
CARACCIOLO, L.; LUCA, A. D.; IANNITTI, S. Trajectory tracking control of a four-wheel
differentially driven mobile robot. In: IEEE International Conference on Robotics &
Automation. Detroit, Michigan: [s.n.], 1999.
CHOSET, H.; BURGARD, W.; HUTCHINSON, S.; KANTOR, G.; KAVRAKI, L. E.;
LYNCH, K.; THRUN, S. book. Principles of Robot Motion: Theory, Algorithms, and
Implementation. [S.l.]: MIT Press, 2005.
DONALD, B.; XAVIER, P.; CANNY, J.; REIF, J. Kinodynamic motion planning. J. ACM,
ACM, New York, NY, USA, v. 40, n. 5, p. 10481066, 1993. ISSN 0004-5411.
Egervry Research Group on Combinatorial Optimization. LEMON Graph Library. 2010.
Disponvel em: <http://lemon.cs.elte.hu/trac/lemon>.
FERGUSON, D.; STENTZ, A. Anytime rrts. In: Proceedings of the 2006 IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS 06). [S.l.: s.n.], 2006. p.
5369 5375.
GERKEY, B. P.; VAUGHAN, R. T.; HOWARD, A. The player/stage project: Tools for
multi-robot and distributed sensor systems. In: In Proceedings of the 11th International
Conference on Advanced Robotics. [S.l.: s.n.], 2003. p. 317323.
KOZLOWSKI, K.; PAZDERSKI, D. Modeling and control of a 4-wheel skid-steering mobile
robot. International Journal of Applied Mathematics and Computer Science, v. 14, n. 4, p.
477496, 2004.
KRISHNAMURTHY, D. A. Modeling and simulation of skid steered robot Pioneer - 3AT.
Dissertao (Mestrado) Florida State University College of Engineering, 2008.
LARSEN, E.; GOTTSCHALK, S.; LIN, M. C.; MANOCHA, D. Fast distance queries with
rectangular swept sphere volumes. In: Robotics and Automation, 2000. Proceedings. ICRA
00. IEEE International Conference on. [S.l.: s.n.], 2000. v. 4, p. 37193726 vol.4.
LAVALLE, S. M. Planning algorithms. [S.l.]: Cambridge University Press, 2006.
LAVALLE, S. M.; KUFFNER, J. J. Rapidly-exploring random trees: Progress and prospects.
In: Algorithmic and Computational Robotics: New Directions. [S.l.: s.n.], 2000. p. 293308.
LAVALLE, S. M.; KUFFNER, J. J. Randomized kinodynamic planning. The International
Journal of Robotics Research, v. 20, n. 5, p. 378400, May 2001.
66
NOURANI-VATANI, N.; BOSSE, M.; ROBERTS, J.; DUNBABIN, M. Practical path planning
and obstacle avoidance for autonomous mowing. In: In Proc. of the Australasian Conference
of Robotics and Automation. [S.l.: s.n.], 2006.
RODRIGUEZ, S.; TANG, X.; LIEN, J.-M.; AMATO, N. An obstacle-based rapidly-exploring
random tree. In: Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE
International Conference on. [S.l.: s.n.], 2006. p. 895 900. ISSN 1050-4729.
VALLEJO, D.; JONES, C.; AMATO, N. M. An adaptive framework for single shot motion
planning: A self-tuning system for rigid and articulated robots. In: In Proceedings of the IEEE
International Conference on Robotics and Automation. [S.l.: s.n.], 2001. p. 2126.
VAZ, D. Alves Barbosa de O.; INOUE, R. S.; GRASSI, V. Kinodynamic Motion Planning
of a Skid-Steering Mobile Robot Using RRTs. In: Robotics Symposium and Intelligent
Robotic Meeting (LARS), 2010 Latin American. [s.n.], 2010. p. 7378. Disponvel em:
<http://dx.doi.org/10.1109/LARS.2010.27>.