Distributed Multi-robot Exploration and Mapping

Transcription

1 1 Disribued Muli-robo Exploraion and Mapping Dieer Fox Jonahan Ko Kur Konolige Benson Limkekai Dirk Schulz Benjamin Sewar Universiy of Washingon, Deparmen of Compuer Science & Engineering, Seale, WA Arificial Inelligence Cener, SRI Inernaional, Menlo Park, CA Universiy of Bonn, Deparmen of Compuer Science III, Bonn, Germany Absrac Efficien exploraion of unknown environmens is a fundamenal problem in mobile roboics. In his paper we presen an approach o disribued muli-robo mapping and exploraion. Our sysem enables eams of robos o efficienly explore environmens from differen, unknown locaions. In order o ensure consisency when combining heir daa ino shared maps, he robos acively seek o verify heir relaive locaions. Using shared maps, hey coordinae heir exploraion sraegies so as o maximize he efficiency of exploraion. Our sysem was evaluaed under exremely realisic real-world condiions. An ouside evaluaion eam found he sysem o be highly efficien and robus. The maps generaed by our approach are consisenly more accurae han hose generaed by manually measuring he locaions and exensions of rooms and objecs. I. INTRODUCTION Efficien exploraion of unknown environmens is a fundamenal problem in mobile roboics. As auonomous exploraion and map building becomes increasingly robus on single robos, he nex challenge is o exend hese echniques o eams of robos. Compared o he problems occurring in single robo exploraion, he exension o muliple robos poses several new challenges, including (1) coordinaion of robos, (2) inegraion of informaion colleced by differen robos ino a consisen map, and (3) dealing wih limied communicaion. Coordinaion: Increasing efficiency is one of he key reasons for deploying eams of robos insead of single robos. The more robos ha explore an environmen, he more imporan he coordinaion beween heir acions becomes. The difficuly of he coordinaion ask srongly depends on he knowledge of he robos. If he robos know heir relaive locaions and share a map of he area hey explored so far, hen effecive coordinaion can be achieved by guiding he robos ino differen, non-overlapping areas of he environmen [1], [29], [37], [2]. This can be done by assigning he robos o differen exploraion froniers, which are ransiions from explored free-space o unexplored areas [36], [1]. However, if he robos do no know heir relaive locaions, hen i is far less obvious how o effecively coordinae hem, since he robos do no share a common map or frame of reference. Map merging: In order o build a consisen model of an environmen, he daa colleced by he differen robos has o be inegraed ino a single map. Furhermore, such an inegraion should be done as early as possible, since he availabiliy of a shared map grealy faciliaes he coordinaion beween robos. If he iniial locaions of he robos are known, map merging is a raher sraighforward exension of single robo mapping [32], [6], [35], [20]. This is due o he fac ha he daa races of he individual robos can be reaed as if hey were colleced by a single robo. Consisen inegraion of he daa when he robos do no know heir relaive locaions is more difficul, since i is no clear how and where he robos races should be conneced. Limied communicaion: During exploraion of large-scale environmens, communicaion beween he robos and a conrol saion migh fail. To achieve robusness agains such failures, each robo has o be able o explore on is own, i.e., wihou guidance by a cenral conrol node. Furhermore, groups of robos should be able o coordinae heir acions wihou he need of a cenral conrol node, and each robo should be able o ake over he ask of coordinaion. In his paper we presen an inegraed muli-robo mapping and exploraion sysem ha addresses all hese challenges. The approach enables eams of robos o efficienly build highly accurae maps of unknown environmens, even when he iniial locaions of he robos are unknown. In order o avoid wrong decisions when combining heir daa ino shared maps, he robos acively verify heir relaive locaions. Using shared maps, hey coordinae heir exploraion sraegies so as o maximize he efficiency of exploraion. Our sysem was evaluaed horoughly by an ouside evaluaion eam. The resuls of his es showed ha our approach is highly efficien and robus. The maps generaed by our robos are consisenly more accurae han hose generaed by manually measuring he locaions and exensions of rooms and objecs. This paper is organized as follows. In he nex secion, we provide an overview of our muli-robo coordinaion echnique, followed by a descripion of an approach o esimaing relaive locaions beween robos. Then, in Secion IV, we show how he daa colleced by muliple robos can be inegraed ino a consisen map of an environmen. The following secion describes experimens supporing he reliabiliy of our echniques. We conclude in Secion VI. II. DECISION-THEORETIC COORDINATION ARCHITECTURE We will now discuss he concep underlying our mulirobo coordinaion echnique; implemenaion deails will be provided in he experimenal resuls Secion V. A. Relaed Work Virually all exising approaches o coordinaed muli-robo exploraion assume ha all robos know heir locaions in a shared (parial) map of he environmen. Using such a map, effecive coordinaion can be achieved by exracing exploraion froniers from he parial map and assigning robos o froniers based on a global measure of qualiy [1], [29],

