A Monte Carlo Algorithm for Multi-Robot Localization

Transcription

1 A Mote Carlo Algorithm for Multi-Robot Localizatio Dieter Fox, Wolfram Burgard, Haes Kruppa, Sebastia Thru March 1999 CMU-CS School of Computer Sciece Caregie Mello Uiversity Pittsburgh, PA Abstract This paper presets a statistical algorithm for collaborative mobile robot localizatio. Our approach uses a sample-based versio of Markov localizatio, capable of localizig mobile robots i a ay-time fashio. Whe teams of robots localize themselves i the same eviromet, probabilistic methods are employed to sychroize each robot s belief wheever oe robot detects aother. As a result, the robots localize themselves faster, maitai higher accuracy, ad high-cost sesors are amortized across multiple robot platforms. The paper also describes experimetal results obtaied usig two mobile robots, usig computer visio ad laser rage fidig for detectig each other ad estimatig each other s relative locatio. The results, obtaied i a idoor office eviromet, illustrate drastic improvemets i localizatio speed ad accuracy whe compared to covetioal sigle-robot localizatio. This research is sposored i part by NSF, DARPA via TACOM (cotract umber DAAE07-98-C-L032) ad Rome Labs (cotract umber F ), ad also by the EC (cotract umber ERBFMRX-CT ) uder the TMR programme. The views ad coclusios cotaied i this documet are those of the author ad should ot be iterpreted as ecessarily represetig official policies or edorsemets, either expressed or implied, of NSF, DARPA, TACOM, Rome Labs, the Uited States Govermet, or the EC.

2 Keywords: mobile robots, localizatio, Markov localizatio, robotic visio, multi-robot cooperatio, Mote Carlo methods

3 A Mote Carlo Algorithm for Multi-Robot Localizatio 1 1 Itroductio Sesor-based robot localizatio has bee recogized as of the fudametal problems i mobile robotics. The localizatio problem is frequetly divided ito two subproblems: Positio trackig, which seeks to idetify ad compesate small dead reckoig errors uder the assumptio that the iitial positio is kow, ad global self-localizatio, which addresses the problem of localizatio with o a priori iformatio. The latter problem is geerally regarded as the more difficult oe, ad recetly several approaches have provided soud solutios to this problem. I recet years, a flurry of publicatios o localizatio which icludes a book solely dedicated to this problem [6] documet the importace of the problem. Accordig to Cox [15], Usig sesory iformatio to locate the robot i its eviromet is the most fudametal problem to providig a mobile robot with autoomous capabilities. However, virtually all existig work addresses localizatio of a sigle robot oly. The problem of cooperative multi-robot localizatio remais virtually uexplored. At first glace, oe could solve the problem of localizig N robots by localizig each robot idepedetly, which is a valid approach that might yield reasoable results i may eviromets. However, if robots ca detect each other, there is the opportuity to do better. Whe a robot determies the locatio of aother robot relative to its ow, both robots ca refie their iteral believes based o the other robot s estimate, hece improve their localizatio accuracy. The ability to exchage iformatio durig localizatio is particularly attractive i the cotext of global localizatio, where each sight of aother robot ca reduce the ucertaity i the locatio estimated dramatically. The importace of exchagig iformatio durig localizatio is particularly strikig for heterogeeous robot teams. Cosider, for example, a robot team where some robots are equipped with expesive, high accuracy sesors (such as laser rage fiders), whereas others are oly equipped with low-cost sesors such as ultrasoic rage fiders. By trasferrig iformatio across multiple robots, high-accuracy sesor iformatio ca be leveraged. Thus, collaborative multi-robot localizatio facilitates the amortizatio of high-ed high-accuracy sesors across teams of robots. Thus, phrasig the problem of localizatio as a collaborative oe offers the opportuity of improved performace from less data. This paper proposes a efficiet probabilistic approach for collaborative multi-robot localizatio. Our approach is based o Markov localizatio [53, 64, 37, 9], a family of probabilistic approaches that have recetly bee applied with great practical success to sigle-robot localizatio [7, 70, 19, 29]. I cotrast to previous research, which relied o grid-based or coarse-graied topological represetatios, our approach adopts a samplig-based represetatio [17, 23], which is capable of approximatig a wide rage of belief fuctios i real-time. To trasfer iformatio across differet robotic platforms, probabilistic detectio models are employed to model

4 2 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru the robots abilities to recogize each other. Whe oe robot detects aother, detectio models are used to sychroize the idividual robots believes, thereby reducig the ucertaity of both robots durig localizatio. To accommodate the oise ad ambiguity arisig i real-world domais, detectiomodels are probabilistic, capturig the reliabilityad accuracy of robot detectio. The costrait propagatio is implemeted usig samplig, ad desity trees [42, 51, 54, 55] are employed to itegrate iformatio from other robots ito a robot s belief. While our approach is applicable to ay sesor capable of (occasioally) detectig other robots, we preset a implemetatio that uses color cameras for robot detectio. Color images are cotiuously filtered, segmeted, ad aalyzed, to detect other robots. To obtai accurate probabilistic models of the detectio process, a statistical learig techique is employed to lear the parameters of this model usig the maximum likelihood estimator. Extesive experimetal results, carried out usig data collected i two idoor eviromets, illustrate the appropriateess of the approach. I what follows, we will first describe the Mote Carlo Localizatio algorithm for sigle robots. Sectio 2 itroduces the ecessary statistical mechaisms for multi-robot localizatio, followed by a descriptio of our samplig-based ad Mote Carlo localizatio techique i Sectio 3. I Sectio 4 we preset our visio-based method to detect other robots. Experimetal results are reported i Sectio 5. Fially, related work is discussed i Sectio 6, followed by a discussio of the advatages ad limitatios of the curret approach. 2 Multi-Robot Localizatio Let us begi with a mathematical derivatio of our approach to multi-robot localizatio. Throughout the derivatio, it is assumed that robots are give a model of the eviromet (e.g., a map [69]), ad that they are give sesors that eable them to relate their ow positio to this model (e.g., rage fiders, cameras). We also assume that robots ca detect each other, ad that they ca perform dead-reckoig. All of these seses are typically cofouded by oise. Further below, we will make the assumptio that the eviromet is Markov (i.e., the robots positios are the oly measurable state), ad we will also make some additioal assumptios ecessary for factorial represetatios of joit probability distributios as explaied further below. Throughout this paper, we adopt a probabilistic approach to localizatio. Probabilistic methods have bee applied with remarkable success to sigle-robot localizatio [53, 64, 37, 9, 25, 8], where they have bee demostrated to solve problems like global localizatio ad localizatio i dese crowds.

