NAVEGAÇÃO USANDO UNIDADE INERCIAL DE BAIXO CUSTO (IMU MEMS) E RECEPTOR GPS COM APLICAÇÃO DO FILTRO DE KALMAN SIGMA PONTO

Transcrição

1 AVGAÇÃO USAO UIA ICIAL BAIXO CUSTO (IMU MMS) CPTO GPS COM APLICAÇÃO O FILTO KALMA SIGMA POTO Walter woegerer*, Hélo Kot Kuga*, Paulo Gácomo Mla* eumo - te trabalho apreeta um método de tegração da formaçõe de aceleração e taxa agular de uma Udade de Medda Iercal (IMU) de baxo cuto, com tecologa Mcro lectro-mechacal Sytem (MMS) à medda de poção e velocdade obtda de um receptor GPS. A euaçõe dfereca utlzada para tegrar o movmeto e a attude de um tema móvel a cofguração oldára à plataforma (trapdow) ão apreetada. a eüêca, decrevem-e o procedmeto e reultado obtdo da tegração de medda da IMU a um receptor GPS em um expermeto em codçõe dâmca. Um algortmo baeado o Fltro de Kalma Sgma-Poto (FKSP) fo deevolvdo para aplcação à avegação a fm de realzar a fuão da formaçõe de poção e velocdade va GPS e a coordeada calculada atravé da IMU. O reultado da trajetóra obtda deta tegração é comparado à trajetóra eperada. O objetvo dete trabalho é o de obter o domío da técca de Mecazação de Cetra de avegação Iercal, permtdo a determação do erro evolvdo a mplemetação do algortmo de avegação uado a técca do Fltro de Kalma Sgma-Poto. Palavra-Chave: IMU, MMS, Fltro de Kalma, Fltro de Kalma Sgma Poto. Abtract - The propoal of th wor to preet a proce of fuo of the accelerato ad agular rate meauremet of a low cot Iertal Meauremet Ut (IMU) MMS (Mcro lectro-mechacal Sytem) baed techology, corporatg meaured poto ad peed data from a GPS recever. The, t detal the euato that have bee ued to tegrate the movemet ad the atttude of a ytem trapdow cofgurato, through approprate algorthm ug the method of the Sgma-Pot Kalma flter, verfyg t performace ad comparg reult. It alo detal the meauremet tae dyamc codto ug the IMU ad GPS multaeouly ad decrbe the ue of th data o a Sgma-Pot Kalma Flter developed to fue the GPS poto ad peed formato wth the coordate calculated from the IMU. Keyword: IMU, MMS, Kalma flter, Sgma Pot Kalma Flter. I. ITOUÇÃO Uma Udade de Medda Iercal (IMU) baeada em tecologa MMS (Mcro lectro-mechacal Sytem), devdo à ua grade dpobldade e baxo cuto, erá au utlzada para a aplcação a determação de um algortmo de avegação. O dado obtdo de uma IMU-MMS (Mcro lectro- Mechacal Sytem) permtem cematcamete a tegração da euaçõe dfereca do movmeto a taxa de amotragem elevada (até 133Hz a udade utlzada). tretato, em vrtude de eu baxo cuto, cotém derva ue com o paar do tempo acumulam erro crecete em relação à poção e velocdade medda ao logo do tempo. O receptor GPS, por ua vez, opera a taxa de amotragem meore (1 Hz) forecedo formaçõe ma preca de poção e velocdade, porém é ujeto a perda de a em fução de obtáculo fíco. Para compear ee tpo de lmtação pretede-e a tegrar a IMU ao receptor GPS, o ue permte uma olução de avegação ue ala a depedêca da IMU, de baxa precão, com a etabldade do receptor GPS, ujeto a perda de al e tpo de tegração e tora cada vez ma comum devdo ao crecete deevolvmeto dete tpo de IMU pobltado ão apea a ua tegração a receptore GPS, ma também a outro eore e dpotvo de apoo à avegação. A caracterítca da IMU-MMS utlzada, bem como o procedmeto para a obteção de eu parâmetro ão detalhadamete decrto a refereca [1] e [2]. O detalhameto do reultado correpodete a ete expermeto etão reportado e aalado a referêca [3]. II. MCAIZAÇÃO A AVGAÇÃO ICIAL ete trabalho, para o euacoameto abaxo ão coderada: a IMU a cofguração oldára a plataforma ("trapdow"), o tema de coordeada grate com a Terra e a varáve de etado repreetada pela coordeada de poção, velocdade, e attude. eta forma, a euaçõe dfereca ue mecazam a cemátca de avegação para determar a trajetóra e a attude da IMU-MMS, ão dada por 4 :. V. V. V f = f f & ϕ 1 & λ = h& + g 1 ecoϕ V V 1 V 2 V V V ta ϕ 2 Ω ev e ϕ + e V ( V ta ϕ + V ) + 2Ω e ( V e ϕ + V co ϕ ) + e 2 2 V V 2 Ω ev co ϕ e (1) (2)

