UNSCENTED KALMAN FILTER POSITION ESTIMATION FOR AN AUTONOMOUS MOBILE ROBOT

Transcription

1 Bulletn of the rnslvn Unversty of Brşov Vol. 3 (52) - 21 Seres I: Engneerng Scences UNSCENED KALMAN FILER OSIION ESIMAION FOR AN AUONOMOUS MOBILE ROBO C. SULIMAN 1 F. MOLDOVEANU 1 Abstrct: he Klmn flters hve been wdely used for moble robot nvgton nd system ntegrton. So tht t my operte utonomously, moble robot must now where t s. Accurte loclzton s ey prerequste for successful nvgton n lrge-scle envronments, prtculrly when globl models re used, such s mps, drwngs, topologcl descrptons, nd CAD models. hs pper presents the loclzton of moble robot usng one vrton of the trdtonl Klmn flter: the unscented Klmn flter (UKF). For ths purpose the flter ws mplemented for nown nemtc model of the robot. Key words: utonomous moble robot, Klmn flter, unscented Klmn flter, poston estmton. 1. Introducton Flterng s very used method n engneerng nd embedded systems. A good flterng lgorthm cn reduce the nose from sgnls whle retnng the useful nformton. he Klmn flter (KF) [8] s mthemtcl tool tht cn estmte the vrbles of wde rnge of process. It estmtes the sttes of lner system. hs type of flter wors very well n prctce nd tht s why t s often mplemented n embedded control system nd becuse we need n ccurte estmte of the process vrbles. he KF hs been wdely used for moble robot nvgton [1], [2] nd system ntegrton. So tht t my operte utonomously, moble robot must now where t s. Accurte loclzton s ey perquste for successful nvgton n lrge-scle envronments, prtculrly where globl models re used. he KF hs mny lmttons nd tht s why mny uthors proposed vrous fxes nd modfctons to better estmte the process vrbles [4]. One of the mny vrtons of the Klmn flter s the unscented Klmn flter (UKF). hs flter s bsed on the unscented trnsform (U) [5]. he UKF wors by pproxmtng Gussn dstrbuton, whch s much eser thn pproxmtng n rbtrry nonlner functon. he UKF s computtonlly more costly thn the EKF, but t reduces estmton errors. One of the dvntges of the UKF over the EKF s tht t doesn t need to derve the Jcobn mtrces. he most mportnt pplcton of the UKF s 1 Dept. of Automtcs, rnslvn Unversty of Brşov.

2 3 Bulletn of the rnslvn Unversty of Brşov Vol. 3 (52) - 21 Seres I n smultneous loclzton nd mp buldng (SLAM). It hs been ppled to ground moble robots [3], [6], but hs been successfully ppled even to unmnned erl vehcles (UAV) [7]. hs pper presents the mplementton for the unscented Klmn flter for n utonomous moble robot bsed on Acermn steerng. he smultons n ths pper wll show the ccurcy of the UKF. In secton 2 of the pper we wll present the trdtonl KF. In secton 3 we present the of the UKF mplementton for n utonomous moble robot. he lst two sectons of the pper contn the smulton results nd the conclusons drwn from these smultons nd future wor. 2. he Klmn Flter he Klmn flter s composed from set of mthemtcl equtons tht provde the computtonl mens for estmtng the stte of process, n wy tht mnmzes the men of the squred error. hs type of flter s very powerful becuse: t supports estmtons of pst, present, nd even future sttes, nd t cn do so even when the precse nture of the modeled system s unnown. he Klmn flter estmtes process by usng form of feedbc control: the flter estmtes the process stte t some tme nd then obtns feedbc n the form of nosy mesurements. he equtons of the Klmn flter fll nto two ctegores: tme updte equtons nd mesurement updte equtons. he tme updte equtons re responsble for projectng forwrd (n tme) the current stte nd error covrnce estmtes to obtn the pror estmtes for the next tme step. he mesurement updte equtons re responsble for the feedbc, for ncorportng new mesurement nto the pror estmte to obtn n mproved posteror estmte. he specfc equtons for the predcton step (tme nd mesurement updtes) re: x ˆ + B u, (1) = F xˆ = F F Q, (2) + where: F s the stte trnston model whch s ppled to the prevous stte x ˆ ; B s the control-nput model whch s ppled to the control vector u. From the bove equtons we cn see how the stte nd covrnce estmtes re projected forwrd n tme, from the tme step 1 to step. In the observton step we hve: ~ y z H xˆ, (3) = = H H R, (4) S + where: y~ s clled the nnovton or mesurement resdul; H s the observton model whch mps the true stte spce nto the observed spce; represents the predcted ( pror) estmte covrnce; Q s the covrnce of the process nose. For the mesurement updte step we hve: = H S K, (5) ˆ xˆ 1 + x = K ~ y, (6) = ( ) I K H. (7) where: K s the optml Klmn gn nd S s the nnovton (or resdul) covrnce. he frst ts durng the mesurement updte step s to compute the Klmn gn (5). he next step s to ctully mesure the process to obtn z, nd then to generte n posteror stte estmte by ncorportng the mesurement s n (6). he fnl step s to obtn n posteror error covrnce estmte v (7). After ech tme nd mesurement updte pr, the process s repeted wth the prevous posteror estmtes used to project or predct the new pror estmtes.

