Estimação em Tempo Real de Posição e Orientaçãode Robôs Móveis Utilizando Sensores com Diferentes

Taxas de AmostragemFranz Alberto Sandi Lora'

Elder M. Hemerly/Waiter Fetter Lages'

Departamento de Controle e Conversão de EnergiaInstituto Tecnológico de Aeronáutica - ITA

CTA - ITA - IEEE 12228-900 São José dos Campos SP - Brasillfranz@ele.ita.cta.br2hemerly@ele.ita.cta.br

3w . f e t t e r @i e e e. o r g

Abstract: A location system for a self-navigation vehicle is presented. This location system has twoindependent measuring subsystem that are combined via a Kalman filter. The first is a ' dead-reckoning subsystem based on rotation counts of right and left vehicle's wheels, and the second is adigital compassoExperimental results show that accurate estimates, in orientation and position, areobtained by the system.

Resumo: Apresenta-se neste artigo um sistema de localização para 'navegação de veículosautônomos, baseado na integração via filtro de Kalman de dois subsistemas independentes. Oprimeiro é o dead-reekoning, que utiliza a leitura de pulsos de dois eneoders alocados nas, rodasdireita e esquerda do veículo, e o segundo é uma bússola digital. Resultados experimentais mostramque boa acurácia, em orientação e posição, é obtida mediante esta integração.

I. IntroduçãoA automatização de tarefas via robótica móvel, comdiversas aplicações [I] e [2], requer o conhecimentoda posição do veículo (coordenadas espaciais doveículo em relação a um sistema de referênciadeterminado). Inúmeros trabalhos têm tratado doproblema da navegação autônoma usando o métodoclássico dead-reekoning, cuja precisão dependediretamente da qualidade dos sensores utilizados,sendo impossível evitar a acumulação de erro deposição. Assim, diversas técnicas de integração desensores têm sido utilizadas para compensar este erro.

Em [3] integra-se a informação do número depulsos detectados por eneoders instalados nas rodas doveículo (odômetro) com a informação deacelerômetros e girômetros solidários ao veículo. Em[4] e [5] empregam-se algoritmos de processamento deimagens que integrados à informação de eneodersestimam posição e orientação, mostrando-se que anavegação baseada só na informação de eneodersapresenta a sua maior deficiência no cálculo da

orientação. Em [6] apresenta-se um 'método quecombina as informações de um girômetro ,e umodômetro, de modo a se estimar a orientação de umveículo móvel, mas corno são integrados dois sensoresque apresentam erros crescentes no tempo, énecessária uma modelagem 'estatística de ambos ossensores.

Neste trabalho integra-se a informação deorientação fornecida por um odômetro com ainformação de uma bússola digital mediante um filtrode Kalman . Esta orientação estimada é então utilizadapara calcular a posição do veículo em cada instante detempo.

11.Estimação de OrientaçãoA plataforma móvel utilizada, descrita em detalhes em[7] e apresentada na Fig. I, é movimentada através' deduas rodas dianteiras operando em modo diferencial.Cada uma possui um eneoder que permite medir asua velocidade angular. Na parte traseira 'existe umaterceira roda de apoio que gira livremente. Uma

453

o

Fig. 1- Estrutura do veículo implementado.

(12)

(13)

Na Fig. 3"tem-se:Pc : interseção entre o eixo de simetria e das rodas.

: deslocamento da roda esquerda no período deamostragem.: deslocamento da roda direita no período deamostragem.

Xc , Yc : coordenadas do veículo com origem em Pc.X, Y : coordenadas do sistema de referência.4> : ângulo medido entre "o eixo de simetria da

plataforma e o eixo X.b : distância entre as rodas e o eixo de simetria.

De forma similar a [4] e admitindo-se somente aexistência de movimento na direção do "eixo desimetria, obtemosyc coso - Xc sen 4> = O (1)

onde (y c' xc) são as coordenadas de Pc em relação àscoordenadas de referência (X, Y).

Considerando-se que as rodas do veículo nãodeslizam, advémédr = (2)

éer = Yc seno + xccos4>- (3)onde r é o raio das rodas, Sd e Se são osdeslocamentos angulares das rodas direita e esquerda,respectivamente.

Subtraindo-se (3) de (2) obtém-se. édr-é r (4)4> = e

2be somando-se (2) e (3) resulta

. . édr+éer. (5)xc cos 4> + Yc sen 4> = = D

2onde D é a velocidade do veículo na direção do seueixo de simetria.

Resolvendo-se (1) e (5) para xce Yc obt ém-se

Xc =Dcos o (6)

Yc =Dsen 4> (7)

4> = O) (8)