2 ode & φ 1 & = θ ψ& f f f p b b r b eφ taθ eφ coθ p r coφ 2 p coφ taθ eφ coφ coθ Ω ecoϕ + V e V Ω eeϕ Vtaϕ (3) fu b u = p2 fv bv (4) fw bw e [ ϕ,λ, h ] ão a lattude geodétca, a logtude, e a alttude; [V,V,V ] ão o compoete de velocdade da plataforma a dreçõe orte, Lete, e adr local; [ φ, θ, ψ ] ão o âgulo de rolameto, arfagem e guada ("roll, ptch, yaw"); e é o rao terretre lete acrecdo da alttude h; é o rao terretre orte acrecdo da alttude h; p2 é a matrz de rotação do tema do corpo (plataforma) para o tema de avegação (orte, Lete, e adr); Ω e é a rotação da Terra; g é a gravdade local computada levado em cota a atração gravtacoal e a aceleração cetrípeta; [p,, r] ão a taxa agulare medda pelo gro e [b p, b, b r ] ão ua derva repectvamete; [f u, f v, f w ] ão a medda acelerométrca e [b u, b v, b w ] ão eu ba repectvamete; e [f, f, f ] ão a medda acelerométrca traformada para o tema. Coderamo ada ue o "ba" e derva do acelerometro e gro, a ua forma ma mple, podem er modelado cotate por trecho repectvamete fazedo: III. b& b& a g =, =, FILTO ÃO LIA KALMA SIGMA-POTO Para problema ão-leare, o fltro ão leare covecoa, como o fltro eteddo de Kalma, podem apreetar uma performace pobre em razão de problema erete ao tema ão leare, prcpalmete devdo à egute upoçõe,6,7,8 Learzação é uma boa aproxmação do proceo (Jacobao da dâmca e medda); Proceo ão Gauao memo para problema altamete ão leare; ormalmete omete a méda (prmero mometo) é predto como ão-lear, vto ue a covarâca é learzada. e (5) O método do FKSP cote em obter um cojuto mímo de amotra, o gma-poto, em toro da méda 9,1,11 de teree e ue eja ada repreetatvo do tema ão lear. te método procura obter formaçõe obre o prmero mometo (méda, covarâca, tercero mometo cetral ou ametra e curtoe) da amotra ecolhda crteroamete. Poterormete, o gma- poto ão predto ão learmete e também calculada ua covarâca predta 1. Com eta aproxmação ão é eceára a learzação do tema, como para o fltro de Kalma teddo (KF), evtado Jacobaa aalítca complexa ue ecetam er calculada a cada etapa da tegração. Para tal, defdo como a dmeão do vetor a er etmado, é gerado um cojuto de 2+1 gma-poto por: o χˆ = xˆ ˆ ˆ + (6) χ = x ( + κ) Pˆ, = 1, K, + χˆ ˆ ) ˆ 1 = x ( + κ P, = 1, K, Ode ( + κ ) Pˆ é a -éma lha ou colua da matrz raz uadrada de ( + κ) Pˆ, e o fator κ é ecolhdo de maera a ecaloar o mometo uperore a 3. Se ( + κ) = 3, também é poível ecaloar algu do uartomometo (curtoe) uado x é gauao 1,11. O peo para poderar a méda e covarâca predta ão dado por: W W o = 1 + = κ ( + κ ) ( 2 + 2κ ), = 1, K, ( 2 + 2κ ), = 1, K,. (7) W = 1 A fae de predção do FKSP é mplemetada atravé da tegração da euacõe (1)-(3), e (5), para cada gma-poto:, +1 ( χ ) & χ = f. (8), A méda e a covarâca predta ão etão calculada por: x = W χ, + 1 (9) = [ χ x ] [ χ ] T 2 P + 1 = W, , + 1 x + 1 (1) = uado o peo de acordo com a euação (7). A abordagem va FKSP ete problema é aplcada omete à fae da predção. A fae de correção do fltro ("meauremet update") ua a euaçõe covecoa do Fltro de Kalma 8, po a medda ão leare em relação ao vetor de etado a er etmado: t K = P H t Pˆ = P H xˆ = x + K t ( H P H + ) ( I - K H ) P [ y H x ] 1. (11) Am edo, um Fltro de Kalma (FK) ão lear de tempo real deomado fltro de Kalma Sgma-Poto (ou Uceted ) (FKSP ou FKU) é a opção a er deevolvda.

3 IV. FILTO ÃO LIA KALMA SIGMA-POTO A TMIAÇÃO OS PAAMTOS A IMU-MMS Foram deevolvdo à pror, coforme referêca [1] e [2], o procedmeto para a determação do ba e derva da IMU-MMS atravé da técca do FKSP. a eüêca foram aalado o deempeho e o erro evolvdo fazedo comparaçõe etre: * Itegração mple, CI exata, coderado b a = e b g =, * Itegração mple, CI exata com valore de b a e b g précalculado atravé do FKSP, * Utlzado FKSP com = 9 elemeto, ão coderado b a e b g, * Utlzado FKSP com = 15 elemeto, coderado b a e b g. A cocluõe deta mulaçõe determaram ue é eceáro levar em cota o erro temátco, eja etmado-o prevamete, ou etmado-o cojutamete o FKSP. Tato meor o erro como ma rápda a covergêca do valore de ba ou derva e ete erro b a e b g fzerem parte do etado a er etmado em ualuer fltro projetado para avegação. ta abordagem tora o fltro robuto também para varaçõe loca da IMU ode em regme de trabalho dferete ete erro teham a cotrbução de derva o tempo, fator de ecala e dealhameto, amplfcado. O receptor GPS 13 utlzado para ete trabalho, Ahtech Z- 12, faz uo completo do tema de pocoameto e provê precão de avegação com ualfcação aeroáutca. O cojuto é um tema compoto de receptor, batera, atea e cabo coaxal. O dado podem er armazeado teramete ao receptor e depo recuperado va comucação eral S- 232, para pó-proceameto, ou etão podem er utlzado em tempo real valedo-e deta mema porta de comucação. ete trabalho foram utlzada a oluçõe de avegação (poção, velocdade, pdop e o tempo correpodete) forecda pelo receptor. ete expermeto, para efeto de comparação, é de teree o erro cometdo a medção da dtâca ao atélte a determação da poção dete. É caracterzada pelo fatore OP (luto of Preco), e ete trabalho é uado epecfcamete o pdop, ue é uma medda de erro de poção trdmeoal etre receptor e atélte. V. FILTO ÃO LIA KALMA SIGMA-POTO O ALGOITMO A ITGAÇÃO IMUGPS O Fltro de Kalma Sgma-poto para a tegração IMUGPS codera o repectvo ba do acelerometro e derva do gro como elemeto do vetor de etado. Ao algortmo ão forecda a codçõe ca e a letura eüêca e crozada etre o valore obtdo pela IMU e pelo receptor GPS. É feta a mplemetação do fltro FKSP com a geração do gma-poto e a poteror tegração umérca da euaçõe de mecazação da cetral ercal. São obtda a méda e covarâca predta para cada vetor de etado com a poteror correção (atualzação) atravé do Fltro de Kalma covecoal para a attude e a avegação. A faldade do algortmo de avegação é propagar a formaçõe de poção, velocdade e attude em fução de cada letura recebdo do acelerômetro e gro, etre o tempo de letura do receptor GPS e também uado ete últmo tem eu a bloueado por obtáculo fíco. Fg. 1 - eceptor GPS, cojuto IMU e computador O eupameto da fgura 1 fo motado em um automóvel e o ambete ecolhdo e a trajetóra percorrda para o tete é exbdo a vta aérea motrada a fg. 2. VI. MOTAGM O XPIMTO O expermeto uou uma IMU-MMS de baxo cuto, um cojuto receptor GPSbatera e um computador para recepção e armazeameto do dado coforme motrado a fgura 1. A IMU-MMS (Modelo Crobow IMU-C4-2) 5 é um tema de medda de 6-exo, para medda de aceleração lear em toro de 3 exo tr-ortogoa; e taxa de rotação em toro de 3 exo tr-ortogoa, ue pobltam medda completa da dâmca do tema. A medda da IMU ão obtda de uma porta de comucação eral S-232 para armazeameto da medda o computador. Fg. 2 Vta aérea do trajeto percorrdo

4 O cojuto percorreu o trajeto armazeado o dado da IMU (aceleração, taxa agulare e tempo correpodete) o computador. Por e tratar de um expermeto pó-proceado, a correlação etre a dferete amotra do aruvo armazeado fo obtda atravé do cohecmeto do elemeto comum à mema, ou eja, o tempo. É obrgatóro o cohecmeto da repectva marcaçõe de tempo em ue e deu cada medda, em relação a um padrão (GMT), de forma ue eja poível uma crozação etre letura do dado ao tempo comum da IMU, do computador e do receptor GPS. A fgura 3 reume o fucoameto do algortmo e motra ue apó a fae de predção, uado da extêca de dado dpoíblzado pelo receptor GPS (P,V, pdop,t) pode er feta uma atualzação ou correção do valore de poção e velocdade obtdo pela tegração da medda forecda pela IMU (aceleraçõe e taxa agulare). A decão da atualzação ou ão da medda é tomada em fução do valore de pdop (ualdade) do GPS para a correpodete medção. eta forma, o valor de pdop erá uado como referêca para a ualdade do al GPS o fltro FKSP. Attude:,1 o para oll e Ptch e,5 o para Yaw Ba do acelerômetro:1,e-3 m 2 erva do Grocópo: 5,e-3 rad O valore ão típco para medda de poção e velocdade obtda para GPS 12. etre o âgulo de attude, o âgulo de yaw (guada) fo coderado o meo obervável (maor devo-padrão) a prátca. A matrz de dedade epectral de potêca fo coderada dagoal e cotate com o ruído: vv θθ baba bgbg =,125m =,5 =,125m =,25 correpodete ao ruído a euaçõe dfereca de velocdade (2), de taxa de attude (3), de bae e derva (5) do gro e acelerômetro. por fm a varâca do ruído da medda ( ) adotada: 3 3 VII. Fg. 3 agrama IMUGPS com tegração ao FKSP SIMULAÇÕS Para o algortmo do Fltro de Kalma Sgma-Poto (FKSP), a aplcação da euaçõe (6) a (9), foram utlzado o valore: a) para o vetor de etado x em P1: X(1) - Lattude Geodéca = 23, S X(2) - Logtude Geodéca = 314, X(3) - Alttude Geodéca = 613,866 m X(4) - Velocdade orte = m X(5) - Velocdade Lete = m X(6) - Velocdade adr = m X(7) - oll = X(8) - Ptch = -,49 X(9) - Yaw = 314,47 X(1) - b ax = -,12 m 2 X(11) - b ay =,57 m 2 X(12) - b az = -,13 m 2 X(13) b gx =,15 o eg X(14) b gy =,14 o eg X(15) b gz =,38 o eg b) para a covarâca Ical [ P ]: Poção: 1 metro em coordeada horzota (lattude e logtude) e 3 metro a coordeada vertcal, Velocdade:,1 m para a trê compoete, (1,1) = 2,45465e-12 rad 2 (lattude) (2,2) = 2,45465e-12 rad 2 (logtude) (3,3) = 9 m 2 (alttude) (4,4) =,1 (m) 2 (V ) (5,5) =,1 (m) 2 (V ) (6,6) =,1 (m) 2 (V ) (7,7) = e-7 rad 2 (roll) (8,8) = e-7 rad 2 (ptch) (9,9) = e-5 rad 2 (yaw) O expermeto fo amotrado a uma taxa de 2Hz para a letura da UMI e a 2 Hz para o receptor GPS. A trajetóra bem como a medda de dtâca ão apreetada em coordeada e ua udade em metro. O algortmo, além de procear a trajetóra em fução da letura do acelerômetro e grocópo, recebe de forma cotíua a formaçõe de poção e velocdade determada pelo receptor GPS. Para a aále do deempeho do algortmo em fução da ualdade do al GPS (fltragem uado o valore de pdop) ão etudado do cao ode é poível medr a atuação do fltro e eu deempeho. Cao 1 Coderado toda a letura obtda pelo receptor GPS como válda (em fltragem) Para ete cao, toda a letura obtda pelo receptor GPS ão realmetada para o algortmo de avegação, depedetemete de ua ualdade (em lmtação de pdop). A fgura 4 faz a comparação, em coordeada, etre a trajetóra forecda pelo receptor GPS e a obtda atravé do proceameto do algortmo IMUGPS.