2 2 [37], [2], [31]. As illusraed in Fig. 1, exploraion froniers are borders of he parial map a which explored free-space is nex o unexplored areas [36], [1], [24]. These borders hus represen locaions ha are reachable from wihin he parial map and provide opporuniies for exploring unknown errain, hereby allowing he robos o greedily maximize informaion gain [17], [34]. To measure he qualiy of an assignmen of robos o froniers, he overall ravel disance combined wih an esimae of he unexplored area a each fronier proved o be highly successful in pracice [1], [29]. The assumpion of he availabiliy of a shared map, however, severely resrics he scenarios ha can be handled by such an exploraion sraegy. For insance, a unique, globally consisen map can be generaed only if he robos know heir relaive locaions. If he robos do no know heir relaive locaions, hen i is no clear how hey can combine heir maps ino a global, shared map. Knowledge abou relaive locaions is readily available only if all robos sar a he same locaion or have sensors ha provide locaion esimaes in a global frame of reference. While he laer case can hold when using GPS for oudoor exploraion [26], here exiss no global posiioning sensor for indoor environmens. Thus, in order o deal wih more general exploraion seings, he robos mus be able o handle uncerainy in heir relaive locaions, which direcly ranslaes ino uncerainy in how o combine heir maps. In a full Bayesian reamen, he robos could esimae poserior probabiliy disribuions over heir relaive locaions and hen coordinae heir acions based on he resuling disribuion over shared maps. While such an approach could lead o a highly effecive exploraion sraegy, i does no scale well since he number of possible relaive locaions, and hus maps, grows exponenially in he number of robos. To avoid his complexiy, virually all approaches o muli-robo mapping under posiion uncerainy le he robos explore independenly unil hey have reliable esimaes of heir relaive locaions; a which ime heir maps are merged and he robos sar o coordinae heir exploraion sraegies [3], [6], [35], [32], [16], [14]. To esimae relaive locaions, Howard and colleagues rely on he robos abiliy o deec each oher [14]. Here, all robos explore independenly of each oher unil one coincidenally deecs anoher robo. The robos use such deecions o deermine heir relaive locaion, based on which hey combine heir maps. While such an approach scales well in he number of robos, i can resul in inefficien exploraion, since i can ake arbirarily long unil robos coincidenally deec each oher. For insance, if one robo follows he pah of he oher robo wihou knowing, boh robos migh explore he complee map wihou ever deecing each oher. Oher approaches esablish relaive locaions beween pairs of robos by esimaing one robo s locaion in anoher robo s map. This is ypically done under he assumpion ha one robo sars in he map already buil by he oher robo [6], [35], [32] or ha here exiss an overlap beween he parial maps [3]. Since hese echniques do no verify locaion esimaes, hey migh erroneously merge maps, which ypically resuls in inconsisen maps. Our approach combines and exends hese ideas in order o generae an efficien and robus exploraion sysem. In conras f 1 r1 f 3 c 11 r2 c 12 f2 f4 f5 f r 7 8 f 3 6 f r f 4 9 Fig. 1. Coordinaion example: Parial map buil by exploraion cluser of four robos (red circles r 1,..., r 4). Addiionally, wo locaion hypoheses (blue circles c 11, c 21) have been generaed for robo c 1, which is no ye par of cluser. The map has nine exploraion froniers (f 1,..., f 9), indicaed by green lines. o [6], [35], [32], [3], our echniques makes no assumpions abou he relaive locaions of robos. Furhermore, i adds robusness by verifying hypoheses for he relaive locaion of robos. Similar o [14], his is done by using robo deecions. However, in conras o [14], hese deecions are no coincidenal; hey are pursued acively. B. Decision-heoreic Coordinaion Our echnique for exploraion wih unknown sar locaions inegraes robo deecions ino a Bayesian, decision-heoreic exploraion sraegy. In a nushell, our sysem works as follows. Iniially, he robos migh no know heir relaive locaions. In such a case, each robo explores on is own, mapping an increasingly large porion of he environmen. As soon as wo robos can communicae, hey sar o exchange sensor daa and esimae heir relaive locaion. Once hey have a good hypohesis for heir relaive locaion, hey acively verify his hypohesis using a rendez-vous echnique. If successful, he robos form an exploraion cluser: hey combine heir daa ino a shared map and sar o coordinae heir exploraion acions. On he oher hand, if he relaive locaion hypohesis urns ou o be wrong, he robos coninue o explore independenly and exchange sensor daa so as o refine heir esimaes of heir relaive locaion. During exploraion, he size of exploraion clusers increases as more robos deermine heir relaive locaions, ending in a single cluser conaining all robos. As long as a robo is no par of an exploraion cluser, i individually explores an environmen by moving o he closes exploraion fronier in is parial map [36], [17]. To coordinae he robos wihin an exploraion cluser, we exend he decision-heoreic approaches of [1], [29], [37], [2] o he case of relaive posiion uncerainy [16]. To do so, we assume ha he robos wihin an exploraion cluser share a map and ha he posiions r i of all robos in he shared map are known. Fig. 1 shows an exploraion cluser of four robos sharing a parial occupancy grid map. Exploraion froniers f i are indicaed by hick green lines. The figure also shows hypoheses c 11 and c 21 for he locaion of a robo no ye par of he cluser. In general, le c ij denoe he i-h hypohesis for he unknown locaion of robo j. p(c ij ) is he probabiliy ha robo j acually is a his hypohesis (how hypoheses and heir probabiliies are deermined will be described in Secion III).

