A New Approach for Large-Scale Localization and Mapping: Hybrid Metric-Topological SLAM

Transcription

1 27 IEEE Inernaonal Conference on Robocs and Auomaon Roma, Ialy, -14 Aprl 27 ThB1.5 A New Approach for Large-Scale Localzaon and Mappng: Hybrd Merc-Topologcal SLAM Jose-Lus Blanco, Juan-Anono Fernández-Madrgal, Javer Gonzalez Dep. of Sysem Engneerng and Auomaon Unversy of Malaga Málaga, Span {lblanco,afma,gonzalez}@cma.uma.es Absrac Mos successful works n Smulaneous Localzaon and Mappng (SLAM am o buld a merc map under a probablsc vewpon employng Bayesan flerng echnques. Ths work nroduces a new hybrd mercopologcal approach, where he am s o reconsruc he pah of he robo n a hybrd connuous-dscree sae space whch naurally combnes merc and opologcal maps. Our fundamenal conrbuons are: ( he esmaon of he opologcal pah, an mprovemen smlar o ha of Rao- Blackwellzed Parcle Flers (RBPF and FasSLAM n he feld of merc map buldng; and ( he applcaon of grounded mehods o he absracon of opology (ncludng loop closure from raw sensor readngs. I s remarkable ha our approach could be sll represened as a Bayesan nference problem, becomng an exenson of purely merc SLAM. Besdes provdng he formal defnons and he bascs for our approach, we also descrbe a praccal mplemenaon amed o real-me operaon. Promsng expermenal resuls mappng large envronmens wh mulple nesed loops (~3. m 2, ~2Km robo pah valdae our work. Index Terms Moble robos, Large-scale maps, Loop closure, Rao-Blackwellzed Parcle Flers, SLAM. I. INTRODUCTION Smulaneous Localzaon and Mappng (SLAM has beng nensvely suded by researchers n he las decade, leadng o approaches ha can be classfed no hree welldfferenaed paradgms dependng on he underlyng map srucure: merc ([5],[12],[24],[26], opologcal ([22],[23], or hybrd represenaons ([7], [15],[28]. In hs work we focus on he formulaon of hybrd merc-opologcal SLAM n erms of Bayesan esmaon from all he avalable robo acons and observaons. Thus, our maor conrbuon consss of a new formulaon of hybrd SLAM as he esmaon of he sequence of areas he robo raverses (opologcal par and he local pose of he robo whn hose areas (merc par, provdng an esmae of he spaal relaon beween elemens, no her absolue posons [9]. Our proposal s grounded on he fac ha he robo wll be always a one area, n whose merc scope SLAM s a solved problem by sandard mehods, eher Exended Kalman Flers [5] or RBPF [6]. As long as he sze of he areas (local maps s kep bounded, so does he complexy of he local SLAM mehod. In hs paper we dscuss ssues such deecng when he robo goes ou of he curren area, eners a new one, or reeners a prevously known one (loop closure. In our proposal wo esmaon processes are carred ou concurrenly: ( he robo pose relave o he curren area (merc pah, and ( he sequence of areas he robo goes hrough (opologcal pah. Under hs perspecve, loop closure becomes fndng a paron n he sequence of all he areas n he map [22]. The meanng of he opologcal map n hs work srongly dffers from he one consdered n many oher works. In he leraure we can fnd works ha consder dsncve places as nodes ([4],[15], whle ohers cu he map no dson areas ([7],[28]. Our model s closer o hose of appearance-based maps ([2],[3], where opologcal nodes are he resul of absracng low-level robo observaons gahered a a gven area. Acually, he sze of areas wll be auomacally deermned by he naure of sensors, more concreely, by he covsbly beween observaons [2]. As an example, observaons gahered by a laser scanner whn a room may be grouped no a sngle area, bu usng a narrow feld-of-vew camera may generae a number of areas nsead. Furhermore, paronng observaons, as dsnc from he physcal space, mples ha he same locaon may be assgned dfferen relave coordnaes, one for each area n he opologcal map. In addon o provdng a unfed heorecal suppor for hybrd merc-opologcal SLAM (whch we call HMT- SLAM, n hs work we propose a praccal mplemenaon framework. Ths sysem processes local merc nformaon n real-me by means of a parcle fler wh a consan-me complexy, whle he opologcal srucure of he envronmen s esmaed n an anyme fashon. We clam ha he presen work s a promsng base for negrang many prevous separae conrbuons, such as: Rao-Blackwellzed Parcle Flers (RBPF for mappng, whch suffer maor problems when dealng wh large or nesed loops, requrng a dynamc number of parcles [8] or arfacs o preven he loss of dversy [25], respecvely. These problems are he resul of map buldng under a global-coordnae approach. In he conex of occupancy grd map buldng, advanced echnques ([],[11] are requred o reduce he memory requremens of RBPF mappng, due o he manenance of a global map n each parcle. In our approach each parcle keeps a map of he curren area only, hence achevng mproved scalably wh a grea reducon n sorage requremens. Appearance-based map paronng mehods ([2],[3] have never been negraed before no a hybrd SLAM framework, whereas hey naurally f no he nducon of opologcal areas from merc observaons /7/$2. 27 IEEE. 261

2 Global localzaon (he robo awakenng problem, can be managed n a more effcen manner n our hybrd sae-space han whn a global merc map. The robo can frs localze n whch area s, and hen ry o esmae s merc whn ha area. However, hs ssue wll no be addressed here for he sake of brevy. The res of hs paper s srucured as follows. In secon II we examne prevous works relaed o boh merc and opologcal SLAM. Nex, we provde he probablsc foundaons of our approach, whle some of s relevan elemens are dscussed n secon IV. A praccal sysem ha mplemens our deas s presened n secon V, and expermenal resuls wh real robos n large-scale scenaros are dscussed n secon VI. Fnally, some conclusons and fuure work are oulned. II. PREVIOUS RESEARCH In he followng we brefly dscuss prevous works n he felds of merc, opologcal, and herarchcal mappng, hghlghng her relaon wh he presen paper. Merc approaches ([12],[17],[19],[2],[24],[26] am o reconsruc he spaal arrangemen of map elemens, n he form of landmark maps [19], occupancy grds [2], or ses of range scans [17] (please refer o [27] for a more dealed classfcaon. Alhough some non-probablsc mehods have been proposed o buld merc maps ([12],[17], he vas maory of works on merc SLAM rely on a probablsc represenaon of he robo pose and he map, where Bayesan flerng esmaes he correspondng probably dsrbuons [5]. The hardes problem n hese mehods s daa assocaon (ha s, esablshng correspondences among observaons and he map [21], a problem ha aggravaes when he robo closes a loop: a prevously known place s revsed hrough a new and unknown pah. Esablshng wrong correspondences grealy compromses he conssency of esmaed maps. Snce he uncerany n he robo pose and he map ncreases as he robo explores new areas, he hardness of fndng he correc daa assocaon ncreases wh he scale of maps. Recenly, hs problem has been successfully addressed from a new vewpon: esmang he whole robo pah, nsead of only he mos recen pose. We can hen apply a convenen facorzaon, called Rao-Blackwellzaon n Esmaon Theory [6], ha has enabled he mappng of relavely largeszed envronmens wh boh occupancy grds (Rao- Blackwellzed Parcle Flers [] and landmark maps (FasSLAM [18]. However, he number of parcles requred o close a loop ncreases wh s lengh, whch may evenually urns no a sorage capacy lmaon snce each parcle mus carry a hypohess of he whole map. Buldng a opologcal map s an aracve alernave o merc maps. Among oher properes, hey have reduced sorage requremens and can be easly negraed wh symbolc plannng. Alhough Bayesan esmaon has been repored for hese maps [22], s assumed here ha he robo can deec wheher s close o one of a se of dsncve places, whch are represened as nodes n he map. We hnk hs s a oo resrcve assumpon where he dversy of merc nformaon s los. A more appealng approach s o consder hybrd maps, where opologcal nodes conan local merc nformaon ([3],[7],[14],[15],[16]. However, loop closure for hese maps n prevous works has been consdered only under he merc pon-of-vew,.e. by fndng he global coordnaes ransformaon compable wh he loop closure [7]. For hs reason he assocaon problem for observaons remans beng a maor ssue, whch can be smplfed by nferrng probablsc opologcal loop closure hypoheses [22]. Our approach urns he landmark-o-landmark daa assocaon problem no a node-o-node one, whch we clam s an easer problem due o s reduced ambguy. Addonally, o he bes of our knowledge no prevous work has proposed he probablsc esmaon of he opologcal pah followed by he robo, whch may be seen as he dual of Rao- Blackwellzaon for merc mappng. III. PROBABILISTIC FOUNDATIONS OF HMT-SLAM The problem of merc SLAM s saed as o smulaneously esmae he map m and he robo pose x a any gven nsan of me. Se ou as a Bayesan flerng problem condoned o he sequence of robo acons u ={u 1,,u } and observaons z ={z 1,,z } [5], he probably dsrbuon o be esmaed s 1 : (, p x m u, z (1 Under hs formulaon of he problem, all he observaons z depend on he whole map m. Ths s n conras wh he localy of real observaons, whch ypcally cach a small par of he envronmen a once. Our proposal for a hybrd SLAM bulds upon he assumpon ha he map can be convenenly dvded no a se of n merc sub-maps { M} =1..n, whch we wll call areas. Each area has s own coordnae reference frame, whle edges { ab } beween sub-maps defne he ransformaon beween dfferen frames leadng o he opologcal vew of he map. Thus, a hybrd map s a 2-uple: k ab { },{ } a 1.. m = k= 1.. n = n b= 1.. n M (2 Accordngly, he robo pose becomes a hybrd mercopologcal (HMT varable, saed as s =(x,γ, where he dscree par γ denfes a sub-map (area, and he connuous x s he pose relave o he coordnae reference of he area γ M. Gven he above defnon of a HMT map m, we sae he HMT-SLAM problem as he esmaon of he followng dsrbuon: (,, p s m u o (3 where o ={o 1,,o } s he sequence of hybrd observaons o =(z, ψ. The purpose behnd defnng o as hybrd s o convenenly separae merc observaons z, and area dependan, qualave observaons ψ (hs dvson s especally useful when facng he problem of global 1 For clary, hroughou hs paper we wll denoe sequences of varables of he form x 1:k ={x 1,,x k } as x k. 262

3 M localzaon. The esmaon of he opologcal and merc pahs enables he Rao-Blackwellzaon of he on mappah esmaon [6]. I mus be hghlghed he clear advanage of hs model o face he hard problem of loop closure: whle merc SLAM mehods am o compue he exac coordnaes of he robo afer raversng a cycle (.e. compung connuous varables, closng he loop n he opologcal scope s equvalen o esablshng a paron n he (dscree space of opologcal areas. Ths ssue was rgorously dscussed elsewhere [22]. One could argue ha he merc elemens n he HMT map (edges ab beween areas sll have o be consdered. However, hey become analycally racable when condoned o hypoheses of opologcal-level loop closure: f we facorze (3 hrough he defnon of condonal probably: (,, (, (,, p s m u o = p s u o p m s u o (4 he map m becomes analycally racable gven he hybrd robo pah s ([6],[18]. Nex, we ake advanage of havng defned he map as a se of areas. Ideally, we could expec hs paronng o acheve he ndependence of observaons whn dfferen areas, as llusraed n he DBN of Fg. 1. Mahemacally, from hs follows he condonal ndependence beween robo pah porons from dfferen parons, condoned o he sarng pose of he robo no each sub-map: Unforunaely, n pracce, paronng he map wll rarely generae srcly ndependen observaons beween sub-maps. However, we wll sll assume ha (5 holds as an approxmaon whle only a small quany of nformaon s los. Ths enables he followng facorzaon of (4: p s, m u, o ( k k p( s u, o p( m s, u, o n k = 1 n o s s 1 u 1 o 1 u n n k k k ab k k (, (,, ({ }{, },, M M p s u o p s u o p s u o k= 1 k= 1 s n o n = s s 1 Hdden varables Observed varables Fg. 1 The graphcal model for HMT-SLAM. Here segmens of he robo pah are condonally ndependen gven he sarng pose from each segmen. The relave pose beween areas s a random varable bu defned as an analycal funcon of robo poses. ( k l { } Ths heorecal resul suggess ha HTM-SLAM can be acheved hrough a se of n separae esmaon processes, one for he robo pah whn each area. Then, map hypoheses are compued accordng o he correspondng esmaed pahs. Snce he robo sae s s a Markov process, we can sequenally esmae va he Bayes rule: M I s, s s, s, k, l > (5 o u 1 o 1 u 2 s 2 o 2 (6 Poseror Bayes (, p s u o Observaon lkelhood p o u o p s s o u p s u o ds (, (,, (, Transon model Pror Ceranly, an exac soluon for he negral above s no avalable n he general case, forcng an approxmae approach. Furhermore, he nracable growh n he number of possble opologcal pahs [22] mposes a sample based approxmaon. Noce ha he esmaed pah s defnes he opologcal srucure of he HMT map, as llusraed wh some examples n Fg. 2. In hs work we compue (7 by means of a Sequenal Imporance Resamplng (SIR fler [6]. Assume ha he dsrbuon of he robo pah unl he las me sep (-1 s avalable as a se of M parcles: { s 1,[ ] } p ( s 1 u 1, o 1 = 1.. M (7 (8 The fler n (7 can be mplemened as a predcon sep followed by he correspondng parcle weghs updae: { s [] } p ( s u, o 1, [] [] ( [] [] ω ω p o s 1, m = 1.. M (9 Nex, a selecve resamplng sep s requred o allevae he parcle depleon problem [1]. Recallng he Markov assumpon, we can now exend he robo pah esmaon as: {, },[ ] 1,[ ] [ ] s s s ( and repea all he seps n he SIR fler recursvely over me. Remember ha Rao-Blackwellzaon grounds on he map dsrbuon o be analycally compuable from (, hus each parcle carres s own map esmaon ha s updaed ndependenly. In our conex hs means ha each parcle carres a hypohess of he curren area s merc map only, snce prevous local sub-maps are removed from he hsory of parcles each me he robo eners a new area. I may be somemes desrable o compue he dscree probably mass funcon (PMF of he opologcal pah γ, e.g. for vsualzng exsng opologcal srucure hypoheses. Ths operaon can be smply acheved by summaon over γ = γ = γ = M {,1, 2,3, 4} {,1, 2,3,} {,1, 2,3,1} Sequence of raversed areas 1 M 2 M 3 M Some opologcal hypoheses 4 M Paron: {,1, 2,3, 4} Paron: {{, 4 },1, 2,3} Paron: {, { 1,4 },2,3} Fg. 2 Our approach akes he sequence of areas raversed by he robo (on he op, and esmaes he opologcal srucure of he envronmen accordng o he feasbly of possble parons n ha sequence. The boom graphs show some examples of poenal opologes and assocaed opologcal pahs γ. 263

4 all he HMT pah hypoheses: ( γ = γ ', = (, γ ',, P u o p x m u o dx dm Parcle aproxmaon { } ω, Ω= : γ = γ ' Ω [],[] (11 I s worh menonng ha vrually all he complexy n our approach ress on he drawng process from he dsrbuon (9, manly due o he esmaon of he opologcal pah. An mplemenaon s proposed n a laer secon o deal wh he complexy of hs ask n real-me applcaons, by means of dvdng no small, parallel subprocesses, and by posponng some operaons. IV. RELEVANT ELEMENTS OF HMT-SLAM Afer exposng he heorecal foundaons of our Bayesan approach o hybrd mappng, n hs secon we furher clarfy some key elemens from hs framework. A. Paronng he map The hybrd map m s an annoaed graph defned by he 2-uple n (2, where nodes are he se of areas { 1 M,, n M } and edges represen coordnaes ransformaon beween hem. Le ab be he ransformaon of coordnaes from he area a M no he reference sysem of b M. Provded for convenence ha he frs pose whn each sub-map s he local coordnae reference, and beng û he nverse pose composon operaor, we have: ab (, a b (, p u o p x û x u o (12 In hs work we compue hs dsrbuon by margnalzaon over all he parcles: a b p x û x u, o = ( ( û,,, (,, Parcle aproxmaon a b p x x s m u o p s m u o ds dm δ ( x û x x û x ω a b a [] b [] [] (13 whch s a pon mass approxmaon drecly avalable from he RBPF employed for local merc SLAM. The proposed model of sub-maps and coordnae ransformaons beween hem has been repored n oher works ([7],[13],[14], hough we also propose a well grounded mehod ha mnmzes he loss of nformaon n he map paronng process: he mnmum normalzed-cu (mn-ncu n he graph of observaons [2],[3]. Ths s n conras wh prevous works, whch ypcally assume heursc crera. To compue hs paronng, a weghed undreced graph s bul where nodes are he robo observaons and edge weghs represen he covsbly beween hem. Perodcally s appled a hghly-effcen specral bsecon algorhm o oban he cluserng ha mnmzes a gven measure of ndependence (namely, he normalzed cu. Noce ha hs bsecon should be acceped only f he measuremen rses over a gven hreshold whch saes he requred ndependence beween sub-maps. The paronng process was already repored n deal elsewhere [2]. B. Uncerany dereference/proecon An ssue n any merc-opologcal approach o SLAM s he proecon of uncerany hrough dfferen coordnae references. Ths process akes any pose dsrbuon p( a x referenced o a gven area a M, and compues s dsrbuon p( b x relave o a second reference sub-map b M. The nverse operaon s uncerany dereferencng, whch s a fundamenal mechansm n our framework: each me he robo eners a new area, s pose uncerany s dereferenced no he new coordnae sysem, as graphcally llusraed n Fg. 3. Ths nvolves a pose nverse compoundng operaon: b a ab x = x û (14 Referencng uncerany o he local frame provdes a maor advance over non-hybrd approaches o SLAM: he farher he robo s from he coordnae reference, he more complcaed becomes o model he pose probably dsrbuon. For EKF mehods hs enals a rase of lnearzaon and non-gaussany errors, whereas n RBPFs more parcles are requred o assure bounded approxmaon errors. Acually, hese problems are a consequence of amng o esmae global coordnaes, whch s avoded here. The oppose suaon (proecng a pose dsrbuon owards a dfferen area across he opologcal graph s carred ou by he pose composon operaor : a ab b x = x (15 whch should be exended recursvely f here are mulple ransformaons (edges beween he orgn and arge areas. Noce ha, n general, many possble opologcal pahs may exs beween a gven par of areas, hus many dfferen and probably nconssen ransformaons are smulaneously applcable. We have adoped he soluon repored n [3], where he Dksra algorhm s appled for fndng he shores opologcal pah, n erms of smalles uncerany. Ths uncerany proecon has a drec applcaon n fndng absolue coordnaes, a requred sep n compung a global map. Alhough our mappng approach does no need a all, compung a global map relave o an arbrary reference sll s an appealng way of vsualzng maps readable o humans, hence he neres n her consrucon. Expermenal resuls n nex secons show examples of such global maps, where nconssences may exs due o he problem menoned above. One can devse opmzaon mehods o reduce such nconssences [3] by means of deas prevously appled o conssen scan machng [17], bu hey are no appled here. In our curren mplemenaon, uncerany propagaon n (14-(15 s performed by Mone Carlo smulaons, snce dsrbuons are already gven as dscree samples from he RBPF. In he case of EKF-based SLAM, approxmae closed-form soluons exs [7]. V. IMPLEMENTATION FRAMEWORK In secon III we nroduced he heorec foundaons of HMT-SLAM. Nex a praccal framework s presened whch mplemens hose deas whle keepng n mnd ha a moble robo may demand accurae merc localzaon n real-me (e.g. for navgaon or manpulaon purposes, whereas mananng he conssency of he opologcal map, 264

5 Prevous coordnae reference x Uncerany proecon +1 x New reference Uncerany dereference New reference Fg. 3 Example of uncerany dereference/proecon from a real expermen. The op fgure shows he evoluon of parcles as he se of esmaed pahs from each hypohess. When enerng no a new area s deeced, he uncerany s dereferenced no a new coordnae reference (see op fgure. Relavely o he new area, he pah uncerany has been grealy reduced, as shown n he boom fgure. The ellpses ndcae 99.97% confdence nervals for approxmae Gaussan dsrbuons. hus, solvng loop closures, may be deal n an any-me fashon. The sysem s skeched n Fg. 4, where can be seen he layered srucure: merc local SLAM s performed n he low level, whle more absrac (opologcal represenaons are managed n upper levels. The npus o he sysem are acons and observaons from he robo, whch are kep n a me-samp-ordered queue unl hey can be processed. Whn he sysem, here are a number of processes runnng concurrenly whch nerac by readng and wrng operaons no he hree levels, llusraed n Fg. 4: he local merc map of he curren area, he sequence of raversed areas, and he space of opologcal pah hypoheses. I mus be remarked he parallel naure of he sysem, snce he processes do no run n a predefned, sequenal order. Nex we descrbe he concurren processes n he sysem and her relaons wh he dfferen level descrbed above: Merc SLAM: Ths process handles he robo localzaon and mappng whn he local merc map for he curren area, by processng acons and observaons and negrang hem no he Bayes fler. RBPFs represen an Acons Observaons Probablsc opologcal level Hybrd mercopologcal map level Tmesamp-ordered queue Local mercmap level HMT mappng framework TSBI M 4 M SLAM 1 M M 3 M TLCA 2 M 2 M AAM 3 M 3 M 1 M MCO +1 x RTL Real-me localzaon esmaon Fg. 4 Overvew of a proposed mplemenaon for our hybrd mappng framework, desgned as a praccal soluon o he heorecal esmaon process nroduced n he paper. Please refer o he ex for a dealed descrpon. aracve choce here due o her consan-me operaon, an essenal feaure o enable relable real me low-level SLAM. Ths process s relaed o he esmaon of he merc par of he robo pah descrbed n (7-(. Area Absracon Mechansm (AAM: Appearance-based mehods are appled here o deec clusers of (approxmaely ndependen observaons n he sequence gahered by he robo ([2],[3],.e. wheher he robo has enered no a new area. In such an even, observaons from he las area are absraced no a new area n he upper level. In our curren mplemenaon we assume ha he paronng of areas does no change wh me, whch does no represen a hurdle snce he paronng mehod s robus enough o produce approxmaely he same clusers of a gven area ndependenly of he robo pah across. Topologcal Space Bayesan Inference (TSBI: Ths process assgns values o he opologcal pah γ of he robo accordng o he curren map hypoheses [22], and s relaed o he opologcal par of he drawng process n (9. In our curren mplemenaon he nference on he opologcal pah γ s posponed unl he AAM algorhm sars a new sub-map. We have found ha by dong so, he TSBI process can be performed hrough a smple maxmum lkelhood esmaon (MLE approach: γ ( γ = arg max P u, o, s, m [],* 1 1,[] [] γ Bayes 1 1,[ ] [ ] 1,[ ] [ ] ( γ,,, ( γ ( k γ,, P u o s m P p o s m k (16 where no nformaon abou he pror P(γ s assumed,.e. we consder a unform dsrbuon over all pahs. The mehod above smplfes he mplemenaon, snce (16 can be easly evaluaed ponwse, whle sll performng well for large-scale map buldng. However, hs smplfcaon s no suable for global HMT localzaon, an ssue no addressed n he presen work. Topologcal Loop Closure Accepance (TLCA: Due o compuaon and sorage lmaons, may be requred ha he robo forges par of s opologcal pah γ. The TLCA process performs hs ask, whch s equvalen o accepng par of he opologcal srucure hypohess as correc. However, n pracce hs can be done for hghly domnan (or unque hypoheses, whch can be deermned by evaluang (11, hence he loss of nformaon shall be neglgble n mos cases. Maps Conssency Opmzaon (MCO: Ths ask s n charge of opmzng he relave pose of observaons whn each sub-map. The mehod from Lu and Mlos [17], runnng n O(n 2 wh he map sze, s approprae here snce he sze of he local maps s bounded. Real Tme Localzaon (RTL: Ths process guaranees an esmaon of he robo poson n a mely fashon. If he npu queue s empy, he bes pose esmaon s s, already updaed by he SLAM algorhm. However, f here s pendng acons n he queue, he RTL compues he pror dsrbuon, e.g. p(s +1 s,u +1, as a more updaed esmaon of he acual robo pose. Ths s clearly no he opmal soluon, bu can be easly updaed n real me. 265

6 Alhough our mplemenaon does no explo all he poenal of he developed heorecal framework (mosly due o he posponed opologcal loop-closure hrough maxmum lkelhood esmaon, has demonsraed s suably for large-scale map buldng of complex envronmens. VI. EXPERIMENTAL VALIDATION We have esed our mappng framework wh wo dfferen daa ses, boh wh odomery readngs and laser range scans n large-scale planar scenaros. One daa se was gahered by he auhors a he unversy of Malaga, and comprses almos 5 laser scans colleced along a 1.9Km pah. The oher daa se was recorded a Edmonon Convenon Cenre by Nck Roy, and s freely avalable onlne. Please, consder vewng he vdeos of he expermens [29] o ge a beer grasp of he resuls. To compare he effcency of our approach wh prevous mehods we have also bul he correspondng global merc maps wh a hghly effcen RBPF echnque proposed n [] for global merc mappng. The performance n boh compuaon me and memory requremens s summarzed n Table I. Resuls are for a 2.GHz Penum M (1Gb RAM, and for occupancy grd maps wh a cell sze of 12cm. TABLE I PERFORMANCE COMPARISON BETWEEN GLOBAL RBPF-MAPPING AND OUR APPROACH FOR HMT-SLAM Memory requremens Compuaon me Mehod Global HMT- Global HMT- Daa se _ RBPF SLAM RBPF SLAM Edmonon Mb Mb 39 mn. 7 mn. Malaga Mb Mb 3 mn. 24 mn. I s noceable ha HMT-SLAM ouperforms global RBPF for boh daa ses. The mprovemen n he memory requremens follows from he fac ha parcles n our approach carry a hypohess of he local merc map only, whereas n RBPF hose hypoheses are for he whole global map. Therefore, he advanage of HMT maps becomes more and more relevan for ncreasngly larger envronmens. Regardng he lower compuaon me of our approach, s a drec consequence of he reduced number of parcles. However, by usng a few parcles only n local SLAM (we use 15 parcles, our approach can aan a much beer represenaon of he uncerany (hrough Mone Carlo smulaons of he uncerany proecon n (15 han he one aanable by a global RBPF wh a praccal number of parcles (e.g. less han 5. Ths urns no more precse loop closures n HMT-SLAM han n merc RBPF. The map esmaon before and afer a loop closure are shown n Fg. 5(a-(b for he Malaga daa se. In hs suaon, he robo eners a new area, labeled 11, leavng he prevous area. Then, he domnan opologcal pah hypohess becomes ha of esablshng he paron { 1, 11 }, ha s, he mos lkely explanaon for robo observaons s ha area 11 acually corresponds wh he area 1. I can be apprecaed n Fg. 5(c-(d how he loop closure affecs he pose uncerany of surroundng areas due o he nroducon of a new edge ha modfes he Dksra shores pahs employed o generae hose global maps (noce ha ellpses n he fgure exaggerae he acual uncerany by a facor of 5 for ease of vsualzaon. The fnal HMT maps bul from he Malaga and Edmonon daa ses are ploed n Fg. 5(e-(f, respecvely, as global maps where global coordnae references have been arbrarly se o he frs nodes n each map. Alhough GPS readngs are no avalable o measure absolue localzaon errors, our work s no focused on obanng an accurae global merc map, bu on relably esmang he opologcal srucure of he envronmen, whch has been successfully carred ou n boh expermens. VII. CONCLUSIONS AND FUTURE WORK In hs work we have nroduced a new vewpon for solvng he problem of large-scale SLAM whch consss of esmang he hybrd merc-opologcal (HMT pah followed by he robo. I has been demonsraed ha our approxmaon s suppored by he probablsc srucure of he SLAM problem under weak assumpons. We have also presened a relavely smple mplemenaon of our deas n he form of a real-me/anyme sysem. Ths mplemenaon has demonsraed o be effcen for mappng large scale envronmens wh mulple loops, bu furher research s needed o fully explo he versaly of he HMT-SLAM framework agans harder problems. For example, mappng hghly ambguous envronmens, or effcenly solvng he robo awakenng problem whn large (even parally unknown envronmens, are ssues ha can be hardly deal wh exsng mehods. We beleve ha he proposed paradgm of HMT-SLAM s a promsng approach for hese problems. REFERENCES [1] M.S. Arulampalam, S. Maskell, N. Gordon, T. Clapp, A Tuoral on Parcle Flers for Onlne Nonlnear/Non-Gaussan Bayesan Trackng, n IEEE Transacons on Sgnal Processng, v.5, pp , 22. [2] J.L. Blanco, J. González, J.A. Fernández-Madrgal, Conssen Observaon Groupng for Generang Merc-Topologcal Maps ha Improves Robo Localzaon, n ICRA6, pp , 26. [3] M. Bosse, P. Newman, J. Leonard, M. Soka, W. Feen, S. Teller, An Alas framework for scalable mappng, n ICRA3, pp , 23. [4] H. Chose, K. Nagaan, Topologcal smulaneous localzaon and mappng (SLAM: oward exac localzaon whou explc localzaon, n IEEE Trans. on Rob. and Auo., v.17, pp , 21. [5] M. Dssanayake, P. Newman, S. Clark, H.F. Durran-Whye, M.A. Csorba, A soluon o he smulaneous localzaon and map buldng (SLAM problem, n IEEE Transacons on Robocs and Auomaon, v.17, no. 3, pp , 21. [6] A. Douce, N.D. Freas, K. Murphy, S. Russell, Rao-Blackwellsed Parcle Flerng for Dynamc Bayesan Neworks, n Proc. of he 16h Conf. on Uncerany n Arfcal Inellgence, 2. [7] C. Esrada, J. Nera, J.D. Tardós, Herarchcal SLAM: Real-Tme Achúrae Mappng of Large Envronmens, n IEEE Transacons on Robocs, v.21, no. 4, pp [8] D. Fox, KLD-samplng: Adapve parcle flers and moble robo localzaon, n Advances n NIPS, 21. [9] U. Frese, A Dscusson of Smulaneous Localzaon and Mappng, n Auonomous Robos, v.2, pp.25-42,

7 Sep #9 Sep #12 Malaga daa se y (m (a Sep # x (m (c y (m (b 12 Sep # x (m 12 (d (e Edmonon daa se [] G. Grse, C. Sachnss, W. Burgard, Improvng Grd-based SLAM wh Rao-Blackwellzed Parcle Flers by Adapve Proposals and Selecve Resamplng, n ICRA5, pp , 25. [11] G. Grse, G.D. Tpald, C. Sachnss, W. Burgard, D. Nard, "Speedng-Up Rao-Blackwellzed SLAM", n ICRA6, pp , 26. [12] J.S. Gumann, K. Konolge, Incremenal mappng of large cyclc envronmens, n IEEE Inernaonal Symposum on Compuaonal Inellgence n Robocs and Auomaon, pp , [13] M.E. Jefferes, M.C. Cosgrove, J.T. Baker, W.K. Yeap, The Correspondence Problem n Topologcal Merc Mappng-Usng Absolue Merc Maps o Close Cycles, n In. Conf. on Knowledgebased Inellgen Informaon and Engneerng Sysems, 24. [14] K. Kouzoubov, D. Ausn, Hybrd opologcal/merc approach o SLAM, n ICRA4, v.1, pp , 24. [15] B. Kupers, Y.T. Byun, A Robo Exploraon and Mappng Sraegy Based on a Semanc Herarchy of Spaal Represenaons, n Robocs and Auonomous Sysems, v.8, pp , 21. [16] B. Lsen, D. Morales, D. Slver, G. Kanor, I. Rekles, H. Chose, The herarchcal alas, n IEEE Trans. on Robocs, v.21, pp , 25. [17] F. Lu, E. Mlos, Globally Conssen Range Scan Algnmen for Envronmen Mappng, n Auonomous Robos, v. 4, pp , [18] M. Monemerlo, S. Thrun, D. Koller, B. Wegbre, FasSLAM: A facored soluon o he smulaneous localzaon and mappng problem, n Proc. of he AAAI Nal. Conf. on Ar. Inell., pp , 22. [19] P. Mouarler,R. Chala, Sochasc mulsensory daa fuson for moble robo locaon and envronmen modelng, n 5h In. Symposum on Robocs Research, pp , (f 2 meers Memory (Mb (h HMT-SLAM Merc RBPF Tme (seps Fg. 5 (a-(b The map us before and afer closng a loop by mergng nodes {1, 11}. I s shown n (c-(d how hs opologcal loop closure reduces he uncerany n he poson of surroundng nodes (uncerany s represened by 95% confdence nervals, where uncerany has been exaggeraed by a facor of 5 for clary. The fnal global map obaned for he Malaga daa se s shown n (e, and n (f overlapped wh a saelle phoo of he acual place. The graph (g shows he global map obaned wh our mehod for he Edmonon daa se. The memory requremens of our approach n conras wh hose of a global merc RBPF-mappng are also ploed n (h for he Malaga daa se. [2] H. Moravec, A. Elfes, Hgh resoluon maps from wde angle sonar, n Robocs and Auomaon, v.2, pp , [21] J. Nera, J.D. Tardós, Daa Assocaon n Sochasc Mappng Usng he Jon Compably Tes, n IEEE Transacons on Robocs and Auomaon, v.17, no. 6, pp , 21. [22] A. Ranganahan, E. Menega, F. Dellaer, Bayesan Inference n he Space of Topologcal Maps, n IEEE Trans. on Rob., pp. 92-7, 26. [23] F. Savell, B. Kupers, Loop-closng and planary n opologcal mapbuldng, n IROS4, v.2, pp , 24. [24] R. Sm, P. Elnas, M. Grffn, A. Shyr, J.J. Lle, Desgn and analyss of a framework for real-me vson-based SLAM usng Rao- Blackwellsed parcle flers, n Canadan Conf. on Compuer and Robo Vson, 26. [25] C. Sachnss, G. Grse, W. Burgard, Recoverng Parcle Dversy n a Rao-Blackwellzed Parcle Fler for SLAM Afer Acvely Closng Loops, n ICRA5, pp , 25. [26] J.D. Tardos, J. Nera, P.M. Newman, J.J. Leonard, Robus Mappng and Localzaon n Indoor Envronmens Usng Sonar Daa, n The Inernaonal Journal of Robocs Research, v.21, pp , 22. [27] S. Thrun, Roboc Mappng: A Survey, n Explorng Arfcal Inellgence n he New Mllennum, 22. [28] S. Thrun, A. Bücken, Inegrang grd-based and opologcal maps for moble robo navgaon, n Na. Conf. on Ar. Inell., pp , [29] Webse for mulmeda conen for expermenal resuls, avalable n hp:// [3] Z. Zvkovc, B. Bakker, B. Krose, Herarchcal map buldng usng vsual landmarks and geomerc consrans, n IROS5, pp , 25. (g 267