DE UM ROBO MOVEL LOCALIZAC AO BASEADA EM ODOMETRIA E MARCOS NATURAIS UTILIZANDO FILTRO DE KALMAN EXTENDIDO M. Santana , Adelardo A.D.

A.D. Medeiros Andre

Departamento de Engenharia de Computa ca o e Automa ca o - DCA Universidade Federal do Rio Grande do Norte - UFRN Natal,RN,Brasil Email: [andremacedo,adelardo]@dca.ufrn.br

Abstract This work proposes a localization system for mobile robots using the Extended Kalman Filter. The robot navigates in an known environment where the lines of the oor are used as natural landmarks and identiqued by using the Hough transform.The prediction phase of the Kalman lter is implemented using the odometry model of the robot. The update phase directly uses the parameters of the lines detected by the Hough algorithm to correct the robots pose. Keywords Robot Localization , Kalman Filter, Sensor Fusion.

Resumo Este trabalho prop oe um sistema de localiza ca o para rob os m oveis com acionamento diferencial utilizando Filtro de Kalman Extendido. O sistema em quest ao considera que o rob o navega em um ambiente conhecido onde as arestas do piso podem ser identicadas como marcos naturais utilizando a transformada de Hough.A fase de predi ca o do ltro e implementada usando o modelo de odometria do rob o e a fase de atualiza ca o usa os par ametros da linhas detectadas por Hough diretamente nas suas equa co es para corrigir a pose do rob o. Palavras-chave Localiza ca o de Rob os, Filtro de Kalman, Fus ao Sensorial.

Introdu c ao

Borenstein et al. (1997) classicam os m etodos de localiza ca o em duas grandes categorias: m etodos de localiza ca o relativa, que d ao a pose do rob o em rela ca o a uma pose inicial e m etodos de localiza ca o absoluta, que d ao a pose global do rob o e n ao necessitam das posi co es calculadas anteriormente. No caso de rob os com rodas e comum o uso de encoders acoplado aos eixos de rota ca o das rodas, na t ecnica denominada odometria (Borenstein et al., 1997). Entretanto, a id eia b asica da odometria e a integra ca o da informa ca o da movimenta ca o em um dado per odo de tempo, o que leva ao ac umulo de erros (Park et al., 1998). As t ecnicas de localiza ca o absoluta utilizam marcos para localizar o rob o. Estes marcos podem ser articiais, quando introduzidos no ambiente com a nalidade de auxiliar na localiza ca o do rob o, ou naturais, quando j a se encontram no importante ressaltar que mesmo as ambiente. E t ecnicas de localiza ca o absolutas s ao imprecisas devido aos ru dos oriundos dos sensores utilizados. A literatura mostra trabalhos utilizando medidas de dist ancia a marcos naturais (paredes por exemplo) para localizar o rob o. A obtenc a o destas medidas geralmente e feita utilizando sonar, laser e vis ao computacional (Lizzaralde et al., 2003; Kim e Kim, 2004; P eres et al., 1999). Bezerra (2004) utilizou no seu trabalho as retas do piso que comp oem o ambiente como marcos naturais. Kiriy e Buehler (2002) utilizaram ltro de Kalman extendido para seguir diversos marcos articiais dispostos de forma n ao estruturada. Launay et al. (2002) usaram as l ampadas do teto de um corredor para localizar o rob o.

O presente artigo prop oe um sistema capaz de localizar um rob o m ovel em um ambiente onde as linhas do ch ao formam uma grade bidimensional. Para tal, as linhas s ao identicadas como marcos naturais e suas caracter sticas, juntamente com o modelo de odometria do rob o, s ao incorporados em um ltro de Kalman a m de obter sua pose. Al em desta se ca o, este documento possui outras quatro. A Se ca o 2 descreve a teoria b asica necess aria para entender o funcionamento do ltro de Kalman. A Se ca o 3 que trata da modelagem do problema. Os resultados encontrados s ao ilustrados na Se ca o 4 e, por m, a Se ca o 5 traz as conclus oes e as perspectivas deste trabalho. 2 Filtro de Kalman

A modelagem do Filtro de Kalman Discreto - FKD pressup oe que o sistema seja linear e descrito pelo modelo de equa co es do sistema 1: st = At st1 + Bt ut1 + t zt = Ct st + t (1)