3 3 The robos in an exploraion cluser rade off exploring unknown errain and verifying hypoheses for he locaions of oher robos. Hypohesis verificaion is done by sending one of he robos o he hypohesis and physically esing wheher here acually is anoher robo. In our sysem, similar o [14], robo deecions are performed by marking robos wih highly reflecive ape and using laser range-finders o deec hese markers. Once a locaion hypohesis is verified, he daa of his robo can be added o he cluser map and he robo can paricipae in coordinaed exploraion. A any poin in ime, each robo in he exploraion cluser can be assigned eiher o an exploraion fronier or o a hypohesized locaion of a robo ouside he cluser. Coordinaion beween he robos can be phrased as he problem of finding he assignmen from robos o froniers and hypoheses ha maximizes a uiliy-cos radeoff. To see, le θ denoe an assignmen ha deermines which robo should move o which arge (froniers and hypoheses). Each robo is assigned o exacly one arge and θ(i, j) = 1 if he i-h robo in he exploraion cluser is assigned o he j-h arge. Among all assignmens we choose he one ha maximizes expeced uiliy minus expeced cos: θ = argmax θ (i,j) θ θ(i, j) (U(i, j) C(i, j)) (1) The cos and uiliy of each robo arge pair (i, j) can be compued as follows. Cos: If he arge is a fronier hen he cos is given by he minimum cos pah from he robo s posiion r i o he fronier posiion f k. Minimal cos pahs can be compued efficienly by A search. For hypohesis verificaion, he cos is given by he minimal pah o a meeing poin beween he robos plus he cos o esablish wheher he wo robos acually mee or no. { dis(ri, f k ) if j-h arge is fronier f k C(i, j) = (2) verify(r i, c pq ) if j-h arge is hypohesis c pq Uiliies: If he arge is a fronier, hen he uiliy is given by he expeced area he robo will explore a ha fronier. This area is esimaed by he size of he unknown area visible from he fronier [1]. If he arge is a locaion hypohesis, say c pq, hen he uiliy is given by he expeced uiliy of meeing robo r q. The funcion coord esimaes his uiliy by measuring he map size of he oher robo plus he expeced uiliy of coordinaed exploraion versus independen exploraion. Since i is no known wheher he oher robo is a he locaion hypohesis, he uiliy of meeing is weighed by he probabiliy of he hypohesis, denoed p(c pq ). { explore(ri, f k ) if j-h arge is fronier f k U(i, j) = (3) p(c pq )coord(r q ) if j-h arge is hypohesis c pq Once he pairwise uiliies and coss are compued, we use a linear program solver o find he opimal assignmen. Finding opimal assignmens can be performed in ime O(mn), where m is he number of robos and n is he number of goals [11]. In exploraion scenarios involving up o six robos, we found he overall compuaion ime for his decision sep o be negligible compared o he oher cos involved in exploraion (less han 1 second). Using he rade-off beween Eq. 2 and Eq. 3, robos ypically move o exploraion froniers and only choose a hypohesis as a arge if i is no oo far away and is probabiliy is very high. III. ESTIMATING RELATIVE POSITIONS We now discuss an algorihm for sequenially esimaing he relaive locaions beween pairs of robos exploring an environmen [16]. In order o perform his esimaion, robos exchange laser range scans and odomery moion informaion whenever hey are in communicaion range. Our approach considers only pairs of robos since he complexiy of esimaing relaive locaions is exponenial in he number of robos considered joinly. One approach o esimaing he overlap beween parial maps of wo robos is o compue he correlaions beween he maps for each possible overlap. Unforunaely, such an approach does no adequaely handle uncerainy in mapping and does no lend iself o an incremenal implemenaion. We overcome his problem by using an adaped paricle filer in combinaion wih a predicive model of indoor environmens in order o sequenially deermine wheher and how he parial maps of wo robos overlap. A. Paricle Filer for Parial Map Localizaion Exising approaches o robo localizaion have only addressed he problem of localizing a robo in a complee map of an environmen. Paricle filers have been applied wih grea success o his problem [8], [21], [15], [7]. The main difference beween localizing a robo in a complee and in a parial map of an environmen is due o he fac ha he robo migh no be inside he parial map and ha he robo can ener or exi he map a any poin in ime. We now show how o leverage he represenaional capabiliies of paricle filers so as o address his more complex esimaion problem. A sraighforward approach o parial map localizaion would be o esimae a robo s posiion boh inside and ouside he map. However, such an approach could be exremely inefficien since he area ouside a parial map can be arbirarily large. Forunaely, for he purpose of map merging, i is no necessary o reason abou all possible locaions ouside he map. Insead, we are only ineresed in robo locaions ha are par of rajecories ha overlap wih he parial map (nonoverlapping rajecories correspond o cases in which he wo maps do no overlap a all). Similar o he applicaion of Rao-Blackwellised paricle filers for mobile robo mapping [23], [13], [5], one can consider a paricle filer as recursively compuing poserior probabiliy disribuions over robo rajecories. A paricle filer represens such poserior disribuions by ses S = { x (i) e i:, w (i) i = 1,..., N} of N weighed samples disribued according o he poserior. Here each x (i) e i: is a rajecory, and he w (i) are nonnegaive numerical facors called imporance weighs, which sum up o one. In our case, he rajecories have differen lenghs, as indicaed by e i, he ime when rajecory i firs enered he parial map. We proceed as shown in Table I, in order o generae poseriors over such rajecories.

4 4 Robo posiion Enry samples Robo posiion Bes rajecory mach Froniers Bes rajecory mach Enry samples Robo posiion (a) (b) Fig. 2. Parial map localizaion: (a) The paricle filer generaes enry poin samples along he arcs a each fronier. (b) (d) Sample ses a differen poins in ime. The picures also show he rue robo locaion, he enry poin samples (blue), and he mos likely hypohesis for he oher robo s posiion along wih he rajecory of his hypohesis. (b) Iniially, he samples are spread uniformly hroughou he parial map. (c) Afer only shor overlap, he mos likely rajecory does no mach he rue pah of he robo. (d) The robo exis he map and he mos likely paricle is a he correc posiion. 1. Inpus: S 1 = { x (i) e i : 1, w(i) 1 i = 1,..., N}, conrol informaion u 1, observaion z, parial map M, probabiliy of non-overlapping rajecories n 1, probabiliy of enering parial map ε, number of enry poin samples N ε 2. S := // Iniialize 3. for all samples x (i) e i : 1, w(i) 1 in S 1 do // Exend exising rajecories 4. sample x (i) from p(x x (i) e i : 1, u 1) 5. S := S { x (i) e i :, w (i) 1 } (c) // Predic nex posiion using moion // Inser ino nex se 6. for i := 1,..., N ε do // Generae N ε new rajecories saring a enry poins 7. sample x (i) from an enry poin ino he map // Enry poins are given by ransiion from free space o unexplored 8. w (i) // Se weighs accordingly = ε n 1 N ε 9. S := S { x (i), w (i) } // Inser ino se 10. n = (1 ε) n 1 // Subrac fracion ε migraed from non-overlapping rajecories ino map 11. for all samples x (i) e i :, w (i) in S do // Inegrae observaion ino individual rajecories 12. if x (i) M hen w (i) := w (i) p(z x (i) ) // Trajecories currenly inside he map else := w (i) p(z ouside) // Trajecories currenly ouside he map 13. n = n p(z ouside) // Updae probabiliy of non-overlapping rajecories 14. α 1 w (i) = n + P i=1,...,n+n ε w (i) // Compue normalizaion facor 15. n = α n ; i : w (i) = α w (i) // Normalize 16. resample samples in S based on heir weighs // Samples N rajecories TABLE I: OUTLINE OF PARTICLE FILTER BASED IMPLEMENTATION OF PARTIAL MAP LOCALIZATION. (d) The algorihm akes as inpu he previous sample se along wih he oher robos mos recen conrol informaion and observaion sen via wireless communicaion. Addiionally, i requires he curren parial map M and n 1, which is he probabiliy ha so far here was no overlap beween he oher robos rajecory and he parial map. As soon as M 0 is sufficienly large, he algorihm is sared as follows: A = 0, each rajecory consiss of only one robo locaion x 0. In his degenerae case, only locaions inside he parial map correspond o overlapping rajecories. Since here is no knowledge abou he iniial locaion of he robo, hese rajecories x 0 are sampled uniformly hroughou he parial map. n 0, he probabiliy ha he oher robo iniially is ouside he parial map, is se according o an esimae of he raio beween he sizes of he parial map and he enire environmen. Fig. 2(b) shows a parial map along wih such a uniformly iniialized sample se. The figure also shows he rue robo locaion, which iniially is ouside he parial map. Then, a each ieraion of he paricle filer, he rajecories are updaed based on he following reasoning. A ime, a rajecory can overlap wih he parial map if and only if i already overlapped a ime 1 or if he robo jus enered he parial map for he firs ime. The firs case is handled by Seps 3-5 of he algorihm. Here, each overlapping rajecory of he previous ime sep is exended using he moion informaion u 1. Then, in Seps 6 hrough 9, he algorihm generaes N ε locaions ha correspond o rajecories ha ener he parial map for he firs ime. These rajecories do no conain locaions prior o ime, since we are mosly ineresed in heir locaions inside he parial map (in fac, our efficien implemenaion only keeps rack of he mos recen locaion in each rajecory). The enry poins in Sep 7 are generaed based on he assumpion ha he robo can only ener he parial map a is fronier cells. These cells are shown in Fig. 2(a), and he corresponding enry samples are indicaed in he sample se shown in Fig. 2(c). The weighs of hese samples are se in Sep 8 such ha he combined weighs of all new enry samples is ε n 1. The parameer ε represens he probabiliy ha he robo eners he map a any poin in ime given ha i previously was ouside. Sep 10 adjuss he probabiliy of non-overlapping rajecories by subracing he weighs of rajecories ha enered he map. A his poin in ime, he

5 5 weighs of all overlapping rajecories (exended and new ones) plus he probabiliy n of non-overlapping rajecories sum up o one (he only change is due o a shif of probabiliy from non-overlapping o jus enering rajecories). So far, we only considered robo moion, he mos recen observaion is inegraed in Seps of he algorihm. Here we have o consider wo cases depending on wheher he locaion is inside or ouside he parial map. Sep 12 handles locaions inside he parial map by muliplying he rajecory weigh wih he observaion likelihood, which can be exraced from he map, exacly as in regular robo localizaion [8], [9], [34]. Trajecories ha overlap wih he map bu exied i a one poin in ime are weighed by he likelihood of observing he measuremen ouside he map (we will discuss our approach o compuing his likelihood in he nex secion). The same likelihood is used o weigh he probabiliy n of non-overlapping rajecories in Sep 13. The normalizaion facor is deermined in he following sep and muliplied ino n and all weighs in Sep 15. The final sep samples N rajecories from he weighed samples and ses he weighs such ha hey sum up o 1 n. I can be shown ha each ieraion of his paricle filer generaes samples ha are disribued according o he poserior over rajecories ha overlapped wih he parial map a some poin in ime. If he size of he parial map increases during his process, we move he enry poin froniers accordingly. B. Predicive Model for Observaions Ouside he Parial Map A key quaniy esimaed by he paricle filer algorihm is n, which is he probabiliy of wheher or no here is an overlap beween he parial map and he oher robos pah. This quaniy is crucial o assess he weigh p(c pq ) of a map merge hypohesis used by he coordinaion algorihm in Eq. 3. In order o deermine n, i is necessary o esimae he likelihood of sensor measuremens ouside he parial map (Seps 12 and 13). Unforunaely, an accurae esimae of his likelihood is no possible, since he robos do no know which measuremens o expec ouside he explored area. One soluion o his problem is o use a fixed likelihood for all observaions z made a locaions ouside he parial map. However, such an approach ignores valuable informaion abou he srucure of an environmen and resuls in brile esimaes of map overlaps [30]. To acquire p(z ouside), he likelihood of observing z in unexplored areas, we developed a srucural model of indoor environmens ha can be used o predic he observaions made by a robo. An in-deph discussion of his approach is beyond he scope of his paper, we refer he reader o [30], [10] for more deails. In a nushell, he srucural model is a hidden Markov model ha generaes sequences of views observed by a robo when navigaing hrough an environmen. In our approach, we exrac discree views v from laser rangescans. These views roughly correspond o map paches such as hallways, openings, rooms, ec. A every updae of he paricle filer, he nex view is prediced based on he previous view and he view ransiion probabiliy. Boh view and ransiion probabiliies are esimaed during exploraion via so-called Dirichle hyper-parameers. More specifically, le v denoe he random variable over views observed a ime. I can be shown [10] ha he predicive disribuion for observing view i a ime given ha he robo jus observed view j is given by p(v =i v 1 =j, α j, f j ) = f i j + α ij i f i j + α i j. (4) Here, f j is he number of imes view j has already been observed in his environmen, and α j is a Dirichle prior coun learned from previous environmens. Accordingly, f i j and α ij are he view ransiions observed in his environmen and given as prior couns, respecively. The Dirichle prior couns are learned using a hierarchical Bayesian approach. A each ieraion of he paricle filer, Eq. 4 is used o esimae p(z ouside) in Seps 12 and 13 of he algorihm. In [30], [10] we showed ha his predicive model resuls in significanly beer esimaes of wheher or no he maps of wo robos overlap. By updaing he probabiliies of he HMM as he robos explore an environmen, our model achieves an addiional improvemen in predicive qualiy [10]. A each ieraion of he paricle filer, hypoheses for he locaion of a robo are exraced from he sample se and hen used by he decision-heoreic coordinaion echnique described in Secion II-B. Once he coordinaion approach considers a hypohesis valuable enough, i verifies his hypohesis by assigning i o a robo. If his robo deecs he oher robo a he hypohesized posiion, is daa can be merged ino he cluser map, as described nex. If, however, he hypohesis urns ou o be incorrec, hen he paricle filer naurally incorporaes such informaion by giving he samples a he wrongly hypohesized locaion exremely low weighs. The low weighs resul in he removal of hese paricles in he nex re-sampling sep, hereby increasing he probabiliy of alernaive hypoheses. IV. MULTI-ROBOT MAP MERGING We will now describe how o build a consisen map from daa colleced by muliple robos. A. SLAM Paradigms and Local Maps The key problem in mobile robo mapping is caused by he uncerainy in a robo s posiion as i explores an environmen. This posiion uncerainy has o be considered when generaing a map from he observaions made by he robo. I is his connecion beween robo posiion and map uncerainy ha makes he SLAM (simulaneous localizaion and mapping) problem compuaionally demanding [4], [33]. Over he las years, various research groups have developed efficien soluions o he SLAM problem. These echniques range from spliing maps ino sub-maps [27], o hin juncionree approximaions [28], o sparse exended informaion filers [33], o Rao-Blackwellised paricle filers [25], [23], [13], [5], o graph srucures modeling spaial consrains [22], [12], [18]. In his projec, we build on he laer class of echniques, which are appropriae because hey can be made o be independen of he coordinae sysem in which he

6 6 consrains are expressed [19], an obvious advanage when combining local maps ha have differen coordinae sysems. Here, we only provide an inuiive descripion of our approach, more deails can be found in [22], [12], [19]; a good exposiion of general consrain graphs can be found in he Graph-SLAM algorihm [34]). B. Local Consrains and Opimizaion The key o combining informaion from muliple local maps is o form probabilisic consrains ha are invarian o rigid ransformaions. Such consrains can be combined direcly, because hey are no ied o any paricular local map coordinae sysem. Consrains are generaed from four sources: 1) Odomery beween successive poses. 2) Scan-maching beween nearby poses. 3) Loop closure, when scans from wo hisorically disan poses are mached. 4) Colocaions beween poses in parial maps. Formally, consrains in our sysem are measuremen equaions beween pairs of poses. For wo poses p 0 and p 1, he equaion is he difference beween he wo poses f(p 1, p 0 ) = p 1 p 0. The measured disance is given by a difference d 01 and is covariance C 01. For example, using odomery he difference comes from he measured wheel movemen, and he covariance from a model of he moion errors. The oher source of consrains comes from scan maching beween poses (cases 2-4). As in he case of odomery, he oupu of he scan mach is an esimaed difference beween he poses, and a covariance of he esimae. Thus all of he consrains we are working wih can be pu ino he form of measuremens of he difference beween wo poses. Unforunaely, consrains ha are pure pose differences are no local hey can vary depending on he global locaion of he poses. Consider he wo poses locaed a p 0 = (0, 0, 0) and p 1 = (0, 1, 0). Their difference is d 01 = (0, 1, 0). Now roae he coordinae sysem 90 degrees, so ha p 0 = (0, 0, pi/2) and p 1 = (1, 0, pi/2). Obviously heir difference d 01 in he new coordinae sysem will change. When we pu ogeher consrains from differen parial maps, each of which has is own coordinae sysem, he consrains are no comparable. Our soluion is o always express consrains in a form which is invarian o a rigid ransformaion of he pose coordinaes. Insead of using pose differences in he global sysem, we express he difference in he coordinae sysem of p 0, ha is, as if p 0 were a (0, 0, 0). We wrie 0 (p 1 p 0 ) for his relaive pose difference; i is easy o verify ha i is invarian o any rigid ransformaion of he global coordinaes. Given a se of local pose consrains, he maximum likelihood soluion is found by minimizing he covariance-based errors. For each consrain k, define: ɛ k = d ij i (p j p i ) (5) The oal covariance-based squared error (also called he squared Mahalanobis disance) is given by E = k ɛ T k C 1 k ɛ k (6) Sar posiion Curren posiion Fig. 3. Pose consrains before (lef) and afer (righ) linking he sar and end of a loop. Minimizaion of he consrains afer he robo reurned ino he hallway o he righ resuls in consisen scan locaions. Robo rajecory is shown in gray, spaial consrains as hin black lines aached o he rajecory. Any paricular se of values for he pose variables will yield a value for E. Finding he values ha minimize E is a non-linear opimizaion problem (he consrains are nonlinear because of he angular dependencies). Given an iniial soluion ha is close enough o he opimal soluion, here are efficien mehods for solving his problem, mos noably conjugae gradien descen [22], [12], [18], [34]. In pracice hese mehods work very well, and can solve sysems of (for example) 1,000 poses in under a second. C. Map Merging Examples The consrain graph is ideal for inegraing map informaion wih uncerain alignmen. In he case of odomery and local scan maches, he sysem looks a jus a small local neighborhood o enforce consisency [12], which can be done in consan ime. The more ineresing cases are enforcing global consisency: loop closure in a local map, and parial map merging. In loop closure, a robo is building a local map using is own scans and he scans of any co-locaed robos. A some poin, he robo reurns o a posiion i has previously visied, bu accumulaed error causes i o be misaligned (Figure 3 lef). Here he robo has raversed an inerruped loop, going ou of he op of he figure before coming back. Once scan maching esablishes links wih poses a he beginning of he loop, addiional consrains can be added o he graph. Based on hese consrains, he mapping algorihm deermines he opimal posiion for all scan locaions by maximizing he poserior probabiliy of all consrains in he graph, using Eq. 6. In pracice, he iniial soluion esablished by enforcing local consrains gives a close enough soluion o sar he minimizaion process. In he righ side of Fig. 3, scan maching has esablished links wih poses a he beginning of he loop, resuling in a consisen map afer minimizaion of he consrain sysem. Because he opimizaion is efficien, i can be performed online as he robo explores an environmen, causing no more han a second or so of hesiaion as consisency is enforced. The consrain represenaion naurally faciliaes he merging of parial maps buil by differen robos. For example, he upper panels of Fig. 4 show hree parial maps buil by hree robos. Suppose we can link he pose marked o in he lef map o he pose marked o in he middle map, and he poses marked x in he middle and he righ maps. Then, we can move he hree maps ogeher o regiser hem in he same

