A Monte Carlo Localization Algorithm for 2-D Indoor Self-Localization Based on Magnetic Field

Transcription

1 03 8h Inernaonal Conference on Communcaons and Neworkng n Chna (CHINACOM) A Mone Carlo Localzaon Algorhm for -D Indoor Self-Localzaon Based on Magnec Feld Xaohuan Lu, Yunng Dong College of Communcaon and Informaon Engneerng Nanjng Unversy of Poss and Telecommuncaons Nanjng, Chna luxaohuanglmpes@6.com, dongyn@njup.edu.cn Xnheng Wang School of Compung Unversy of he Wes of Scoland Pasley, Scoland xnheng.wang@uws.ac.uk Absrac -Evdence shows ha a large varey of anmals use Earh s magnec feld for navgaon. Inspred by hs nrgung ably of anmals, we propose a MCL algorhm ha ulzes local anomales of magnec feld o acheve -D ndoor selflocalzaon. Mone Carlo Localzaon (MCL) s one of he mos popular probablsc echnques due o he hgh effcency and accuracy, bu one poenal problem s parcle mpovershmen. In order o furher mprove he performance of MCL, we employ a cluserng approach o ge he cluserng nformaon and hus resolvng he problem of losng effecve parcles. The approach has been mplemened and nensvely esed n real-world envronmens. The resul shows ha he proposed approach provdes a smple, robus, low-cos soluon for ndoor localzaon. Keywords-magnec feld; cluserng; MCL; ndoor localzaon I. INTRODUCTION Researchers have found ou ha a large varey of anmals possess an ably of magnec sense []. Mgraory brds use magnec cues o fnd her way souh n fall and norh n sprng []. Salamanders and frogs use he magnec feld for orenaon when hey have o fnd he drecon of he neares shore [3]. Behavoral expermenalss have used hese naural movemen paerns o desgn expermens ha allow hem o nvesgae n whch way Earh s magnec nformaon s used for orenaon. Ths shows ha hese anmals have he ably o ge posonal nformaon from naural clues ha arse from he local anomales of he Earh s magnec feld. Modern buldngs wh seel and renforced concree srucures are supposed o have sac and exremely lowfrequency magnec felds [4]. Havernen and Glber use hs phenomenon o desgn a self-localzaon sysem o rack a robo along a corrdor [4]. I s argued ha f he anomales of he magnec felds have compeen varably, s possble o use he magnec felds for user locaon. Thus, as an analogy o he anmal s percepon of Earh s magnec feld, combned wh probablsc approach, we propose an algorhm o ulze he local anomales of magnec feld for -D self-localzaon [5]. In self-localzaon, here are wo key problems need o be solved, one s global poson esmaon and he oher s local poson rackng [6]. Global poson esmaon s he ably o deermne he arge s poson n a prevously esablshed map, gven no oher nformaon han ha he arge s somewhere on he map. Afer he poson esmaon process, poson rackng s o keep rack of ha poson all he me. In hs paper, we manly deal wh he global localzaon problem. Afer successful localzaon, eher of he wo pars should work precsely and effecvely. Many algorhms for self-localzaon have been proposed. Probablsc approaches are among he mos popular mehods n self-localzaon due o he effecveness and robusness, such as Kalman-fler based echnques [7], grd-based Markov localzaon approach [8], Topologcal Markov Localzaon approach [9], and Mone Carlo Localzaon (MCL) [0]. In hese algorhms, he uncerany of he arge s poson s represened by probablsc dsrbuons over he whole sae. Mone Carlo Localzaon approach, also known as parcle fler localzaon [], represens he uncerany n a dfferen way. Insead of descrbng he probably densy self, depcs wh a se of parcles ha are randomly drawn from. The parcles are generaed based on he probably dsrbuon of he sae, and updae he poseror dsrbuon wh mporance resamplng. Bu one poenal problem wh MCL approach s ha of sample mpovershmen: n he resamplng process, samples wh hgh wegh wll be selec many mes, resulng n a loss of dversy. Several mprovemens o he algorhm have been suggesed. In hs paper, we use a cluserng-based MCL echnque [] o esmae he poson of he arge, wh he knowledge of he observaons of he magnec feld and he dynamc nformaon of he arge. The proposed algorhm employs a cluserng approach ha analyzes he dsrbuon of parcles durng he process of MCL. Wh he help of hs nformaon, we could decde how many parcles need o be re-nalze dynamcally, hus resolvng he problem of losng effecve parcles and mprovng he localzaon success rae. The res of hs paper s arranged as follows: The background knowledge s descrbed n secon II. In Secon III, dealed descrpon of our proposed mehodology s gven. Secon IV presens he mplemenaon process and expermenal resuls. Fnally, he conclusons and fuure work are dscussed n secon V IEEE

