FastSLAM: A Factored Solution to the Simultaneous ...

1 FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem Michael Montemerlo and Sebastian Thrun Daphne Koller and Ben Wegbreit School of Computer Science Computer Science Department Carnegie Mellon University Stanford University Pittsburgh, PA 15213 Stanford, CA 94305-9010. Abstract A key limitation of EKF-based approaches is their compu- tational complexity. Sensor updates require time quadratic The ability to simultaneously localize a robot and ac- in the number of landmarks to compute. This complex- . curately map its surroundings is considered by many to ity stems from the fact that the covariance matrix maintained be a key prerequisite of truly autonomous robots. How- by the Kalman filters has elements, all of which must ever, few approaches to this problem scale up to handle be updated even if just a single landmark is observed. The the very large number of landmarks present in real envi- ronments.

2 Kalman filter-based algorithms, for example, quadratic complexity limits the number of landmarks that require time quadratic in the number of landmarks to in- can be handled by this approach to only a few hundred . corporate each sensor observation. This paper presents whereas natural environment models frequently contain mil- FastSLAM, an algorithm that recursively estimates the lions of features. This shortcoming has long been recog- full posterior distribution over robot pose and landmark nized by the research community [6, 8, 14]. locations, yet scales logarithmically with the number of landmarks in the map. This algorithm is based on an ex- In this paper we approach the SLAM problem from a act factorization of the posterior into a product of con- Bayesian point of view. Figure 1 illustrates a generative ditional landmark distributions and a distribution over probabilistic model (dynamic Bayes network) that underlies the rich corpus of SLAM literature.

3 In particular, the robot . robot paths. The algorithm has been run successfully on as many as 50,000 landmarks, environments far be- poses, denoted , evolve over time as a function yond the reach of previous approaches. Experimental of the robot controls, denoted .. Each of the land- results demonstrate the advantages and limitations of mark measurements, denoted . , is a function of the the FastSLAM algorithm on both simulated and real- world data.. position of the landmark measured and of the robot pose at the time the measurement was taken. From this diagram it is evident that the SLAM problem exhibits important condi- tional independences. In particular, knowledge of the robot's Introduction path . renders the individual landmark measure- The problem of Simultaneous localization and mapping, also ments independent. So for example, if an oracle provided us known as SLAM, has attracted immense attention in the mo- with the exact path of the robot, the problem of determin- bile robotics literature.

4 SLAM addresses the problem of ing the landmark locations could be decoupled into inde- building a map of an environment from a sequence of land- pendent estimation problems, one for each landmark. This mark measurements obtained from a moving robot. Since observation was made previously by Murphy [12], who de- robot motion is subject to error, the mapping problem neces- veloped an efficient particle filtering algorithm for learning sarily induces a robot localization problem hence the name grid maps. SLAM. The ability to simultaneously localize a robot and Based on this observation, this paper describes an efficient accurately map its environment is considered by many to be SLAM algorithm called FastSLAM. FastSLAM decomposes a key prerequisite of truly autonomous robots [3, 7, 16]. the SLAM problem into a robot localization problem, and The dominant approach to the SLAM problem was in- a collection of landmark estimation problems that are con- troduced in a seminal paper by Smith, Self, and Cheese- ditioned on the robot pose estimate.

5 As remarked in [12], man [15]. This paper proposed the use of the extended this Factored representation is exact, due to the natural con- Kalman filter (EKF) for incrementally estimating the poste- ditional independences in the SLAM problem. FastSLAM. rior distribution over robot pose along with the positions of uses a modified particle filter for estimating the posterior the landmarks. In the last decade, this approach has found over robot paths. Each particle possesses Kalman fil- widespread acceptance in field robotics, as a recent tutorial ters that estimate the landmark locations conditioned on paper [2] documents. Recent research has focused on scal- the path estimate. The resulting algorithm is an instance of ing this approach to larger environments with more than a the Rao-Blackwellized particle filter [5, 13]. A naive im- . few hundred landmarks [6, 8, 9] and to algorithms for han- plementation of this idea leads to an algorithm that requires dling data association problems [17].