onde s Rn e o vetor de estados; u Rl e o vetor das entradas de controle; z Rm e o vetor de medi co es; a matriz n n, A, e a matriz de transic a o de estados; B, n l, e a matriz de coecientes de entrada; a matriz C, m n, e a matriz de observa ca o; Rn representa o vetor de ru dos do processo e Rm o vetor de erros de medi ca o. Os ndices t e t 1 representam os instantes de tempo atual e anterior respectivamente. O ltro opera em modo de predi ca o-atualizac a o levando em considera ca o as propriedades estat sticas do ru do. Um modelo interno do sistema

 e usado para atualiza ca o e um esquema de realimenta ca o realiza as medi co es. As etapas de predi ca o e atualiza ca o para o FKD podem ser descritas pelos sistemas 2 e 3 respectivamente. t = At t1 + Bt ut1 t = At t1 AT t + Rt 1 T T Kt = t Ct (Ct t Ct + Qt ) t = t + Kt (zt Ct t ) t t = (I Kt Ct ) (2)

Modelagem

3.1

Fase de Predi ca o: Modelo de Odometria do Movimento do Rob o

(3)

O ltro de Kalman representa o vetor de estados st no tempo t por sua m edia t e covari ancia t . As matrizes R, n n, e Q, l l, s ao as matrizes de covari ancia dos ru dos de processo ( ) e medi ca o ( ) respectivamente e a matriz K, n m, representa o ganho do sistema. Uma deriva ca o do ltro de Kalman aplicado a sistemas n ao-lineares e o: : Filtro de Kalman Extendido - FKE. A id eia do FKE e linearizar as fun co es em torno da estima ca o corrente usando as derivadas parciais do processo e das fun co es de medi ca o para calcular as estima co es, mesmo em face a rela co es n ao-lineares. O modelo do sistema para o FKE e dado pelo sistema 4: st = g (ut1 , st1 ) + t zt = h(st ) + t (4)

Tradicionalmente, o comportamento do movimento de um rob o e descrito por seu sistema din amico. A modelagem deste tipo de sistema e bastante complexa pois existem muitas vari aveis envolvidas (massas e momentos de in ercia, atritos, atuadores, etc.). Mesmo os sistemas mais elaborados n ao conseguem retratar dedignamente o comportamento do movimento dos rob os. Um m etodo cl assico utilizado para calcular a pose de um rob o e a odometria. Este m etodo utiliza sensores, encoders oticos por exemplo, que medem as rota co es das rodas do rob o. Usando o modelo cinem atico do rob o sua pose e calculada atrav es da integra ca o dos seus movimentos a partir de um referencial xo. Thrun et al. (2005) prop oem que as informac o es odom etricas n ao funcionem como medidas sensoriais, mas sim, que elas sejam incorporadas ao modelo do rob o. Este modelo consiste em utilizar as leituras dos sensores odom etricos (encoders no nosso caso) funcionando como sinais de controle aplicados ao modelo do rob o na fase de predi ca o do ltro de Kalman. Considere um rob o com acionamento diferencial em que os sinais de controle aplicados a seus atuadores n ao sejam tens ao mas sim, deslocamentos angulares, conforme a Figura 1. Com esta id eia, e supondo que as velocidades s ao constantes no per odo de amostragem, pode-se determinar o modelo geom etrico do movimento do rob o (sistema 9).

onde g (ut1 , st1 ) e uma fun ca o n ao-linear que representa o modelo do sistema, e h(st ) e uma func a o n ao-linear que representa o modelo das medic o es. Suas etapas de predi ca o e atualiza ca o podem ser obtidas pelos sistemas de equa co es 5 e 6 respectivamente. t = g (ut1 , t1 ) t = Gt t1 GT t + Rt 1 T T Kt = t Ht (Ht t Ht + Qt ) t = t + Kt (zt h( t )) t = (I Kt Ht )t (5)

(6)

A matriz G, n n, e o jacobiano que lineariza o modelo, e H, l n, o jacobiano que lineariza o vetor de medi co es. Tais matrizes s ao denidas pelas equa co es 7 e 8. Gt = g (ut1 , st1 ) st1 (7)

Dq

DL

Dq b
r
d

h(st ) (8) Ht = st A seguir ser a descrita a modelagem do problema bem como denidas as matrizes que ser ao utilizadas no ltro de Kalman.

Dq

Figura 1: Vari aveis do modelo geom etrico.

Para facilitar a legibilidade do sistema 9, que representa o modelo de odometria do rob o, foram utilizadas duas vari aveis auxiliares L e L = (d rd + e re )/2 = (d rd e re )/b (10)

xt =xt1 + L [sin(t1 + ) sin(t1 )] yt =yt1 L [cos(t1 + ) cos(t1 )] t =t1 +

(9)