5 A Mote Carlo Algorithm for Multi-Robot Localizatio Data Let N be the umber of robots, ad let d deote the data gathered by the -th robot, with 1 ç ç N. Obviously, each d is a sequece of three differet types of iformatio: 1. Odometry measuremets. Each cotiuously moitors its wheel ecoders (dead-reckoig) ad geerates, i regular itervals, odometric measuremets. These measuremets, which will be deoted a, specify the relative chage of positio accordig to the wheel ecoders. 2. Eviromet measuremets. The robots also queries their sesors (e.g., rage fiders, cameras) i regular time itervals, which geerates measuremets deoted by o. The measuremets o establish the ecessary referece betwee the robot s local coordiate frame ad the eviromet s frame of referece. I our experimets below, o will be laser rage scas. 3. Detectios. Additioally, each robot queries its sesors for the presece or absece of other robots. The resultig measuremets will be deoted r. Robot detectio might be accomplished through differet sesors tha eviromet measuremets. Below, i our experimets, we will use a combiatio of visual sesors (color camera) ad rage fiders for robot detectio. The data of all robots is deoted d with d = d1 ë d2 ë :::ëd N : (1) 2.2 Markov Localizatio Before turig to the topic of this paper collaborative multi-robot localizatio let us first review a commo approach to sigle-robot localizatio, which our approach is built upo: Markov localizatio. Markov localizatio uses oly dead reckoig measuremets a ad eviromet measuremets o; it igores detectios r. I the absece of detectios (or similar iformatio that ties the positio of oe robot to aother), iformatio gathered at differet platforms caot be itegrated. Hece, the best oe ca do is to localize each robot idividually, idepedetly of all others. The key idea of Markov localizatio is that each robot maitais a belief over its positio. Thebeliefofthe-th robot at time t will be deoted Bel ètè èçè. Hereç deotes a robot positio (we will use the terms positio, pose ad locatio iterchageably), which is typically a threedimesioal value composed of a robot s x-y positio ad its headig directio ç. Iitially, at time t =0, Bel è0è èçè reflects the iitial kowledge of the robot. I the most geeral case, which is beig cosidered i the experimets below, the iitial positio of all robots is ukow, hece Bel è0è èçè is iitialized by a uiform distributio.

6 4 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru At time t, the belief Bel ètè èçè is the posterior with respect to all data collected up to time t: Bel ètè èçè = Pèç ètè j d ètè è (2) where ç ètè deotes the positio of the -th robot at time t, add ètè deotes the data collected by the -the robot up to time t. By assumptio, the most recet sesor measuremet i d ètè is either a odometry or a eviromet measuremet. Both cases are treated differetly, so let s cosider the former first: 1. Sesig the eviromet: Suppose the last item i d ètè is a eviromet measuremet, deoted o ètè. Usig the Markov assumptio (ad exploitig that the robot positio does ot chage whe the eviromet is sesed), we obtai Bel ètè èçè = Pèç ètè j d ètè = Pèoètè = Pèoètè = æpèo ètè = æpèo ètè = æpèo ètè è j ç ètè ;d èt,1è è Pèç ètè Pèo ètè j d èt,1è j ç ètè è Pèç ètè Pèo ètè j d èt,1è è jçètè èpèç ètè è j d èt,1è jç ètè èpèç èt,1è j d èt,1è è è è jdèt,1è j d èt,1è è jç ètè èbel èt,1è èçè (3) where æ is a ormalizer that does ot deped o ç ètè. Notice that the posterior belief Bel ètè after icorporatig o ètè is obtaied by multiplyig the perceptual model Pèo ètè the prior belief. This observatio suggest the icremetal update equatio: èçè j ç ètè è with Bel èç è è, Pèo ètè j ç ètè è Bel èç è (4) The coditioal probability Pèo j ç è is called the eviromet perceptio model of robot. I Markov localizatio, it is assumed to be give. The probability Pèo j ç è ca be approximated by Pèo j o ç è, which is the probability of observig o coditioed o the expected measuremet o ç at locatio ç. The expected measuremet is easily computed usig ray tracig. Figure 1 shows this perceptio model for laser rage fiders. Here the x-axis is the distace o ç expected give the world model, ad the y-axis is the distace o measured by the sesor. The fuctio is a mixture of a Gaussia desity ad a geometric distributio. It itegrates the accuracy of the sesor with the likelihood of receivig a radom measuremet (e.g., due to obstacles ot modeled i the map [22]).

