Simultaneous

Localization and Mapping Problem

Michael Montemerlo,Sebastian Thrun,Daphne Koller,Ben Wegbreit

February2002

CMU-CS-02-112

School of Computer Science

Carnegie Mellon University

Pittsburgh,PA15213

This research is sponsored by by DARPA’s MARS Program(Contract number N66001-01-C-6018)and the Na-tional Science Foundation(CAREER grant number IIS-9876136and regular grant number IIS-9877033),all of which is gratefully acknowledged.The views and conclusions contained in this document are those of the author and should not be interpreted as necessarily representing official policies or endorsements,either expressed or implied,of the United States Government or any of the sponsoring institutions.Abstract

The ability to simultaneously localize a robot and accurately map its surroundings is considered by many to be a key prerequisite of truly autonomous robots.However,few approaches to this problem scale up to handle the very large number of landmarks present in real environments. Kalmanfilter-based algorithms,for example,require time quadratic in the number of landmarks to incorporate each sensor observation.This paper presents FastSLAM,an algorithm that recursively estimates the full posterior distribution over robot pose and landmark locations,yet scales loga-rithmically with the number of landmarks in the map.This algorithm is based on a factorization of the posterior into a product of conditional landmark distributions and a distribution over robot paths.The algorithm has been run successfully on as many as50,000landmarks,environments far beyond the reach of previous approaches.Experimental results demonstrate the advantages and limitations of the FastSLAM algorithm on both simulated and real-world data.

Keywords:Robot mapping,Kalmanfilters,Bayesian techniques,particlefilters,robot navi-gation,robot localization1Introduction

The problem of simultaneous localization and mapping,also known as SLAM,has attracted im-mense attention in the mobile robotics literature.SLAM addresses the problem of building a map of an environment from a sequence of landmark measurements obtained from a moving robot. Since robot motion is subject to error,the mapping problem necessarily induces a robot localiza-tion problem—hence the name SLAM.The ability to simultaneously localize a robot and accu-rately map its environment is considered by many to be a key prerequisite of truly autonomous robots[3,7,15].

The dominant approach to the SLAM problem was introduced in a seminal paper by Smith, Self,and Cheeseman[14].This paper proposed the use of an extended Kalmanfilter(EKF)for incrementally estimating the posterior distribution over robot pose along with the positions of the landmarks.In the last decade,this approach has found widespread acceptance infield robotics, as a recent tutorial paper[2]documents.Recent research has focused on scaling this approach to larger environments with more than a few hundred landmarks[9,6,8]and to algorithms for handling data association problems[16].

A key limitation of EKF-based approaches is their computational complexity.Sensor updates require time quadratic in the number of landmarks to compute.This complexity stems from the fact that the covariance matrix maintained by the Kalmanfilters has elements,all of which must be updated even if just a single landmark is observed.The quadratic complexity limits the number of landmarks that can be handled by this approach to only a few hundred—whereas natural environment models frequently contain millions of features.This shortcoming has long been recognized by the research community[6,8,13].

In this paper we approach the SLAM problem from a Bayesian point of view.Figure1illus-trates a generative probabilistic model(dynamic Bayes network)that underlies the rich corpus of SLAM literature.In particular,the robot pose,denoted,evolve over time as a function of the robot controls,denoted.Each of the landmark measurements,denoted, are functions of the position of the landmarks measured(),and of the robot pose at the time the measurement was taken.From this diagram it is evident that the SLAM problem exhibits im-portant conditional independences.In particular,knowledge of the robot’s path renders the individual landmark measurements independent.So for example,if an oracle provided us with the exact path of the robot,the problem of determining the landmark locations could be decoupled into independent estimation problems,one for each landmark.

Based on this observation,this paper describes an efficient SLAM algorithm called FastSLAM. FastSLAM decomposes the SLAM problem into a robot localization problem,and a collection of landmark estimation problems that are conditioned on the robot pose estimate.This factored repre-sentation is exact,due to the natural conditional independences in the SLAM problem.FastSLAM

1

Figure1:The SLAM problem:The robot moves from pose through a sequence of controls,.As it moves,it measures nearby landmarks.At time,it observes landmark out of two landmarks,. The measurement is denoted(range and bearing).At time,it observes the other landmark,,and at time ,it observes again.The SLAM problem is concerned with estimating the locations of the landmarks and the robot’s path from the controls and the measurements.The gray shading illustrates a conditional independence relation.