2 II. BACKGROUND A. Self-Localzaon Durng self-localzaon, he nformaon of he arge and envronmen s called sae. Parcularly we wll work wh a hree-dmensonal sae vecor X = [ x, y, θ], where x, y s he poson and θ s he orenaon of he arge a me. In order o localze self, he arge has o oban conrol acons and measuremens whch gvng he feedback nformaon abou s acons and he suaon of he envronmen. Through he wo knds of neracons, he arge receves wo daa ses: movemen daa and measuremen daa. The measuremen daa s denoed as Z = { z, =,... } up ll he curren me and he movemen daa a me s denoed as u. Gven hs nformaon, he arge has o deermne s locaon as accurae as possble, whch s an nsance of he Bayesan flerng problem. We are neresed n consrucng he poseror densy p( X Z ), whch sands for he curren sae condon on all measuremens. Thus, he problem urns no how o recursvely calculae he densy p( X Z ) a each me-samp. Ths process s dvded no wo phases: one s Predcon Phase, and he oher s Updae Phase [3], correspondng o process he wo daa ses. Predcon Phase: In he frs par, based on he densy px ( Z ) derved from las eraon, we use a moon model denoed as px ( X, u ) o predc he curren sae of he robo px ( Z ) (assumng he curren sae X s only dependen on he prevous sae X accordng o Markov propery). The predcve densy s compued as follows: px ( ) (, ) ( Z px X u px Z ) dx = () Updae Phase: In he second par, combned wh measuremen daa se Z, we use a measuremen model n erms of p( z X ), whch represens he condonal probably of z gven he sae X, o updae he sae of he arge p( X Z ). Here, we propose he measuremen daa z s ndependen of earler measuremens. The poseror densy s compued as follows: ( ) ( ) ( pz ) X px Z px Z pz ( Z ) = () The localzaon process s repeaed wh hese wo seps recursvely. Abou he nal sae, we assume a unform densy p( x 0) over all possble posons. B. Gaussan Process The class of Gaussan process s one of he mos wdely used famles of sochasc process for modelng dependen daa observed over me, or space, or me and space [4]. The fundamenal characerscs, as descrbed below, of a Gaussan process s ha all he fne dmensonal dsrbuons have a mulvarae normal (or Gaussan) dsrbuon. In parcular he dsrbuon of each observaon mus be normally dsrbued. A real-valued sochasc process { X, T}, where T s an ndex se, s a Gaussan process f all he fne-dmensonal dsrbuons have a mulvarae normal dsrbuon. Tha s, for any choce of dsnc values,... k T, he random vecor X = ( X,... X )' k has a mulvarae normal dsrbuon wh mean vecor μ = EX and covarance marx = cov( X, X), whch s denoed by ~(, ) X μ. The random vecor X has a Gaussan probably densy funcon gven by: ( /) / f ( ) ( ) n (de ) exp( ( )' X x = π xμ ( x μ)) (3) C. Mone Carlo Localzaon Mone Carlo Localzaon algorhm s one of he samplngbased mehods [5], who represens he densy p( X Z ) by a se of N random parcles S { k = s k, =... N}. Smlar o he robo localzaon mehods, he key problem s o compue he se of samples S a each me-samp whch s drawn from he poseror densy p( X Z ). The localzaon process s oulned n wo phases: Predcon Phase: sarng wh he parcle se S, we apply he moon model o each parcle s, whch s drawn from he densy px ( s, u ), and ge a new sample s ' whch s drawn from he predcve densy px ( Z ). Updae Phase: jonng he measuremen daa z, we apply he measuremen model o each parcle s ', weghng each parcle by m ( ' = p z s ). Afer ha, we generae he updaed parcle se S by resamplng process. In dong so he parcle wh hgher wegh enjoys a hgher probably o be pcked up. Smlarly, he nal sae S 0 s samplng from he pror. Afer hese wo phases, he localzaon process s p( X 0) repeaed recursvely. D. Cluserng Mehod The goal of cluserng s o reduce he amoun of daa by caegorzng or groupng smlar daa ems ogeher [6]. The mos mporan par s o selec a proxmy measure ha deermnes how he smlary of wo daa ses. Here we choose he Eucldean dsance = + as proxmy measure durng d(p,p j) ( x xj) (y y j) P mean (P ) cluserng and use he mean pon = N as he represenave of a cluser. There are several algorhms n he felds of cluserng, n hs paper we apply he sequenal algorhm Basc Sequenal Algorhmc Scheme due o s smplcy and easy mplemenaon. 564

3 E. MCL III. METHODOLOGY The unqueness of varaons and perurbaons of magnec feld from one locaon o he nex n an ndoor envronmen enables he developmen of a magnec aded poson algorhm. MCL algorhm s ulzed here o esmae he poson of he localzaon arge ha sars from an unknown poson Xsar = [ x, y, θ ] and moves a a random speed and drecon. As MCL uses a parcle fler mehod [7] amng o denoe he dsrbuon of p( X Z ), s necessary o sample he densy p( X ) frs. The nal dsrbuon of N parcles S ( N) s unformly and randomly sampled from p( X 0 ). Wh he movemen model and measuremen model alernavely acng on he parcles, we assgn o each parcle a wegh m ( ' = p z X = s ) n relaonshp wh he measuremen daa. Afer normalzaon and resamplng he wegh se {( s ( n ), m ( n ) )} can approxmaely represen p ( X Z ) whch sands for choosng one of he S ( N) wh probably m. As N, samples from p ( X Z ) are gradually equvalen o samples from ( ) F. Cluserng and Inlsaon p X Z. Durng he MCL process, he parcles gradually gaher ogeher o a ceran small area. Based on he suaon, many parcles could overlap wh each oher and hus lead o a wase of compuaon and a loss of effecve parcles. We use a Basc Sequenal Algorhmc Scheme (BSAS) cluserng mehod o ge he deals abou he convergence degree of he parcles. Wh he help of hs nformaon, we could decde he number of parcles o represen he correspondng area. By pckng up a ceran percenage parcles from he old parcle se and renalzng hs par of he parcles we successfully resolve he problem of losng effecve parcles and mprove he localzaon success rae. The specfc seps of BSAS cluserng are shown n Algorhm. Algorhm m =, C m = {x m} for all N- parcles do d(x (),C k) = mn j md(x (),C j) f d(x (),C K ) > θ m = m +, C m = {x () } else Ck = Ck {x()} end f end for Do { n= Max(C K ) selec p parcles from n re-nalze (n-p) parcles }Whle (n>p) In Algorhm, θ s he hreshold of cluserng dsance. For each new parcle, based on hs dsance from he exsed cluser, he algorhm eher creaes a new cluser or assgns o an already formed one. p s he hreshold of cluserng sze, once he number of cluserng parcles exceed he value, we renalze he par. The man seps of whole proposed mehod are presened n order shown n Algorhm, Algorhm Generae N parcles wh normalzed dsrbuon xyθ xmax ymax π (,, ) {[0, ],[0, ],[0, ]} ( xy, ) sar ( xy, ) robo Δ( xy, ) 0 whle Δ ( xy, ) <Δ ( xy, ) max do ( xy, ) robo ( xy, ) robo wθ ( ) z = hobs (( x, y) robo ) = + movemen model for all N parcles do ( x, ) ( n) ( ( ) ( ), ) n ( n y = x y + w θ ) movemen model m ( n) ( n) ( ) ( (, ) n = m p z x y ) measuremen model end for resample process draw N samples from p ( X Z ) re-nalze par ( n ) m are normalzed o sum ( xy N, ) ( n) (, ) ( n) esmae = m xy n= 0 e= ( x, y) esmae ( x, y) robo end whle In Algorhm, ( xy, ) sar represens an unknown sarng poson of he arge n he begnnng. The maxmum dsance Δ ( xy, ) max s he allowable dsance for he arge o ravel. Obvously, he localzaon process should fnsh durng ha ravel. Nex, we apply movemen model ( xy, ) = ( xy, ) + wθ ( ) on he arge n order o smulae he moon of he arge where w( θ ) ~ U[ 0.5,0.5]. Also, wh he help of observaon daa he magnec measuremen daa z s obaned by mappng he correspondng poson. For all he parcles, based on MCL algorhm, frs ulze he same movemen model o updae parcle posons hen apply mulvarae Gaussan measuremen model wh mean hmap ( x, y) and covarance R: p ( z x, y ) = exp( ( (, )) T ( (, ))) ( ) N / / map map z h x y R z h x y (4) π R where z s he measuremen daa from observaon daa, whle hmap ( x, y ) s made from map daa whch we have esablshed beforehand. Beween resample process, parcle s weghs are updaed n he mulplcave manner and generae a new se of N parcles ha are drawn from he poseror p ( X Z ) wh he probably 565

4 ( n) m. Afer ha, re-nalze par s performed by nalzng ceran parcles n order o keep he effecveness of parcles. ( n) A las, he weghs m are normalzed o sum and hus we could calculae he esmaon locaon ( xy, ) esmae. If he esmaon error e.0 m, he expermen was consdered faled. IV. IMPLEMENTATION G. Expermen Seup and Daa Collecon In hs par, we wll demonsrae he specfc envronmen seup process and daa collecon seps. The framework of buldngs we have chosen as expermen envronmen s made from renforced concree and seel, whch s supposed o have sac and exremely low-frequency magnec felds [8]. The phoo of our expermenal envronmen s shown n Fg., whch presens he lab of Wux Chgoo Ineracve Technology Co Ld. We seleced he mddle 5 5 m space of he lab o ac as our localzaon area due o few obsacles and convenen measuremen. Fg. The 3-D map of magnec feld Apar from magnec map daa, observaon daa ses were acqured usng he same way and used as measuremen daa n he expermens. In order o verfy he sably of magnec feld, we monor a pon n he localzaon area o record he value for days connuously, as shown n Fg 3. The observaon shows ha he magnec feld s no sensve o weaher condons, non-magnec dynamc or sac objecs. Afer acqurng he observaon daa, he localzaon expermens are conduced off-lne n Malab. Fg. Phoos of he ndoor expermenal envronmen. Fg 3. Magnec feld over days. The goal of our expermen s o esmae he poson of he localzaon arge n a -D square space whle he arge s movng a random speed and drecon. The measuremen seup for expermens s presened n Fg.. The sensor used s McroMag3 3-Axs Magnec Sensor Module. The magnec feld B s measured every s (0.5Hz) producng a hreedmensonal vecor m= [ mx, my, mz] conssng of he hree drecon componens, n uns of μt. Magnec maps of magnec feld of he square area n he expermenal envronmens were provded pror o he localzaon expermens. We have wo mehods of geng he fnal magnec maps, one s performng measuremen every 0.5m and applyng a lnear nerpolaon by usng a 0.m sep sze. The oher s obanng he magnec daa drecly every 0.m. I s meconsumng bu more accurae. The magnec map for our expermen envronmen s showed n Fg., whch s generaed by he acual measuremen. Possble odomerc errors were no consdered. H. Expermen Resul In hs secon, we wll demonsrae he expermenal resuls of he proposed algorhm and he basc algorhm n smulaon on PC. Fg. 4 shows an example of a -D global localzaon expermen. The arge sars from an unknown poson. Afer ravelng approxmaely 0.5 m he rue poson (shown wh red crcle) has been correcly esmaed (shown wh green crcle). We can capure he converge procedure by analyzng he rend of he parcles movemen. Whn he range of allowable error, he reducng par successfully conrol he problem of losng effecve parcles. Fg. 5 shows a par of he localzaon resul usng he basc MCL algorhm whou re-nalze par. As we can see ha he densy of parcles s hgher compared wh Fgure 4 and he localzaon faled whle he arge s movng o he oppose poson. 566

5 (a) (b) approach provdes a promsng and smple alernave for solvng he global ndoor -D self-localzaon problem. In he fuure work, he proposed approach can be generalzed o 3-D localzaon problems, combned wh amospherc pressure value o dsngush dfferen heghs. Also, we can ncorporae acve localzaon approach [9,0] no our proposed algorhm, whch means acvely selecng he mos effcen robo s moon drecon and sensor drecon or ake advanage of oher algorhms o acheve a beer performance. ACKNOWLEDGEMENT Ths work was suppored n par by Naonal Naural Scence Foundaon of Chna (No. 6733, ), he Mnsry of Educaon (Chna) Ph.D. Programs Foundaon (No ). Fg 4. Fg 5. (c) (e) A specfc example of -D global localzaon expermen usng he proposed algorhm. (a) (c) (d) A par of he -D global localzaon resul usng he basc algorhm whou re-nalzng. V. CONCLUSION Ths paper presens a dynamc sze MCL algorhm ulzes local anomales of magnec feld o acheve a -D ndoor selflocalzaon. The repored expermens demonsrae he feasbly and effecveness of he proposed approach. The (b) (d) (f) REFERENCES [] Larry C. Boles, Kenneh J. Lohmann, True navgaon and magnec maps n spny lobsers, Naure 4 (003) [] H. Moursen, G. Feenders, M. Ledvogel, and W. Kropp, Mgraory brds use head scans o deec he drecon of he earhs magnec feld, Curren Bology 9(004) [3] Francsco J. Dego-Raslla, John B. Phllps, Magnec Compass Orenaon n Larval Iberan Green Frogs, Ehology, (007) [4] Janne Havernen, Anss KemppanenGlber, Global ndoor selflocalzaon based on he amben magnec feld, Robocs and Auonomous Sysems 57 (009) [5] Gozck B, Subbu K.P, Danu R, and Maeshro T, Magnec Maps for Indoor Navgaon, Insrumenaon and Measuremen IEEE Transacons (0) [6] Burgard W, Derr A, Fox D, and Cremers A.B, Inegrang global poson esmaon and poson rackng for moble robos: he dynamc Markov localzaon approach, Inellgen Robos and Sysems IEEE/RSJ Inernaonal Conference on (998) vol.. [7] Greg Welch, Gary Bshop, An nroducon o he Kalman Fler, UNC- Chapel Hll, TR 95-04, 006. [8] Okada, K. Yokohama, Grd-based localzaon and mappng mehod whou odomery nformaon, IEEE Indusral Elecroncs Socey, 0: [9] Kosecka J, Fayn L, Vson based opologcal Markov localzaon, Robocs and Auomaon IEEE Inernaonal Conference on (004) vol.. [0] F. Dallae, D. Fox, W. Burgard, and S. Thrun, Mone Carlo localzaon for moble robos, n: Inernaonal Conference on Robocs and Auomaon, vol., 999, pp [] Sanjeev Arulampalam, Maskell S, Gordon N, and Clapp T, A uoral on parcle flers for onlne nonlnear/non-gaussan Bayesan rackng, Sgnal Processng IEEE Transacons on (00) [] Yuefeng Wang, Dan Wu, and Lbng Wu, A Dynamc Sze MCL Algorhm for Moble Robo Localzaon, Proceedngs of he 00 IEEE Inernaonal Conference on Robocs and Bommecs, (00) [3] Aruro Gl, Óscar Renoso, Asuncón Vcene, César Fernández, and Lus Payá, Mone Carlo Localzaon Usng SIFT Feaures, Paern Recognon and Image Analyss, (005) [4] Carl Edward Rasmussen, Gaussan processes for machne learnng, 006. [5] Sebasan Thruna, Deer Foxb, Wolfram Burgardc, and Frank Dellaera, Robus Mone Carlo localzaon for moble robos, Arfcal Inellgence,00,pp [6] B.Ever, S.Landau and M.Leese, Cluser Analyss, Arnold Publsher,( 00). [7] S.Thrun, W.Burgard and D.Fox, Probablsc Robocs, The MIT Press,(005). 567

6 [8] K. Yamazak, K. Kao, K. Ono, H. Saegusa, K. Tokunaga, Y. Lda, S. Yamamoo, K. Ashho, K. Fujwara, N. Takahash, Analyss of magnec dsurbance due o buldngs, IEEE Transacons on Magnecs 39 (003) [9] D.Fox, W.Burgard and S.Thrun. Acve markov localzaon for moble robos, Robocs and Auonomous Sysems(998). [0] Deer Fox, Wolfram Burgard, Hannes Kruppa, and Sebasan Thrun, A Probablsc Approach o Collaborave Mul-Robo Localzaon, Auonomous Robos,(000)pp