7 A Mote Carlo Algorithm for Multi-Robot Localizatio 5 probability expected distace [cm] measured distace [cm] 200 Fig. 1: Perceptio model for laser rage fiders. The x-axis depicts the expected measuremet, the y-axis the measured distace, ad the vertical axis depicts the likelihood. The peak marks the most likely measuremet. The robots are also give a map of the eviromet, to which this model is applied. 2. Odometry: Now suppose the last item i d ètè is a odometry measuremet, deoted a ètè. Usig the Theorem of Total Probability ad exploitig the Markov property, we obtai Bel ètè èçè = Pèç ètè j d ètè = Z Pèç ètè = Z Pèç ètè è j d ètè ;çèt,1è è Pèç èt,1è j a ètè ;çèt,1è è Pèç èt,1è j d ètè è dç èt,1è j d èt,1è è dç èt,1è (5) which suggests the icremetal update equatio: Z Belèç è è, Pèç j a ètè ;ç0 èbelèçè 0 dç 0 (6) Here Pèç j a; ç 0 è is called the motio model of robot. Figure 2 shows a example for the mobile robots used i our experimets. The straight lie represets the trajectory of the robot, which moved straight from left to right. I the begiig Bel è0è èçè was iitialized by a Dirac-Distributio. After 30 meters the robot is highly ucertai about its locatio which is represeted by the baaa-shaped distributio Bel ètè èçè. As the figure suggests, a motio model is basically a model of robot kiematics aotated with ucertaity. These equatios together form the basis of Markov localizatio, a icremetal probabilistic algorithm for estimatig robot positios. The Markov localizatio algorithm cosists of the followig steps:

8 6 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru Fig. 2: Motio model represetig the ucertaity i robot motio. Step 1. Iitialize Bel èçè by a uiform distributio. Step 2.1. For each eviromet measuremet o do Bel èç è è, Pèo j ç è Bel èç è: (7) Step 2.2. For each odometry measuremet a do Z Belèç è è, Pèç j a ;çèbelèç 0 è 0 dç 0 (8) Thus, Markov localizatio relies o kowledge of Pèo j çè ad Pèç j a; ç 0 è, The former coditioal typically requires a model (map) of the eviromet. As oticed above, Markov localizatio has bee applied with great practical success to mobile robot localizatio. However, it is oly applicable to sigle-robot localizatio, ad caot take advatage of robot detectio measuremets. Thus, i its curret form it caot exploit relative iformatio betwee differet robots positios i ay sesible way. 2.3 Multi-Robot Markov Localizatio The key idea of multi-robot localizatio is to itegrate measuremets take at differet platforms, so that each robot ca beefit from data gathered by robots other tha itself. At first glace, oe might be tempted to maitai a sigle belief over all robots locatios, i.e., ç = fç1;:::;ç N g (9) Ufortuately, the dimesioality of this vector growths with the umber of robots: If each robot positio is described by three values (its x-y positio ad its headig directio ç), ç is of dimesio

9 A Mote Carlo Algorithm for Multi-Robot Localizatio 7 3N. Distributios over ç are, hece, expoetial i the umber of robots. Thus, modelig the joit distributio of the positios of all robots is ifeasible for larger values of N. Our approach maitais factorial represetatios; i.e., each robot maitais its ow belief fuctio that models oly its ow ucertaity, ad occasioally, e.g., whe a robot sees aother oe, iformatio from oe belief fuctio is trasfered from oe robot to aother. The factorial represetatio assumes that the distributio of ç is the product of its N margial distributios: Pèç1 ètè ;:::;çètè N jdètè è = Pèç1 ètè jd ètè èæ:::æpèç ètè N jdètè è (10) Strictly speakig, the factorial represetatio is oly approximate, as oe ca easily costruct situatios where the idepedece assumptio does ot hold true. However, the factorial represetatio has the advatage that the estimatio of the posteriors is coveietly carried out locally o each robot. I the absece of detectios, this amouts to performig Markov localizatio idepedetly for each robot. Detectios are used to provide additioal costraits betwee the estimated of pairs of robots, which will lead to refied local estimates. To derive how to itegrate detectios ito the robots beliefs, let us assume the last item i d ètè is a detectio variable, deoted r ètè. For the momet, let us assume this is the oly such detectio variable i d ètè, ad that it provides iformatio about the locatio of the m-th robot relative to robot (with m 6= ). The Bel ètè m = Pèç ètè m = Pèç ètè m = Pèç ètè m j d ètè è j d ètè m è Pèç ètè m j d ètè è j dètè m è Z Pèç ètè m j çètè ;rètè èpèç ètè jdèt,1è è dç èt,1è (11) which suggests icremetal update equatio: Belèç m è è, Belèç m è Z Pèç ètè m j ç ètè ;rètè èbelèç è dç (12) Of course, this is oly a approximatio, sice it makes certai idepedece assumptios (it excludes that a sesor reports I saw a robot, but I caot say which oe ), ad strictly speakig it is oly correct if there is oly a sigle r i the etire ru. However, this gets us aroud modelig the joit distributio Pèç1;:::;ç N j dè, which is computatioally ifeasible as argued above. Istead, each robot basically performs Markov localizatio with these additioal probabilisticcostrais, hece estimates the margial distributios Pèç jdè separately. The reader may otice that, by symmetry, the same detectio ca be used to costrai the -th robot s positio based o the belief of the m-the robot. The derivatio is omitted sice it is fully symmetrical.

10 8 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru 2.4 Additioal Cosideratios If the data set cotais more tha oe costrait r betwee two robots m ad, the situatio becomes more complicated. Basically, repeated itegratio of differet robots belief accordig to (11) ca lead to usig the same evidece twice; hece, robots ca get overly cofidet i their positio. I our approach, this effect is dimiished by a set of rules that basically reduce the dager arisig from the factorial distributio. 1. To dimiish these effects, our approach igored egative sights, i.e., evets where a robot does ot see aother robot. 2. It also icludes timer that, oce a robot has bee sighted, blocks the ability to see the same robot agai for a pre-specified duratio. I practice, these two restrictios are sufficiet to yield superior performace, as demostrated below. However, the reader should otice that they imply that detectio iformatio may ot be used. At the curret poit, we are ot aware of a approach that would utilize more iformatio yet maitai the highly coveiet factorial represetatios. 3 Samplig ad Mote Carlo Localizatio The previous sectio left ope how the belief is represeted. I geeral, the space of all robot positios is cotiuous-valued ad o parametric model is kow that would accurately model arbitrary beliefs i such robotic domais. However, practical cosideratios make it impossible to model arbitrary beliefs usig digital computers. The key idea here is to approximate belief fuctios usig a Mote Carlo method. More specifically, our approach is a extesio of Mote Carlo localizatio (MCL), which was recetly proposed i [17, 23]. MCL is a versio of Markov localizatio that relies o sample-based represetatio ad the samplig/importace re-samplig algorithm for belief propagatio [60]. MCL represets the posterior beliefs Bel èçè by a set of K weighted radom samples, or particles, deoted S = fs i j i =1::Kg. A sample set costitutes a discrete distributio. However, uder appropriate assumptios (which happe to be fulfilled i MCL), such distributios smoothly approximates the correct oe at a rate of p 1= K as K goes to ifiity [66]. A particularly elegat algorithm to accomplish this has recetly bee suggested idepedetly by various authors. It is kow alteratively as the bootstrap filter [27], the Mote-Carlo filter [40], the Codesatio algorithm [35, 36], or the survival of the fittest algorithm [38]. These methods are geerically kow as particle filters, or samplig/importace re-samplig [60], ad a overview ad discussio of their properties ca be foud i [18].