7 7 Fig. 4. Upper panels: Parial maps buil by hree robos in he UW Allen Cener. The o s and x s provide connecion poins beween he lef and he middle map, and he middle and he righ map, respecively. Lower panels: The lef picure shows he map generaed from he hree parial maps by opimizaion of he global consrain graph generaed by zippering he maps ogeher a he connecion poins. (righ) Map generaed by simply overlaying he parial maps, wihou any addiional global opimizaion (only laser scans are shown for clariy). meric space. This is done by aking each consrain in he middle map and adding i o he consrain graph underlying he lef map, jus as if all scans were colleced by a single robo. In addiion, we generae an iniial soluion as inpu o he global opimizaion, by ransforming all he poses in he middle map, and making a rigid ransformaion so ha hey line up wih he colocaed pose a is correc posiion. A his poin, alhough he maps are aligned correcly for he colocaed poses, hey can differ on poses ha are disan from his poin see he lower righ panel in Fig. 4. An addiional zippering process is performed, in which all he poses ha are now close in he colocaed wo parial maps are scanmached for addiional consrains. By consolidaing he poses ino spaial buckes, his process can ake place in order N, he number of poses in he parial map. Opimizaion of Eq. 6 yields a globally consisen map. Finally, he scans observed by he hird robo are added o his map, using he same process. The occupancy grid map resuling from opimizing he global consrain graph is shown in he lower lef panel in Fig. 4. Absracly, he zippering process les us ake any parial maps produced by any robos and pu hem ogeher, once a common locaion (colocaion) beween heir rajecories has been idenified. In our sysem, colocaion informaion is esimaed by he paricle filer described in Secion III, and verified via acively riggered robo deecions, as described in Secion II. Our map merging echnique is ransiive in he sense ha if robo A knows robo B s locaion inside is parial map and robo B knows he locaion of robo C inside is parial map, hen i is possible o consisenly merge C s map ino robo A s map (possibly afer merging B s map ino A s map). The reader may noice ha merging maps in differen orders migh lead o slighly differen maps, which is due o he approximaions performed by our approach (sequenially adding spaial consrains migh resul in differen consrain sysems). In pracice, however, we found his approach o map merging highly reliable. V. EXPERIMENTAL EVALUATION We will now describe he evaluaion of our exploraion sysem. Addiional aspecs of he approach are evaluaed in [10], [16], [18], [30]. A. Implemenaion Deails 1) Coordinaion and Mapping: We implemened he decision-heoreic coordinaion echnique described in Secion II-B. Maps are represened compacly as ses of laser range-scans annoaed wih robo poses and probabilisic links (scans are recorded only every 50 cm of ranslaion or 30

8 8 degrees of roaion). Wihin an exploraion cluser, each robo inegraes is observaions ino is own map, and broadcass he informaion o he oher robos. While mos of he oher robos only sore his daa, he eam leader of he cluser inegraes all he sensor informaion i receives. Thus he eam leader, which is chosen as he robo wih he smalles ID, has a complee and consisen map represening he daa colleced by all robos in he cluser. This map is used o coordinae he robos in he cluser. Whenever wo clusers mee and merge heir maps, he eam leader wih he smaller ID becomes he leader of he new exploraion cluser. Frequenly, he eam leader broadcass he map o he oher robos, in order o guaranee consisency. This daa can be sen very compacly, since only updaed robo poses and links have o be ransmied (scans are already sored by he oher robos). The mos complex broadcas follows whenever a robo closes a loop, since he opimizaion of he consrain sysem modifies all robo poses in a map (Secion IV). In pracice, broadcasing even his informaion ypically involves sending only several kilobyes of daa, which is well below he capaciy of ypical wireless communicaion capaciy and can be done in a fracion of a second. A crucial siuaion occurs when a robo moves ino he communicaion range of a robo from anoher, possible singlerobo, cluser. A his poin in ime, he robos exchange all heir sensor and moion informaion and sar esimaing heir relaive locaions using he paricle filer discussed in Secion III. Our curren sysem allocaes his ask o he eam leader. The oher robos in he eam do no generae hypoheses. In he wors case, if all robos are in singlerobo clusers and wihin communicaion range, he number of paricle filers run on each robo can be as high as he oal number of robos minus one. In simulaion experimens, we found his simple approach o work efficienly enough for up o six robos. However, i does no scale o very large eams of robos and more hough mus be pu ino more inelligen allocaion of compuaion asks. 2) Dealing wih Limied Communicaion: Our exploraion sysem achieves robusness o communicaion loss by enabling every robo o explore he environmen on is own. Whenever a robo in an exploraion cluser reaches an assigned goal poin, i keeps on exploring based on is own map unil i receives a new goal poin. Thus, if a robo moves ouside he communicaion range of is cluser, i auomaically keeps on building is own map unil i ges back ino communicaion range. Afer geing back ino communicaion, robos exchange all he relevan daa ha was los. Such a sync operaion only involves he communicaion of raher small daa ses and can ypically be done in less han a second. Our approach is also robus o loss of he eam leader, since any oher robo in he cluser can explore on is own or ake over he eam leader role. In he exreme, if none of he robos can communicae wih each oher, each robo will explore he environmen independenly of he oher robos. The resul will sill be a complee map; only buil less efficienly. We added hand shaking and various imeous o he decision making in order o make acive hypohesis verificaion robus o loss of communicaion. This implemenaion ask urned ou o be raher edious since i required exensive esing of he sysem in order o deermine all siuaions in which one robo migh leave he communicaion range of anoher robo. As an example of our approach, when a robo sends a Mee signal o anoher robo for which i has a good locaion hypohesis, i wais for an acknowledgmen of his signal. If he acknowledgmen is no received afer several seconds, he robo keeps on exploring and reconsiders a meeing only afer an addiional imeou and he oher robo is back in communicaion. B. Predicive Model for Esimaing Relaive Locaions As described in Secion III, our sysem relies on paricle filers o esimae he relaive posiions of robos. In order o esimae wheher or no he maps of wo robos overlap, we compare he likelihood of measuremens z inside a parial map wih he likelihood of observing z ouside he map, denoed p(z ouside). To esimae he ouside likelihood, we developed a hierarchical Bayesian echnique ha learns priors from previously explored environmens (see Secion III-B). To evaluae he suiabiliy of his approach for parial map merging, we generaed 15 parial maps from 5 differen environmens and esimaed he locaion of a robo relaive o hese maps using he approach described in Secion III. For each parial map, we ook several sensor logs colleced in he same environmen. The sensor logs were chosen randomly and some of hem had no overlap wih he corresponding parial map a all. For each map-rajecory pair we proceeded as follows. A each ieraion of he paricle filer, we deermined he mos likely hypohesis for he robo s locaion. If he probabiliy of his hypohesis exceeded a cerain hreshold θ hen his hypohesis was considered valid. For each hreshold θ, precision measures he fracion of correc valid hypoheses, i.e. hypoheses above he hreshold. Correcness is esed by comparing he posiion of he hypohesis o a ground ruh esimae compued offline. To deermine recall, we firs checked a wha imes he robo was in he parial map. Recall, hen, measures he fracion of his ime for which he approach generaed a correc hypohesis, i.e. a he correc posiion and wih probabiliy above he hreshold θ. We compared our approach o an alernaive mehod ha uses a fixed likelihood p(z ouside) for locaions ouside he parial map (his corresponds o virually all exising mapping echniques). The precision-recall rade-offs for differen hresholds θ are shown in Fig. 5(a) (each poin represens a differen hreshold). The solid line represens he resuls obained wih our approach and he dashed lines are resuls for he fixed approach using differen likelihoods p(z ouside) for measuremens ouside he maps (daa poins are omied for clariy). The graphs clearly show he superior performance of our approach. I achieves 26% higher precision han he bes likelihood value for he alernaive mehod. Noe ha high precision values are more imporan han high recalls since low precision resuls in wrong acive colocaion decisions, while low recall only delays he map merging process. Noe also ha one canno expec very high recall values since a robo has o be in he parial map for a cerain duraion before a valid hypohesis can be generaed.