3 Sulmn, C., et l.: Unscented Klmn Flter oston Estmton for n Autonomous Moble Robot he UKF Implementton o model the robot poston, we wsh to now ts x nd y coordntes nd ts orentton. hese three prmeters cn be combned nto vector clled stte vrble vector. he robot uses n overhed cmer to obtn the nformton bout how fr the robot hs trveled nto the envronment so tht t cn clculte ts poston. hese mesurements nclude component of error. If trgonometry s used to clculte the robot s poston t cn hve lrge error nd cn chnge sgnfcntly from frme to frme dependng on the mesurement t the tme. For the UKF mplementton we hve used the nemtc model of n utonomous moble robot bsed on Acermnn steerng (see Fgure 1). he moble robot s supposed to move n 2D coordnte system. dstnce between the rer xel nd front one, nd Φ s the steerng ngle. From the nonlner system of equtons presented bove, we deduced tht we cnnot use the trdtonl form of the KF, nsted we use the UKF form. he problem of propgtng Gussn rndom vrble through nonlner functon cn lso be pproched usng the unscented trnsform. Insted of lnerzng the functon, ths method uses set of ponts nd propgtes the through the nonlner functon, nd thus elmntng the process of lnerzton (see Fgure 2). he ponts re chosen n such mnner tht ther men, covrnce nd possbly ther hgher order moments mtch the Gussn rndom vrble. Fg. 1. he utonomous moble robot representton n 2D coordnte system he nemtc equtons for the moble robot bsed on Acermnn steerng re presented n the followng: x& = V cosθ, (8) y& = V snθ, (9) V tn Φ θ & =. (1) L In the bove system of equtons V s the velocty of the robot, L represents the Fg. 2. Unscented Klmn flter propgton of sgm ponts from Gussn dstrbuton through nonlner functon, nd recreton of the Gussn dstrbuton by computng the men nd covrnce of the results he men nd the covrnce cn be reclculted from the propgted ponts, nd thus we cn obtn more ccurte result compred to the ordnry functon lnerzton. he mn de s lso to pproxmte the probblty dstrbuton nsted of the functon. hs strtegy decreses the computtonl complexty nd, t the sme tme, t ncreses estmte ccurcy, thus gvng more ccurte results.

4 32 Bulletn of the rnslvn Unversty of Brşov Vol. 3 (52) - 21 Seres I he flter strts by ugmentng the stte vector to L dmensons, where L s the sum of dmensons n the orgnl stte-vector, model nose nd mesurement nose. he covrnce mtrx s smlrly ugmented to L 2 mtrx. ogether ths forms the ugmented stte estmte vector xˆ nd covrnce mtrx ˆ : xˆ 1 x ˆ 1 =, (11) 1 = Q. (12) R he next step conssts of cretng + 1 sgm-ponts n such wy tht they together cpture the full men nd covrnce of the ugmented stte. he X mtrx s chosen to contn these ponts, nd ts columns re clculted s follows: x x = =, X x, (13) X 1, = 1,..., L, (14) = x + ( L + λ) X 1 = x ( L + λ), = L +1,...,, (15) L 1 where: x nd represent the men nd covrnce of the rndom vrble x. hen the sgm ponts re propgted through the nonlner functon: X 1 = f ( X ), =,...,, (16) from whch the men nd covrnce cn be pproxmted: x ˆ = W X, (17) = m = Wc ( X xˆ )( X xˆ ) = (18) Ech sgm-pont s lso ssgned weght. hese weght re derved by comprng the moments of the sgmponts wth ylor seres expnson of the models whle ssumng Gussn dstrbuton. he resultng weghts for men m nd covrnce c estmtes then becomes: λ W m =, (19) L + λ λ 2 W c = + (1 α + β), (2) L + λ Wm = Wc =.5( L + λ), = 1,...,, (21) 2 λ = α ( L + ) L. (22) In the bove equtons α nd control the spred of the sgm ponts nd β s relted to the dstrbuton of x. he α prmeter s chosen n the ntervl < α 1, nd t s set to low vlue,.1, to vod non-locl effects. When we frst use the flter we need to ntlze t:

