XX X Vol. XX, No. X 200X X ACTA AUTOMATICA SINICA Month, 200X 1 1 1.,,, ; (MCMC, ;, MCMC, ;.,,, TP242.6 Robut Robot Monte Carlo Localzaton WU Er-Yong 1 XIANG Zh-Yu 1 LIU J-Ln 1 Abtract A robot localzaton algorthm baed on partcle flter preented. Frtly n order to mprove the flterng effect and decreae the number of partcle needed, one parallel extended Kalman flter ued a the propoal denty of partcle flter, thu partal obervaton nformaton wll be nfued nto the flterng proce. Secondly, n order to enhance the partcle refnng capacty, one mproved Marov Chan Monte Carlo (MCMC reamplng method wth varable boundary of propoal denty put forward; Fnally, the robot localzaton algorthm wth mproved MCMC reamplng advanced, thu the effect of partcle mpoverhment wll be decreaed and the localzaton accuracy wll be mproved. Experment reult how that th algorthm ha the advantage n computatonal complexty, localzaton accuracy and robutne. Key word Robot localzaton, partcle flter, Marov Monte Carlo, reample 1 [1] [2],.,., (EKF,, [1] [3].,, (Monte-Carlo [4] [5],.. Thrun Fox 2007-03-27 2007-09-14 Receved March 27, 2007; n reved form September 14, 2007 (60534007, (2005C14008, (60505017 Supported by Key Project of Natonal Natural Scence Foundaton of P. R. Chna (60534007, Scence Plannng Project of Zhejang Provnce of P. R. Chna (2005C14008, Natonal Natural Scence Foundaton of P. R. Chna (60505017 1. 327 1. Inttute of Informaton & Communcaton Engneerng, Zhejang Unverty, Hangzhou 327 DOI: 10.1360/aa-007-xxxx, Mxture-MCL, [6]. Fox, KD-tree [7]. Fox Burgard, [8] [9]. Dellaert Revere-Jump MCMC,, (MCMC [10].,,, [6] 000, [7] 400,,,,. 1:, ; 2:,,, ;,,,., :., : ;,
2 XX ; MCMC,,, ; ;. 2 x, x (1.,, [4]. p(x z 1: = p(z x p(x z 1: 1 /p(z z 1: 1 (1 (1. {x, w } N =1 p(x z 1:, {x, = 0...N }, {w, = 0...N }. [11] : N p(x 0: z 1: wδ(x 0: x 0: (2 =1 w [11], w p(x 0: z 1: /q(x 0: z 1:, q(x 0: z 1:, (1 : w = p(z x p(x x 1p(x 0: 1 z 1: 1 p(z z 1: 1 1 q(x x 0: 1, z 1: q(x 0: 1 z 1: 1 p(z w x p(x x 1 1 q(x x 0: 1, z 1: (3 q(x x 0: 1, z 1: = q(x x 1, z, x 1 z, : p(z w w x p(x x 1 1 q(x x 1, z (4 : N p(x z 1: wδ(x x (5 =1 N, (5 x. (5. 3 3.1.,, (x, y, θ., q(x x 1, z p(x x 1, ˆx = f (x 1, u, z., ;,,.,., : 1: x 1, ˆx, (6 ; 2:, z,, (7 (8 ; 3:, (9. ˆx = f(x 1, u ˆP = f x ˆP 1 f T x + Q ẑz = h(ˆx, θ ˆP z = ˆP hx h T x + h ˆP θ f h T θ + R K = ˆP f T x ˆP z (6 (7 x = ˆx + K(z ẑz (8 P = ˆP K f ˆP x ˆx = drawsample(x, P ; ˆP = [0] (9 h θ, h x h(x, θ θ x. R Q., (8, N, p(x x 1 p(z x. 3.2 MCMC,, [6]., N eff [11]. N eff [11],.,. [11],.,,.
X 3 (MCMC: Marov Monte Carlo, [10].,,,, ; ;. MCMC,. :,, ;, ; MCMC Metropol- Hatng, (propoal denty functon Q(x, x,.,.,,., 1. 2 x, x, ± x x /2., KD-Tree [12], N 2 N log 2 N, N. 3.1,, P, x,.,.,.,, ;,.,,. 1. 1 Fg. 1 VPDMetropolHatng ( x Q ( x ; x Q( x ; x x acceptance rato p( x z p( x z 1 x 1 x x x x x Metropol-Hatng Metropol-Hatng algorthm wth varable boundary of probablty denty functon,. MCMC.,,., MCMC, 2.,, Metropol-Hatng., ;,,. j j N w j 1 [{ x, } ] N VMCMCReamplng ({ x, w } 1 N ( 0, c = c w c 1 1, for u u N j j x j w N j j 1: N u U[0, N ] -1 1-1 1 ( -1 whle uj c +1 end whle end for 1 1 VPDMetropolHatng ( x 2 MCMC Fg. 2 Improved MCMC reamplng method, (IMCMCRL: Improved Marov Monte Carlo Reamplng Robot Localzton 3. Fg. 3 z w 3 x IMCMCRL The IMCMCRL algorthm advanced n th paper 4 (CMU,, 20 m, 0.5, 361 ;
4 XX, 5 m. CMU Wean Hall, 4,,, 80 m 40 m. 1, (Beam enor model [16],. 000, (SIR: Sequental Importance Reamplng ( 500, 5.,,,,., 3.1,,,. MCMC,.., [11] N eff,.,, ( 820,.,,,. The percent of effectve partcle [%] 120 80 60 40 20 0 0 500 0 1500 6 Fg. 6 MCMC The percent of non-duplcated effectve partcle ung MCMC reamplng method wth fxed boundary 110 Fg. 4 400 350 4 The probabltc grd map ued n the experment IMCMCRL SIR partcle flter The percent of effectve partcle [%] 90 80 70 60 50 40 Localzaton RMS error [cm] 300 250 200 150 50 0 0 20 40 60 80 5 Fg. 5 The experment comparon for localzaton error. 1, 2 5000, MCMC. (0.21 m, 0.13 m, 0.0087 rad, Fg. 7 7 5 30 20 0 500 0 1500 The percent of non-duplcated effectve partcle ung our algorthm,, ;, MCMC., MCMC (IMCMCRL..,,
X 5. Reference 1 Thrun S. Bayean landmar learnng for moble robot localzaton. Machne Learnng, 1998, 33(1: 41-76 2 Mour A I, Roumelot S I, Burdc J W. SC-KF moble robot localzaton: a tochatc clonng Kalman flter for proceng relatve-state meaurement. IEEE Tranacton on Robotc, 2007, 23(4: 717-730 3 Davon A J, Red I D, Molton N D, Stae O. MonoSLAM: real-tme ngle camera SLAM. IEEE Tranacton on Pattern Analy and Machne Intellgence, 2007, 29(6: 1052-1067 4 Arulampalam M S, Maell S, Gordon N. A tutoral on partcle flter for onlne nonlnear/non-gauan Bayean tracng. IEEE Tranacton on Sgnal Proceng, 2003, 50(2: 174-188 5 Lmeta B, Fox D, Ln Lao. CRF-Flter: Dcrmnatve Partcle Flter for Sequental State Etmaton. In: Proceedng of IEEE Internatonal Conference on Robotc and Automaton, Roma, Italy: IEEE, 2007. 3142-3147 6 Thrun S, Fox D, Burgard W. Robut Monte Carlo localzaton for moble robot. Artfcal Intellgence, 2000, 128(1: 99-141 7 Fox D. Adaptng the ample ze n partcle flter through KLD-Samplng. Internatonal Journal of Robotc Reearch, 2003, 22(1: 985-4 8 Fox D, Ko J, Konolge K, Lmeta B, Schulz D, Stewart B. Dtrbuted multrobot exploraton and mappng. Proceedng of the IEEE, 2006, 94(7: 1325-1339 9 Burgard W, Moor M, Stachn C, Schneder F. Coordnated mult-robot exploraton. IEEE Tranacton on Robotc, 2005, 21(3: 376-378 10 Khan Z, Balch T, Dellaert F. MCMC-baed partcle flterng for tracng a varable number of nteractng target. IEEE Tranacton on Pattern Analy and Machne Intellgence, 2005, 27(1: 1805-1819 11 Doucet A, Freta N de, Gordon N. Sequental Monte Carlo method n practce. New Yor: Sprnger-Verlag, 2001 12 Fredman J H, Bentley J L, Fnel R A. An algorthm for fndng bet matche n logarthmc expected tme. ACM Tranacton on Mathematcal Software, 1977, 3(3: 209-226 13 Gamn Danayae M W M, Newman P. A oluton to the multaneou localzaton and map buldng (SLAM problem. IEEE Tranacton Robotc and Automaton, 2001, 17(3: 229-241 14 Baley T. Moble Robot Localzaton and Mappng n Extenve Outdoor Envronment. [Ph. D. dertaton], Unverty of Sydney, 2002 15 Montemerlo M. Fatlam: a factored oluton to the multaneou localzaton and mappng problem. [Ph. D. dertaton], Carnege Mellon Unverty, 2003 16 Fox D, Burgard W. Thrun S, Marov localzaton for moble robot n dynamc envronment. Journal of Artfcal Intellgence Reearch, 1999, 11(1: 391-427.. E- mal: wueryong343@ohu.com (WU Er-Yong Ph. D. canddate at Zhejang Unverty. H reearch nteret nclude the navgaton of ntellgent robot and pattern recognton... E-mal: xangzy@zju.edu.cn (XIANG Zh-Yu Aocate Profeor at Department of Informaton Scence & Electronc Engneerng, Zhejang Unverty. H reearch nteret nclude ntellgent robot and machne learnng. Correpondng author of th paper... Emal: jllu@zju.edu.cn (LIU J-Ln Profeor at Department of Informaton Scence & Electronc Engneerng, Zhejang Unverty. H reearch nteret nclude ntellgent tranportaton and mage proceng.