5 Fg. 4 Trajetóra percorrda GPS X algortmo em lmtação de pdop O trecho percorrdo é ujeto a trecho com perda de al GPS, ue é a regão de teree para a avalação do deempeho do algortmo em etudo. Para a codçõe deta aále, a perda de valore cofáve de poção e velocdade forecda pelo receptor GPS é a repoável pelo devo a trajetóra proceada pelo fltro devdo à falta de ualdade do al. A fgura 5 detalha o trecho de teree ode houve a perda de al GPS e a repota do fltro eta regão.. Fg. 6 Trajetóra percorrda GPS X algortmo com lmtação de pdop A tedêca do algortmo é egur a attude formada ao fltro matedo a trajetóra aproxmadamete correta o trecho ode há obtáculo ou lmtaçõe de atélte víve ue reultam em uma ualdade feror do al GPS. Fg. 5 egão ode há perda de al GPS e comportameto do algortmo em fltragem de al GPS de má ualdade Fg. 7 egão com perda de al GPS e atuação do algortmo elmado-e o a GPS com Pdop>5 Cao 2 - Coderado apea a letura obtda pelo receptor GPS com Pdop 5 (com fltragem) m codçõe dêtca à do cao 1, o algortmo recebe de forma cotíua a formaçõe de poção e velocdade do receptor GPS, cotudo, para ete cao ão atualzada e proceada dete, apea a com ualdade de Pdop 5. Para a medda com pdop >5, ão utlzada a medda de poção e velocdade obtda a partr da medda da IMU. eulta ete cao para o cojuto IMUGPS a ova trajetóra obtda pelo algortmo ue é o motrado a Fg. 6. a aále do reultado obtdo a regõe com perda de al GPS e com geometra rum (Pdop>5), a tegração UMIGPS garatu a terpolação da trajetóra com erro meore ue o cometdo pelo GPS, dede ue elmado dado GPS trecamete ru. O reultado obtdo motram ue a trajetóra obtda é depedete da correçõe mpota à olução de avegação forecda pelo receptor GPS. Como a ualdade obtda do receptor GPS é fução da geometra local, to afeta dretamete a ualdade de avegação. Por ete motvo é mprecdível a elmação de valore ru obtdo doreceptor GPS, permtdo ue a trajetóra eja calculada apea atravé da UMI para ete valore.