onde O) é a velocidade angular de Pc com relação àscoordenadas (X,Y) .

Discretizando-se (6), (7) e (8), de forma similar a[8], e assumindo-se que a trajetória percorrida por Pc éum arco de circunferência conforme Fig. 3, temos

Xc (k + I) = Xc (k) + + (9)2

Yc(k + I) = Yc (k) + + O)2

4>(k + 1) = 4>(k) + (11)onde

= m(k)

= (k) + (k)2

BÚSSOLA

y

Fig. 2- Foto do lEEE-Rover.

x

bússola digital é montada no centro do eixo das rodasdianteiras, permitindo medir aorientação do veículo.

Uma foto do robô móvel pode ser vista na Fig. 2.

Il.I, Dead-Reckoning com o OdômetroO modelo cinemático do veículo é desenvolvido combase na Fig. 3.

Fig. 3- Representação do sistema de coordenadas da plataforma"

454

1.60

(21)filtro de

+

1.20

OdômctroJBúuolavía filtro de Kalman

Val orverdadelr c

Filtro de Kalman .ru ------u··------.- ------1, ..

0.40 0.80Poslção X Im!

:+

, ,1.. • ...1

,,.-- --. -----_.. ----- ----_.'

I 0 .80

>-o

o 0.40c,

0 .00

.Q.40

0.00

Bússola

: + :, + ' -:........i $o

1.20

1.60

Fig . 4· Diagrama de blocos da integração OdômetrolBússo!a.

,------------------------,, ,. ,, ,, ,

Odômctro

onde t (k) é o valor verdadeiro do incremento de.orientação.

Considerando-se que ' as magnitudes em (11) sãoverdadeiras, subtraindo-se (20) de (11) e utilizando(18) obtemos o erro de orientação, dado por

ô<j>(k+1) = ô<j>(k) + w(k)que constitui a equação de estado para oKalman.Assim, o filtro de Kalman linear é escalar com os

ruídos de medida, v(k), e de estado, w(k) , não-correlacionados, permitindo-nos estimar o erro deorientação, para a seguir subtraí-lo do valor calculadoa partir dos encoders, como é mostrado na Fig. 4. Aorientação corrigida é retroalimentada para calcular aposição, de modo a se reduzir a acumulação de erro.

Fig. 5- Simulação do odômetro e da integração Odômetro/Büssola.

111. Resultados de SimulaçãoA integração dos sistemas foi simulada considerando-se uma trajetória que começa em (0,0) e forma urnacircunferência no plano de 1,37 m de raio. Estemovimento é em sentido anti-horário, conformeindicado na Fig . 1. A Fig. 5 mostra a trajetória doponto Pc só com o odômetro 'e a trajetória utizando-seo filtro de Kalman, para uma velocidade constante doveículo de 0,14 m/s . A Fig. 6 mostra as diferentesorientações.

= (14)2b

Note-se que quando = 0, = m(k)não havendo indeterminação matemática em (12).

As expressões e em (13) e (14)representam respectivamente os deslocamentoslineares das rodas direita ' e esquerda no intervalo[k,k+1], e são dadas por

(k) = 2nr NPd(k) (15)n

= 2m NPe(k) (16)n

onde n é o número de pulsos por revolução dosencoders, e NPd (k) e NPe(k) o número de pulsosdetectados no intervalo [k,k+ 1].

As expressões (9), (10) e (1I) foram desenvolvidasconsiderando-se uma trajetória circular, mas sãotambém exatas quando a trajetória é uma reta.

Existem, no entanto, erros de modelagem devidoao escorregamento das rodas, às irregularidades dosolo, à não exatidão no conhecimento do raio dasrodas , à distância entre elas, etc., que são estudadoscom maior detalhe em [9].

11.2. Orientação com Bússola DigitalO cálculo de orientação baseado na leitura de umabússola digital <!>B (k) é uma das soluções empregadasmais elementares, devido ao modelo simples queapresenta: o valor verdadeiro <!>t (k) é corrompido poro ruído de medida v(k), isto é,

<!>B (k) = <!>t (k) + v(k) (17)Neste experimento foi utilizada a bússola digital