11 A Mote Carlo Algorithm for Multi-Robot Localizatio 9 Start locatio 10 meters Fig. 3: Samplig-based approximatio of the positio belief for a o-sesig robot. The solid lie displays the actios, ad the samples represet the robot s belief at differet poits i time. Samples i MCL are of the type hhx; y; çi;pi (13) where hx; y; çi deote a robot positio, ad p ç 0 is a umerical weightig factor, aalogous to a discrete probability. For cosistecy, we assume P K i=1 p i =1. I aalogy with the geeral Markov localizatio approach outlied i Sectio 2, MCL proceeds i two phases: 1. Robot motio. Whe a robot moves, MCL geerates K ew samples that approximate the robot s positio after the motio commad. Each sample is geerated by radomly drawig a sample from the previously computed sample set, with likelihood determied by their p- values. Let ç 0 deote the positio of this sample. The ew sample s ç is the geerated by geeratig a sigle, radom sample from Pèç j ç 0 ;aè, usig the actio a as observed. The p-value of the ew sample is K,1. Figure 3 shows the effect of this samplig techique for a sigle robot, startig at a iitial kow positio (bottom ceter) ad executig actios as idicated by the solid lie. As ca be see there easily, the sample sets approximate distributios with icreasig ucertaity, represetig the gradual loss of positio iformatio due to slippage ad drift. 2. Eviromet measuremets are icorporated by re-weightig the sample set, which is aalogous to Bayes rule i Markov localizatio. More specifically, let hç; pi (14) be a sample. The p è, æpèojçè (15)

12 10 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru where o is a sesor measuremet, ad æ is a ormalizatio costat that eforces P K i=1 p i = 1. The icorporatio of sesor readigs is typically performed i two phases, oe i which p is multiplied by Pèo j çè, ad oe i which the various p-values are ormalized. A algorithm to perform this re-samplig process efficietly i OèKè time is give i [12]. I practice, we have foud it useful to add a small umber of uiformly distributed, radom samples after each estimatio step [23]. Formally, these samples ca be uderstood as a modified motio model that allows, with very small likelihood, arbitrary jumps i the eviromet. The radom samples are eeded to overcome local miima: Sice MCL uses fiite sample sets, it may happe that o sample is geerated close to the correct robot positio. This may be the case whe the robot loses track of its positio. I such cases, MCL would be uable to re-localize the robot. By addig a small umber of radom samples, however, MCL ca effectively re-localize the robot, as documeted i our experimets described i [23] (see also the discussio o loss of diversity i [18]). 3.1 Properties of MCL A ice property of the MCL algorithm is that it ca uiversally approximate arbitrary probability distributios. As show i [66], the variace of the importace sampler coverges to zero at a rate of p 1= N (uder coditios that are true for MCL). Thus, at least theoretically MCL is superior to all previous localizatio approaches that the authors are aware of, i that it ca approximate a much larger class of distributios. The sample set size aturally trades off accuracy ad computatio. The true advatage, however, lies i the way MCL places computatioal resources. By samplig i proportio to likelihood, MCL focuses its computatioal resources o regios with high likelihood, where thigs really matter. MCL also leds itself icely to a ay-time implemetatio [16, 75]. Ay-time algorithms ca geerate a aswer at ay time; however, the quality of the solutio icreases over time. The samplig step i MCL ca be termiated at ay time. Thus, whe a sesor readig arrives, or a actio is executed, samplig is termiated ad the resultig sample set is used for the ext operatio. 3.2 Multi-Robot MCL The extesio of MCL to collaborative multi-robot localizatio is ot straightforward. This is because uder our factorial represetatio, each robot maitais each ow, local sample set. Whe oe robot detects aother, both sample sets are sychroized usig the detectio model, accordig

13 A Mote Carlo Algorithm for Multi-Robot Localizatio 11 (a) (b) Figure 4: (a) Sample set that correspods to a detectio r, ad (b) its approximatio usig a desity tree. The tree trasforms the discrete sample set ito a cotiuous distributio, which is ecessary to geerate ew importace factors for the idividual sample poits represetig each robot s belief. to the update equatio Belèç m è è, Belèç m è Z Pèç ètè m j ç ètè ;rètè èbelèç è dç (16) Notice that this equatio requires the multiplicatio of two desities. Sice samples i Belèç m è ad Belèç è are draw radomly, it is ot straightforward to establish correspodece betwee idividual samples i Belèç m è ad R Pèç ètè m j ç ètè ;r ètè è Belèç è dç. To remedy this problem, our approach trasforms sample sets ito desity fuctios usig desity trees [42, 51, 54, 55]. These methods approximate sample sets usig piecewise costat desity fuctios represeted by a tree. The resolutio of the tree is a fuctio of the desities of the samples: the more samples exist i a regio of space, the fier-graied the tree represetatio. Figure 4 shows a example sample set alog with the tree that represets this set. Our specific algorithm grows trees by recursively splittig i the ceter of each coordiate axis, termiatig the recursio whe the umber of samples is smaller tha a pre-defied costat. After the tree is grow, each leaf s desityis give by the quotietof the sum of all weights p of all samples that fall ito this leaf, divided by the volume of the regio covered by the leaf. The latter amouts to maximum likelihood estimatio of (piecewise) costat desity fuctios. To implemet the update equatio above, our approach approximates the desity Z Pèç ètè m j ç ètè ;rètè èbelèç è dç (17) usig samples, just as described above. The resultig sample set is the trasformed ito a desity tree. These desity values are the multiplied ito the weights (importace factors) of the samples i Belèç m è, effectively multiplyig both desity fuctios. The result is a refied desity for the m-th robot, reflectig the detectio ad the belief of the -th robot.