6 VIII. COCLUSÕS te trabalho decreveu o procedmeto e o reultado obtdo a determação da trajetóra a do dado de uma IMU e ua tegração a receptor GPS por meo de um algortmo utlzado a técca do Fltro de Kalma Sgma Poto. A técca Sgma-Poto é atratva em tema ode a expreõe da dâmca do tema ão etão dpoíve ou e apreetam a forma a ão permtr uma fácl learzação. Além do, é capaz de ecaloar mometo de ordem uperor aumetado lgeramete a carga computacoal. a avalação do deempeho da tegração de uma IMU- MMS com um receptor GPS, cocluu-e pelo reultado obtdo, ue a fuão da formaçõe de aceleração e velocdade agular de uma IMU com a de poção e velocdade de um receptor GPS atravé de FKSP aumeta a autooma dea mema IMU operado em cao de perda do al GPS. O reultado motraram uma repota rápda do fltro. O trabalho futuro evolverão campaha ma loga para a caracterzação do fltro, avalar ua robutez e a tolerâca à falha. Sympoum o Aeropaceefee Seg, Smulato ad Cotrol, SPI, [1] Juler, S.J. ad Uhlma, J.K., ad urrat-whyte, H.F., 2, A ew method for the olear traformato of mea ad covarace flter ad etmator. I Traacto o Automatc Cotrol, Vol. 45, Iue 3, p [11] Juler, S.J. ad Uhlma, J.K., 24, Uceted Flterg ad olear tmato. Proceedg of the I, Vol. 92, o. 3. [12] Mra, P. ad ge, P., 21, "Global Potog Sytem: Sgal, Meauremet ad Performace", Gaga- Jamua Pre, Lcol. [13] Ahtech Surveyg Product, - Ahtech, Z-12 GPS ecever Operato ad eferece Maual, evo A, 999. IX. AGACIMTOS O autore deejam agradecer o orgazadore do eveto SBI 21 pela ateção e dedcação para ue o trabalho pudee er apreetado. X. FÊCIAS [1] Kuga, H.K. ad Lope,.V.F. ad woegerer, W., "xpermetal tatc calbrato of a IMU (Iertal Meauremet Ut) baed o MMS." XIX Cogre of Mechacal geerg - COBM 27, Brala, F, Brazl, 5-9 ovember 27. [2] Kuga, H.K. e Mla, P.G. e woegerer,w.; "xpermeto de alhameto de udade de medda ercal baeada em MMS (Mcro lectro-mechacal Sytem)" V Smpóo Bralero de gehara Iercal V SBI, o de Jaero, J, Brazl, 27. [3] woegerer, W. Mecazação de cetral de avegação ercal com dado a partr da tegração de IMU MMS e receptor GPS p. (IP TI156). ertação (Metrado em Mecâca pacal e Cotrole) - Ittuto acoal de Peua paca, São Joé do Campo. 29. [4]Farrel, Jay A. ad Barth, Matthew 1998, The Global Potog Sytem & Iertal avgato, ew Yor, Y, McGraw-Hll. [5]Crobow, 27, IMU Uer Maual Model IMU3CC, IMU4CC, IMU4C. evo B, February 27, ocumet [6] Maybec, P. S., 1979, Stochatc Model, tmato ad Cotrol, Academc Pre, ew Yor. [7] Brow,. G. ad Hwag, P. Y. C., 1996, Itroducto to adom Sgal ad Appled Kalma Flterg, Joh Wley & So, ew Yor. [8] Berma, G. J., 1977, Factorzato Method for crete Seuetal tmato, Academc Pre, ew Yor. [9] Juler, S.J. ad Uhlma, J.K., 1997, A ew xteo of thee Kalma Flter for olear Sytem. Iteratoal