uses a modified particlefilter for estimating a posterior over robot paths.Each particle possesses Kalmanfilters that estimate the landmark locations conditioned on the path estimate.The resulting algorithm is an instance of the Rao-Blackwellized particlefilter[12,5].A naive imple-mentation of this idea leads to an algorithm that requires time,where is the number of particles in the particlefilter and is the number of landmarks.We develop a tree-based data structure that reduces the running time of FastSLAM to,making it significantly faster than existing EKF-based SLAM algorithms.We also extend the FastSLAM algorithm to situations with unknown data association and unknown number of landmarks,showing that our approach can be extended to to full range of SLAM problems discussed in the literature.

Experimental results using a physical robot and a robot simulator illustrate that the FastSLAM algorithm can handle orders of magnitude more landmarks than present day approaches.We also find that in certain situations,an increased number of landmarks leads to a mild reduction of the number of particles needed to generate accurate maps—whereas in others the number of particles required for accurate mapping may be prohibitively large.

2SLAM Problem Definition

The SLAM problem,as defined in the rich body of literature on SLAM,is best described as a probabilistic Markov chain.The robot’s pose at time will be denoted.For robots operating

2on the plane—which is the case in all our experiments—poses are comprised of a robot’s-coordinate in the plane and its heading direction.

Poses evolve according to a probabilistic law,often referred to as probabilistic motion model:

(1)

Thus,is a probabilistic function of the robot control and the previous pose.In mobile robotics,the motion model is usually a time-invariant probabilistic generalization of robot kine-matics[1].

The robot’s environment possesses immobile landmarks.Each landmark is characterized by its location in space,denoted for.Without loss of generality,we will think of landmarks as points in the plane,so that locations are specified by two numerical values.

To map its environment,the robot can sense landmarks.For example,it may be able to measure range and bearing to a landmark,relative to its own local coordinate frame.The measurement at time will be denoted.While robots can often sense more than one landmark at a time,we follow commonplace notation by assuming that sensor measurements correspond to exactly one landmark[2].This convention is adopted for mathematical convenience.It poses no restriction,as multiple landmark sightings at a single time step can easily be processed sequentially.

Sensor measurements are governed by a probabilistic law,often referred to as the measurement model:

(2) Here is the set of all landmarks,and is the index of the land-mark perceived at time.For example,in Figure1,we have,and,since the robotfirst observes landmark,then landmark,andfinally landmark for a second time. Many measurement models in the literature assume that the robot can measure range and bearing to landmarks,confounded by measurement noise.The variable is often referred to as corre-spondence.Most theoretical work in the literature assumes knowledge of the correspondence or, put differently,that landmarks are uniquely identifiable.Practical implementations use maximum likelihood estimators for estimating the correspondence on-the-fly,which work well if landmarks are spaced sufficiently far apart.In large parts of this paper we will simply assume that landmarks are identifiable,but we will also discuss an extension that estimates the correspondences from data.

We are now ready to formulate the SLAM problem.The SLAM problem,in its most general form,is the problem of determining the location of all landmarks and robot poses from mea-surements and controls.In probabilistic terms,this is expressed by the following posterior:

(3)

3Here we use the superscript to refer to a set of variable from time1to time.If the correspon-dences are known,the SLAM problem is simpler:

(4)

As argued in the introduction to this article,all individual landmark estimation problems are in-dependent if one knew the robot’s path and the correspondence variables.This conditional independence is the basis of the FastSLAM algorithm described in the next section.

3FastSLAM with Known Correspondences

We begin our consideration with the important case where the correspondences

are known,and so is the number of landmarks observed thus far.

3.1Factored Representation

The conditional independence property of the SLAM problem implies that the posterior(4)can be factored as follows:

(5)

Put verbally,the problem can be decomposed into estimation problems,one problem of estimating a posterior over robot paths,and problems of estimating the locations of the landmarks conditioned on the path estimate.This factorization is exact and always applicable in the SLAM problem—but to our knowledge has never been utilized before in the context of SLAM.

The FastSLAM algorithm implements the path estimator using a modified particlefilter[4].As we argue further below,thisfilter can efficiently sample from this space, providing a good approximation of the posterior even under non-linear motion kinematics.The landmark pose estimators are realized by Kalmanfilters,using separatefilters for different landmarks.Because the landmark estimates are conditioned on the path estimate,each particle in the particlefilter has its own,local landmark estimates.Thus,for particles and landmarks,there will be a total of Kalmanfilters,each of dimension2(for the two landmark coordinates).This representation will now be discussed in detail.

3.2Particle Filter Path Estimation