Vector 2X Compass Module fabricada pela PrecisionNavigation, Inc. , cujas especificações estão em [10].

11.3. Integração dos SistemasA integração de ambos os sistemas, odômetro ebússola, foi realizada mediante um filtro de Kalmanlinear. A orientação calculada com base nos encoders,<!>E (k), pode ser modelada como o valor verdadeiro<!>t (k) mais um erro de orientação ô<!>(k) , i.e.,

<!>E (k) = <!>t (k) + ô<j>(k) . (18)A medida, y(k), é a diferença de orientação entre

ambos os sistemas, (17) e (18). Portanto,desconsiderando-se o signo do ruído branco v(k),temosy(k)=Ô<!>(k)+v(k) (19)Supondo-se que ruído branco w(k) é adicionado ao

incremento de orientação em cada instante de tempo , aorientação <!>E (k + 1) é calculada com base em (l l ) daseguinte forma

<!>E (k + 1)= <!>E (k) + (k) +w(k) (20)

455

Fig. 8- Diagrama de temporização dos encoders .

Percebe-se, então, que a leitura da bússola nãopode ser feita pelas técnicas convencionais de busy-wait, pois, devido ao seu tempo de processamento emtorno de 13Sms, isto faria com que os encoders nãopudessem ser processados a cada SO ms . Necessita-seportanto de um ambiente multitarefa. Neste caso, aleitura da bússola e a leitura dos encoders são feitaspor tarefas periódicas diferentes, cada uma com seutempo de amostragem. Quando uma tarefa necessitaefetuar um delay ela é posta para dormir durante estetempo, liberando o processador para as demais tarefas.

Este ambiente multitarefa foi implementadoutilizando-se o sistema operacional Linux[ I I] . Éimportante notar, porém, que as tarefas não foramimplementadas como processos, mas sim comothreads[ I2]. Processos operam com espaços deendereçamento diferentes entre si, enquanto threadscompartilham o mesmo espaço de endereço, além devários outros recursos. Uma conseqüência direta destefato é que a troca de dados entre thréads é feita maisfacilmente do que entre processos.

O programa implementado possui quatro threads:I) leitura da bússola, com período de amostragem de200ms; 2) leitura dos eneoders, com período deamostragem de SOms; 3) fusão de dados, sem períodode amostragem fixo. mas sincronizado com os doisthreads anteriores; 4) interface com o usuário, comperíodo de amostragem de Is . Este thread possui a

portas de 1/0, portanto é executado em no maximoalguns us, Os números indicam os delays necessáriosem ms. O símbolo ::r indica o tempo restante dentro doperíodo de amostragem da bússola de 200ms. Note queembora o processamento para leitura da bússolademore em torno de 13Sms, o processamento énecessário apenas durante breves intervalos de algunsus . A maior parte do tempo necessário para leitura dabússola é constituído por delays.

Para implementação em tempo real do sistemaproposto é necessário também efetuar a leitura dosencoders, cujo diagrama de temporizaçã0.rode servisto na Fig . 8. Novamente, o símbolo 11 indicaprocessamento pelo da ordem dealguns us, e o símbolo ':1indica o tempo restantedentro do período de amostragem dos eneoders, que éde SOms.