9 9 Precision Fixed p(z ou) Hierarchical Recall (a) Cumulaive log likelihood Hierarchical Bayes Hierarchical prior Trans. frequencies View frequencies Disance raveled [m] Fig. 5. (a) Precision vs. recall: Each poin represens an average over 375 pairs of parial maps and rajecories. Each curve shows he rade-off for differen hresholds θ ( ). The dashed lines indicae resuls obained wih differen fixed values for p(z ouside) and he solid line represens he resuls for our approach. (b) Predicive likelihood of differen approaches averaged over 45 daa sequences in hree environmens. (b) To evaluae he predicive qualiy of our approach, we used sequences of daa colleced in hree environmens. A each ieraion (afer approximaely 2m of robo moion), we compued he likelihood of he nex view in he daa log given he view predicion obained by our approach. The predicive qualiy is hen deermined by accumulaing he logarihm of he measuremen likelihoods over ime. Fig. 5(b) shows he resuls for alernaive echniques, averaged over 45 daa sequences. The solid line represens he resuls for our approach, i.e. using Eq. 4 o predic he nex view. The dashed line gives he resuls using our approach, bu wihou updaing he model, i.e. only he Dirichle prior couns learned from oher maps are used (f i j and f i j are se o zero in Eq. 4). Even hough he (logarihmic) difference beween hese op wo graphs seems small, he average likelihood of a complee sensor sequence using our adapive approach is approximaely 360 imes as high as wih he prior only approach. This indicaes ha i is imporan o updae he predicive model using observaions obained in he new environmen. The doed line in Fig. 5(b) shows he resul if we predic views using he frequency couns of view ransiions observed in he oher maps. These predicions are clearly inferior o hose of he Dirichle prior learned wih he hierarchical Bayesian approach (dashed line), which shows ha our learning mehod significanly improves he performance over sraighforward ransiion frequency couning. Finally, he dashed-doed line gives he resul based on frequency couns of individual views, i.e. wihou considering ransiions beween views. This graph demonsraes ha considering he conneciviy of environmens is superior o predicing views simply based on heir frequency. These graphs are averages over differen environmens. We found our approach o yield much sronger improvemens in predicable environmens such as ypical office buildings. C. For A.P. Hill Evaluaion Our overall exploraion and mapping sysem was evaluaed horoughly as par of he CeniBos eam wihin he DARPA SDR projec. The SDR projec was unique in having an experimenal validaion conduced by an ouside group. For a week in January 2004, he CeniBos were esed a a 650m 2 building in F. A.P. Hill, Virginia. We were esed under conrolled condiions, wih a single operaor in charge of he robo eams. All compuaion was performed using sae-ofhe-ar lapops onboard he robos. The evaluaion crieria for mapping included ime o creae a map, opological accuracy, and percen of area mapped. Ground ruh for mapping was given by a manually consruced map (Fig. 6(d)). Exensive sofware uning was circumvened by limiing access o only half of he experimenal area during es runs. Even he developer eam was no allowed o inspec he complee environmen before he robos were deployed. Run # Mapping robos Mapping Time Map area min 96% min 97% min 95% min 96% min 97% TABLE II EXPLORATION RUNS DURING THE FT. A.P. HILL EVALUATION. The resuls for five official mapping runs are summarized in Table II. In all runs, he robos were able o auonomously generae a highly accurae map of he environmen. The average mapping ime for single robo exploraion was 24 minues; his ime was reduced o 18 minues when using wo and 15 minues when using hree robos. I should be noed ha he robos frequenly los communicaion wih he overall conrol cener and wih oher robos. In such siuaions, he robos explored on heir own and combined heir sensor daa as soon as hey were in conac again. In addiion o he robusness of our sysem, an imporan resul of his evaluaion was he fac ha he maps buil by our approach were more accurae han hose buil manually by he evaluaion eam. Fig. 6(d) shows one of our maps overlayed wih he ground ruh map. As can be seen, he wo maps do no mach perfecly (see for insance he wo ables in he upper middle room). Three maps buil by our robos in hree differen evaluaion runs are shown in Fig. 6(a)- (c). These maps look virually idenical, even hough hey were buil independenly of each oher, using differen rajecories of he robos. To beer illusrae he similariy of hese hree maps, we overlayed hem on op of each oher. Fig. 6(e) shows he pixels a which he overlayed occupancy grid maps are no idenical. As can be seen, he maps almos perfecly line up. Mismaches are only along he obsacles, which is mosly due o limied resoluion of he maps.