Utilizar a id eia de Thrun implica em uma modica ca o na formula ca o original do FKE, pois o ru do n ao e mais aditivo e sim incorporado ` a func a o que lineariza o modelo como mostra o sistema 15, a ser comparado com o sistema 4: st = p(ut1 , st1 , t ) zt = h(st ) + t (15)

um modelo odom etrico mais simples do rob o (sistema 14), obtido a partir do limite do sistema 12 0. quando xt = xt1 + L cos(t1 ) sin(t1 ) (14) yt = yt1 + L t = t1

onde d e a leitura do encoder direito e funciona para o rob o como o deslocamento angular aplicado a roda direita; e ` e a leitura do encoder esquerdo e funciona como o deslocamento aplicado ` a roda esquerda; b representa a dist ancia de roda a roda do rob o, e rd e re s ao os raios de suas rodas direita e esquerda respectivamente. importante ressaltar que em aplica E co es reais o deslocamento angular efetivamente realizado pela roda difere do medido pelo encoder. Al em disso, a suposi ca o de que as velocidades s ao constantes no per odo de amostragem, que foi utilizada para obter o modelo 9, nem sempre e verdade. Da , existem diferen cas entre os deslocad e e ) mentos angulares reais das rodas ( e os medidos pelos encoders (d e e ). Esta diferen ca ser a modelada por um ru do gaussiano aditivo, conforme o sistema 11. d = d + d e = e + e (11)

onde t = [d e ]T e o vetor de ru do associado ` a odometria. necess E ario, portanto, fazer uma modica ca o na fase de predi ca o do FKE tradicional. O pr oprio Thrun prop oe que a nova fase de predi ca o seja dada pelo sistema 16: t = t1 + p(ut1 , t1 , 0) T t = Gt t1 GT t + Vt Mt Vt (16)

onde a matriz M, l l, e a matriz de covari ancia dos ru dos dos sensores() e V, n m, e o jacobiano que mapeia o ru do dos sensores para o espa co de estado. A matriz V e denida pela equa ca o 17. Vt = p(ut1 , st1 , 0) ut1 (17)