16.0012.008.00Tempo (si

4 .00

Fig. 6- Simulação da orientação

0.00

IV. Implementação em Tempo Real,A fusão de dados de diversos sensores utilizando ofiltro de Kalman é uma técnica que tem sido bastanteutilizada para obter-se uma estimativa mais confiáveldas grandezas medidas. É comum assumir-se quetodos os sensores são capazes de fornecer medidasperiodicamente e que este período é constante e igualpara todos os sensores.

No entanto, quando são utilizados sensores comdiferentes princípios físicos, é comum a situação ondecada sensor exige uma taxa de amostragem diferente.Nestes casos, · a simulação do sistema, ou umaimplementação da fusão de dados off-line não é muitodiferente -do caso com uma única taxa de amostragem.Porém,' uma implementação para fusão dos dados emtempo . real apresenta uma série de problemasrelacionados ao processamento para aquisição dedados de cada sensor, com o sincronismo entre ossensores e destes com o filtro de Kalman.

O diagrama de temporização para acionamento dabússola utilizada pode ser visto na Fig. 7. O símbolo Iindica processamento pelo microcomputador queconsiste apenas da leitura ou escrita de alguns bits nas

0.00 -jL- --,--- ,--------,,-----,- ---,-- --.-- ,------,

0.40

1.20

oc-rn

5 ·O.BO

1.60

2.00

Utilizou-se um período de amostragem TE=O,OSSnos eneoders e T8=0,2s na bússola. O algo ritmo dofiltro de Kalman requer o conhecimento das variânciasdos ruídos de estado e medida Q e R,respectivamente. No que se refere ao ruído de medida,v(k) em (19), sua variância pode ser obtida do manual

_ do usuário[IO], resultando R=0.0698rad2. Quanto aoruído de estado, w(k) em (21), utilizou-se o valorQ=0.08 rad", pois este foi o valor adequado obtidoexperimentalmente, vide seção V.

80 - 100espera leitura (16 bits) desabili

ta ãolivre

Fig. 7- Diagrama de temporização da bússola.

456

-------

c--;Ii controle dos motores

sem posu&(data->cps_data-»ackj):Ii filtragem

Na Fig. 9 são mostrados os períodos deamostragem da bússola e dos encoders. O nível lógicodo sinal é invertido a cada amostragem. Note que aamostragem da bússola não é realizada emsincronismo com a amostragem dos encoders, mas émantida a razão de uma amostragem da bússola paracada quatro amostragens dos encoders.

1,.. 1 LW1JlIUlr

6T=B .Bms.S AVE

TEKTRONIX 2232..U1= 'l .6YU TRIG =-=B .5V..V2='l.72V

menor prioridade e portanto executa apenas quandoos demais threads não necessitam utilizar oprocessador.

O sincronismo entre os threads é realizado atravésde semáforos[13]. Semáforos são vari áveiscontroladas pelo sistema operacional sobre as quaissão definidas as operações post e wait (além dainicialização). A operação wait testa o valor dosemáforo. Se o semáforo é zero, o thread é posto paradormir. Caso contrário, o valor semáforo édecrementado e o. processo continua normalmente. Aoperação post acorda um processo que estejadormindo no semáforo ou, caso não haja nenhumprocesso dormindo, o valor do semáforo . éincrementado.Aqui semáforos são utilizados para garantir: a)

que a operação do sistema em tempo real comeceapenas após todos os threads terem completado a suainicialização, e b) que a fusão de dados ocorre apenasquando os dados da bússola e dos encoders estãodisponíveis , sincronizando a operação entre osthreads de leitura dos sensores e o thread de fusão dedados. Em linhas gerais, os threads de leituras dossensores seguem o algoritmo resumido abaixo (noteque os semáforos são inicializados com valor zero):

void compass(struct cps_param *data){

Ii inicializaçãoserrr postf&data->ready);sem_wait(&data->start) ;for(;;){

Ii leitura do sensorsernposn&data->avail);Ii dormir até próximo período de amostragemsem_wait(&data->ack);

Fig. 9- Períodos de amostragem da bússola e dos encoders.

O sincronismo é feito no thread de fusão de dados,como pode ser verificado na Fig. 10, onde é mostradoo sinal gerado pelo thread de leitura da bússola e osinal gerado pelo thread de fusão de dados.Novamente tem-se uma inversão do nível lógico dosinal por loop da rotina . Note que o período deexecução da fusão de dados não é mantidoexatamente em SOms, variando ligeiramente paraforçar o sincronismo entre os sensores.

Fig. 10- Temporização da bússola e da fusão de dados .

V. Resultados ExperimentaisNavegou-se em uma trajetória circular, com raiomantido constante via elo rígido, de um ângulo de90°. A trajetória nominal percorrida pelo veículo foientão calculada. As covariâncias Q e R foram asmesmas utilizadas na seção Ill, sendo que Q foideterminado experimentalmente, pois o ruído de

6 T =B .Bm sSAVE

.2U

TE KTRONI X 2232.. U1= Y .Ô'lU TRIG.. V2=Y. 72 u

T

KI2 U