10 10 (a) (b) (c) Table posiions Furniure posiion Furniure posiion (d) (e) Fig. 6. (a)-(c) Maps buil during hree auonomous exploraion runs. The maps look almos idenical, even hough hey were buil under very differen circumsances. The similariy beween he maps illusraes he robusness of he sysem and suppors our belief ha hese maps are more accurae han he hand-buil map. (d) Map overlayed wih he ground ruh CAD model of he building. The CAD model was generaed by manually measuring he locaions and exensions of rooms and objecs. (e) Map generaed from overlaying he hree maps shown in (a)-(c). Whie pixels indicae locaions a which all hree maps agree, black pixels show disagreemen on occupancy. D. UW Allen Cener Evaluaions We performed addiional evaluaion runs wih hree robos in he UW Allen Cener. These runs confirmed he reliabiliy of our sysem. Fig. 8 illusraes one of hese runs. An animaion of his run can be found a hp:// In order o evaluae he benefis of acive colocaion, we performed several simulaion runs involving hree robos. To do so, we used he Saphira robo simulaor along wih a map of he firs floor of he Allen Cener (see Fig. 7). The Saphira simulaor accuraely models robos including noise in moion and sensing. A larger environmen was simulaed by limiing he velociy of robos o 20cm/sec and he range of deecions o 1.5m. Fig. 7. Map used for simulaion experimens. Approach Mapping ime Firs meeing Second Meeing [min] [min] [min] Passive 35.2± ± ±7.9 Acive 26.3± ± ±3.8 TABLE III EXPLORATION WITH AND WITHOUT ACTIVE COLOCATION. Table III summarizes he resuls of 12 exploraion runs, six of which were performed using our acive colocaion approach (numbers are mean imes along wih 95% confidence inervals). The oher six runs merged maps only when robos me coincidenally, similar o he approach discussed in [14]. Our map merging echnique generaed accurae, consisen maps in all 12 runs. As can be seen, acively verifying relaive locaion hypoheses significanly reduces he overall exploraion ime. The hird and fourh column of he able indicae why our acive approach is more efficien han is passive counerpar. The hird column provides he ime unil he firs wo robos are able o merge heir maps, and he fourh column gives he ime unil he hird robo joins he exploraion cluser. As can be seen, by acively verifying hypoheses, he robos are able o merge heir maps earlier, which resuls in improved coordinaion beween heir exploraion sraegies. We performed furher simulaion runs in his environmen using six robos, which resuled in faser exploraion of 22.8±4.5 minues. All hese runs resuled in globally consisen maps. Furhermore, hese runs involved acive colocaion beween exploraion clusers of more han one robo each.