14 12 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru The same update rule is applied i the other directio, from robot equatios are completely symmetric, they are omitted here. 3.3 m to robot. Sice the Adaptive Samplig I practice, the best sample set sizes ca vary drastically [42]. Durig global localizatio, a robot may be completely igorat as to where it is; hece, it s belief uiformly covers its full threedimesioal state space. Durig positio trackig, o the other had, the ucertaity is typically small ad ofte focused o lower-dimesioal maifolds. For example, whe a robot kows its relative positio to a adjacet wall but does ot kow what hallway it is i, the belief is focused o a oe-dimesioal sub-maifold similar to a road-map. Thus, may more samples are eeded durig global localizatio to approximate the true desity with high accuracy, tha are eeded for positio trackig. MCL determies the sample set size o-the-fly. The idea is to use the divergece of P ( ) ad P ( j o ), the belief before ad after sesig, to determie the sample sets. More specifically, both motio data ad sesor data is icorporated i a sigle step, ad samplig is stopped wheever the o-ormalized sum of weights p (before ormalizatio!) exceeds a threshold. If the positio predicted by odometry is well i tue with the sesor readig, each idividual p is large ad the sample set remais small. If, however, the sesor readig carries a lot of surprise, as is typically the case whe the robot is globally ucertai or whe it lost track of its positio, the idividual p-values are small ad the sample set is large. MCL directly relates to the well-kow property that the variace of the importace sampler is a fuctio of the mismatch of the samplig distributio (i our case P ( )) ad the distributio that is beig approximated with the weighted sample (i our case P ( j o )). The less these distributios agree, the larger the variace (approximatio error). The idea is here to compesate such error by larger sample set sizes, to obtai approximately uiform error. Robot positio Robot positio Robot positio Fig. 5: Global localizatio: Fig. 6: Ambiguity due to Iitializatio. symmetry. Fig. 7: Achieved localizatio.

15 A Mote Carlo Algorithm for Multi-Robot Localizatio 13 Fig. 8: Traiig data of successful detectios for the robot perceptio model. 3.4 A Global Localizatio Example Figure 5 to 7 illustrate MCL whe applied to localizatio of a sigle mobile robot. Show there is a series of sample sets (projected ito 2D) geerated durig global localizatio of the mobile robot Rhio operatig i a office buildig. I Figure 5, the robot is globally ucertai; hece the samples are spread uiformly over the free-space. Figure 6 shows the sample set after approximately 1.5 meters of robot motio, at which poit MCL has disambiguated the robot s positio maily up to a sigle symmetry. Fially, after aother 4 meters of robot motio, the ambiguity is resolved, the robot kows where it is. The majority of samples is ow cetered tightly aroud the correct positio, as show i Figure 7. All ecessary computatio is carried out i real-time o a low-ed PC. 4 Learig Visual Detectio Models To implemet the multi-robot Mote-Carlo localizatio techique robots must possess the ability to sese each other. The crucial compoet is the detectio model Pèç m j ç ;r èwhich describes the coditioal probability that robot m is at locatio ç m, give that robot perceives robot m with measuremet r. From a mathematical poit of view, our approach is sufficietly geeral to accommodate a wide rage of sesors for robot detectio, assumig that the coditioal desity Pèç m j ç ;r èis adjusted accordigly. We will ow describe a specific detectio method that itegrates iformatio from multiple sesor modalities. This method, which itegrates camera ad rage iformatio, will be employed throughout our experimets.