5 Sulmn, C., et l.: Unscented Klmn Flter oston Estmton for n Autonomous Moble Robot 33 xˆ x ˆ = nd = Q R. γ 1 = 2 = h( X ),... L, (23) z ˆ = W γ. (24) = m he flter then predcts next stte by propgtng the sgm-ponts through the stte nd mesurement models, nd then clcultng weghted verges nd covrnce mtrces of the results: he predctons re then updted wth new mesurements by frst clcultng the mesurement covrnce nd stte-mesurement cross correlton mtrces, whch re then used to determne the Klmn gn: zz xz = W = c = W = c [ γ z ] [ γ z ] ˆ ˆ, (25) [ X x ] [ γ z ] ˆ ˆ, (26) = xz zz K, (27) x xˆ + K ( z zˆ ), (28) ˆ = 1 = K zz K. (29) 4. Expermentl Results In ths secton of the pper we show the results obtned by smultng UKF cse for n utonomous moble robot. For ths purpose the UKF flter ws mplemented n the progrmmng nd smulton envronment clled Mtlb. For our tests we hve consdered very smple cse: robot tht follows predefned pth. he poston of the robot n the envronment s obtned by usng n overhed cmer. On the robot we ve put squre mrer. o obtn the poston of the mrer we ve developed n pplcton n Vsul C++, usng the ugmented relty toolbox ARoolt. he dt obtned from the mrer detecton pplcton s fed nto the UKF whch wll estmte the robot s poston from the rel nosy mesurements. In the below fgure t s presented the estmted pth of the robot compred to the rel pth. he green lne s the predefned pth tht ws chosen rndomly, nd the red lne s the estmted pth. Also, n the smultons, the blue trngle represents the robot s rel poston nd the red one s the robot s estmted poston. 5. Conclusons In ths pper one of the mn vrtons of the Klmn flter used for the poston estmton of n utonomous moble robot bsed on Acermnn steerng. From the smulton results shown erler we hve deduced tht the UKF hs shown promsng results. he dfference n nonlnerty between systems encountered n everydy lfe cn gve bg dfference n the results obtn from the presented flterng technques. he dvntge of the UKF over other vrtons of the clsscl KF ncreses wth the degree of nonlnerty n the mesurement model. In the future we wll try to combne the fuzzy logc the flter presented n ths pper for better poston estmton. Furthermore, we wll use the UKF n vsul bsed SLAM system, where cmer clbrton nd correct feture detecton ply vtl role n solvng the dt ssocton problem.

6 34 Bulletn of the rnslvn Unversty of Brşov Vol. 3 (52) - 21 Seres I Fg. 3. rjectory estmton wth the UKF Acnowledgement hs pper s supported by the Sectorl Opertonl rogrmme Humn Resources Development (SO HRD), fnnced from the Europen Socl Fund nd by the Romnn Government under the contrct number OSDRU/6/1.5/S/6. References 1. Cho, B.S., Lee, J.J.: he oston Estmton of Moble Robot under Dynmc Envronment. In: roceedngs of the 33 rd Annul Conference of the IEEE Industrl Electroncs Socety, 27, p Hu, C., Chen, W., Chen, Y., Lu, D.: Adptve Klmn Flterng for Vehcle Nvgton. In: Journl of Globl ostonng Systems 2 (23) No. 1, p Holmes, S., Klen, G., Murry, D.W.: A Squre Root Unscented Klmn Flter for Vsul MonoSLAM. In: roceedngs of IEEE Interntonl Conference on Robotcs nd Automton - ICRA, 28, p Johnson, R., Ssde, J., Zlews, J.: Klmn Flter Enhncement for UAV Nvgton. In: roceedngs of the Interntonl Symposum on Collbortve echnologes nd Systems, Juler, S., Ulhmnn, J.K.: A New Extenson of the Klmn Flter to Nonlner Systems. In: roceedngs of Areosense: 11 th Interntonl Symposum Aerospce/Defense Sensng, Smulton, nd Controls 368 (1997), p Ko, S., Cho, J., Km, B.: Indoor Moble Loclzton System nd Stblzton of Loclzton erformnce usng re-flterng. In: Interntonl Journl of Control, Automton, nd Systems 6 (28), p Sünderhuf, N., Lnge, S., rotzel.: Usng the Unscented Klmn Flter n Mono-SLAM wth Inverse Depth rmetrzton for Autonomous Arshp Control. In: IEEE Interntonl Worshop on Sfety, Securty nd Rescue Robotcs, 27, p Welch, G., Bshop, G.: An Introducton to the Klmn Flter. In: echncl Report: R95-41, Unversty of North Croln, 26.