Sabe-se que a odometria possui erro acumulativo. Portanto, os ru dos d e e n ao possuem vari ancia constante. Assume-se que estes ru dos t em desvio padr ao proporcional ao m odulo do deslocamento medido. Com essas novas considera co es o sistema 9 passa a ser representado pelo sistema 12: xt =xt1 + L ) sin(t1 )] [sin(t1 + yt =yt1 L ) cos(t1 )] [cos(t1 + t =t1 + d rd + e re )/2 = ( L = ( d rd e re )/b (12)

Utilizando o modelo de odometria do rob o descrito nesta se ca o e as deni co es das matrizes utilizadas pelo ltro de Kalman, tem-se: 1 0 g13 (18) G = 0 1 g23 , onde 0 0 1 L ) cos(t1 )] g13 = [cos(t1 + L ) sin(t1 )] g23 = [sin(t1 + v11 V = v21 rd /b v12 v22 , re /b

onde

(19)

onde

v11 = k 1 cos(k 2) k 3[sin(k 2) sin(t1 )] v12 = k 1 cos(k 2) + k 3[sin(k 2) sin(t1 )] v21 = k 1 sin(k 2) k 3[ cos(k 2) + cos(t1 )] v22 = k 1 sin(k 2) + k 3[ cos(k 2) + cos(t1 )] (13) M= d | 1 | 0 e | 0 2 | (20)

Observa-se que este modelo n ao pode ser uti = 0. Ocorrendo isto, utiliza-se lizado quando

Os valores dos elementos m11 e m22 na equac a o 20 representam o fato dos desvios padr oes de d e e serem proporcionais ao m odulo do deslocamento angular. As vari aveis k 1, k 2 e k 3 s ao dadas pelo sistema 21, consideranado rd = re = r. d + e ) r( k1 = d e ) b( d e ) r( (21) k 2 = + t 1 b e b k3 = d e )/b)2 2(r( 3.2

Fase de Atualiza ca o: Modelo do Sensor para a Detec ca o dos Marcos Naturais

Neste trabalho, utilizaremos como marcos naturais um conjunto de linhas retas formadas pelas arestas do piso do ambiente onde o rob o ir a navegar, pois al em de j a existirem, esta caracter stica do ambiente e muito comum no mundo real. Devido ` a escolha das retas como marcos, a t ecnica adotada para identic a-los e a transformada de Hough. Esta transformada e um m etodo para detectar em uma imagem digital, uma classe de formas geom etricas que pode ser representada por uma curva param etrica (Gonzales, 2000). No caso das retas, e feito um mapeamento entre o espa co cartesiano (X, Y ) e o espa co de par ametros (, ) em que a reta e denida. Hough dene uma reta utilizando sua representa ca o normal como mostra a equa ca o 22, onde o par ametro representa o comprimento do vetor que passa pela origem e e perpendicular ` a reta e o angulo que esse vetor forma com o eixo X. A Figura 2 mostra a representa ca o geom etrica desses par ametros. = x cos() + y sin() (22)

como mostra a gura 3. A id eia e utilizar as informa co es diretamente obitidas do processamento de imagem (, ) na fase de atualiza ca o de um ltro de Kalman extendido para calcular a pose do rob o. Para tal, deve-se deduzir o modelo do sensor (ou seja, o processador de imagens) em fun ca o das vari aveis de estado. O rob o navega em um ambiente onde a posi ca o das retas no mundo e conhecida e a cada passo identica os descritores das retas contidas na imagem I e I . Esses descritores s ao mapeados para o plano de um sistema de coordenadas m ovel obtendo-se M e M . Tal transforma ca o e f acil e depende exclusivamente da calibra ca o correta dos par ametros extr nsecos da c amera. A gura 4 ilustra os sistemas de coordenadas utilizados na dedu ca o matem atica do modelo do sensor. Como nomenclaturas adotadas tem-se que (X F , Y F ) e um sistema de coordenadas xo no mundo; (X M , Y M ) e um sistemas de coordenadas F ) e a coordenada da origem do sism ovel; (xF , y M M tema m ovel mapeado no sistema de coordenadas xo e a vari avel M representa o angulo de rota ca o do sistema de coordenadas m ovel. Parte-se de uma transforma ca o simples que mapeia um ponto no sistema de coordenadas m ovel {M} para o sistema de coordenadas xo {F}

O sistema tratado neste trabalho e baseado em um rob o com acionamento diferencial que possui uma c amera xa acoplada a sua estrutura,

Figura 3: Vis ao geral do sistema.

YF

r a

yF M

qM

X
Figura 2: Par ametros de Hough.

xF M

Figura 4: Sistema de coordenadas.

como mostra o sistema 23.

M M M xF = cos(F )xM sin(F )y + xF M M M M F y F = sin(F )xM + cos(F )y + yM

MAPA DO AMBIENTE 7 6.5

(23)

6 5.5 5 4.5 4 3.5

Da equa ca o 22 tem-se que: F = xF cos(F ) + y F sin(F )

M

=x

cos( ) + y

sin( )

(24)

2.5 2 1.5

Substituindo (23) em (24) obt em-se o sistema 25 que representa o modelo do sensor a ser utilizado no ltro.
F M = F M M F

1 0.5 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

xF M

cos( )

F yM

sin( )

(25)

Figura 5: Localiza ca o por Odometria.

Neste sistema, F e F s ao dados, pois representam a descri ca o do marco no mapa, que e suposto conhecido. As equa co es expressam a rela ca o entre a informa ca o retornada pelo sensor (F , F ) M M e as grandezas que se quer estimar (xM F , yF , F ). Note-se que h a uma rela ca o direta entre esM M tas vari aveis (xM o F , yF , F ) e a pose do rob (xR , yR , R ) que e dada pelo sistema 26 xR = xM F M yR = yF (26) = M + R F 2

MAPA DO AMBIENTE 7 6.5 6 5.5 5 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

O modelo do sistema 25 e incorporado ao ltro de Kalman atrav es da matriz H (equa ca o 8) que e dada pela equa ca o 27. H=
F F cos(M ) sin(M ) 0 0

Figura 6: Localiza ca o por Corre ca o Geom etrica.

MAPA DO AMBIENTE 7

0 1

6.5

(27)

6 5.5 5

Resultados

4.5 4 3.5

As situa co es que ser ao apresentadas nesta se ca o foram obtidas por simula ca o. Procurou-se utilizar a medida dos ru dos dos sensores condizentes com a realidade. Para tal, nos encoders foi implementado um ru do com desvio padr ao proporcional ` a quantidade de pulsos lidos, e para a identica ca o dos par ametros das retas e o desvio padr ao do ru do tamb em obedece uma propor ca o que e regida pelo tamanho que a reta aparece na imagem. Nas guras o ret angulo tracejado representa a posi ca o real do rob o e o ret angulo cont nuo a posi ca o calculada. A Figura 5 apresenta o resultado do sistema de localiza ca o utilizando apenas a odometria. Outro sistema de localiza ca o bastante utilizado tamb em foi implementado: sistema de localiza ca o utilizando corre ca o geom etrica. Neste sistema, a cada passo as linhas s ao identicadas e usadas para calcular a pose do rob o utilizando trigonometria. Quando n ao h a retas identicadas a corre ca o e feita por odometria (Figura 6).

3 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

Figura 7: Localiza ca o utilizando Filtro de Kalman, fundindo odometria e detec ca o de marcos.

Finamente, na Figura 7, mostra-se o resultado do c alculo da pose utilizando a fus ao dos dados da odometria e da detec ca o de marcos pelo FKE. Uma situa ca o em particular foi implementada para testar a robustez dos sistemas de localizac a o. Para tal, uma perturba ca o foi adicionada ao sistema quando o rob o se aproxima da posi ca o (6.5,4.5). Os resultados encontrados utilizando geometria e Kalman s ao exibidos nas guras 8 e 9.

MAPA DO AMBIENTE 7 6.5 6 5.5 5 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

de Part culas, pois este incorpora mais f acilmente as n ao-linearidas do problema al em de tratar com ru dos n ao gaussianos; evoluir esta estrat egia de localiza ca o para uma proposta de SLAM (Simulataneous Localization and Mapping ), de forma que o rob o seja capaz de fazer sua localiza ca o sem um conhecimento pr evio do mapa e simultaneamente mapear o ambiente onde navega. Refer encias Bezerra, C. G. (2004). Localiza ca o de um rob o m ovel usando odometria e marcos naturais, Masters thesis, Universidade Federal do Rio Grande do Norte, UFRN, Natal, RN. Borenstein, J., Everett, H., Feng, L. e Wehe, D. (1997). Mobile robot positioning: Sensors and techniques, Journal of Robotic Systems, 14(4):231249. Gonzales, R. C. (2000). Processamento de Imagens Digitais, Edgard Blucher. Kim, S. e Kim, Y. (2004). Robot localization using ultrasonic sensors, Proccedings of the 2004 IEEE/RSJ International Conference on Inteligent Robots and Systems, Sendal, Japan. Kiriy, E. e Buehler, M. (2002). Three-state extended kalman lter for mobile robot localization, Report Centre for Intelligent Machines - CIM,McGill University. Launay, F., Ohya, A. e Yuta, S. (2002). A corridors lights based navigation system including path denition using a topologically corrected map for indor mobile robots, Proceedings IEEE International Conference on Robotics and Automation, pp.3918-3923. Lizzaralde, F., Nunes, E., Hsu, L. e J.T., W. (2003). Mobile robot navigation using sensor fusion, Proccedings of the 2003 IEEE International Conference on Robotics and Automation, Taipei, Taiwan. Park, K. C., Chung, D., Chung, H. e Lee, J. G. (1998). Dead reckoning navigation mobile robot using an indirect kalman lter, Conference on Multi-sensor fusion and Integration for Intelliget Systems, 9(3):107-118. P eres, J., Catellanos, J., Montiel, J., Neira, J. e Tard os, J. (1999). Continuous mobile robot localization: Vision vs. laser, Proccedings of the 1999 IEEE International Conference on Robotic and Automation. Thrun, S., Burgard, W. e Fox, D. (2005). Probabilistic Robotics, 01 edn, MIT Press.

Figura 8: Efeito da perturba ca o na Localiza ca o por Geometria.

MAPA DO AMBIENTE 7 6.5 6 5.5 5 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

Figura 9: Efeito da perturba ca o na Localiza ca o utilizando Filtro de Kalman. 5 Conclus oes e Perspectivas

Este trabalho prop os um sistema de localiza ca o para rob os m oveis utilizando Filtro de Kalman Extendido. A principal contribui ca o e a modelagem do sensor otico feita de tal forma a proporcionar a utiliza ca o dos par ametros obtidos no processamento de imagem diretamente nas equac o es do ltro de Kalman, sem passar por nenhuma etapa intermedi aria de c alculo de pose ou de dist ancia a partir das informa co es usuais. Analisando as Figuras 5, 6 e 7 percebe-se que o comportamento do sistema de localiza ca o utilizando o ltro de Kalman mostrou-se mais satisfat orio que os utilizando odometria e corre co es geom etricas. No tocante a rejei co es de perturbac o es (Figuras 8 e 9), o sistema baseado no ltro tamb em mostrou-se eciente pois tende a retornar ` a pose real, enquanto que o sistema baseado em corre ca o geom etrica n ao conseguiu o mesmo resultado. Como trabalhos futuros pretende-se: implementar outras formula co es do Filtro de Kalman, por exemplo, o Filtro de Kalman com Observa co es Parciais; substituir o ltro de Kalman por um Filtro