16 14 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru 4.1 Detectio To determie the relative locatio of other robots, our approach combies visual iformatio obtaied from a o-board camera, with proximity iformatio comig from a laser rage fider. Below, i our experimets, oly oe of the robots is equipped with a camera; however, despite the asymmetry, the iformatio coveyed by a detectio eables both robots to refie their iteral belief as to where they are, utilizig the other robot s belief. Camera images are used to detect other robots, ad laser rager fider scas are used to determie the relative positio of the detected robot ad its distace. The top row i Figure 8 shows examples of camera images recorded i the corridor. Each image shows a robot, marked by uique, colored markers to facilitate their recogitio. Eve though the robot is oly show with a fixed orietatio i this figure, the markers ca be detected regardless of a robot s orietatio. To fid robots i a camera image, our approach first filters the image usig Gaussia color filters tued to the colors of the markers (see e.g., [34]). The ceter of the colors are the obtaied by local smoothig, ad thresholdig is applied to determie whether or ot a robot ca be see i the image. The small black rectagles, superimposed at the ceter of each marker i the images i Figure 8, illustrate the ceter of the marker as idetified by this visual routie. Curretly, images are aalyzed at a rate of 1Hz, with the mai delay beig caused by the parallel port over which images are trasferred from the camera to the computer. 1 This slow rate is sufficiet for the applicatio at had. Oce a robot has bee detected, a laser sca is aalyzed for the relative locatio of the robot i polar coordiates (distace ad agle). This is doe by searchig for a covex local miimum i the distaces of the sca, usig the agle obtaied from the camera image as a startig poit. We foud that this method is robust ad gives accurate results eve i cluttered eviromets. The bottom row i Figure 8 shows laser scas ad the result of our aalysis for the example situatios depicted i the top row of the same figure. Each sca cosists of 180 distace measuremets with approx. 5 cm accuracy, spaced at 1 degree agular distace. The dark lie i each diagram depicts the extracted locatio of the robot i polar coordiates, relative to the positio of the detectig robot. All scas are scaled for illustratio purposes. Based o a dataset of 54 successful robot detectios, which were labeled by the true positios of both robots, we foud the mea error of the distace estimatio to be 88.7cm, ad the mea agular error to be 2.36 degree. 4.2 Learig the Detectio Model Next, we have to devise a probabilistic detectio model of the type Pèç m j ç ;r è. To recap, r deotes a detectio evet by the -th robot, which comprises the idetity of the detected robot (if 1 With a state-of-the-art memory-mapped frame grabber the same aalysis would be feasible at frame rate.

17 A Mote Carlo Algorithm for Multi-Robot Localizatio Fig. 9: Gaussia desity represetig the robot perceptio model. The x-axis represets the deviatio of relative agle ad the y-axis the error i the distace betwee the two robots. ay), ad its relative locatio i polar coordiates. The variable ç m is the locatio of the detected robot (here m with m 6= refers to a arbitrary other robot), ad ç is the locatio of the -th robot. As described above, we will restrict our cosideratios to positive detectios, i.e., cases where a robot did detect a robot m. Negative detectio evets (a robot does ot see a robot m) are beyod the scope of this paper ad will be igored. The detectio model is traied usig data. More specifically, durig traiig we assume that the exact locatio of each robot is kow. Wheever a robot takes a camera image, its locatio is aalyzed as to whether other robots are i its visual field. This is doe by a geometric aalysis of the eviromet, exploitig the fact that the locatios of all robots are kow durig traiig. The, the image is aalyzed, ad for each detected robot the idetity ad relative locatio is recorded. This data is sufficiet to trai the detectio model Pèç m j ç ;r è. robot detected o robot detected robot i field of view 64.3% 35.7% o robot i field of view 6.90% 93.1% Table 1: Rates of false-positives ad false-egatives for our detectio routie. I our implemetatio, we employ a parametric mixture model to represet Pèç m j ç ;r è. Our approach models false-positive ad false-egative detectios usig a biary radom variable. Table 1 shows the ratios of these errors i the traiig set. As ca be see there, our curret visual routies have a 35.7% chace of ot detectig a robot i their visual field, but oly a 6.9% chace

18 16 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru to erroeously detectig a robot whe there is oe. The Gaussia distributio show i Figure 9 models the error i the estimatio of a robot s locatio. Here the x-axis represets the agular error, ad the y axis the distace error. This Gaussia has bee obtaied through maximum likelihood estimatio. As is easy to be see, the Gaussia is zero-cetered alog both dimesios, ad it assigs low likelihood to large errors. The correlatio betwee both compoets of the error, agle ad distace, are approximately zero, suggestig that both errors might be idepedet. I our experimets, the true locatio was ot determied maually; istead, MCL was applied for positio estimatio (with a kow startig positio ad very large sample sets). Empirical results i [17] suggest that MCL is sufficietly accurate for trackig a robot with oly a few cetimeters error. The robots positios, while movig at speeds like 30 cm/sec through our eviromet, were the aalyzed geometrically to determie whether (ad where) robots are i the visual fields of other robots. As a result, data collectio is extremely easy as it does ot require ay maual labelig; however, the error i MCL leads to a slightly less cofied detectio model that oe would obtai with maually labeled data (assumig that the accuracy of maual positio estimatio exceeds that of MCL). 5 Experimetal Results Our approach was evaluated systematically usig the two mobile robots (Robi ad Maria) show i Figure 10. Both robots were marked optically by a colored marker, as show i Figure 8. The cetral questio drivig our experimets war: Ca cooperative multi-robot localizatio improve the localizatio accuracy, whe compared to covetioal sigle-robot localizatio? Put differetly, ca the task of global localizatio sped up sigificatly whe multiple robots cooperate durig localizatio? To shed light oto these questios, we operated the robots over exteded periods of time i our uiversity buildig. Figure 11 shows a map of the eviromet which was leared usig a probabilistic mappig algorithm [69, 72]. Notice the log corridor. Due to the lack of features, global localizatio is quite difficult whe the robots operate i this corridor. Previous publicatios (e,g,. [17, 23]) have aalyzed i detail the performace of Markov localizatio ad MCL. Thus, i this paper we will focus o the utility of collaboratio ad detectios i multi-robot localizatio. Throughout our experimets, we cosistetly foud that the collaboratio reduced the time required for global localizatio, ad it also improved the overall accuracy. Figures 11 to 15 show a example i detail, obtaied i oe of our experimets. I particular, Figure 11 shows the belief state of oe of the robots, Robi, at a specific poit i time while performig global localizatio. I this specific experimet, the robot previously

19 A Mote Carlo Algorithm for Multi-Robot Localizatio 17 Fig. 10: Two of the robots used for testig: Maria (left) ad Robi (right). Fig. 11: Belief state of Robi durig global localizatio i a log corridor. Fig. 12: Belief state of Maria operatig i the room.

20 18 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru Fig. 13: Image ad laser sca Maria uses to determie the relative agle ad distace of Robi. Fig. 14: Samplig-based represetatio of the desity geerated by Maria accordig to the detectio of Robi i the curret image. Fig. 15: Belief state of robi after icorporatig the measuremet of Maria.

