A statistical approach to simultaneous mapping and localization for mobile robots

The Annals of Applie d Statistics 2007, V ol. 1, No. 1, 66–84 DOI: 10.1214 /07-A OAS115 c  Institute of Mathematical Statistics , 2 007 A ST A TISTICAL APPR OA CH TO SIMUL T ANEOUS MAPPING AND LOCALIZA TION F OR MOBILE R OBOTS 1 By Anit a Arane da 1 , Stephen E. Fienb erg 2 and Al v aro Soto 1 Pontificia Universidad Cat´ olic a de Chile, Carne gie M e l lon University and Pontificia Universidad Cat´ olic a de Chile Mobile robots require basic informatio n to na vigate through an environmen t: they need to know where they are (localization) and they need to know where they are going. F or the latter, rob ots need a map of the environmen t. Using senso rs of a v ariety of forms, rob ots gather information as they mov e through an environmen t in order to build a map. In this pap er w e present a nov el sampling algo rithm to solving the simultaneous mapping and localization (SLAM) problem in indo or environmen t s. W e approac h the p roblem from a Ba yesi an statistics persp ective. The data co rrespond to a set of range finder and odometer measuremen ts, obtained at discrete time instants. W e focus on the estimation of the p osterior distribution ov er the space of p ossible maps giv en the data. By exploiting different factorizations of this distribution, we derive three sampling algorithms based on imp ortance sampling. W e illustrate th e results of our approach b y testing the algorithms with tw o real data sets obtained through rob ot naviga tion inside office buildings at Carnegie Mellon Universit y and the P ontificia Universidad C atolica de Chile. 1. In tro d uction. Mobile rob ots requ ire basic inform ation to na vigate through an en viron m en t: they need to know where they are (lo calization) and they need to kn o w where they are going. F or the latter, rob ots need a map of th e en vironment . Using sensors o f a v ariet y of forms, rob ots gather information as they mo v e through an environmen t in order to build a map. There are many algorithmic approac h es to deal with this pr ob lem; for ex- ample, see the discussion in [ 4 ]. In this pap er w e examine data gathered by Received January 2007; revised April 2007. 1 Supp orted by F ondecy t Grant 1050653 at the Pon tificia Un ivers idad Cat´ olica de Chile. 2 Supp orted in part by NSF Grant SES-9720374 to Carnegie Mellon U niversit y and NSF Gran t DMS-04-39734 to the Institute for Mathematics and It s Application at the Universit y of Minnesota. Supplementary material av ailable to http://i mstat.org/ao as/supplemen ts Key wor ds and phr ases. Ba yesian mo dels, graphical mod els, H idden Marko v mo dels, imp ortance sampling, particle filtering, SLAM. This is an e lectronic reprint of the o riginal a rticle published by the Institute o f Ma thema tical Statistics in The A nn als of Applie d St atist ics , 2007, V ol. 1, No. 1, 66–84 . This reprint differs from the original in pagination and t ypo graphic deta il. 1 2 A. ARANEDA, S. E. FIENBERG AND A. S O TO t wo m ob ile rob ots, executing a trav ersal through tw o differen t office en vi- ronment s, using an od ometer and a simple set of laser readings f rom sensors. In the p ast, th e pro cessing of su c h data has b enefited enormously from a probabilistic approac h that attempts to use the data to form estima tes and densit y functions of the basic q u an tities of in terest [ 4 , 10 , 2 4 , 2 6 ]. The literature on “prob ab ilistic rob otics” has fo cused hea vily on the p rob- lems of lo calization, k n o wing precisely where the rob ot is, and of mapping the environmen t. These are inte r t win ed , that is, to bu ild a m ap of an en v ir on- men t, the rob ot needs to kno w the lo cations it has visited, but kno win g the lo cations require kno wledge of a map. Therefore, the probabilistic rob otics problem inv olv es the p erformance of these dual tasks and is kn own as S i- m u ltaneous Mapping and Localization (SLAM) [ 16 ]. It is natural to thin k of addressing SLAM usin g a Ba y esian approac h whic h puts a p osterior distri- bution o ver the space of all p ossible maps and then up dates the distribution using the information that th e rob ot acquires as it mov es through the envi- ronment . This Ba y esian solution in some sense maximizes the information a v ailable for SL AM [ 5 ]. Most of the rob otics literature on SLAM utilizes a v ariet y of app ro ximations that allo w for real-time calculations and u p dating and thus, of nece ssit y , simplifies this Bay esian conceptual f ormulation of the SLAM pr oblem. Our fi rst data set comes from an experim ent conducted w ith a mobile rob ot, Pearl, at Carnegie Mellon Univ ersity in W ean Hall (see Figure 1 ). Our second data set comes from a sec on d rob ot, this one na vigating insid e the Computer Science De partmen t at the Pon tificia Univ ersidad Catolica de Chile. Both data sets consist of a set of n oisy m easur emen ts obtained by an o dometer and a laser r ange finder moun ted on the rob ot. O dometer r eadings con vey information ab ou t the rob ot’s relativ e location. They corresp ond to rotational and translational measur es of the rob ot mo v emen ts. Laser read- ings con v ey information ab out the lo cation of landmarks, w ith resp ect to the rob ot’s location. They corresp ond to a set of scalar quantit ies indicating the distances from the rob ot to the nearest obstacle in a set of previously sp ecified directi ons. Using this t yp e of data, w e prop ose a complete pr obabilistic representat ion of the SLAM problem and obtain a Ba y esian solution. W e formalize the problem of mapp ing as the problem of learning the p osterior distribution of the m ap giv en the data. Our k ey idea is b ased on noti ng th at the p osterior distribution of the map is determined b y the p osterior join t distribution of the lo cations visited by the rob ot and the distances to the obstacles from those lo cations. W e deriv e expressions for this p osterior join t distribu tion of lo cations and distances and sho w that th er e is n o closed form for it. By exploiting d ifferen t factorizations of th is distribution, we derive thr ee sampling a lgorithms b ased on imp ortance samp ling. SLAM FOR MOBILE R OBOTS 3 The outline of the pap er is as follo ws. In Section 2 w e discus s previous researc h in SLAM and compare this literature with our approac h . In Sec- tion 3 we describ e in detail the dep en dencies and mod els that define our probabilistic approac h. In Section 4 w e explore the p osterior distribu tion of maps and d ev elop thr ee s ampling algorithms. In Sectio n 5 w e apply these algorithms to the data sets collected in W ean Hall Building at Carnegie Mellon Unive rsit y , and in the Computer S cience Department at the P onti - ficia Univ ersidad Catolica de Chile. Finally , in Section 6 w e discuss br iefly extensions of our mo del and metho d ology , in order to allo w for b oth real time implementati on and more el ab orate form s of data input. 2. Previous SLA M approac hes. Although ther e is an extensiv e rob otics researc h literature dealing with mapping or lo calization for mobile rob ots (e.g., see [ 2 , 8 , 24 , 26 ]) the SLAM problem is a relativ ely new er r esearc h area, where most efforts ha ve b een made ov er the last couple of d ecades. An imp ortant family of approac hes to SLAM is based on ve rsions of the K alman filter. The pioneering d ev elopment in this area was the p ap er by S m ith et al. [ 22 ] whic h pr op osed a basic v ersion of the Hidden Mark o v Model (HMM) approac h widely used to da y , and then used the Kalman filter to address the problem of estimating top ological maps. Th ey assumed a fixed num b er of landmarks in the environmen t where these land marks can b e id entified by their cartesian co ord inates. A t a fi xed p oint in time, the set of landmarks co ordinates and the lo cation of the rob ot are assum ed to b e unobserv able or laten t v ariables. As in the Kalman filter, th e main assumption is that Fig. 1. The mobile r ob ot Pe arl gazing down t he c orridor of We an Hal l, Carne gie Mel lon U ni versity. Courtesy of Seb astian Thrun fr om a vide o available at http: // robots. stanford. edu/ videos. html . 4 A. ARANEDA, S. E. FIENBERG AND A. S O TO the p osterior distributions of all th ese v ariables are Gauss ian and that the observ ations, giv en the laten t v ariables, can b e describ ed by a linear function and a white noise term. These t w o assumptions, Gaussian v ariables and lin earity , are somewh at restrictiv e. The Gaussian assumption mak es this approac h unsu itable for m u ltimo dal distributions that arise when the location of th e rob ot is am- biguous. Th e linearit y assump tion is n ot met in general, sin ce the r elation b et w een o dometry and locations inv olv es trigonometric functions. The Ex- tended Kalman Filter (EKF) [ 7 , 15 ] p artially h andles nonlin earit y us in g a T a ylor series app ro ximation. F or the non-Gaussian ca se, Th run et al. [ 25 ] outlined a ge neral app roac h that can b e used with g eneral distribution functions. Under this a pproac h, ho wev er, maximum lik eliho o d estimation is to o exp ensiv e computationall y . As an alternativ e, Thrun [ 24 ] p resen ted an app lication of the Exp ectation– Maximizatio n (EM) a lgorithm [ 6 ] applied to m ap p ing. He treated the map as the parameter to b e estimated and the lo cations as part of an HMM, maximizing the exp ected log lik elihoo d of the observ ations and the lo cations, giv en the map. A more recen t and successful approac h to the SLAM pr oblem is the F ast- SLAM algorithm [ 17 ]. This approac h applies to top ologica l maps, and is based on a factorizati on of the p osterior distrib ution of maps and lo cations. F or full detail s on b oth the mo dels and their alg orithmic implemen tation, see [ 18 ]. The key factorization of maps and locations is n ot part of the mo d el w e present here and should b e though t of as an appr o ximation whic h allo ws for real-time implemen tation. H¨ ahn el et al. [ 13 ] present an ap p roac h that is also based on the description in [ 24 ], bu t this one applied to o ccupancy grids. This approac h fi nds lo cations iterativ ely ov er time. A t eac h p oin t in time, the algorithm estimates the lo cation visited b y the rob ot as the lo cation that maximizes the pr obabilit y of the current d ata, giv en past data and previous location estimates. The next step fi nds the map, as the map that m aximizes the p osterior probability of the estima ted locations and the observed data. Our mapping approac h applies to o ccup ancy grid maps of static envi- ronment s. Our form u lation of the pr oblem is b ased on the app roac h ini- tially describ ed in Thrun et al . [ 25 ]. W e build a g raphical represen tation of that form ulation where the locations are considered unobserv able v ariables determining the observ ed o d ometer r eadings and, tog ether with the map, determining the o bserv ed laser readings. Th e probabilit y mo del for the en- tire pro cess is determined by motio n and p erception mo d els and a prior distribution for the map. In con trast w ith most of th ese other approac h es, we pro vide a formal probabilistic description of the en tire pro cess a nd dev elop a Ba yesia n solu- tion with th e goal of estimat ing the posterior distribution of the map using SLAM FOR MOBILE R OBOTS 5 Fig. 2. Gr aphic al r epr esentation of the r ob otics SLAM pr oblem. I n our applic ation at Carne gie Mel l on University the r ob ot obtains a total of T = 335 4 r e adings. sim u lation. T h e fact that our app r oac h uses a more general motion mo del than the one used by the Kalman filter and EKF approac hes m akes it appli- cable to a wider set of problems. The adv ant age of this metho d is th at it does not pro vid e a single estimate of the map, as the EM-based solution, bu t it pro du ces m ultiple m aps showing the n otion of v ariabilit y from th e exp ected p osterior map. F or lo calization, w e obtain a sim ulation of t he lo cations vis- ited b y the robot from their p osterior distribution, as an in termediate step while sim ulating maps. 3. F ormalization of the p roblem. Figure 2 provides a graphical repre- sen tation of the SLAM pr ob lem, wh ere n on ob s erv able v ariables hav e b een circled for clarit y . This represen tation forms the basis of our mo del and has some similarities to a suggested represen tation in an earlier pap er b y Mur- phy [ 19 ], as w ell as to p robabilistic graphs of the sort we could form ulate related to Kalman filter approac h es. Our r epresen tation w as dev elop ed in earlier unpublished w ork and has been imp licitly adopted in [ 26 ]. In Fig ure 2 time increases as we go do wn in the fi gu r e. Th e r ob ot starts at lo cation Z 0 , and mo ves to its final lo cation Z T . In the figure U t corresp ond to o dometer data recorded at time t , expressed as differences b et ween lo cations at t imes t and t − 1. The ve ctor Z t corresp onds to the true location visited b y th e rob ot at time t . Two co nsecutiv e lo cations, Z t and Z t − 1 , induce an o dometer tr anslation reading, U t . The random v ariable M = { M ij , ( i, j ) ∈ I } represents the map of the en viron m en t, where M ij tak es the v alue “1 ¯ ,” if the location ( i, j ) is occup ied 6 A. ARANEDA, S. E. FIENBERG AND A. S O TO b y an obstacle, and “0 ¯ ” otherwise, and I is a su itable set of locations. A t eac h p oin t in time, the map M and a give n location of the rob ot, Z t , determine the distances to obstacles, θ t . Finally , these distances determine the d istribution of the laser r eadings V t . According to Fig ure 2 , the d istribution of th e pr o cess is determined b y three mo dels. A motion mo del [ 23 ] describ es the dep en dency of the curr en t lo cation, Z t , on the p r evious one and the current o dometer reading, Z t − 1 and U t , resp ectiv ely . W e adopt a Gaussian motion mo d el. A p er c eption mo del [ 23 ] describ es the dep endency of laser readings V t on th e true distances to ob s tacles, θ t . W e use a truncated Gaussian distrib ution, with standard deviation σ where the limits of the distribution corresp ond to 0 and d max , the maxim um range of the laser device. Finally , for our pr ior d istribution for th e map, M , w e assum e that cells in the map are ind ep endent, ea c h having the same p r obabilit y , p , of b eing o ccupied. Araneda [ 1 ] discusses th ese mo d els in greater detail. In wh at follo ws w e use U , V , Z and θ to denote the sets of o d ometer reading differences, laser readings, locations and distances to obstacl es, from time 1 to T , resp ectiv ely . 4. Imp ortance samp ling for map inf erence. In this section we explore the p osterior d istribution of maps giv en the data and deriv e samp ling strategies based o n Imp ortance Sampling (IS) [ 11 ]. W e note th at t he p osterior d istri- bution o ver the space of p ossible maps, M , is completely determined b y the join t p osterior distribu tion of locations and distances to obstacles, Z and θ . Th us, our approac hes are in tended to explore this last p osterior distri- bution, and they mainly differ in th e particular factorization used, this last one b eing either P ( Z , θ | U , V ) = P ( Z | U , V ) P ( θ | Z , U , V ) , (4.1) or P ( Z , θ | U , V ) = P ( θ | U , V ) P ( Z | θ , U , V ) . (4.2) Our first algorithm, based on factorization ( 4.1 ), app r o ximates the p oste- rior distrib ution of th e lo cations b y discarding laser readings. F rom the ap- plication of this algorithm to the W ean Hall d ata set, we learn that o dometry alone d o es not help the rob ot to r eco v er from odometry error. Our second al gorithm, based on factorization ( 4.2 ), appro ximates the p os- terior distr ibution of distances to o bstacles by d iscard ing o dometer readings. The application of t his algorithm brings upfron t the problem p osed b y the man y restrictions of the sampling space. W e are not able to handle these restrictions analytical ly and, th u s, we sample o v er b roader sampling spaces. Our main algorithm, also b ased on factorization ( 4.2 ), corresp onds to a partially p robabilistic algorithm, wher e the restrictions of th e sampling s p ace SLAM FOR MOBILE R OBOTS 7 are relaxed, a llo wing obs er v ations to lie outside the restrictiv e domain of the distributions. This algo rithm is successful in reco vering from od ometry err or b y using all odometer an d laser information when sampling locations. In the next sect ions w e describ e the deriv ation of the algorithms. Th e fi rst algorithm is describ ed b riefly , an d the details of this deriv ation are sho wn in the App end ix . As the second algorithm corresp onds to the basis of our main algorithm, w e sho w its der iv ation in more d etail. W e finally refer to our main algo r ithm. Th e r esults we mentio n ab o v e are based on the app lication of the alg orithm s that we d escrib e in Sectio n 5 . 4.1. Derivation of the first algorith m. Th e main feature of the first al go- rithm is the appro ximation of the posterior distribution of the lo cations b y the distribution implied by the moti on mo d el. In other words, this approac h considers o d ometry in formation only , when sampling lo cations, and discards the information ab out the lo cations that is con tained in laser readings. In particular, consider the factorization in ( 4.1 ). W e appro ximate the first term in this pro duct, P ( Z | U , V ), by the motion mod el, P ( Z | U ). F or the second term in the facto rization, P ( θ | Z , U , V ) , w e find that w e need to fo cus on terms of the form P ( θ tk | Z , V , θ t − 1 ), where θ tk corresp onds to the d istance to the obs tacle lo cated in the direction of the k th b eam of the laser d evice at time t , and θ t − 1 denotes the matrix con taining all distance s up to time t − 1. W orking this expression, and under a Gaussian p erception mo del, w e obtain P ( θ tk | Z , V , θ t − 1 ) ∝ φ (( V tk − θ tk ) /σ ) Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) P ( θ tk | Z , θ t − 1 , V ( tk ) ) (4.3) × I (0 < V tk < d max ) I ( θ tk > 0) , where the term V ( tk ) corresp onds to the set of a ll la ser rea dings exc ept for reading V tk . The functions φ and Φ corresp ond to the d ensit y and cum u- lativ e fun ction of the standard Gaussian distribu tion, resp ectiv ely (see the App end ix for details). Equation ( 4.3 ) implies that we can implement an IS algorithm by samp lin g θ tk from a Gaussian distribution with imp ortance weigh ts giv en b y ω ( θ tk ) = 1 Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) P ( θ tk | Z , θ t − 1 , V ( tk ) ) (4.4) × I (0 < V tk < d max ) I ( θ tk > 0) . The distribution P ( θ tk | Z , θ t − 1 , V ( tk ) ) in ( 4.4 ) can b e ap p ro ximated by a truncated ge ometric d istribution, that we denote by T r . Ge om ( C tk , p ). See the App endix for an outline of the deriv atio n. Th e imp ortan t consideration 8 A. ARANEDA, S. E. FIENBERG AND A. S O TO in obtaining this result corresp onds to the fact that the sampling space of θ tk , giv en Z t − 1 , θ t − 1 and Z t , C tk , gets narro wer as time increases. W e say that the v alues in this space are c onsistent with the v alues of Z t − 1 , θ t − 1 and Z t . T he parameter p in the tru ncated geometric distribu tion corresp onds to the p rior pr obabilit y of a cell o f b eing o ccupied. With this appro xim ation in hand, w e can go bac k to equation ( 4.4 ), a nd rewrite th e we igh t of eac h sampled v alue θ tk as ω ( θ tk ) = T r . Ge om ( C tk , p ; θ tk ) Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) (4.5) × I (0 < V tk < d max ) I ( θ tk > 0) . 4.2. Derivation of the se c ond algorithm. This algorithm samples dis- tances to the closest obstacles fi rst, and locations afterw ard . It disregards information carr ied b y o dometer r eadings w hen exp lainin g distances to ob- stacles, appro ximating their p osterior distribu tion by the p erception mo d el. The approxima tion us ed in this second algorithm is more accurate th an the one us ed in the fir s t algorithm, since o d ometry err or accum ulates o v er time, while laser error do es not. In addition, the h igh p recision of laser sensors p ro duces accurate readings and , thus, highly v aluable information. Th us, dropping o dometer readings has a smaller impact than droppin g lase r readings. In particular, consider the facto rization of the p osterior distribution giv en in ( 4.2 ). T o s amp le θ from th e first term, P ( θ | U , V ) , we use th at P ( θ | U , V ) ≈ P ( θ | V ) = T Y t =1 N Y k =1 P ( θ tk | V tk ) ∝ T Y t =1 N Y k =1 P ( V tk | θ tk ) P ( θ tk ) (4.6) = T Y t =1 N Y k =1 φ (( V tk − θ tk ) /σ ) Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) (1 − p ) θ tk − 1 p. Th us, w e can implemen t an IS algorithm by samp lin g v alues of θ tk from a Gaussian distribution, and a sso ciating a w eigh t ω ( θ tk ) = 1 Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) (1 − p ) θ tk − 1 p, (4.7) to eac h observ ation. F or the second te r m in equation ( 4.2 ), P ( Z | θ , U , V ), we ha ve that P ( Z | θ , U , V ) = T Y t =1 P ( Z t | θ , U , Z t − 1 ) SLAM FOR MOBILE R OBOTS 9 (4.8) = T Y t =1 P ( Z t | θ , U T t , Z t − 1 ) . F rom this expression, applying Ba yes’ th eorem, we get P ( Z | θ , U , V ) = T Y t =1 P ( Z t | U T t , Z t − 1 ) P ( θ | Z t , U T t , Z t − 1 ) P ( θ | U T t , Z t − 1 ) = T Y t =1 P ( Z t | U t , Z t − 1 ) P ( θ | Z t , U T t , Z t − 1 ) P ( θ | U T t , Z t − 1 ) . Th us, w e can implemen t an IS strategy by s amp ling lo cations, Z t , from the motion mo d el, P ( Z t | U t , Z t − 1 ), and assigning weigh ts ω ( Z t ) = P ( θ | Z t , U T t , Z t − 1 ) , as the term P ( θ | U T t , Z t − 1 ) in the denominator d o es not dep end on Z t . Decomp osing θ o v er time, a s in ω ( Z t ) = t − 1 Y i =1 P ( θ i | Z t , U T t +1 , Z t − 1 , θ i − 1 ) × P ( θ t | Z t , U T t +1 , Z t − 1 , θ t − 1 ) (4.9) × T Y i = t +1 P ( θ i | Z t , U T t +1 , Z t − 1 , θ i − 1 ) , w e iden tify three types o f terms. When i is smaller than t , as in th e terms in the first pro duct in ( 4.9 ), there is no distance in formation a v ailable at time t to matc h with Z t , so w e can dr op the conditioning on Z t . T h u s, the terms in this p ro du ct do not dep end on Z t and w e can drop them from the we igh t. When i is greater that t , the terms in the second pr o duct b ecome P ( θ i | U i , M ( Z t − 1 , θ t − 1 )) , where M ( Z t − 1 , θ t − 1 ) corresp ond s to the partial map built w ith information up to time t − 1. As the lo cation inf orm ation at time i , U i , is random, computing this term requires an additional in tegration. T o a void this, w e appro ximate the w eigh ts in ( 4.9 ) by ω ( Z t ) ≈ P ( θ t | Z t , U T t +1 , Z t − 1 , θ t − 1 ) = P ( θ t | Z t , M ( Z t − 1 , θ t − 1 )) (4.10) = N Y k =1 P ( θ tk | Z t , M ( Z t − 1 , θ t − 1 )) . 10 A. ARANEDA, S. E. FIENBERG AND A. S O TO The terms inside t he pro duct in equ ation ( 4.1 0 ) corresp ond to a tru n cated geometric distribution (see the App end ix for details). W e note that the weigh t, ω ( Z t ) = P ( θ t | Z t , M ( Z t − 1 , θ t − 1 )), represen ts the degree of agreemen t b et w een true distances to obstacles at time t , θ t , and the s amp led location, Z t , within the partial map implied by Z t − 1 and θ t − 1 . 4.3. Derivation of our main algorithm. Our third alg orithm bu ilds up on the d eriv ation of the second al gorithm. The ke y d ifference lie s on the weigh ts used when samplin g lo cations Z t . As w e sh o w in the application of the second algorithm in Section 5 , pr oblems arise with th e w eigh ts of the lo cations in ( 4.10 ), due to the imp osition of consistency b et w een observ ations. Our third algorithm relaxes th e concept of consistency b y r edefining weigh ts. This induces a new probabilistic mo del for the pro cess and w e hav e not explored the p robabilistic consequences of using the new wei gh ts. Consider a dra w at a single ti me instan t t , ( Z t , θ t ). A t this p oin t, there are a v ailable dra ws of lo cations and distances up to time t − 1, Z t − 1 and θ t − 1 , resp ectiv ely . Since we are no w dealing with a d ifferen t co ncept of consistency , the draws obtained up to time t − 1 do not necessarily imply a consistent map, in the sense consistency w as und ersto o d b efore. Th at is, there ma y b e conflicting information in previous d ra ws so that a certai n cell is determined as empt y by some observ ations and as o ccup ied b y others. Thus, we red efi ne the m ap up to time t − 1 as a probabilistic map that w e la b el ˜ M t − 1 . T o un derstand the d efinition of ˜ M t − 1 , consider eac h p air ( Z l , θ l ), l = 1 , . . . , t − 1. Ev ery time one of these pairs is drawn, the s tatus of eac h cell in the map is determined as empt y , o ccupied or un kno wn . W e defin e the v alue of cel l ( i, j ) in ˜ M t − 1 as the prop ortion of times the cell wa s determined as o ccupied, ov er the num b er of times the cell was determined either o ccupied or empty . W e determine, afterwa r d, that a cell in ˜ M t − 1 is empty if its v alue is smaller that π and is occup ied if its v alue is greate r than or equal to 1 − π . F rom calibration of the algorithm on the W ean Hall data set describ ed in the n ext section, w e u se π = 0 . 2. On th e other hand , consider that for laser b eam k , a sampled v alue θ tk and a sa mpled last location Z t obtained f r om the m otion mo d el, dete rmine that the cell at the distance θ tk from Z t in the k th direction is o ccupied, while t he cells b et wee n that cell and Z t are empty . The algorithm consid er s the d egree of agreemen t b etw een these t wo sources of inf ormation: th e partial map ˜ M t − 1 and the information obtained from θ tk and the sampled v alue Z t . Tw o cel ls, one in ˜ M t − 1 and the same one in the ma p determined by θ tk and Z t , agree if they ha v e b een assigned the same v alue b y the tw o sources. O therwise, they disagree. Unlab eled cells are discarded. W e define th e w eight of Z t with resp ect to the k th direction, SLAM FOR MOBILE R OBOTS 11 Fig. 3. Map of We an Hal l. ω k ( Z t ), as t he prop ortion of cells that ag ree, o ver the total n u mb er of cells that either agree or d isagree. Finally , w e d efine the w eigh t of Z t as ω ( Z t ) = N Y k =1 ω k ( Z t ) . (4.11) 5. Implemen tation and emp irical r esults. W e fi rst apply the three algo- rithms describ ed in Section 4 to data obtained f rom an exp erimen tal r un in W ean Hall, at C arnegie M ellon Univ ersit y , by Pea rl (see Figure 1 ), a rob ot equipp ed with an o d ometer and a laser sensor. (The data were pro vided by Nic holas Ro y .) Pea rl na vigated inside the 5th fl o or of W ean Hall building going bac k and forth a long t w o corridors sho wn in th e map in Figure 3 . Pea r l collect ed d ata at ab out 10 recordings p er second. In her j ourney , she to ok 3354 measur emen ts, eac h of them consisting in a pair ( U t , V t ), t = 1 , . . . , 3354. Her laser sensor sent b eams ev ery degree and thus there are 180 d istances recorded for eac h laser reading. The laser sensor had a maximum distance range of d max = 10 m. A map d ra w n from ra w o dometer and laser readings is shown in Figure 4 . This figure sh o ws ho w o d ometry er r or accum ulates so that it seems that P earl visited three differen t corridors, instead of t wo. The smo othness of the depicted w alls, how ev er, suggests that error in laser sensor readings is small compared t o error in o dometer rea dings. 5.1. R esults of the first algor ithm. Figure 5 shows a t yp ical path obtained using the first algorithm. W e see that large o b serv ed rotations in o dometry 12 A. ARANEDA, S. E. FIENBERG AND A. S O TO cause large rotations to o ccur in the samp led path, whic h explains its curv ed undesirable shap e. Figure 5 sho w s that paths are un able to reco v er f rom the error ac cum u lated b y odometry . The lac k of a b ilit y of the motion m o del to reco v er f rom o d ometry error s h o ws the n eed of incorp orating th e information carried by laser sensor readings when sampling paths, as ou r second and main a lgorithms d o. The second step of our sampling pro cess simulates distances to the obsta- cles from th eir p osterior distrib ution giv en the data and the sampled paths. Figure 6 sho w s the map obtained for the same path sho wn in Figure 5 . The figur e sho ws that laser readings app ear to induce larger errors in the sampled maps. Th is impression is not true, h ow ev er, if we n otice that the sampled lo cations carry large errors in orien tations, wh ic h could not b e seen Fig. 4. M ap obtaine d fr om r aw data. Fig. 5. Path sample d by the first algorithm. SLAM FOR MOBILE R OBOTS 13 in Figure 5 . This causes the obstacles in the map to b e lo cated in the wrong places. 5.2. R esults of the se c ond algorithm. Under t his algorithm, w e obtain a sample of distances to obstacles θ from a Gaussian p erception mo d el, and assign wei gh ts ω ( θ tk ) in equation ( 4.7 ). It is not p ossible to visualize th e observ ations obtained in this step w ithin the map, as there are n o locations a v ailable. Problems arise early during the sec ond sampling step. When sampling Z t , at in stan t t , there is a v ailable a partial map built with information up to time t − 1 , M t − 1 ( Z t − 1 , θ t − 1 ), and a s et of distances, θ t . A t this step, w e sample a v alue of the curren t lo cation Z t from the motion mod el, and assign a w eigh t to this observ ation t hat corresp ond s to the d egree of ag reemen t of this location with the previous informatio n. The partial map that is a v aila ble, M t − 1 , con tains un determined cells. Th us, for a give n lo cation Z t , there is a set of p ossible distances to the closest obstacles. Th e w eight of the lo cation sa mpled, Z t , is zero if θ t is not in that set. Once t increases and more information is a v ailable in M t − 1 , the set of p ossible distances to obstacles from Z t gets smaller and, th us, it b ecomes d ifficult to dra w a v alue of Z t with a p ositiv e weig h t. In other w ord s, it b ecomes hard to fin d a consistent lo cation Z t . This issue app ears rapidly in the imp lementat ion of this algorithm. Once a few locations are sampled, it becomes hard to find consistent subsequen t lo cations, making this implemen tation unfeasible from the compu tational p oint of view. 5.3. R esults of our main algorithm. W e tested our main algorithm w ith b oth data sets, fr om Carn egie Mellon Universit y and from the Po n tificia Univ ersid ad Catolic a de Chile. T o get eac h observ ation ( Z , θ ) in the sample, Fig. 6. Map matching sample p ath i n Figur e 5 . 14 A. ARANEDA, S. E. FIENBERG AND A. S O TO w e start by sampling n sets of distances to obstacles, θ tk , t = 1 , . . . , T , k = 1 , . . . , N , from Gaussian distributions, with means V tk and ˆ σ = 2 cm . Using IS w eights according to equation ( 4.7 ), we resample one of these observ ations. W e s amp le n s ets of lo cations Z t , t = 1 , . . . , t , from the motion mo d el and use the v alue o f θ obtained in th e fi rst step to compute the new we igh ts in equation ( 4.10 ). W e resample one observ ation ac cording to these weigh ts. F or the W ean Hall data, w e run th is algorithm with tw o differen t IS samp le sizes n . Figures 7 and 8 s h o w tw o maps obtained f or n = 10 and n = 100, resp ectiv ely . F or eac h IS sample size, additional observ ations obtained closely resem b le the ones sho w n here. F or the data collected at the Pon tificia Univ ersidad C atolica d e Chile, Figure 9 sho ws the map ob tained f r om raw data, confirming the error accu- m u lation present in o dometry . Figure 10 sho ws the result of the applicatio n or our main algorithm to this data. Figures 7 , 8 and 1 0 sho w that th is algorithm allo ws the rob ot t o reco v er from o d ometry error, aligning th e data into the right n umb er of corridors. Comparing these results w ith the results obtained b y our first alg orithm, w e fin d that incorp orating l aser data w hen sampling lo cations significan tly impro v es the b ehavio r of the sampling tec h nique. Figures 7 and 8 closely r e- sem b le the true W ean Hall map, sho w n in Figure 3 . Figure 10 also resembles the tru e area na vigated by the r ob ot (not sho wn here). Our resu lts also sho w impr o ve men t when we dra w more observ ations in the IS step for sampling lo cations, although ev en a small sample s ize, for example, n = 10, b eh a ve s r easonably well. The compu tational complexit y of the algorithm is O ( n × T ). The complex- it y is linear in the size n of t he I S step, as eac h sampled v alue is pr o cessed indep end en tly , w hic h allo w s to an easy parallelizati on of the algorithm. I n particular, we also r educe pro cessing time by using pr e-computed wei gh ts, when r ep eated observ ations are sampled at the re-sampling step of IS. The complexit y is linear in the n um b er of observ ations in the sample, T , as at eac h p oin t in time, we only up date a fixed num b er of cells in the neigh b orho o d of the sampled l o cation in the map. W e fu rther redu ce the computational complexit y in our implemen tation b y considering that a rob ot u sually obtains data at a h igh rate (ab out 10 measuremen ts p er second). W e discard red undant information w hen the rob ot is static or m o ving too slo wly . With these simplificatio ns, our algorithm requires ab out 1 seco n d to com- pute the w eigh ts of the n observ ations s ampled at eac h point in time, when runn in g in a P en tium pro cessor at 2 GHz. 6. Con tribu tions and open statistical mo deling problems. In this pap er w e ha ve presen ted a fu lly Ba y esian approac h to m o deling the simulta neous SLAM FOR MOBILE R OBOTS 15 Fig. 7. Map of W e an Hal l, at Carne gie Mel lon University, obtaine d fr om pr op ose d m ain algorithm, and IS sample size n = 10 . lo calizat ion and mapp ing (S LAM) problem for mobile rob ots in indoor en vi- ronment s w ith a simplified sensor system. Our metho dology utilizes a prob- abilistic graph ical repr esen tation for data acquisitio n and inference that has already b een adapted to a v ariety o f other S LAM problems in the rob otics literature. W e use imp ortance sampling app roac hes to calculate the p oste- rior distrib ution and the m etho dology , while not imp lemen table in anything appro ximating real time can serve as a b aseline f or assessing v arious real- time appr o ximations, su c h as the particle filtering metho d s as wel l as o ther approac hes. Finally , our results suggest that mo deling o dometry err or is the key com- p onent in mo deling the SLAM p roblem. Extensions to the mo d els here migh t w ell in v olve d istributions ov er fam- ilies p r iors for the maps and o ver motion and p erception mo d els. But to Fig. 8. Map of W e an Hal l, at Carne gie Mel lon University, obtaine d fr om pr op ose d m ain algorithm, and IS sample size n = 100 . 16 A. ARANEDA, S. E. FIENBERG AND A. S O TO Fig. 9. Map of the Com puter Scienc e Dep artment, at Uni versidad Catoli c a de Chi le, obtaine d fr om r aw data. The true ar e a p ossesses only one c orridor. mak e th e metho dology truly useful for r ob otics, we need to explore appro x- imations to the fully Ba yesia n metho ds here and compare these to curr en t tec hnologies suc h as F astSLAM w hic h is built on Kalman filtering id eas and Gaussian ap p ro ximations. Tw o appr o ximation approac hes seem p otentia lly promising: 1. V ariational approximat ions of the sort that pro ved extremely v aluable in a v ariet y of other mac hin e learning problems, for example, see [ 14 ] and [ 3 ], t hat h a ve an EM-like structure. 2. Mo del ap p ro ximations emerging from the literature on tropical geometry whic h ha v e already b een applied to simple hidd en Mark o v mod els [ 20 , 21 ]. V ariational approximat ions should b e rea sonably easy to implement, while the tropical geometry approac h es lik ely will require n ew mathematic al and statistica l w ork. As w e men tioned at the outset, we ha v e addressed a relativ ely simple and st ylized pr obabilistic rob otics problem. Cur ren t sensors in widespread u se pro v id e more elab orate information of en vironmen ts, including pictures, and man y of th e most int eresting en vironments are thr ee-dimensional [ 12 ] and either dynamic or do not ha v e the simp le restricted b ound aries exhibited b y our data an d mo d eling [ 27 ]. O th er settings inv olv e multiple rob ots inte r- acting within an en vironmen t [ 9 ]. These mapp ing and lo calization p roblems Fig. 10. Map of the Computer Scienc e Dep artment, at Universidad Catolic a de Chile, obtaine d fr om pr op ose d main algorithm, and IS sampl e size n = 100 . SLAM FOR MOBILE R OBOTS 17 pro v id e serious c hallenges to b oth mac h ine learning and statistics. F or most of these problems, the F astSLAM metho d ology of Thrun and his colleagues remains the state- of-the-art. APPENDIX: DET AILS F OR FIRS T ALGORITHM Consider P ( θ tk | Z , V , θ t − 1 ) ∝ P ( θ tk | V tk ) P ( θ tk , Z , θ t − 1 , V ( tk ) ) P ( θ tk , V ( tk ) ) . (A.1) Based on the fact that θ tk is in d ep endent of laser readings that do not con tain a reading k at time t , V ( tk ) , when no information ab out lo cations is a v ailable, we can decomp ose the denominator in equation ( A.1 ) in a pro d - uct of marginals. W e also d ecomp ose the term P ( θ tk , Z , θ t − 1 , V ( tk ) ) in th e n u merator and , un d er a Gaussian p erception mo del, w e obtain P ( θ tk | Z , V , θ t − 1 ) ∝ P ( θ tk | V tk ) P ( Z , θ t − 1 , V ( tk ) ) P ( θ tk | Z , θ t − 1 , V ( tk ) ) P ( θ tk ) P ( V ( tk ) ) = φ (( V tk − θ tk ) /σ ) Φ(( d max − θ tk ) /σ ) − Φ( − θ tk /σ ) P ( θ tk | Z , θ t − 1 , V ( tk ) ) × I (0 < V tk < d max ) I ( θ tk > 0) , where the fu nctions φ and Φ are the density and cum u lative fu nction of the standard Gauss ian distribu tion, resp ectiv ely . T ru ncated geometric distribution appro ximation. Consider that in the term P ( θ tk | Z , θ t − 1 , V ( tk ) ), Z t − 1 and θ t − 1 con vey d eterministic information ab out the map. Thus, the sample space of θ tk gets n arro wer once Z t is also kno w n. Let Z T t +1 b e the set of locations b et ween times t + 1 and T . Calculation of the term P ( θ tk | Z , θ t − 1 , V ( tk ) ) is difficult b ecause Z T t +1 and V ( tk ) also conv ey information ab out the map but, since the laser readings carry error, this information is probabilistic instead of deterministic. Th us, in tro ducing this information in volv es in tro ducing additional in tegratio n. T o a vo id this, we use P ( θ tk | Z , θ t − 1 , V ( tk ) ) ≈ P ( θ tk | Z t − 1 , θ t − 1 , Z t ) . The distr ibution in th is equatio n ta k es the v alue zero for those θ tk that are inconsisten t with Z t − 1 , θ t − 1 and Z t . Figure 11 sho ws this situation. The dashed cell corresp onds to the lo cation of the rob ot at time t , Z t . Using the information conv ey ed by Z t − 1 and θ t − 1 , w e la b el eac h cell las either “1 ” or “0” dep end ing on whether w e dete rmine it to b e o ccupied or empty . The arr o w corresp onds to the dir ection of the 18 A. ARANEDA, S. E. FIENBERG AND A. S O TO Fig. 11. Il lustr ation of a trunc ate d ge ometric distribution. k th b eam. W e see that the closest obstacle in that direction can only b e lo cated at the un lab eled cells or at the o ccupied cell, in the d irection of the arro w. W e denote the s et of these p ossible cells as C tk . On the other hand , the pr obabilit y that th e closest obstacle is in one of the cells in C tk corresp onds to a certain p articularizatio n of a Ge o- metric distr ib ution with parameter p , w here p is the prior probabilit y of eac h cell of b eing o ccupied. Th ese considerations imply that the distribu - tion P ( θ tk | Z t − 1 , θ t − 1 , Z t ) corresp onds to the s o-named T runc ate d Ge ometric distribution, T r . Ge om ( C tk , p ). Ac kn o wledgment s. W e thank Sebastian Thrun wh o int ro duced us to the SLAM p roblem, provided feedbac k on the appr oac h presen ted here at v arious stages, and arranged to sh are with us the C arnegie Mellon data analyze d in this pap er. J ay Kadan e pr o vided crucial inp ut on the dev elopmen t and implemen tation of the algorithms. This w ork is partially based on material from the Ph.D. thesis o f t he fi rst author in the D epartmen t of S tatistics at Carnegie Mellon Univ ersity . REFERENCES [1] Araneda, A. (2004). Statistical inference in mapping and localization for mobile robots. Ph.D. thesis, Dept. Statistics, Carnegie Mellon Univ. [2] Bailey, T. and Durrant-Whyte, H. ( 2006). Sim ultaneous lo calization and map - ping (SLAM): Part II : S t ate of t he art. R ob otics and Automation Magazine 13 108–117 . [3] Beal, M. J. and Gha h ramani, Z. (2006). V ariational Ba yesian learning of directed graphical mod els with hidden v ariables. Bayesian A nalysis 1 793–832. [4] Choset, H., L ynch, K., Hutchinson, S., Kantor, G., Bu rgard, W ., Ka vraki, L. and Thrun, S. (2004). Principles of R ob otic Motion : The ory , Algorithms , and I m plementation . MIT Press, Cam bridge, MA. [5] DeGroot, M. (1968). O ptim al De cisi on The ory . McGraw -Hill, New Y ork. [6] Dempster, A. P., Lai rd, N. M. and R ubin, D. B. (1977). Maxim um lik elihoo d from incomplete data via the EM algori thm (with discussion). J. R oy. Statist. So c. Ser. B 39 1–38. MR0501537 SLAM FOR MOBILE R OBOTS 19 [7] Doucet, A., De Fre it as, N. an d Gordon, N. (2001). Se quential Monte Carlo Meth- o ds in Pr actic e . S pringer, New Y ork. MR1847783 [8] Durrant-Whyte , H. and Bailey, T. ( 2006). Sim u ltaneous lo calization and m ap - ping (SLAM): Part I: The ess entia l algorithms. R ob otics and A utomation Mag- azine 13 99–110. [9] Fo x, D., B urgard, W., Krupp a , H. and Thrun, S. (2000). A probabilistic ap- proac h to collaborative m ulti-rob ot localization. A utonomous R ob ots 8 . [10] Fo x, D., Burgard, W. and Thrun, S. (2002). Prob- abilistic techniques for mobile rob ots. Av ailable at http://www .cs.washi ngton.edu/homes/fox/ecai- tutorial/ecai- 02.htm . [11] Ge weke, J. (1989). Bay esian inference in econometric mo dels u sing Monte C arlo integ ration. Ec onometric a 57 1317–13 39. MR1035115 [12] Hahn el, D., Bu rgard, W. and Thrun, S. (2003). Learning compact 3d models of indo or and outdoor environmen ts with a mobile rob ot. R ob otics and Autonomous Systems 44 15– 17. [13] H ¨ ahnel, D., Schulz, D. and Bu rgard, W. (2002). Map building with mobile rob ots in p opulated environmen ts. In Pr o c e e di ngs of the IEEE/RSJ Internat i onal Con- fer enc e on I ntel li gent R ob ots and Syste ms (IROS) . [14] Jordan, M. I. (2004). Graphical mo dels. Statist. Sci. 19 140–155 . MR2082153 [15] Julier, S . J. and Uhlmann , J. K. (1997). A new ex tension of the Kalman Filter to nonlinear sy stems. In Pr o c e e dings of Ae r oSense : The 11t h I nternational Sym- p osium on A er osp ac e / Defense Sensing , Simulation and Contr ols , Multi Sensor F usion , T r acking and R esour c e Management II , SPIE. [16] Leonard, J. and Durrant-Whyte, H. (1991 ). Sim u ltaneous map building and localization for an autonomous mobile robot. In Pr o c e e dings IEEE International Workshop on Intel ligent Ro b ots and Systems 3 1442 –1447. [17] Monteme rlo, M. and T hrun, S. (2003). Sim ultaneous localization and ma pping with unknown data asso ciation using F astSLA M. In Pr o c e e dings of the IEEE International Confer enc e on R ob otics and A utomation , ICRA ’ 03 . [18] Monteme rlo, M. and Thrun, S. (2007). F astSLAM. A scalable metho d for the sim u ltaneous lo calization and mapping problem in rob otics. Springer T r acts i n A dvanc e d R ob otics 27 . Springer, Berlin. [19] Murp h y, K. (1999). Bay esian map learning in dy namic environmen ts. In A dvanc es in Neur al I nf ormation Pr o c essing Systems 12 , NI PS ’ 99 . [20] P achter, L. and Sturmfels, B. (2004). Parametri c inference for biological sequence analysis. Pr o c. Natl. Ac ad. Sci. USA 101 16138–16143 . MR2114587 [21] P achter, L. and Sturmfels, B. (2004). T ropical geometry of statistical mod els. Pr o c. Natl. A c ad. Sci. USA 101 16132– 16137. MR2114586 [22] Smi th, R., Self, M. and Cheeseman , P. (1990). Estimating uncertain spatial re- lationships in robotics. In Autonomous R ob ot V ehicles ( I. J. Cox and G. T. Wilfong, eds.) 167–193. Springer, New Y ork . MR1068141 [23] Thrun, S. (1998). Learning metric-top ological maps fo r ind oor mobile rob ot na vi- gation. Ar tificial Intel li genc e 99 21–71. [24] Thrun, S. (2002). Rob otic mapping: A survey . In Exp loring Artificial Intel l igenc e in the New Mil lenium . (G. Lak emeyer and B. Neb el, eds.). Morgan Kaufmann, San Mateo, CA. [25] Thrun, S., Bu rgard, W. and Fo x, D. (1998). A probabilistic approach to con- current mapping and localization for mobile rob ots. Machine Le arning and A u- tonomous Ro b ots 31 1–25. Joi nt Issue. 20 A. ARANEDA, S. E. FIENBERG AND A. S O TO [26] Thrun, S., Fo x, D . a nd Burgard, W. (2005). Pr ob abil istic R ob otics . MIT Press, Cam b rid ge, MA. [27] Thrun, S., Tha yer, S., W hitt aker, W., Baker, C., Burgard, W., Fergurson, D., H ¨ ahnel, D., M ontemerlo, M., Morris, Z., Omohun d ro , A. and Re- ver te, C. ( 2004). Autonomous exploration and mapping of abandoned min es. IEEE R ob otics and Automat ion 11 79–91 . A. Araneda Dep ar tment of St a tistics Pontificia Universidad Ca t ´ olica de Chile Santiago Chile E-mail: aaraneda@mat.puc.cl S. E. Fienb erg Dep ar tment of St a tistics Carnegie Mellon Un iversity Pittsburgh, Pennsyl v an ia 15213 USA E-mail: fien b erg@stat.cmu.edu A. Soto Dep ar tment of Computer S cience Pontificia Universidad Ca t ´ olica de Chile Santiago Chile E-mail: asoto@ing.puc.cl