11 11 R2 R1 R3 R1 verifies hypohesis for R2 s locaion (a) R2 R1 R1 and R2 me and heir maps are merged R3 (b) R1 R2 and R3 mee coincidenally R2 R3 (c) R1 R3 s daa is merged ino he map R2 R3 (d) R1 closes loop R3 R1 R2 (e) Afer loop closure Fig. 8. Sequence of parial maps generaed during exploraion wih hree robos. Shown is only he map of robo R1 along wih he locaions of he oher wo robos. In his experimen, he robos do no know heir relaive sar locaions. (a) R2 is in R1 s map and R1 has a high probabiliy hypohesis for R2 s locaion. R1 sends R2 a Sop command and decides o verify his hypohesis. (b) Afer R1 mees R2 a he hypohesized locaion, he robos merge heir maps. They now coordinae heir exploraion. (c) R2 and R3 mee incidenally and (d) merge R3s daa ino he map. (e) R1 closes a loop. (f) Since all daa is inegraed ino a global consrain graph, R1 is able o correc he odomery error. The final map of his run is shown in he lower lef panel of Fig. 4. R3 R1 R2 (f) VI. CONCLUSIONS We presened a disribued approach o mobile robo mapping and exploraion. The sysem enables eams of robos o efficienly explore environmens from differen, unknown locaions. The robos iniially explore on heir own, unil hey can communicae wih oher robos. Once hey can exchange sensor informaion wih oher robos, hey esimae heir relaive locaions using an adaped paricle filer. In order o esimae wheher or no he parial maps of wo robos overlap, he filer incorporaes a hidden Markov model ha predics observaions ouside he explored area. The parameers of he model are learned from previously explored environmens using a hierarchical Bayesian approach. During exploraion, he robos updae heir predicive models based on observaions in he new environmen. Our experimens indicae ha his approach suppors map merging decisions significanly beer han alernaive echniques. The esimaion of relaive posiions is inegraed seamlessly ino a decision-heoreic muli-robo coordinaion sraegy. In order o overcome he risk of false-posiive map maches, he robos acively verify locaion hypoheses using a rendez-vous sraegy. If he robos mee a he meeing poin, hey know heir relaive locaions and can combine heir daa ino a shared map. Mapping and map merging uses a SLAM echnique ha models uncerainy by local probabilisic consrains beween he locaions of laser range-scans. Shared maps are used o coordinae he robos and o esimae he locaion of oher robos. Our mapping and exploraion sysem was evaluaed under some of he oughes real-world condiions ye imposed on a roboics projec. Prior o he evaluaion, he developer eam was allowed o es heir robos only in one half of he environmen. The oher half was no accessible during esing. During he evaluaion runs, he robos had o rely on heir own ad-hoc wireless nework o exchange informaion. The robos successfully explored he environmen in all four official evaluaion runs. All maps generaed during hese runs were virually idenical, indicaing he high accuracy and robusness of our sysem. ACKNOWLEDGMENTS We hank Doug Gage and DARPA for suppor under he Sofware for Disribued Roboics Program (Conrac #NBCHC020073). We would also like o hank he evaluaion eam, Erik Krokov and Douglas Hacke, for heir exraordinary effors in designing and running he demonsraion. Oher members of he CeniBos eam (C. Oriz, A. Agno, M. Eriksen, B. Morisse, R. Vincen) were exremely helpful in he deploymen and esing of our approach. Par of his research was funded by he NSF under CAREER gran number IIS