21 A Mote Carlo Algorithm for Multi-Robot Localizatio 19 traversed the corridor from the right to the left, developig a belief that is cetered alog the mai axis of the corridor. However, the robot is uaware of its exact locatio withi the corridor; either does it kow its global headig directio. The secod robot, Maria, operates i our lab, which is the cluttered room adjacet to the corridor. Its belief is show i Figure 12. Because of the o-symmetric ature of the lab, the robot kows fairly well where it is. The key evet, illustratig the utility of cooperatio i localizatio, is a detectio evet. More specifically, Maria, the robot i the lab, detects Robi, as it moves through the corridor. Figure 13 shows the image ad the laser sca, alog with the estimated distace ad orietatio. Usig the detectio model described i Sectio 4 Maria geerates the desity show i Figure:14. It the trasmits this desity to Robi which itegrates it ito its curret belief. Robi s resultig desity is show i Figure 15. As this figure illustrates, this sigle icidet resolves etirely the ucertaity i Robi s belief which would have take miutes if the robots were uable to detect each other. Obviously, this experimet is specifically well-suited to demostrate the advatage of detectios i multi-robot localizatio, sice the robots ucertaities are somewhat orthogoal, makig the detectio highly effective. Nevertheless, we cosistetly observed similarly good performace eve whe operatig the robots i other parts of the eviromet, e.g., whe they both operated i the corridor. 6 Related Work Mobile robot localizatio has frequetly bee recogized as a key problem i robotics with sigificat practical importace. Cox [15] oted that Usig sesory iformatio to locate the robot i its eviromet is the most fudametal problem to providig a mobile robot with autoomous capabilities. A recet book by Borestei, Everett, ad Feg [6] provides a excellet overview of the state-of-the-art i localizatio. Localizatio plays a key role i various successful mobile robot architectures [14, 26, 32, 46, 47, 52, 57, 59, 73] ad various chapters i [43]. While some localizatio approaches, such as those described i [33, 44, 64] localize the robot relative to some ladmarks i a topological map, our approach localizes the robot i a metric space, just like those methods proposed i [3, 67, 71]. Almost all existig approach address sigle-robot localizatio oly. Moreover, the vast majority of approaches is icapable of localizig a robot globally; istead, they are desiged to track the robot s positio by compesatig small odometric errors. Thus, they differ from the approach described here i that they require kowledge of the robot s iitial positio; ad they are ot able to recover from global localizig failures. Probably the most popular method for trackig a robot s positio is Kalma filterig [30, 31, 48, 50, 61, 65], which represet ucertaity by sigle-modal

22 20 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru distributios. These approaches are uable to localize robots uder global ucertaity a problem which Egelso called the kidapped robot problem [20]. Recetly, several researchers proposed Markov localizatio, which eables robots to localize themselves uder global ucertaity [9, 37, 53, 64]. Global approaches have two importat advatages over local oes: First, the iitial locatio of the robot does ot have to be specified ad, secod, they provide a additioal level of robustess, due to their ability to recover from localizatio failures. Amog the global approaches those usig metric represetatios of the space such as MCL lad [9, 8] ca deal with a wider variety of eviromets tha those methods relyig o topological maps. For example, they are ot restricted to orthogoal eviromets cotaiig pre-defied features such as corridors, itersectios ad doors. I additio, most existig approaches are restricted i the type features that they cosider. May approaches reviewed i [6], a recet book o this topic, are limited i that they require modificatios of the eviromet. Some require artificial ladmarks such as bar-code reflectors [21], reflectig tape, ultrasoic beacos, or visual patters that are easy to recogize, such as black rectagles with white dots [4]. Of course, modifyig the eviromet is ot a optio i may applicatio domais. Some of the more advaced approaches use more atural ladmarks that do ot require modificatios of the eviromet. For example, the approaches of Kortekamp ad Weymouth [44] ad Matarić [49] use gateways, doors, walls, ad other vertical objects to determie the robot s positio. The Helpmate robot uses ceilig lights to positio itself [39]. Dark/bright regios ad vertical edges are used i [13, 74], ad hallways, opeigs ad doors are used by the approach described i [41, 62, 63]. Others have proposed methods for learig what feature to extract, through a traiig phase i which the robot it told its locatio [28, 56, 67, 68]. These are just a few represetative examples of may differet features used for localizatio. Our approach differs from all these approaches i that it does ot extract predefied features from the sesor values. Istead, it directly processes raw sesor data. Such a approach has two key advatages: First, it is more uiversally applicable sice fewer assumptios are made o the ature of the eviromet; ad secod, itca utilizeall sesoriformatio, typicallyyieldigmoreaccurate results. Other approaches that process raw sesor data ca be foud i [30, 31, 48]. The issue of cooperatio betwee multiple mobile robots has gaied icreased iterest i the past (see [11, 1] for overviews). I this cotext most work o localizatio has focused o the questio how to reduce the odometry error usig a cooperative team of robots. Kurazume ad Shigemi [45], for example, divide the robots ito two groups. At every poit i time oly oe of the groups is allowed to move, while the other group remais at its positio. Whe a motio commad has bee executed, all robots stop, perceive their relative positio, ad use this to reduce errors i odometry. While this method reduces the odometry error of the whole team of robots it is ot able to perform global localizatio; either ca it recover from sigificat sesor errors.

