x sbai simpósio brasileiro de automação inteligente 18 a ... · multiplos encoders e gps ... x...

6
X SBAI – Simpósio Brasileiro de Automação Inteligente 18 a 21 de setembro de 2011 São João del-Rei - MG - Brasil ISSN: 2175-8905 - Vol. X 1268 LOCALIZA ¸ C ˜ AO EM ROB ´ OTICA TERRESTRE: FUS ˜ AO ENTRE ODOMETRIA POR M ´ ULTIPLOS ENCODERS E GPS Renato Jos´ e Martins §, , Samuel Siqueira Bueno , Luiz G. Bizarro Mirisola , Ely Carneiro de Paiva , Paulo A. Valente Ferreira § Centro de Tecnologia da Informa¸ c˜ao Renato Archer - CTI Divis˜ ao de Rob´ otica e Vis˜ ao Computacional - DRVC Campinas, S˜ ao Paulo, Brasil Universidade Federal do ABC - UFABC Centro de Matem´ atica, Computa¸ c˜ao e Cogni¸ c˜ao-CMCC Santo Andr´ e, S˜ ao Paulo, Brasil Universidade Estadual de Campinas - UNICAMP Faculdade de Engenharia Mecˆanica - FEM Campinas, S˜ ao Paulo, Brasil § Universidade Estadual de Campinas - UNICAMP Faculdade de Engenharia El´ etrica e de Computa¸ c˜ao-FEEC Campinas, S˜ ao Paulo, Brasil Emails: [email protected], [email protected], [email protected], [email protected], [email protected] Abstract— Robot localization (i.e pose estimation) in outdoor environments is the basis for control and autonomous navigation strategies. This paper proposes a localization methodology based on GPS and odometry fusion. An important aspect is a new odometry formulation, wich results from a least squares optimization of the information gathered from multiple encoders (four wheels and steering) of an outdoor robotic vehicle. Sensor fusion is developed by using both the Extended Kalman Filter and the Unscented Kalman Filter. Experimental data are used to validate the methodology and to compare the results of both filters. Keywords— Localization, Multisensor Fusion, Odometry, Outdoor Robotics. Resumo— Adetermina¸c˜ ao da localiza¸ ao (i.e. estima¸ ao da pose ) de um robˆ o em ambiente externo constitui a base para estrat´ egias de controle e navega¸ ao autˆ onoma. Esse artigo prop˜ oe uma metodologia de localiza¸ ao de robˆ os baseada na fus˜ao entre dados de GPS e odometria. Um aspecto relevante ´ e uma nova formula¸c˜ ao de odometria, que resulta da otimiza¸ ao, por m´ ınimos quadrados, das informa¸c˜ oes provenientes de m´ ultiplos encoders (das quatro rodas e da dire¸c˜ ao) de um ve´ ıculo rob´ otico terrestre. A fus˜ao sensorial ´ e desenvolvida usando tanto o Filtro de Kalman Estendido quanto o Unscented Kalman Filter. Dados experimentais s˜ao usados para validar a metodologia e comparar os resultados de ambos os filtros. Palavras-chave— Localiza¸ ao, Odometria, Fus˜ ao Sensorial, Rob´ otica de Exterior. 1 Introdu¸ ao Alocaliza¸c˜ ao de um ve´ ıculo rob´ otico ´ e uma das mais importantes tarefas no desenvolvimento de estrat´ egias de controle e navega¸ ao autˆ onoma. Entende-se por localiza¸c˜ ao a determina¸ ao da pose (posi¸c˜ ao e orienta¸c˜ ao) do ve´ ıculo no ambiente onde ele evolui. De uma maneira simples, a pose pode ser obtida atrav´ es da integra¸ ao da odometria das rodas, de sensores inerciais de velocidade e acele- ra¸ ao (chamados proprioceptivos), atrav´ es de sis- temas de posicionamento absoluto (Global Posi- tion System - GPS) ou utilizando-se sensores ex- ternoceptivos como cˆ ameras, lasers e sonares. En- tretanto em alguns cen´ arios (ambientes externos e/ou urbanos) o simples uso da percep¸ ao pro- prioceptiva ´ e inaceit´ avel, devido ` a acumula¸ ao de imprecis˜ oes com o tempo (erro em posi¸c˜ ao ilimi- tado), ao passo que o sistema GPS pode n˜ ao estar dispon´ ıvel ou fornecer medidas aberrantes (devido a oclus˜oes, interferˆ encias,transi¸c˜ oes entre sat´ eli- tes). Dentre estas alternativas de localiza¸c˜ ao, a odometria simples ´ e, sem d´ uvida, um dos m´ eto- dos mais utilizados em estruturasde locomo¸c˜ ao a rodas. No entanto al´ em de sofrer com a integra¸ ao de imprecis˜ oes, ela consideraa hip´otese limitadora de planaridade do movimento. Uma maneira de melhorar a estimativa de pose consiste em combinar as diferentes fontes de informa¸ ao de maneira a considerar as incertezas intr´ ınsecas de cada sensor, processo comumente chamado de fus˜ao multisensorial (Durrant-Whyte and Henderson, 2008). O processo de fus˜ao frequentemente utilizado consiste em variantes simples de filtragem Bayesiana, como o Filtro de Kalman (Kalman Filter - KF). O Filtro de Kalman apresenta grande versatilidade de aplica¸c˜ ao, tendo como ´ unicas restri¸ oes que os ru´ ıdos do modelo e de observa¸ ao sejam normais (gaussianos) n˜ ao-correlacionados e que os mode- los empregados sejam lineares. Uma vez que a

Upload: lykhanh

Post on 25-Jan-2019

220 views

Category:

Documents


0 download

TRANSCRIPT

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1268

LOCALIZACAO EM ROBOTICA TERRESTRE: FUSAO ENTRE ODOMETRIA POR

MULTIPLOS ENCODERS E GPS

Renato Jose Martins§,∗, Samuel Siqueira Bueno∗, Luiz G. Bizarro Mirisola†, Ely

Carneiro de Paiva‡, Paulo A. Valente Ferreira§

∗Centro de Tecnologia da Informacao Renato Archer - CTI

Divisao de Robotica e Visao Computacional - DRVC

Campinas, Sao Paulo, Brasil

†Universidade Federal do ABC - UFABC

Centro de Matematica, Computacao e Cognicao - CMCC

Santo Andre, Sao Paulo, Brasil

‡Universidade Estadual de Campinas - UNICAMP

Faculdade de Engenharia Mecanica - FEM

Campinas, Sao Paulo, Brasil

§Universidade Estadual de Campinas - UNICAMP

Faculdade de Engenharia Eletrica e de Computacao - FEEC

Campinas, Sao Paulo, Brasil

Emails: [email protected], [email protected],

[email protected], [email protected], [email protected]

Abstract— Robot localization (i.e pose estimation) in outdoor environments is the basis for control andautonomous navigation strategies. This paper proposes a localization methodology based on GPS and odometryfusion. An important aspect is a new odometry formulation, wich results from a least squares optimization ofthe information gathered from multiple encoders (four wheels and steering) of an outdoor robotic vehicle. Sensorfusion is developed by using both the Extended Kalman Filter and the Unscented Kalman Filter. Experimentaldata are used to validate the methodology and to compare the results of both filters.

Keywords— Localization, Multisensor Fusion, Odometry, Outdoor Robotics.

Resumo— A determinacao da localizacao (i.e. estimacao da pose) de um robo em ambiente externo constituia base para estrategias de controle e navegacao autonoma. Esse artigo propoe uma metodologia de localizacaode robos baseada na fusao entre dados de GPS e odometria. Um aspecto relevante e uma nova formulacaode odometria, que resulta da otimizacao, por mınimos quadrados, das informacoes provenientes de multiplosencoders (das quatro rodas e da direcao) de um veıculo robotico terrestre. A fusao sensorial e desenvolvidausando tanto o Filtro de Kalman Estendido quanto o Unscented Kalman Filter. Dados experimentais sao usadospara validar a metodologia e comparar os resultados de ambos os filtros.

Palavras-chave— Localizacao, Odometria, Fusao Sensorial, Robotica de Exterior.

1 Introducao

A localizacao de um veıculo robotico e uma dasmais importantes tarefas no desenvolvimento deestrategias de controle e navegacao autonoma.Entende-se por localizacao a determinacao da pose(posicao e orientacao) do veıculo no ambiente ondeele evolui. De uma maneira simples, a pose podeser obtida atraves da integracao da odometria dasrodas, de sensores inerciais de velocidade e acele-racao (chamados proprioceptivos), atraves de sis-temas de posicionamento absoluto (Global Posi-

tion System - GPS) ou utilizando-se sensores ex-ternoceptivos como cameras, lasers e sonares. En-tretanto em alguns cenarios (ambientes externose/ou urbanos) o simples uso da percepcao pro-prioceptiva e inaceitavel, devido a acumulacao deimprecisoes com o tempo (erro em posicao ilimi-tado), ao passo que o sistema GPS pode nao estardisponıvel ou fornecer medidas aberrantes (devidoa oclusoes, interferencias, transicoes entre sateli-

tes). Dentre estas alternativas de localizacao, aodometria simples e, sem duvida, um dos meto-dos mais utilizados em estruturas de locomocao arodas. No entanto alem de sofrer com a integracaode imprecisoes, ela considera a hipotese limitadorade planaridade do movimento.

Uma maneira de melhorar a estimativa depose consiste em combinar as diferentes fontes deinformacao de maneira a considerar as incertezasintrınsecas de cada sensor, processo comumentechamado de fusao multisensorial (Durrant-Whyteand Henderson, 2008). O processo de fusaofrequentemente utilizado consiste em variantessimples de filtragem Bayesiana, como o Filtrode Kalman (Kalman Filter - KF). O Filtrode Kalman apresenta grande versatilidade deaplicacao, tendo como unicas restricoes que osruıdos do modelo e de observacao sejam normais(gaussianos) nao-correlacionados e que os mode-los empregados sejam lineares. Uma vez que a

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1269

distribuicao normal e completamente conhecidaa partir dos dois primeiros momentos (media evariancia), este estimador calcula a media e avariancia condiconal do estado atual dada todasas observacoes passadas, como sendo a melhorestimativa do estado atual, segundo um criterioquadratico.

No entanto, muitos processos apresentammodelagens nao-lineares e/ou perturbacoes quenao podem ser representadas como gaussianas.Nestes casos, momentos estatısticos de ordemsuperior devem ser calculados para a determi-nacao da propagacao da funcao de distribuicao.As implementacoes que consideram a estimacaodos momentos adicionais possuem o reves dedimensionalidade, visto que a complexidadecomputacional do algoritmo e exponencial emrelacao ao tamanho do vetor de estados (Crassidisand Junkins, 2004). Para permitir a factibilidadecomputacional, adota-se usualmente uma solucaoaproximada que se restringe a dois momentos(media e variancia), ou seja, sub-otima; exemplossao o Filtro de Kalman Estendido - Extended

Kalman Filter - EKF e, mais recentemente,o Unscented Kalman Filter - UKF (Julieret al., 2000).

Este trabalho tem como foco a localizacao deveıculos roboticos terrestres de quatro rodas, compropulsao nas duas rodas traseiras e dirigibilidadeassegurada pela orientacao das duas rodas dian-teiras. Ele apresenta uma estrategia de estimacaoda pose do veıculo, calcada na fusao entre dadosde GPS e de odometria, usando tanto EKFquanto o UKF e comparando os desempenhosobtidos. A contribuicao, neste caso, consistejustamente na formulacao da odometria, comodetalhado em (Martins et al., 2011), a saber: i)a odometria explora os cinco encoders presentesno veıculo (deslocamentos das quatro rodas eangulo de direcao), o que resulta em um conjuntode equacoes apresentando nao-linearidade; ii)uma transformacao executada nessas equacoespermite estabelecer uma formulacao linear; iii) aodometria e determinada como a solucao de umprocesso de otimizacao por mınimos quadrados.

A metodologia e validada com dados expe-rimentais obtidos a partir do veıculo eletrico doprojeto VERO (VEıculo RObotico de Exterior)(Bueno et al., 2009) (Mirisola et al., 2011).

Na sequencia do artigo: A Secao 2 introduz amodelagem do problema. A Secao 3 resume a for-mulacao da odometria usando multiplos encoders.A fusao entre odometria e GPS, via EKF e UKF,e tratada na Secao 4. A Secao 5 apresenta os re-sultados experimentais e a Secao 6 as conclusoes.

2 Modelagem do Problema

A evolucao da pose do veıculo pode ser determi-nada geometricamente como exemplificado na Fi-gura 1, segundo a notacao fornecida pela Tabela1. Assim, entre dois instantes consecutivos (tk)e (tk+1), o ponto P percorre uma distancia △d etem uma variacao na orientacao de △θ, conformeas equacoes cinematicas:

xk+1

yk+1

θk+1

=

xk +△d sinc(△θ

2) cos(θk +

△θ

2)

yk +△d sinc(△θ

2) sin(θk +

△θ

2)

θk +△θ

(1)onde o vetor de estados do sistema e dado por:

x(tk) = [x(tk) y(tk) θ(tk)]T (2)

Tem-se tambem sinc(x) =sin(x)

xe △d = v△T ,

sendo △T o perıodo de amostragem. Adota-se noentanto, em (1), a simplificacao comumente usadapara altas taxas de amostragem: sinc(△θ/2) ≈ 1.

O modelo de observacao adotado correspondeas coordenadas x e y, aqui determinadas pelos da-dos obtidos de um GPS embarcado.

Figura 1: Evolucao da pose do veıculo entre doisinstantes.

Tabela 1: Notacao e Parametros Geometricos do Veıculo

P centro do eixo das rodas traseiras.ψ angulo da direcao.θ orientacao.δL, δR angulos rodas esquerda e direita.v veloxidade no ponto P.△drl, △drr deslocamento rodas traseiras esqu. e dir.△dfl, △dfr deslocamento rodas dianteiras esqu. e dir.u1 = △d.u2 = △θ.D distancia entre as rodas.L distancia entre os eixos.

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1270

y(tk+1) =

[

x(tk+1)y(tk+1)

]

=

[

1 0 00 1 0

]

x(tk+1)

(3)Ressalta-se que, embora o GPS tambem fornecauma informacao de orientacao que poderia seracrescentada como terceiro elemento no vetor deobservacao, esta informacao apresenta sensibili-dade, principalmente a baixas velocidades do veı-culo, sendo portanto descartada na presente for-mulacao.

3 Odometria Usando Multiplos Encoders

Para veıculos com rodas, frequentemente adota-sea simplificacao da cinematica como sendo a deum veıculo diferencial. Nesse caso, as equacoesde odometria sao desenvolvidas utilizando-sesomente as informacoes dadas pelos encoders

montados nas duas rodas traseiras. Outra sim-plificacao comumente usada, mas mais completaque a precedente, e o modelo triciclo o qualresulta do acrescimo, nas equacoes de odometria,da informacao dada pelo encoder de direcao. Asimplificacao pelo modelo bicicleta tambem usaa informacao da direcao mas substitui as duasrodas traseiras por uma unica. No entanto, parafins de odometria, pode-se tambem explorar asinformacoes advindas dos encoders montadosnas duas rodas dianteiras. Essas diferentesformulacoes da cinematica e das equacoes deodometria para veıculos de quatro rodas da classeconsiderada nesse trabalho, sao discutidas em(Martins et al., 2011).

Para a estimacao da pose do veıculo usando osquatro encoders das rodas e o encoder de direcaoem um esquema de otimizacao por mınimos qua-drados (Martins et al., 2011), parte-se do modelocinematico dado pela equacao (1), em que as en-tradas sao as variacoes em distancia △d e angular△θ. Estabelece-se um conjunto de equacoes querelacionam essas entradas aos dados dos cinco en-coders, que podem ser descritas em cinco funcoesdependentes de △d e △θ, segundo a notacao dadana Tabela 1:

tan(ψ) = L△θ

△d

△drr =2△d+D△θ

2

△drl =2△d−D△θ

2

△dfr =2△d+D△θ

2 cos(δR)

△dfl =2△d−D△θ

2 cos(δL)

(4)

Tem-se assim um sistema de equacoes sobredeter-

minado nao-linear, na forma, yi = Hi(△d,△θ). Aprimeira equacao, nao-linear, pode ser facilmentereescrita de maneira a obter um sistema de equa-coes lineares:

0 = y1△d− L△θ +m1

y2 =2△d+D△θ

2+m2

y3 =2△d−D△θ

2+m3

y4 =2△d+D△θ

2 cos(δR)+m4

y5 =2△d−D△θ

2 cos(δL)+m5

(5)

em que wi e uma variavel aleatoria (V.A) comdistribuicao normal, representando a incerteza decada medida. A minimizacao do erro quadraticoponderado por M e usado como criterio para asolucao do sistema. Ou seja:

Φ = argminφ1,φ2

5∑

i=1

‖zi −HiΦ‖2M (6)

Em que

• Φ = [φ1 φ2]T = [△d △θ]T ;

• H1 = [y1 −L], H2 = H4 = [1 D/2] e H3 =H5 = [1 −D/2];

• z1 = 0; z2 = z3 = yi para i = 2, 3; z4 =y4 cos(δR) e z5 = y5 cos(δL);

• M = diag(

η−21 , η−2

2 , η−23 , η−2

4 , η−25

)

, com η2isendo a variancia de cada V.A mi.

Tem-se como solucao:

Φ =[

HT MH]−1HTMz (7)

conhecida como Estimador deMarkov ouMınimosQuadrados Ponderados.

4 Fusao entre GPS e Odometria:

Estimacao de Estados

Esta secao apresenta as solucoes de estimacao deestados nao-linear para fusao de GPS e odometriausando o EKF e o UKF. Para tanto, considere osistema representado pelas equacoes

x(tk+1) = F (x(tk),u(tk),v(tk))y(tk+1) = C (x(tk+1),u(tk+1),w(tk+1))

(8)

com F(.) dado pelo modelo cinematico em (1) eC(.) pelo modelo de observacao em (3). Os ruıdosdo modelo v(tk) e de observacao w(tk) sao consi-derados gaussianos e nao-correlacionados entre si,satisfazendo a:

• E{v} = 0(3×1) e E{vvT } = Q(3×3)

• E{w} = 0(2×1) e E{wwT } = R(2×2)

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1271

4.1 Filtragem pelo EKF

A utilizacao do EKF supoe que os modelos de evo-lucao/observacao sejam contınuos e diferenciaveis.O princıpio basico deste filtro consiste na lineari-zacao do modelo de evolucao em (1), onde a ma-triz A(.) (Jacobiano) e obtida da aproximacao doprimeiro termo da serie de Taylor.

x(tk+1) = A(x(tk),u(tk))x(tk)

y(tk+1) = Cx(tk+1) (9)

com,

A(x,u) =

1 0 −u1 sin(x3 + u2/2)0 1 u1 cos(x3 + u2/2)0 0 1

(10)

As equacoes para o calculo da media e vari-ancia condicionais do EKF correspondem as eta-pas de predicao e atualizacao convencionais do Fil-tro de Kalman (Bar-Shalom et al., 2002)(Crassidisand Junkins, 2004).

4.2 Filtragem pelo UKF

O princıpio basico do filtro UKF consiste em criarum conjunto de partıculas (sigma points) cujamedia corresponda ao estado, enquanto que acovariancia e calculada a partir da variancia daspartıculas e nao propagadas explicitamente comono caso do EKF.

No UKF os ruıdos/incertezas das funcoes nao-lineares tambem devem ser estimados. Assim,para o sistema determinıstico em (1-3) e sua ver-sao estocastica em (8), nos quais o modelo de evo-lucao e nao-linear, deve-se criar o vetor de estadosaumentado xa pela inclusao da incerteza v do mo-delo:

xa(tk) =

[

x(tk)v(tk)

]

(11)

resultando em dim{xa} = N = 6.

Este filtro apresenta uma primeira etapa degeracao de partıculas, que e seguida pelas etapasde predicao e atualizacao como no EKF.

4.2.1 Geracao de Partıculas

No inıcio de cada ciclo sao criados o vetor de es-tados aumentado xa e sua matriz de covarianciaaumentada correspondente Pa, tais que:

xa(tk) =

[

x+(tk−1)03×1

]

(12)

Pa(tk) =

[

P+x (tk−1) 03×3

03×3 Q

]

(13)

onde x+(tk−1) e P+x (tk−1) correspondem respec-

tivamente ao estado e sua matriz de covariancia

estimados apos a ultima fase - de atualizacao, nainteracao precedente do filtro.

A geracao das (2N +1) partıculas iXa no ins-tante atual, e feita a partir desses estado e varian-cia aumentados - equacoes (12) e (13), segundo:

0Xa(tk) = xa(tk)

iXa(tk) = xa(tk) +(

(N + λ)Pa(tk))

j,

i = j = 1, . . . , N (14)

iXa(tk) = xa(tk)−(

(N + λ)Pa(tk))

j,

i = N + 1, . . . , 2N, j = 1, . . . , N

O termo(

(N + λ)Pa(tk))

jequivale a j-esima

coluna da matriz obtida a partir de uma decom-posicao de Cholesky, onde λ corresponde a umfator de normalizacao:

λ = α2(N + κ)−N (15)

Segundo (Julier et al., 2000) α e κ indicam a dis-percao das partıculas em torno da media (suge-rindo α = 0.001 e κ = 0). As ponderacoes associ-adas a cada partıcula sao tais que

0Wm =λ

λ+N

0Wc =λ

λ+N+ (1− α2 + β) (16)

iWm = iWc =1

2(λ+N), i = 1, . . . , 2N

onde Wm sao utilizados para o calculo da me-dia e Wc para a covariancia, sendo determinadosapenas uma vez. O fator β aplica correcoes paramomentos de ordem superior dada a distribuicaoda incerteza dos estados (sugerindo β = 2 parao caso gaussiano). Geradas as partıculas, da-sesequencia aos processos de predicao e estimacaodo filtro.

4.2.2 Predicao

A etapa de predicao se inicia com a insercao daspartıculas resultantes de (14) e denotadas como:

iXa = [x1 x2 x3 v1 v2 v3]T ;

no modelo de evolucao em (17), esse ultimo ad-vindo de (1).

iX (tk+1) = F(iXa(tk),u(tk)), i = 0, . . . , 2N

Com

F(iXa,u) =

x1 + v1 + u1 cos(x3 + v3 + u2/2)x2 + v2 + u1 sin(x3 + v3 + u2/2)

x3 + v3 + u2

(17)

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1272

Apos a passagem das partıculas pelo modelo, ocalculo da media ponderada segundo (18) resultano proximo estado, enquanto que a covariancia eestimada por (19).

x−(tk+1) =

2N∑

i=0

iWmiX (tk+1) (18)

P−x (tk+1) =

2N∑

i=0

iWc

[

iX (tk+1)− x−(tk+1)]

[

iX (tk+1)− x−(tk+1)]T

(19)

E importante salientar que dim{iXa(tk)} = (6×1)e dim{iX (tk)} = (3 × 1). A observacao previstaconsiste em aplicar as partıculas preditas no mo-delo de observacao:

iY(tk+1) = C(iX (tk+1)), i = 0, . . . , 2N (20)

y(tk+1) =

2N∑

i=0

iWmiY(tk+1) (21)

4.2.3 Calculo do Ganho e Atualizacao

O calculo do ganho necessita das medidas de co-variancia da saıda Pyy e da covariancia cruzadaentre os estados e a saıda Pxy, definidas como:

Pxy(tk+1) =

2N∑

i=0

iWc

[

iX (tk+1)− x−(tk+1)]

[

iY(tk+1)− y(tk+1)]T

(22)

Pyy(tk+1) =

2N∑

i=0

iWc

[

iY(tk+1)− y(tk+1)]

[

iY(tk+1)− y(tk+1)]T

+R (23)

A atualizacao do estado obtido na etapa de predi-cao e realizado em (25) atraves da inovacao pon-derada pelo ganho computado em (24)

K(tk+1) = Pxy(tk+1) [Pyy(tk+1)]−1 (24)

x+(tk+1) = x−(tk+1) +K(tk+1) [y(tk+1)−

y(tk+1)] (25)

Finalmente, a covariancia do estado e atualizadasegundo (26) e um novo ciclo do processo de fil-tragem e iniciado.

Px+(tk+1) = P−

x (tk+1)−

K(tk+1)Pyy(tk+1)KT (tk+1) (26)

5 Resultados Experimentais

A avaliacao experimental foi realizada com dadosadquiridos pelo sistema embarcado do veıculoeletrico do projeto VERO, durante duas voltasrealizadas em um percurso retangular sob controlede trajetoria por pontos de passagem (Mirisola

et al., 2011). Os sensores usados foram: i) encoderincremental das 4 rodas (HOHNER/Serie 13) de2048 pulsos por volta - o que resulta em umaresolucao de 4096 bits na configuracao de mon-tagem utilizada; ii) encoder absoluto de direcao(HOHNER/Serie 65), resolucao de 4096 bits -embora, pela variacao dos angulos de direcao,apenas aproximadamente 1/6 dessa faixa sejaefetivamente utilizada; iii) GPS (NOVATEL/Su-perstar II - SSII-5-5Hz), de 12 canais e Circular

Error Probability (CEP) de 5m. Os encoders

sao amostrados a 40Hz e o GPS a 5Hz. A fu-sao odometria-GPS e computada na taxa de 20Hz.

A Figura 2 apresenta a posicao do veıculosegundo a odometria otimizada, o GPS e a fusaopor UKF entre ambos. Apesar da imprecisaoinerente do GPS, os dados por ele fornecidos saoconsistentes visto que o ambiente e aberto e naoapresenta oclusoes (arvores, edifıcios), emborahaja ocorrencias de saltos. O efeito desses saltose reduzido pelo esquema de fusao como ilustrao detalhe do canto superior esquerdo, que eretratado na figura 3. Extrapola-se que, emcasos de ambientes mais propensos a oclusoese/ou mudancas na constelacao de satelites, osresultados sejam mais significativos.

A estimacao da orientacao do veıculo e ilus-trada na figura 4, que mostra o heading resultanteda fusao por UKF em comparacao ao sinal forne-cido pelo GPS. Ressalta-se que esta informacaodo GPS nao e utilizada no processo de filtragem.Observa-se que o sinal de GPS e ruidoso e apre-senta um dado aberrante aos 340 segundos; a fu-sao fornece uma estimativa com menor varianciae elimina a informacao aberrante.

−50 −40 −30 −20 −10 0

0

5

10

15

20

25

30

35

40

metros

met

ros

Odom−otimi

Fusion UKF

GPS

Figura 2: Estimativas de posicao: GPS (azul);Odometria otimizada (vermelho); Fusao GPS-odometria (verde).

O mesmo caso de estimacao foi tambem im-plementado usando EKF (com as mesmas matri-zes de sintonizacao). Os resultados obtidos foramsimilares aos anteriores; no entanto, a diferencada estimativa de orientacao entre ambos os filtroschega a atingir 10◦, como mostra a figura 5.

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 1273

Ressalta-se que, para comparacoes conclusi-vas acerca do desempenho dos filtros e avaliacaoda robustez da propria metodologia em si, e ne-cessaria uma analise baseada no conhecimento doground truth, o que sera realizado futuramentecom a utilizacao de um sistema de localizacao AP-PLANIX, integrado ao sistema embarcado do veı-culo.

−44 −42 −40 −38 −36 −34 −32

32

33

34

35

36

37

38

39

40

41

42

metros

met

ros

Figura 3: Detalhe da posicao: GPS (azul) e FusaoGPS-odometria (verde).

0 50 100 150 200 250 300 3500

50

100

150

200

250

300

350

tempo

grau

s

theta gps

theta fusion

Figura 4: Estimativa da orientacao pela fusao porUKF (verde) em comparacao com o heading doGPS (azul).

0 50 100 150 200 250 300 350 400

−10

−5

0

5

10

tempo [s]

Dife

renc

a Θ

EK

F−

UK

F [g

raus

]

Figura 5: Diferenca nas estimativas de orientacaopelo UKF e EKF com mesma sintonizacao.

6 Conclusoes

Este artigo apresentou uma metodologia para a lo-calizacao de um veıculo robotico terrestre em am-biente externo, caracterizada por duas etapas. Naprimeira, partindo da cinematica do veıculo, as in-formacoes provenientes de multiplos encoders (das

quatro rodas e de direcao) fornecem uma odome-tria mais precisa que a odometria simples diferen-cial. Na segunda etapa, essa odometria e combi-nada com dados de posicao proveniente de GPS,em um esquema de fusao sensorial empregando-setanto o Filtro de Kalman Estendido (EKF) quantoo Unscented Kalman Filter (UKF). A metodologiaproposta reduz os efeitos de deriva da odometriae de saltos/dados aberrantes advindos do GPS.Trabalhos futuros abordarao a sua avaliacao facea ground truth e fusao com outros sensores.

Agradecimentos

Os autores agradecem os financiamentos doprograma PCI-CTI/MCT (551022/2011 − 6)e dos projetos NAGUIVA (490722/2010 −5 - CNPq/FCT - Portugal) e INCT-SEC(573963/2008 − 8 - CNPq e 08/57870 − 9 - FA-PESP), bem como a atuacao de Douglas Figuei-redo e Tiago Tarossi no veıculo e seus sistemas, eaos pesquisadores do projeto VERO.

Referencias

Bar-Shalom, Y., Li, X. and Kirubarajan, T.(2002). Estimation with Application to Trac-

king and Navigation, Wiley & Sons, Inc.

Bueno, S., Azevedo, H., Mirisola, L., Paiva, E. D.,Ramos, J., Victorino, A. and Azinheira, J.(2009). Uma plataforma para pesquisa e de-senvolvimento em robotica terrestre de exte-rior, IX Simposio Brasileiro de Automacao

Inteligente, Brasılia-DF, Brasil.

Crassidis, J. L. and Junkins, J. L. (2004). Optimal

Estimation of Dynamic Systems, Chapman &Hall Inc.

Durrant-Whyte, H. and Henderson, T. C. (2008).Multisensor data fusion, Handbook of Robo-

tics, Springer Verlag.

Julier, S., Uhlmann, J. and Durrant-Whyte, H. F.(2000). A new method for the nonlineartransformation of means and covariances infilters and estimators, IEEE Transactions on

Automatic Control 45(3): 477–482.

Martins, R. J., Bueno, S. S., Mirisola, L. G. B.,de Paiva, E. C. and Ferreira, P. A. V. (2011).Cinematica e localizacao em robotica ter-restre usando multiplos encoders, X Simpo-

sio Brasileiro de Automacao Inteligente, SaoJoao del-Rei, Brasil.

Mirisola, L., Azevedo, H., Ramos, J., Bueno, S.,Azinheira, J. and de Paiva, E. (2011). Va-lidacao experimental de um veıculo roboticoterrestre para ambientes externos, X Simpo-

sio Brasileiro de Automacao Inteligente, SaoJoao del-Rei, Brasil.