sem_wait(&(data-c-cpsjíata-c-availl);Ii processamento da bússola

sem_wait(&(data->enc_data->avaiI»;Ii processamento dos encoderssernposu&(data-c-encjíata-c-ackj);if(c==O){

Ii inicializaçãoint c=3 ; Ii c = contador de sincronismosemposu&data->ready);sem_wait(&data->start);for(;;){

O thread de fusão de dados executa o seguintealgoritmo:

void fusion(struct fus_param *data){

457

amostragem diferentes, o que motivou odesenvolvimento de um software em ambientemultitarefa no sistema operacional Linux[lI] .

A utilização da orientação e posição determinadasneste trabalho em um sistema de controle tipo fuZ2Y,para guiagem de robôs móveis, será reportadaoportunamente.

Referências[1] S. Asami, "Robots in Japan: Present andFuture", IEEE Robotics and AutomationMagazine, Voi. 1, No. 2, June 1994.

[2] R. D. Schraff, "Mechatronics and Robotics forService Applications", IEEE Robotics andAutomation Magazine, Voi. 1, No. 4, Dec ., 1994.

[3] Y. Fuke and E. Krotkov, "Dead Reckoning for a'Lunar Rover on Uneven Terrain", Proceedingsof the 1996 IEEE Inte rnational Conference OnRobotics and Automation, Minneapolis,Minnesota, April, 1996.

[4] S. Murata and T. Hirose , "Onboard LocatingSystem Using Real-Time Image Processing fora Self-Navigation Vehic1e", IEEE Trans. onIndustrial Electronics, Voi. 40, No. 1, Feb., 1993.

[5] W. F. Lages, E. M. Hemerlye L. F. A. Pereira,"Controle Línerizante de uma PlataformaMóvel Empregando Realimentação Visual", XICBA , São Paulo, 1996.

[6] J. Borenstein and L. Feng, "Gyrodometry: ANew Method for Combining Data from Gyrosand Odornetry in Mobile Robots", Proc . of the1996 IEEE International Conference on Roboticsand Automation, Minneapolis, April 22-28, 1996.

[7] E. M. Hemerly e C. .Rodrigues, "Guiagem deVeículos Autônomos Utilizando Sensor deVisão", /0 0CBA , Rio de Janeiro, 1994 .

[8] Y. Yamamoto, & X. Yun, "CoordinatingLocomotion and Manipulation of a MobileManípulador", IEEE Trans . on AutomaticControl , Voi. 39, No. 6, June 1994.

[9] J. Borenstein and L. Feng , "UMBmark - AMethod for Measuring, Comparing, andCorrecting Dead-reckoning Errors in MobileRobots", University of Michigan TechnicalReport UM-MEAM-94-22 , December, 1994.

[lO] Vector 2X Compass Module, "ApplicationNotes", Precision Navigation, Inc. , Version 1.03,january 1996.

[11] V. Yodaiken, "Cheap Operating SystemsResearch and Teaching with Linux", disponívelon-line em http://luz.cs.nmt.edul-rtlinux.

[12] A. Tanenbaum, "Modero Operating Systems",Prentice-Hall: Englewood Cliffs, 1992 .

[13] M. Ben-Ari, "Principies of ConcurrentProgramming" Prentice-Hall : EnglewoodCliffs , 1982.

1.60

20.0016.00

1.200.80Posição À [m]

8.00 12.00Tempo (si

0.40

4.00

0.00

0.00

Fig. 12- Orientações obtidas em tempo real.

1.20

I>-o

0.80

c,

0.40

1.20

o.ao Valor Bli u u lanom ina l

o

m

-c; 0.40O

0.00

Note-se que a posição estimada oscila em torno datrajetória nominal crescendo com o tempo (devido àacumulação do erro de posição), mas sempre estámais perto do valor nominal do que o odômetro. AFig. 12 mostra as várias orientações relativas a estarealização.

1.60

1.60

0.00 -+---''':==---,..----,----,-----.-- --.--..,.----,

Fig. 11- Posicão calculada pelo odômetro e pela integraçãoOdômetrolBússola em tempo real. '

VI. ConclusõesFoi apresentado neste artigo um sistema de estimaçãode -orientação e posição para veículos autônomos,utilizando a integração, via filtro de Kálman, de .orientação fornecida por uma bússola digital[10] coma orientação calculada a partir de um odômetro comencoders. Os subsistemas apresentam períodos de

estado depende do ruído na leitura dos encoders, eesta dependência é dada por (14) , onde apareceinclusive a distância b entre as rodas e o eixo desimetria. Desta forma, convém determinar estavariância via experimentação. O valor adequadoassim obtido foi Q=0 .08rad2. A Fig . Ii mostra a. trajetória nominal e as posições calculadas com basena integração odômetro/bússola e com o odômetroapenas.

458