FastSLAM employs particlefilters for estimating the path posterior in(5),using a filter that is similar(but not identical)to the Monte Carlo localization(MCL)algorithm[1].MCL

4

is an application of particlefilter to the problem of robot pose estimation(localization).At each point in time,both algorithms maintain a set of particles representing the posterior, denoted.Each particle represents a“guess”of the robot’s path:

(6) We use the superscript notation to refer to the-th particle in the set.

The particle set is calculated incrementally,from the set at time,a robot control ,and a measurement.First,each particle in is used to generate a probabilistic guess of the robot’s pose at time:

(7)

This guess is obtained by sampling from the probabilistic motion model.This estimate is then added to a temporary set of particles,along with the path.Under the assumption that the set of particles in is distributed according to(which is an asymptotically correct approximation),the new particle is distributed according to:

(8)

This distribution is commonly referred to as proposal distribution of particlefiltering.

After generating particles in this way,the new set is obtained by sampling from the tem-porary particle set.Each particle is drawn(with replacement)with a probability proportional to a so-called importance factor,which is calculated as follows[10]:

target distribution

(9) The exact calculation of(9)will be discussed further below.The resulting sample set is dis-tributed according to an approximation to the desired pose posterior,an approxi-mation which is correct as the number of particles goes to infinity.We also notice that only the most recent robot pose estimate is used when generating the particle set.This will allows us to silently“forget”all other pose estimates,rendering the size of each particle independent of the time index.

3.3Landmark Location Estimation

FastSLAM represents the conditional landmark estimates in(5)by Kalman filters.Since this estimate is conditioned on the robot pose,the Kalmanfilters are attached to indi-vidual pose particles in.More specifically,the full posterior over paths and landmark positions in the FastSLAM algorithm is represented by the sample set

(10)

5Here and are mean and covariance of the Gaussian representing the-th landmark, attached to the-th particle.In the planar robot navigation scenario,each mean is a two-element vector,and is a2by2matrix.

The posterior over the-th landmark pose is easily obtained.Its computation depends on whether or not,that is,whether or not was observed at time.For,we obtain

(11)

For,we simply leave the Gaussian unchanged:

(12)

The FastSLAM algorithm implements the update equation(11)using the extended Kalmanfil-ter(EKF).As in existing EKF approaches to SLAM,thisfilter uses a linearized version of the perceptual model[2].Thus,FastSLAM’s EKF is similar to the traditional EKF for SLAM[14]in that it approximates the measurement model using a linear Gaussian func-tion.We note that,with an actual linear Gaussian observation model,the resulting distribution is exactly a Gaussian,even if the motion model is not linear.This is a conse-quence of the use of sampling to approximate the distribution over the robot’s pose.

One significant difference between the FastSLAM algorithm’s use of Kalmanfilters and that of the traditional SLAM algorithm is that the updates in the FastSLAM algorithm involve only a Gaussian of dimension two(for the two landmark location parameters),whereas in the EKF-based SLAM approach a Gaussian of size has to be updated(with landmarks and3robot pose parameters).This calculation can be done in constant time in FastSLAM,whereas it requires time quadratic in in standard SLAM.

3.4Calculating the Importance Weights

Let us now return to the problem of calculating the importance weights needed for particle filter resampling,as defined in(9):

µµµµµµµµ8,Σ87,Σ76,Σ65,Σ5

4,Σ43,Σ32,Σ21,Σ1[m][m][m][m][m][m][m][m][m][m][m][m][m][m][m][m]Figure 2:A tree representing landmark estimates within a single particle.

µ8,Σ8µ7,Σ7µ6,Σ6µ5,Σ5µ4,Σ4µ3,Σ3µ2,Σ2µ1,Σ1[m][m][m][m][m][m][m][m][m][m][m][m][m]

[m][m][m]Figure 3:Generating a new particle from an old one,while modifying only a single Gaussian.The new particle receives only a partial tree,consisting of a path to the modified Gaussian.All other pointers are copied from the generating tree.This can be done in time logarithmic in .

8

(a)(b)(c)

Figure4:(a)Physical robot mapping rocks,in a testbed developed for Mars Rover research.(c)Raw range and path data.(c)Map generated using FastSLAM(dots),and locations of rocks determined manually(circles).

all Gaussians.An example is shown in Figure3:Here we assume,that is,only the Gaussian parameters and are updated.Instead of generating an entire new tree,only a single path is created,leading to the Gaussian.This path is an incomplete tree.To complete the tree,for all branches that leave this path the corresponding pointers are copied from the tree of the generating particle.Thus,branches that leave the path will point to the same(unmodified) subtree as that of the generating tree.Clearly,generating such an incomplete tree takes only time logarithmic in.Moreover,accessing a Gaussian also takes time logarithmic in,since the number of steps required to navigate to a leaf of the tree is equivalent to the length of the path (which is by definition logarithmic).Thus,both generating and accessing a partial tree can be done in time.Since in each updating step new particles are created,an entire update requires time in.