6 Time, where . is the number of particles in the 1.. surement at time $ will be denoted . While robots can often sense more than one landmark at a time, we follow com- z z 1 3. monplace notation by assuming that sensor measurements s 1. s 2. s 3.. s t correspond to exactly one landmark [2]. This convention is adopted solely for mathematical convenience. It poses no u u u restriction, as multiple landmark sightings at a single time 2 3 t step can be processed sequentially. z 2. z t Sensor measurements are governed by a probabilistic law, .. often referred to as the measurement model: 2. ' ( 21 (2). Figure 1: The SLAM problem: The robot moves from pose Here 3 -54 76 is the set of all landmarks, and through a sequence of controls, . As it moves, it 8:4;/ 6 is the index of the landmark perceived at 1 9. time $ . For example, in Figure 1, we have 1 <-=/ 21 -?> , observes nearby landmarks.)

7 At time , it observes landmark out of two landmarks, . The measurement is denoted . and 1A@B-C/ , since the robot first observes landmark , then landmark , and finally landmark for a second time.. (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 Many measurement models in the literature assume that the robot's path from the controls and the measurements . The gray robot can measure range and bearing to landmarks, con- shading illustrates a conditional independence relation. founded by measurement noise. The variable 1 is often . referred to as correspondence. Most theoretical work in the particle filter and is the number of landmarks. We de- literature assumes knowledge of the correspondence or, put velop a tree-based data structure that reduces the running time of FastSLAM to !

8 #" . , making it significantly differently, that landmarks are uniquely identifiable. Practi- cal implementations use maximum likelihood estimators for faster than existing EKF-based SLAM algorithms. We also estimating the correspondence on-the-fly, which work well extend the FastSLAM algorithm to situations with unknown if landmarks are spaced sufficiently far apart. In large parts data association and unknown number of landmarks, show- of this paper we will simply assume that landmarks are iden- ing that our approach can be extended to the full range of tifiable, but we will also discuss an extension that estimates SLAM problems discussed in the literature. the correspondences from data. Experimental results using a physical robot and a robot We are now ready to formulate the SLAM problem. Most simulator illustrate that the FastSLAM algorithm can han- generally, SLAM is the problem of determining the location dle orders of magnitude more landmarks than present day.

9 Of all landmarks and robot poses from measurements approaches. We also find that in certain situations, an in- .. - and controls - . In probabilis- tic terms, this is expressed by the posterior '. creased number of landmarks leads to a mild reduction of the number of particles . needed to generate accurate D(. where we use the superscript to refer to a set of variables , maps whereas in others the number of particles required from time 1 to time $ . If the correspondences are known, the for accurate mapping may be prohibitively large.. SLAM problem is simpler: ' E( 21 (3). SLAM Problem Definition The SLAM problem, as defined in the rich body of litera- As discussed in the introduction, all individual landmark es- . ture on SLAM, is best described as a probabilistic Markov timation problems are independent if one knew the robot's chain. The robot's pose at time $ will be denoted.))

10 For path and the correspondence variables 1 . This condi- robots operating in the plane which is the case in all of our tional independence is the basis of the FastSLAM algorithm experiments poses are comprised of a robot's % -& coordi- described in the next section. nate in the plane and its heading direction. Poses evolve according to a probabilistic law, often re- FastSLAM with Known Correspondences . ferred to as the motion model: We begin our consideration with the important case where ' )( +* (1) the correspondences 1 -=1 . 21 are known, and so is Thus, is a probabilistic function of the robot control . the number of landmarks observed thus far. and the previous pose + * . In mobile robotics, the motion Factored Representation model is usually a time-invariant probabilistic generalization of robot kinematics [1]. The conditional independence property of the SLAM prob.