23 A Mote Carlo Algorithm for Multi-Robot Localizatio 21 Rekleitis ad colleagues [58] preset a cooperative exploratio method for multiple robots, which also addresses localizatio. To reduce the odometry error, they use a approach closely related to the oe described [45]. Here, too, oly oe robot is allowed to move at ay poit i time, while the other robots observe the movig oe. The statioary robots track the positio of the movig robot, thus providig more accurate positio estimates tha could be obtaied with pure deadreckoig. Fially, i [5], a method is preseted that relies o a compliat likage of two mobile robots. Special ecoders o the likage estimate the relative positios of the robots while they are i motio. The author demostrates that the dead-reckoig accuracy of the compliat likage vehicle is substatially improved. However, all these approaches oly seek to reduce the odometry error. Noe of them icorporates evirometal feedback ito the estimatio, ad cosequetly they are uable to localize roots relative to each other, or relative to their eviromets, from scratch. Eve if the iitial locatio of all robots are kow, they ultimately will get lost but at a slower pace tha a comparable sigle robot. The problem addressed i this paper differs i that we are iterested i collaborative localizatio i a global frame of referece, ot just reducig odometry error. I particular, our approach addresses cooperative global localizatio i a kow eviromet. 7 Coclusio 7.1 Summary We have preseted a statistical method for collaborative mobile robot localizatio. At its core, our approach uses probability desity fuctios to represet the robots estimates as to where they are. To avoid expoetial complexity i the umber of robots, a factorial represetatio is advocated where each robot maitais its ow, local belief fuctio. A fast, uiversal samplig-based scheme is employed to approximate beliefs. The probabilistic ature of our approach makes it possible that teams of robots perform global localizatio, i.e., they ca localize themselves from scratch without iitial kowledge as to where they are. Durig localizatio, robots ca detect each other. Here we use a combiatio of camera images ad laser rage scas to determie other robot s relative locatio. The reliability of the detectio routie is modeled by learig a parametric detectio model from data, usig the maximum likelihood estimator. Durig localizatio, detectios are used to itroduce additioal probabilistic costraits, represeted by tree-like structure, that tie oe robot s belief to aother robot s belief fuctio. To combie differet sample sets collected geerated at differet robots (each robot s belief is represeted by a separate sample set), our approach trasforms detectios ito desity trees, which trasform discrete sample sets ito piecewise costat desity fuctios. These trees are the used to refie the weightig factors (importace factors) of other robots beliefs, thereby

24 22 Dieter Fox, Wolfram Burgard, Haes Kruppa, ad Sebastia Thru reducig their ucertaity i respose to the detectio. As a result, our approach makes it possible to amortize data collected at multiple platforms. Experimetal results, carried out i a idoor eviromet, demostrate that our approach ca reduce the ucertaity i localizatio sigificatly, whe compared to covetioal siglerobot localizatio. Thus, whe teams of robots are placed i a kow eviromet with ukow startig locatios, our approach ca yield much faster localizatio the covetioal, sigle-robot locatio at approximate equal computatio costs ad relatively small commuicatio overhead. 7.2 Implicatios for Heterogeeous Robot Teams Eve though the experimet reported here were carried out usig homogeeous robots, the work reported here offers some iterestig perspectives for teams of heterogeeous robots. Traditioally, heterogeeity has ofte bee suggested as a meas to achieve a wide rage of tasks, requirig a collectio of differet actuators, maipulators, or locomotio modalities (wheels, legs). I the cotext of behavior-based robotics, heterogeeity has ofte studied the effect of differet software architectures o the overall task performace [2]. Our approach ca exploit heterogeeity i the robots sesors. Cosider, for example, a team of robots where oly a small umber of robots are equipped with sesors that support highaccuracy localizatio. For example, laser rage fiders typically provide highly accurate rage measuremets, but they are bulky, expesive, ad they cosume sigificatly more eergy tha comparable, low-accuracy sesors such as soar sesors. It might therefore be desirable to equip oly a small umber of robots with laser rage fiders. As oted above, our approach makes it possible to amortize sesor data across multiple robotic platforms durig localizatio. Thus, it potetially eables a heterogeeous team of robots to maitai highly accurate locatio estimates, eve if oly a small umber of robots are equipped with the ecessary high-accuracy sesors. I the extreme, oe might thik of heterogeeous robot teams where oly a small umber of robots is capable of performig localizatio. Our approach would eable these robots to localize other robots i the team, ot capable of localizig themselves autoomously, thereby provide a uique service to the etire heterogeeous team. 7.3 Limitatios ad Discussio The curret approach possesses several limitatios that warrat future research. æ I our curret system, oly positive detectios are processed. Not seeig aother robot is also iformative, eve though ot as iformative as positive detectios. Icorporatig such egative detectios is geerally possible i the cotext of our statistical framework (usig the iverse weightig scheme). However, such a extesio would drastically icrease the com-

25 A Mote Carlo Algorithm for Multi-Robot Localizatio 23 putatioal overhead, ad it is uclear as to whether the effects o the localizatio accuracy justify the additioal computatio ad commuicatio. æ Aother limitatio of the curret approach arises from the fact that our detectio approach must be able to idetify idividual robots hece they must be marked appropriately. Of course, simple meas such as bar-codes ca provide the ecessary, uique labels. However, from a academic stad poit of view it might be iterestig to devise methods that ca detect, but ot idetify robots. The geeral problem with such a settig lies i our factorial represetatio, which caot model statemets such as either robot A or robot B is straight i frot of me. To model such situatios, oe would have to compute distributios over the joit space of all robots coordiates, which would make it impossible that each robot carries its ow, local positio estimate. I additio, the complexity of the estimatio routie would ow deped super-liearly o the umber of robots (as poited out above, i the worst case it would scale expoetially istead of liearly). I fact, the latter observatio is the key reaso as to why factorial represetatios are chose here. æ The collaboratio described here is purely passive, i that robots combie iformatio collected locally, but they do ot chage their course of actio so as to aid localizatio. I [10, 24], we proposed a algorithm based o iformatio-theoretic priciples, for active localizatio, where a sigle robot actively explores its eviromet so as to best localize itself. A desirable objective for future research is the applicatio of the same, iformatio-theoretic priciple, to coordiated multi-robot localizatio. æ Fially, the robots update their istatly wheever they perceive aother robot. I situatios i which both robots are highly ucertai at the time of the detectio it might be more appropriate to delay the update. For example, if oe of the robots afterwards becomes more certai by gatherig further iformatio about the eviromet or by beig detected by aother, certai robot, the the sychroizatio result ca be much better if it is doe retrospectively. This, however, requires that the robots keep track of their actios ad measuremets after detectig other robots. Despite these ope research areas, our approach does provide a soud statistical basis for iformatio exchage durig collaborative localizatio, ad empirical results illustrate its appropriateess i practice. These results suggest that robots actig as a team are superior to robots actig idividually. While we were forced to carry out this research o two platforms oly, we cojecture that the beefits of collaborative multi-robot localizatio icrease with the umber of available robots.