3.6Data Association

In many real-world problems,landmarks are not identifiable,and the total number of landmarks cannot be obtained trivially—as was the case above.In such situations,the robot has to solve a data association problem between momentary landmarks sightings and the set of landmarks in the map.It also has to determine if a measurement corresponds to a new,previously unseen landmark,in which case the map is augmented accordingly.

In most existing SLAM solutions based on EKFs,these problems are solved via maximum likelihood.More specifically,the probability of a data association is given by

9(14) The step labeled“PF”uses the particlefilter approximation to the posterior.Thefinal step assumes a uniform prior,which is commonly the case in the literature[2].The maximum likelihood data association is simply the index that maximizes(14).If the maximum value of—with careful consideration of all constants in(14)—is below a threshold, the landmark is considered previously unseen and the map is augmented accordingly.

In FastSLAM,the data association is estimated on a per-particle basis:

(15)

As a result,different particles may rely on different values of.They might even possess different numbers of landmarks in their respective maps.This constitutes a primary difference to EKF approaches,which determine the data association only once for each sensor measurement.It has frequently been observed that false data association will make conventional EKF approach fail catastrophically[2].FastSLAM is more likely to recover,thanks to its ability to pursue multiple data associations simultaneously.Particles with wrong data association are(in expectation)more likely to disappear in the resampling process than those who guess the data association right.

Under mild assumptions(e.g.,minimum spacing between landmarks and bounded sensor er-ror),the data association search can be implemented in time logarithmic in.One possibility is the use of kd-trees as indexing scheme in the tree structures above,instead of the landmark number, as proposed in[11].

4Experimental Results

The FastSLAM algorithm was tested extensively under various conditions.Real-world experi-ments were complimented by systematic simulation experiments,to investigate the scaling abili-ties of the approach.Overall,the results indicate favorably scaling to large number of landmarks and small particle sets.Afixed number of particles(e.g.,)appears to work well across a large number of situations.

Figure4a shows the physical robot testbed,which consists of a small arena set up under NASA funding for Mars Rover research.A RWI Pioneer robot equipped with a SICK laser rangefinder was driven along an approximate straight line,generating the raw data shown in Figure4b.The resulting map generated with samples is depicted in Figure4c,with manually determined landmark locations marked by circles.The robot’s estimates are indicated by dots,illustrating the

10

(b)

Figure5:Maps and estimated robot path,generated using sensors with(a)large and(b)small perceptualfields. The correct landmark locations are shown as dots,and the estimates ellipses,whose size corresponds to the residual uncertainty.

high accuracy of the resulting maps.FastSLAM resulted in an average residual map error of8.3 centimeters,when compared to the manually generated map.

Unfortunately,the physical testbed does not allow for systematic experiments regarding the scaling properties of the approach.In extensive simulations,the number of landmarks was in-creased up to a total of50,000,which FastSLAM successfully mapped with as few as250particles. Here the number of parameters in FastSLAM is approximately0.1%of that in the conventional EKF.However maps with as many as50,000landmarks are entirely out of range for conventional SLAM techniques,due to their enormous computational complexity.Figure5shows example maps with smaller numbers of landmarks,for different maximum sensor ranges as indicated.The ellipses in Figure5visualize the residual uncertainty when integrated over all particles and Gaus-sians.

In a set of experiments specifically aimed to elucidate the scaling properties of the approach, we evaluated the map and robot pose errors as a function of the number of landmarks,and the number of particles,respectively.The results are graphically depicted in Figures6and7. Figure6illustrates that an increase in the number of landmarks mildly reduces the error in the map and the robot pose.This is because the larger the number of landmarks,the smaller the robot pose error at any point in time.Increasing the number of particles also bears a positive effect on the map and pose errors,as illustrated in Figure7.In both diagrams,the bars correspond to95% confidence intervals.

11

Number of landmarks K P o s i t i o n E r r o r (s t a n d a r d d e v i a t i o n )Figure 6:Accuracy of the FastSLAM algorithm as a function of the number of landmarks

,Large number of

landmarks reduce the robot localization error,with little effect on the map error.

Figure 7:Accuracy of the FastSLAM algorithm as a function of the number of particles

.Good results can be

