Inernaonal Journal of Compuer and Informaon Engneerng Novel Rao-Blackwellzed Parcle Fler for Moble Robo SLAM Usng Monocular Vson Maoha L Bngrong Hong esu Ca and Ronghua Luo Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 Absrac hs paper presens he novel Rao-Blackwellsed parcle fler (RBPF) for moble robo smulaneous localzaon and mappng (SLAM) usng monocular vson he parcle fler s combned wh unscened Kalman fler (UKF) o exendng he pah poseror by samplng new poses ha negrae he curren observaon whch drascally reduces he uncerany abou he robo pose he landmark poson esmaon and updae s also mplemened hrough UKF Furhermore he number of resamplng seps s deermned adapvely whch serously reduces he parcle depleon problem and nroducng he evoluon sraeges (ES) for avodng parcle mpovershmen he 3D naural pon landmarks are srucured wh machng Scale Invaran Feaure ransform (SIF) feaure pars he machng for mul-dmenson SIF feaures s mplemened wh a KD-ree n he me cos of O(log 2 N ) Expermen resuls on real robo n our ndoor envronmen show he advanages of our mehods over prevous approaches Keywords Moble robo smulaneous localzaon and mappng Rao-Blackwellsed parcle fler evoluon sraeges scale nvaran feaure ransform I INRODUCION key prerequse for a ruly auonomous robo s ha can A smulaneously localze self and accuraely map s surroundngs [1] he problem of achevng hs s one of he mos acve areas n moble robocs research whch s known as Smulaneous Localzaon and Mappng (SLAM) One of he popular successful aemps a he SLAM problem was he exended Kalman fler (EKF)[23] One of he lmaons of he EKF s her compuaonal complexy [4] he sandard EKF approach requres me quadrac n he number of feaures n he map for each ncremenal updae he oher s ha requres ha feaures n he envronmen be unquely denfable oherwse hs can cause excessve daa assocaon dffculy [5] Recenly parcle flers have been a he core of soluons o hgher dmensonal robo problems such as SLAM whch when phrased as a sae esmaon problem Murphy Manuscrp receved March 12 2006 hs research s suppored by he Naonal Naural Scence Foundaon of Chna (69985002) and he Naonal H-ech Research and Developmen Program of Chna (2002AA735041) Maoha L s wh he Deparmen of Compuer Scence and echonology Harbn Insue of echonology CO 150001 Chna (e-mal: lmaoha@ heducn) Bngrong Hong s wh he Deparmen of Compuer Scence and echonology Harbn Insue of echonology CO 150001 Chna esu Ca s wh he Deparmen of Compuer Scence and echonology Harbn Insue of echonology CO 150001 Chna Ronghua Luo s wh he Deparmen of Compuer Scence and echonology Harbn Insue of echonology CO 150001 Chna adoped Rao-Blackwellzed parcle flers (RBPF) [6] as an effecve way of represenng alernave hypoheses on robo pahs and assocaed maps Monemerlo e al [7] exended hs mehod o effcen landmark-based SLAM usng Gaussan represenaons of he landmarks and were he frs o successfully mplemen on real robos More recenly RBPF s used wdely o buld map [8910] Daley descrbe he applcaon of FasSLAM usng a rnocular sereo camera [11] Se e al [12] demonsrae he use of Scale Invaran Feaure ransform (SIF) pon feaures as landmarks for he SLAM problem usng a rnocular sereo camera Davson e al [13] demonsrae a sngle-camera SLAM algorhm capable of learnng a se of 3D pon feaures Mos of hese vson-based mehods use he sereo camera o oban sraghly he 3D feaure and he assocaon problem eher beween feaures n successve camera frames or beween observed feaures and map feaures s solved ambguously In hs paper we presen an nvesgaon no he use of monocular vson for SLAM n ndoor envronmen wh 3D feaure landmarks whch are srucured from he SIF feaure machng pars hese 2D SIF feaures are used o srucure 3D landmarks because hey are nvaran o mage scale roaon and ranslaon as well as parally nvaran o llumnaon changes and affne or 3D projecon and her descrpon s mplemened wh mul-dmensonal vecor [14] hs combnaon can resul n many hghly dsncve landmarks from envronmen whch smplfes he daa assocaon problem o only dsngushng unque landmarks We presens a fas and effcen algorhm for machng feaures n a KD-ree n he me cos of O(log 2 N ) [15] Followng [67] our approach apples RBPF o esmae a poseror of he pah of he robo where each parcle has assocaed wh an enre map n whch each landmark s esmaed and updaed by he unscened Kalman fler (UKF) [16] and UKF s used o sample new poses ha negrae he curren observaon whch drascally reduces he uncerany abou he robo pose Furhermore he number of resamplng seps s deermned adapvely [17] whch serously reduces he parcle depleon problem and nroducng he Evoluon sraeges (ES) for avodng parcle mpovershmen [18] All of hese specales can make daa assocaon n hs paper more robus han oher mehods and he bul precse map only need a small number of parcles he paper s organzed as follows: In he nex secon he RBPF for SLAM problem s brefly revewed and hen he novel RBPF mehod s descrbed n deal and secon 3 Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 976 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 provdes a dealed s mplemenaon for monocular vson-based SLAM n unknown ndoor envronmen Expermen resuls and dscussons are presened n secon 4 wh concluson n secon 5 II NOVEL RAO-BLACKWELLIED PARICLE FILER FOR SLAM Consder he case of a moble robo movng hrough an unknown envronmen conssed of a se of landmarks he landmark n s denoed θ n he robo moves accordng o a known probablsc moon model p(s u s -1 ) where s denoes he robo sae a me and he conrol npu u carred ou n he me nerval [-1 ] As he robo moves around akes measuremens z of s envronmen hrough observaon model p(z s θn ) where θ s he se of all landmarks and n s he ndex of he parcular landmark observed a me he SLAM problem s o recover he poseror dsrbuon p(s θ 1 θ M z u n ) where M s he number of landmarks observed so far and he noaon s denoes s 1 s (and smlarly for oher varables) In [6] Murphy e al provde an mplemenaon of RBPF for SLAM: M θ1 θm θ n 1 n p( s z u n ) p( s z u n ) p( s z n ) (1) hs can be done effcenly snce he facorzaon decouples he SLAM problem no a pah esmaon problem and ndvdual condonal landmark locaon problems and he quany p(θ n s z n ) can be compued analycally once s and z are known and he amoun of compuaon needed for each ncremenal updae says consan regardless of he pah lengh Each map s consruced gven z and he rajecory s represened by he correspondng parcle Each parcle s of he form S () {s () µ () 1 Σ () 1 µ () M Σ () M } where () ndcaes he ndex of he parcle; s () () s s pah esmae µ m () and Σ m are he mean and varance of he Gaussan represenng he m-h landmark locaon Our novel RBPF updae s performed n he followng seps: Pror Lkelhood Fg 1 Movng he samples n he pror o regons of hgh lkelhood s mporan f he lkelhood les n one of he als of he pror A Samplng New Poses Usng UKF Here we need o calculae he poseror over robo pahs p(s u z n ) approxmaed by a parcle fler Each parcle n he fler represens one possble robo pah s from me 0 o me Snce he map landmark esmaes p(θ n s z n ) depend on he robo pah he parcles samplng sep s very mporan However mos mehods use he sae ranson pror p(s u s -1 ) o draw parcles Because he sae ranson does no ake no accoun he mos recen observaon z especally when he lkelhood happens o le n one of he als of he pror dsrbuon or f s oo narrow as showed n Fg 1 If an nsuffcen number of parcles are employed here may be a lack of parcles n he vcny of he correc sae leadng o dvergence of he fler hs s known as he parcles depleon problem In our mehods he -h new pose s () s drawn from he poseror p(s s -1() u z n ) whch akes he measuremen z no consderaon along wh he landmark n and s -1() s he pah up o me -1 of he -h parcle An effecve approach o accomplsh hs s o use he unscened ransformaon (U) generaed Gaussan approxmaon: ps s u z n Ns s% P N (2) 1() () () ( ) ~ ( ; ) 12 U can compue he mean and covarance up o he hrd order of he aylor seres expanson of he nonlnear observaon funcon g(θ n s ) Le L be he dmenson of s he U compues mean and covarance as follows: 1) Deermnscally generae 2L+1 sgma pons S {χ W }: χ0 s% χ s% + ( ( L+ λ) Ps ) 1 L χ s% ( ( L+ λ) Ps ) 12 L+ L (3) m c m 2 W0 λ ( L+ λ) W0 W0 + (1 α + β) m 2 W 1 (2 ( L+ λ)) 12 L λ α ( L + γ) L (4) Where γ s a scalng parameer ha conrols he dsance beween he sgma pons and he mean s α s a posve scalng parameer ha conrols he hgher order effecs resuled from he non-lnear funcon g β s a parameer ha conrols he weghng of he 0-h sgma pon α0 β0 and γ2 are he opmal values for he scalar case ( (L+λ) P s ) s he -h column of he marx square roo 2) Propagae he sgma pons hrough he nonlnear ransformaon: g( θ χ ) 02 L (5) n 3) Compue he mean and covarance of as follows: 2L m 2L c 0 z 0 z % W P W ( z % )( z % ) (6) Now we follow UKF algorhm o exend he pah s () by samplng he new poses s () from he poseror p(s s -1() u z n ): 1) Calculae he sgma pons: χ { s% s% ± ( L+ λ) P } (7) () () () () 1 1 1 1 2) Usng moon model o predc: Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 977 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 *() () () () 2L m() *() -1 f 1 u s% 1 W j 0 j j -1 χ ( χ ) χ () 2 L c () *() () *() () 1 [ 0 j χ j -1- % 1 ][ χ j -1- % 1] j P W s s 3) Incorporang new observaon z : *( ) *( ) ( ) 2L m( ) *( ) 1 ( χ -1 θn ) 1 0-1 j j j () 2 L c() *( ) () *( ) () zz [ -1-0 1 ][ -1-1] Wj j z% j j z% () 2 L c () *( ) () *( ) () sz W [ 0-1- 1 ][ -1-1] j j χ j s% j z% () () () 1 () () () () () % sz zz % 1 + 1 () () () () () 1 z z g z% W (9) P P K P ( P ) s s K ( z z ) P P K P K 4) Samplng new pose s () and exendng he pah s () : s ~ p( s s u z ) N( s ; s% P ) s s s () 1() () () ( ) 1( ) ( ) ( ) (8) (10) (11) (12) B Updang he Observed Landmark Esmae In hs sep we updae he poseror over he landmark esmaes represened by he mean µ () n-1 and he covarance Σ () n-1 he updaed values µ () n and Σ () n are hen added o he emporary parcle se ψ along wh he new samplng pose s () he updae depends on wheher or no a landmark n was observed a me For n n he poseror over he landmark remans unchanged: µ () n µ () n-1 Σ () n Σ () n-1 For he observed feaure nn he updae s specfed hrough he followng Equaon: p( θ s n z ) ( ) n ( ) 1 ( ) 1 θn θ n ( ) 1 pz ( s n z ) () 1() 1 1 pz n s n p n s n z ( ) ( ) ( ) ~ N( z; g( θn s ) R) ~ N ( θ n ; µ Σ ) n 1 n 1 p( z s n z ) p( s n z ) η ( θ ) ( θ ) 1442443 144424443 (13) he probably p(θ n s -1() z -1 n -1 ) a me -1 s represened by a Gaussan wh mean µ n-1 () and covarance Σ n-1 () For he new esmae a me o also be Gaussan we need generae Gaussan approxmaon for he percepual model p(z θ n s () n ) Our mehods also use U o approxmae he non-lnear measuremen funcon g(θ n s () ): 1) Calculae he sgma pons: ξ { µ µ ± ( L + λ) Σ (14) () () () () n 1 n 1 n 1 n 1 2) Usng observaon model o compue he mean and covarance of he observaon as follows: () () () () 2L m() () n ξn 1 n j 0 j j n () 2 L c() () () () () [ 0 - ][ z - ] n j j j n n j n n g( s ) z W P W z z (15) 3) Under hs approxmaon he poseror for he locaon of landmark n s ndeed Gaussan he new mean and covarance are obaned usng he followng measuremen updae: K Σ P ( P Σ P + R ) () () () () () () 1 n 1 zn zn n 1 zn () () () () n 1 ( ) n + K zz () () () () n ( ) 1 I K Pzn n µ µ Σ Σ C Adapve Resamplng (16) Nex we resample from emporary se of parcles ψ hen form he new parcle se ψ Resamplng s a common echnque n parcle flerng o correc for such msmaches and avodng parcles degeneracy By weghng parcles n ψ and resamplng accordng o hose weghs he resulng parcle se ndeed approxmaes he arge dsrbuon Afer he resamplng all parcle weghs are hen rese o w () 1/N However resamplng can delee good parcles from he sample se n he wors case he fler dverges Accordngly s mporan o fnd a creron when o perform a resamplng sep Lu [19] nroduced he so-called number of parcles N eff 1/Σ 1 N (w () ) 2 o esmae how well he curren parcle se represens he rue poseror Our approach deermnes wheher or no a resamplng should be carred ou accordng o N eff We resample each me N eff drops below a gven hreshold whch was se o 06N where N s he number of parcles In our expermens we found ha hs echnque drascally reduces he rsk of replacng good parcles because he resamplng operaons are only performed when needed D Inroducng Evoluon Sraegy he resamplng sep descrbed before helps o avod parcle degeneracy bu also leads o an undesrable loss of parcle dversy as resamplng may resul n mulple copes of only a few or n he lm only one parcle In hs case here s a severe depleon of samples In order o nroduce sample varey afer resamplng whou affecng he valdy of he approxmaon we nroduce he ES Because he evoluon operaor can search for opmal parcles he samplng process s more effcen and he number of parcles requred o represen he poseror densy can be reduced consderably he wo operaors: crossover and muaon work drecly over he floang-pons o avod he rouble brough by bnary codng and decodng he crossover and muaon operaor are defned as followng: Crossover: selec wo paren parcles (s (p1) w (p1) ) and (s (p2) w (p2) ) randomly from populaon ψ he crossover operaor maes hem by he followng equaon o generae wo chldren parcles: Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 978 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng (1) c ( p1) ( p2) (1) c (1) c s κs + (1 κ) s + τ w p( z s ) ( c2) ( p2) ( p1) ( c2) ( c2) (17) s κs + (1 κ) s + τ w p( z s ) Where κ~u[01] τ~n(0σ) and U[01] represens unform dsrbuon and N(0Σ) he normal dsrbuon hen replace he parens {s (p1) s (p2) } by hers chldren{s (c1) s (c2) }accordng o he followng creron: he chld s (c1) would be acceped f p(z s (c1) )>max(p(z s (p1) )p(z s (p2) ))value else would be acceped wh probably Haa!Haa! In he smlar form s acceped or rejeced he chld s (c2) Muaon: selec one paren parcle (s (p) w (p) ) he muaon operaor on s defned as followng: mached SIF feaure pars of successve mages capured a relavely close posons along he robo s pah by a monocular vson sysem Gven a SIF key-pons se E and a arge key-pon vecor d hen a neares neghbor of d d s defned as: k 2 ( ) 1 d" E d d' d d" d d' d d' (20) Where d s he -h componen of d We mplemen he SIF key-pons machng algorhm whch based on neares neghbor search algorhm n a KD-ree Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 s s + σ w p( z s ) σ ~ N(0 Σ ) (18) ( c) ( p) ( c) ( c) hen he new parcle s (c) s acceped f p(z s (c) )>p(z s (p) ) else s acceped wh probably p(z s (c) )/p(z s (p) ) For more effcen he crossover operaor wll perform adapvely wh probably p c and muaon operaor wll perform adapvely wh probably p m : ( pc 1 pc2)( fc favg) pc 1 fc favg c max avg p f f pc 1 fc < favg ( pm 1 pm2)( fmax fm) pm 1 fm favg pm fmax favg pm 1 fm < favg (19) Where f max s he bgges fness value n he populaon and f avg s he fness average value f c s he bgger fness value of wo crossover ndvduals f m s he fness value of muaon ndvdual In hs paper we se p c1 085 p c2 065 p m1 01 p m2 0001 III IMPLEMENAION DEAILS OF USING MONOCULAR VISION A SIF Feaure Exracon he Scale Invaran Feaure ransform (SIF) was proposed n [14] as a mehod of exracng and descrbng key-pons whch are robusly nvaran o common mage ransforms he SIF algorhm has four major sages: 1) Scale-space exrema deecon 3) Orenaon assgnmen 4) Key-pon descrpor An mporan aspec of he algorhm s ha generaes a large number of hghly dsncve feaures over a broad range of scales and locaons he number of feaures generaed s dependen on mage sze and conen as well as algorhm parameers For a more dealed dscusson see [14] In hs paper we use he vecors wh 128 elemens as key-pon descrpor Fg 2 shows an example of SIF feaure exracon B KD-ree Based Feaure Machng hs secon descrbes KD-ree algorhm for deermnng he Fg 2 ypcal exraced SIF feaures wh her locaons represened by + he radus of he crcle represens her scales: he 320 240 pxel es mage aken a (a) 1618mm; (b) 756mm; and he resul s (a) 278 key-pons; (b) 267 key-pons Fg 3 he SIF feaure maches based on KD-ree and he machng pars are represened by red and he dsance of he key-pons s represened usng he Eucldean dsance beween her accordng 128 dmensonal descrpor vecor (Equaon 20) and we can use he followng equaon o judge he machng for wo key-pons: kp kp kp kp < λ (21) 1 2 Where λ s consan and 0<λ<1(n hs paper λ s evaluaed as 07) f hs equaon s sasfed hen he machng s successful and smulaneously elmnaes he false machng Fg 3 shows an example of SIF feaure machng for a par mage from he labor corner wh dfferen scale and drecon and we oban 67 mached pars whch he machng accurae rae s hgher han 80% C 3D Srucure Afer he SIF feaure machng we oban he 2D SIF mage feaure machng pars along he robo s rajecory In Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 979 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 hs secon we use hese feaure pars o srucure he 3D spaal landmarks whch are n a sngle world model Le p 1 (u 1 v 1 ) and p 2 (u 2 v 2 ) be he machng par ha observed from wo dfferen vewpons and p 1 p 2 assocae he 3D spaal pon landmark P(X w Y w w ) as shown n Fg 4 usng he pnhole camera model: [ ] [ ] [ ] [ ] z 1 u 1 M 1 1 v1 X Y (22) c w w w z 1 M 1 c2 u2 v2 Xw Yw w (23) he soluon of hree unknown varans X w Y w and w can be obaned hrough he leas square mehod and he projecon marx M: α x 0 u0 0 R M 0 α y v0 0 0 1 (24) 0 0 1 0 Where moon model provdes exrnsc camera roaons R and ranslaons for each mage Offlne calbraon [23] yelds he camera s nrnsc parameers α x α y u 0 v 0 as shown n able 1 D Moon Model he moon model p(s u s -1 ) predcs he movemen and saus over me of he robo As shown n prevous mehods O C 1 W Y X X p 1 e 1 when a conrol u conssng of forward and angular velocy s appled o he robo: ps ( u s 1) f( u s 1) + ε x 1 x + v cos( ϕ + ω ) (25) y+ 1 + ε y + v sn( ϕ + ω ) + ε ϕ + 1 ϕ ϕ + ω Where (x y φ ) s he robo s locaon and bearng a me for all parcles 1 N v s he lne velocy ω s he angular eppolar lne Y e 2 baselne X O C 2 Y Fg 4 wo vewpons geomery and he eppolar consran ABLE I HE INRINSIC PARAMEERS AND EXRINSIC PARAMEERS OF CAMERA Inrnsc Parameers P p 2 Exrnsc Parameers α x 36882620 x cr 21039 mm α y 36990239 z cr 1001742 mm u 0 15967029 θ cr 90 v 0 12154136 velocy a me s he me sep and ε are nose n erms of a normal dsrbuon N(0P ) E Observaon Model Every me he robo s rggered he CCD camera vson sysem capures he consecuve dgal mages and afer SIF feaure exracng machng curren observed SIF feaure wh he map daabase conaned wh 3D spaal naural landmarks hrough KD-ree based neares neghbor search algorhm Le F {f 1 f k } be he k SIF feaure key-pons observed a me n whch here are n key-pons machng wh he 3D landmarks n he map daabase: n l {f 1 ~L f1 f n ~L fn } and here are m key-pons machng he 2D SIF feaure key-pons whch observed a me -1 and are no reconsruced and added o he map daabase: n v {f n+1 ~V fn+1 f n+m ~V fn+m } hen he lkelhood of he observaon z beng obaned s: pz s θ n pz s θ n pz s θ n (26) () l () l v () v ( ) ( ) ( ) l v Where z represens he observaon F l {f 1 f n } and z represens he observaon F v {f n+1 f n+m } p(z l s () n l ) l represens he lkelhood of he observaon z gven he machng relaon n l and p(z l s () n v ) represens he lkelhood of he observaon z v gven he machng relaon n v hese wo lkelhood can be calculaed separaely as follows: l () n () θ j 1 j fj ln pz ( s ) ln p( f s L) (27) v () n + m () θ j n+ 1 j fj ln pz ( s ) ln p( f s V) (28) Where p(f j s () L fj ) represens he lkelhood of he () observaon beng f j when robo a pose s observng he landmark L fj and p(f j s () V fj ) represens he lkelhood of he observaon beng f j when robo a pose s () observng he SIF feaure V fj Le he 3D coordnaes of he landmark L fj be (x (j) w y (j) w z (j) w ) hen we can oban lnp(f j s () L fj ) as follows: $ $ () 1 ln p( f j s Lfj) 05mn( l( I j I j) S ( I j I j)) S J( RG fjr ) J (29) Where J s he Jacoban marx of he observaon equaon G fj s he covarance of L fj he maxmum observaon nnovaon l s consan (n our case 30) whch s seleced so as o preven ouler observaons from sgnfcanly affecng he observaon lkelhood Whle he feaure V fj has no 3D spaal nformaon lnp(f j s () V fj ) s only calculaed accordng o eppolar consran: () ln p( f j s V fj) 05( ds( I j H fj) ds( I fj H j)) + (30) Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 980 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng Where I fj s he mage coordnae of he feaure V fj H fj s he eppolar lne on he mage plane correspondng o V fj a me and H j s he eppolar lne on he mage plane correspondng o he feaure f j a me -1 ds( ) s he funcon of he dsance beween pon and lne Afer calculang he observaon model p(z s θn ) whch can be used o evaluae he -h parcle wegh w () and w () s aken as he fness value n evoluonary process: robo s movng ahead he mage frames are capured and processed buldng he map of he wall Fg 6 shows some frames of sze 320 240 (38 frames n oal) A he end a oal of 1468 SIF landmarks wh 3D posons are gahered n he map whch are relave o he nal coordnaes frame w pz s θ n (31) ( ) () () ( ) N () pz 1 s j θ n Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 IV EXPERIMENAL RESULS AND DISCUSSION he expermens are performed on a Poneer 3-DX moble robo ncorporang an 800 MHz Inel Penum processor as shown n Fg 5a Moor conrol s performed on he on-board compuer whle a 3 GHz PC conneced o he robo by a wreless lnk provdes he man processng power for vson processng and he SLAM sofware A monocular color CCD camera mouned a he fron of he robo s used for deecng he landmarks he es envronmen s a robo laboraory wh lmed space shown n Fg 5b For llusrang he advanages of our mehods over prevous approaches we mplemen SLAM wh our novel RBPF and prevous mehod he expermen s descrbed as follows (a) (b) Fg 5 (a) Poneer 3 moble robo; (b) expermenal envronmen Fg 6 he mage sequence of he wall Frsly he robo s se a he dsance of 2m from he lab wall and he robo orenaon s parallel wh he wall a he same me le he CCD camera vson face wh he wall Whle he (1a) (1b) (2a) (2b) Fg 7 Expermen resuls of map buldng based on (1) convenonal RBPF: (a) 100 parcles (b) 500 parcles; (2) novel RBPF: (a) 50 parcles (b) 100 parcles Fg 7 shows he expermen resuls In he map S represens he sar pon of robo pah and E represens he end pon of robo pah he red pon represens he pah parcle he 2D vew of 3D landmarks n he map s represened wh blue pons As shown n Fg 7 (1) f we ncrease he number of parcles he performance of convenonal RBPF wll be mproved largely however he sorage requremen and calculaon burden s severely aggravaed ownng o each parcle assocaed wh a vew of he map Fg 7 (2) shows he bul map wh he novel RBPF whch adops separaely 50 parcles and 100 parcles and 8 evoluonary seps For execung he evoluon sraeges he mos parcles can be convergen o he regon hgh wegh and approxmae he poseror only wh few parcles he performance of he novel RBPF changes a lle wh ncreasng he number of parcles specfcally we can buld precse map only wh few parcles he more deal comparson of performance wh dfferen numbers of parcles s shown as Fg 8 obvously he robo pose and landmark esmaon error s largely reduced and we only need a few parcles o reach remarkable resuls by means of ncorporang curren observaon and hnkng abou evoluon sraegy and adapve resamplng as well as he effecve managemen srucure based on Kd-ree However ES sep can aggravae he compuaon burden hs negave mpac can be largely reduced for less and less parcles wh he runnng process he resuls are compared wh prevous mehods ndcae superor performance of presened mehod Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 981 scholarwaseorg/1307-6892/15275
Inernaonal Journal of Compuer and Informaon Engneerng Anoher expermen was carred ou n our sngle lab room where he compac map s bul wh our mehod and 186 mage frames of sze 320 240 are capured Fg 9 shows he brd s-eye vew of he 3D spaal map he robo pah and UKF for esmang he map Furhermore he number of resamplng seps s deermned adapvely whch serously reduces he parcle depleon problem and nroducng ES sep afer he resamplng for avodng parcle mpovershmen Expermen resuls on real robo n our ndoor envronmen show he advanages of our mehods over prevous approaches Inernaonal Scence Index Compuer and Informaon Engneerng waseorg/publcaon/15275 Fg 8 Resuls of our novel RBPF SLAM algorhm compared wh convenonal RBPF Fg 9 Brd s-eye vew of he SIF landmarks n he map S ndcaes he nal robo poson E ndcaes he pah end he do lne ndcaes he esmaed robo pah and ndcaes he robo movng drecon V CONCLUSION hs arcle descrbed a novel algorhm for SLAM problem usng monocular CCD camera Lke many prevously publshed SLAM algorhms our mehod calculaes poseror probably dsrbuons over 3D SIF feaured maps and robo locaons I does so recursvely based on a key propery of he SLAM problem: he condonal ndependence of feaure esmaes gven he vehcle pah hs condonal ndependence gves rse o a facored represenaon of he poseror usng a combnaon of parcle flers for esmang REFERENCES [1] D Korenkamp RP Bonasso and R Murphy edors AI-based Moble Robos: Case sudes of successful robo sysems MI Press Cambrdge 1998 pp 91 122 [2] R C Smh P Cheeseman On he Represenaon and Esmaon of Spaal Uncerany Inernaonal Journal of Robocs Research vol 5 no 4 pp 56 68 1986 [3] J Leonard J D ard os S hrun and H Chose edors Workshop Noes of he ICRA Workshop on Concurren Mappng and Localzaon for Auonomous Moble Robos n Proc IEEE In Conf Robocs and Auomaon Washngon DC 2002 [4] J E Guvan E M Nebo Opmzaon of he smulaneous localzaon and map-buldng algorhm for real-me mplemenaon IEEE rans Robocs and Auomaon vol 17 no 3 pp 242 257 2001 [5] A J Davson and D W Murray Smulaneous localzaon and map buldng usng acve vson IEEE rans Paern Analyss and Machne Inellgence vol 24 no 7 pp 865 880 2002 [6] K Murphy and S Russell Rao-blackwellzed parcle flerng for dynamc bayesan neworks n Sequenal mone carlo mehods n pracce Sprnger Verlag 2001 [7] M Monemerlo and S hrun Smulaneous localzaon and mappng wh unknown daa assocaon usng FasSLAM n Proc IEEE In Conf Robocs and Auomaon ape 2003 [8] A J Davson Real-me smulaneous localsaon and mappng wh a sngle camera n Proc of he Nnh In Conf on Compuer Vson ICCV'03 Nce France 2003 pp 1403 1410 [9] C Sachnss G Grse and W Burgard Recoverng Parcle Dversy n a Rao-Blackwellzed Parcle Fler for SLAM Afer Acvely Closng Loops n Proc IEEE In Conf Robocs and Auomaon 2005 pp 667 672 Barcelona Span [10] R Sm P Elnas M Grffn and J Lle Vson-based SLAM usng he Rao-Blackwellzed Parcle Fler n Workshop Reasonng wh Uncerany n Robocs Ednburgh Scoland 2005 [11] M N Daley and M Parnchkun Landmark-based smulaneous localzaon and mappng wh sereo vson n Proc of he 2005 Asan Conf on Indusral Auomaon and Robocs 2005 [12] S Se D Lowe and J Lle Moble robo localzaon and mappng wh uncerany usng scale-nvaran vsual landmarks Inernaonal Journal of Robocs Research 21(8): 735 758 2002 [13] A Davson Y Cd and N Ka Real-me 3D SLAM wh wde-angle vson n Proceedngs of he IFAC Symposum on Inellgen Auonomous Vehcles 2004 [14] D Lowe Dsncve mage feaures from scale-nvaran keypons In J of Compuer Vson vol 60 no 2 pp 91 110 2004 [15] A W Moore An nroducory uoral on kd-rees Robocs Insue Carnege Mellon Unversy Psburgh echncal Repor No 209 Compuer Laboraory Unversy of Cambrdge 1991 [16] R Merwe A Douce N Freas and E Wan he Unscened Parcle Fler echncal Repor CUED/FINFENG /R 380 Cambrdge Unversy Engneerng Deparmen 2000 [17] A Douce On sequenal smulaon-based mehods for Bayesan flerng echncal repor Sgnal Processng Group Deparemen of Engeneerng Unversy of Cambrdge 1998 [18] Ducke A genec algorhm for smulaneous localzaon and mappng n Proc of he IEEE Inernaonal Conference on Robocs and Auomaon 2003 pp 434 439 [19] J S Lu and R Chen Sequenal Mone Carlo mehods for dynamcal sysems J Amer Sas Assoc vol 93 pp 1032 1044 1998 [20] hang Flexble camera calbraon by vewng a plane from unknown orenaons Proc ICCV pp 666 671 1999 Inernaonal Scholarly and Scenfc Research & Innovaon 2(3) 2008 982 scholarwaseorg/1307-6892/15275