achieved with as few as 100particles.12

We have presented an algorithm called FastSLAM,as an efficient new solution to the concurrent mapping and localization problem.This algorithm utilizes a Rao-Blackwellized representation of the posterior,integrating particlefilter and Kalmanfilter representations.It is based on a condi-tional independence property of the SLAM problem which previous algorithms failed to exploit. Landmark estimates are efficiently represented using tree structures.Updating the posterior re-quires time,where is the number of particles and the number of landmarks.This is in contrast to the complexity of the common Kalman-filter based approach to SLAM. Experimental results illustrate that FastSLAM can build maps with an order of magnitude more landmarks than previous methods.They also demonstrate that under certain conditions,a small number of particles works well regardless of the number of landmarks.

6Acknowledgments

We are indebted to Kevin Murphy and Nando de Freitas for insightful discussions on this topic.

References

[1]F.Dellaert,D.Fox,W.Burgard,and S.Thrun.Monte carlo localization for mobile robots.

In Proceedings of the IEEE International Conference on Robotics and Automation(ICRA), 1999.

[2]G.Dissanayake,P.Newman,S.Clark,H.F.Durrant-Whyte,and M.Csorba.An experimen-

tal and theoretical investigation into simultaneous localisation and map building(SLAM).

In P.Corke and J.Trevelyan,editors,Lecture Notes in Control and Information Sciences: Experimental Robotics VI,pages265–274,London,2000.Springer Verlag.

[3]G.Dissanayake,P.Newman,S.Clark,H.F.Durrant-Whyte,and M.Csorba.A solution to

the simultaneous localisation and map building(SLAM)problem.IEEE Transactions of Robotics and Automation,2001.In Press.

[4]A.Doucet,J.F.G.de Freitas,and N.J.Gordon,editors.Sequential Monte Carlo Methods In

Practice.Springer Verlag,New York,2001.

[5]A Doucet,N.de Freitas,K.Murphy,and S.Russell.Rao-Blackwellised particlefiltering for

dynamic Bayesian networks.In Proceedings of the Sixteenth Conference on Uncertainty in Artificial Intelligence,pages176–183,Stanford,2000.

13[6]J.Guivant and E.Nebot.Optimization of the simultaneous localization and map building

algorithm for real time implementation.IEEE Transaction of Robotic and Automation,May 2001.In press.

[7]D.Kortenkamp,R.P.Bonasso,and R.Murphy,editors.AI-based Mobile Robots:Case studies

of successful robot systems,Cambridge,MA,1998.MIT Press.

[8]J.J.Leonard and H.J.S.Feder.A computationally efficient method for large-scale concurrent

mapping and localization.In J.Hollerbach and D.Koditschek,editors,Proceedings of the Ninth International Symposium on Robotics Research,Salt Lake City,Utah,1999.

[9]F.Lu and E.Milios.Globally consistent range scan alignment for environment mapping.

Autonomous Robots,4:333–349,1997.

[10]N.Metropolis,A.W.Rosenbluth,M.N.Rosenbluth,A.H.Teller,and E.Teller.Equations of

state calculations by fast computing machine.Journal of Chemical Physics,21:1087–1091, 1953.

[11]A.W.Moore.Very fast EM-based mixture model clustering using multiresolution kd-trees.

In Advances in Neural Information Processing Systems(NIPS),Cambridge,MA,1998.MIT Press.

[12]K.Murphy and S.Russell.Rao-Blackwellized particlefiltering for dynamic Bayesian net-

works.In A.Doucet,N.de Freitas,and Gordon.N.,editors,Sequential Monte Carlo Methods in Practice,pages499–516.Springer Verlag,2001.

[13]P.Newman.On the Structure and Solution of the Simultaneous Localisation and Map Build-

ing Problem.PhD thesis,Australian Centre for Field Robotics,University of Sydney,Sydney, Australia,2000.

[14]R.Smith,M.Self,and P.Cheeseman.Estimating uncertain spatial relationships in robotics.

In I.J.Cox and G.T.Wilfong,editors,Autonomous Robot Vehnicles,pages167–193.Springer-Verlag,1990.

[15]C.Thorpe and H.Durrant-Whyte.Field robots.In Proceedings of the10th International

Symposium of Robotics Research(ISRR’01),Lorne,Australia,2001.

[16]S.Thrun,D.Fox,and W.Burgard.A probabilistic approach to concurrent mapping and local-

ization for mobile robots.Machine Learning,31:29–53,1998.also appeared in Autonomous Robots5,253–271(joint issue).

14下载本文