3 外文原文.pdf

1、FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem Michael Montemerlo and Sebastian Thrun School of Computer Science Carnegie Mellon University Pittsburgh, PA 15213 , Daphne Koller and Ben Wegbreit Computer Science Department Stanford Uni

2、versity Stanford, CA 94305-9010 , Abstract The ability to simultaneously localize a robot and ac- curately map its surroundings is considered by many to be a key prerequisite of truly autonomous robots. How- ever, few approaches to this problem scale up to handle the very large

3、number of landmarks present in real envi- ronments. Kalman fi lter-based algorithms, for example, require time quadratic in the number of landmarks to in- corporate each sensor observation. This paper presents FastSLAM, an algorithm that recursively estimates the full posterior distribution over rob

4、ot pose and landmark locations, yet scales logarithmically with the number of landmarks in the map. This algorithm is based on an ex- act factorization of the posterior into a product of con- ditional landmark distributions and a distribution over robot paths. The algorithm has been run successfully

5、 on as many as 50,000 landmarks, environments far be- yond the reach of previous approaches. Experimental results demonstrate the advantages and limitations of the FastSLAM algorithm on both simulated and real- world data. Introduction Theproblemofsimultaneouslocalizationandmapping,also knownas SLAM

6、, has attractedimmenseattentionin the mo- bile robotics literature.SLAM addresses the problem of building a map of an environment from a sequence of land- mark measurements obtained from a moving robot. Since robotmotionis subject to error,the mappingproblemneces- sarilyinducesarobotlocalizationprob

7、lemhencethename SLAM. The ability to simultaneously localize a robot and accurately map its environment is considered by many to be a key prerequisite of truly autonomous robots 3, 7, 16. The dominant approach to the SLAM problem was in- troduced in a seminal paper by Smith, Self, and Cheese- man 15

8、.This paper proposed the use of the extended Kalman fi lter (EKF) for incrementally estimating the poste- rior distribution over robot pose along with the positions of the landmarks. In the last decade, this approach has found widespread acceptance in fi eld robotics, as a recent tutorial paper 2 do

9、cuments. Recent research has focused on scal- ing this approach to larger environments with more than a few hundred landmarks 6, 8, 9 and to algorithms for han- dling data association problems 17. AkeylimitationofEKF-basedapproachesistheircompu- tational complexity. Sensor updates require time quadr

10、atic in the number of landmarks ? to compute. This complex- ity stems fromthe fact thatthe covariancematrixmaintained bytheKalmanfi ltershas? ? elements,all ofwhichmust be updated even if just a single landmark is observed. The quadratic complexity limits the number of landmarks that can be handled

11、by this approach to only a few hundred whereasnaturalenvironmentmodelsfrequentlycontainmil- lions of features. This shortcoming has long been recog- nized by the research community 6, 8, 14. In this paper we approach the SLAM problem from a Bayesian point of view. Figure 1 illustrates a generative p

12、robabilistic model (dynamic Bayes network) that underlies the rich corpus of SLAM literature. In particular, the robot poses, denoted? ? ? ? ?, evolve over time as a function of the robot controls, denoted?. Each of the land- mark measurements, denoted?, is a function of the position of the landmark

13、 measured and of the robot pose at the time the measurementwas taken. From this diagramit is evident that the SLAM problem exhibits important condi- tionalindependences. Inparticular,knowledgeoftherobots path ? ? ? ? ? rendersthe individuallandmarkmeasure- ments independent. So for example, if an or

14、acle providedus with the exact path of the robot, the problem of determin- ing the landmark locations could be decoupled into ? inde- pendent estimation problems, one for each landmark. This observation was made previously by Murphy 12, who de- veloped an effi cient particle fi ltering algorithm for

15、 learning grid maps. Basedonthisobservation,this paperdescribesaneffi cient SLAM algorithmcalled FastSLAM. FastSLAM decomposes the SLAM problem into a robot localization problem, and a collection of landmark estimation problems that are con- ditioned on the robot pose estimate. As remarked in 12, th

16、is factored representation is exact, due to the natural con- ditional independences in the SLAM problem. FastSLAM uses a modifi ed particle fi lter for estimating the posterior over robot paths. Each particle possesses ? Kalman fi l- ters that estimate the ? landmark locations conditioned on the pat

17、h estimate. The resulting algorithm is an instance of the Rao-Blackwellized particle fi lter 5, 13. A naive im- plementation of this idea leads to an algorithm that requires ? ? time, where is the number of particles in the s1s 2 s t u 2 u t 2 1 z 1 z 2 s 3 u 3 z 3 z t .?.?. Figure 1: The SLAM probl

18、em: The robot moves from pose? through a sequence of controls, ? ? ?. As it moves, it observes 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 ?

19、again. The SLAM problem is concerned with estimating the locations of the landmarks and the robots path from the controls ? and the measurements . The gray shading illustrates a conditional independence relation. particle fi lter and ? is the number of landmarks. We de- velop a tree-based data struc

20、ture that reduces the running time of FastSLAM to?! # ? ? , making it signifi cantly faster than existing EKF-based SLAM algorithms. We also extend the FastSLAM algorithm to situations with unknown data association and unknown number of landmarks, show- ing that our approach can be extended to the f

21、ull range of SLAM problems discussed in the literature. Experimental results using a physical robot and a robot simulator illustrate that the FastSLAM algorithm can han- dle orders of magnitude more landmarks than present day approaches. We also fi nd that in certain situations, an in- creased numbe

22、r of landmarks ? leads to a mild reduction of the number of particles needed to generate accurate mapswhereas in others the number of particles required for accurate mapping may be prohibitively large. SLAM Problem Defi nition The SLAM problem, as defi ned in the rich body of litera- ture on SLAM, i

23、s best described as a probabilistic Markov chain. The robots pose at time $ will be denoted ? . For robots operating in the planewhichis the case in all of our experimentsposes are comprised of a robots % - / ? 6 is the index of the landmark perceived at time $ . For example, in Figure 1, we have 1

24、?, and1AB-C/ , since the robot fi rst observes landmark?, thenlandmark ? , and fi nally landmark?fora second time. Many measurement models in the literature assume that the robot can measure range and bearing to landmarks, con- founded by measurement noise. The variable 1 is often referred to as cor

25、respondence. Most theoretical work in the literature assumes knowledge of the correspondence or, put differently, that landmarks are uniquely identifi able. Practi- cal implementationsuse maximum likelihood estimators for estimating the correspondence on-the-fl y, which work well if landmarks are sp

26、aced suffi ciently far apart. In large parts ofthis paperwe will simplyassume that landmarksareiden- tifi able, but we will also discuss an extension that estimates the correspondences from data. We are now ready to formulate the SLAM problem. Most generally, SLAM is the problem of determining the l

27、ocation of all landmarks and robot poses ?from measurements - ?and controls - ?. In probabilis- tic terms, this is expressed by the posterior ? ? D( ? , where we use the superscript to refer to a set of variables from time 1 to time $ . If the correspondencesare known, the SLAM problem is simpler: ?

28、 E( 21 ? (3) As discussed in the introduction,all individual landmark es- timation problems are independent if one knew the robots path ? and the correspondence variables 1 . This condi- tional independence is the basis of the FastSLAM algorithm described in the next section. FastSLAM with Known Cor

29、respondences We begin our consideration with the important case where the correspondences 1 -=1?21 are known, and so is the number of landmarks ? observed thus far. Factored Representation The conditional independence property of the SLAM prob- lem implies that the posterior (3) can be factored as f

30、ollows: ? E( 21 ? - ? ( 21 ?GF ? H( ? 21 ? (4) Putverbally,theproblemcanbedecomposedinto ?JI / esti- mationproblems,oneproblemofestimating a posteriorover robot paths ? , and ? problems of estimating the locations of the ? landmarks conditioned on the path estimate. This factorization is exact and a

31、lways applicable in the SLAM problem, as previously argued in 12. The FastSLAM algorithm implements the path estimator ? ? ( 21 ? using a modifi ed particle fi lter 4. As we argue further below, this fi lter can sample effi ciently from this space, providing a good approximation of the poste- rior e

32、ven under non-linear motion kinematics. The land- mark pose estimators ? ( ? 1 ? are realized by Kalman fi lters, using separate fi lters for differentlandmarks. Because the landmark estimates are conditioned on the path estimate, each particle in the particle fi lter has its own, lo- cal landmark e

33、stimates. Thus, for particles and ? land- marks, there will be a total of ? Kalman fi lters, each of dimension 2 (for the two landmark coordinates). This repre- sentation will now be discussed in detail. Particle Filter Path Estimation FastSLAM employs a particle fi lter for estimating the path post

34、erior ? ( 1 ? in (4), using a fi lter that is similar (but not identical) to the Monte Carlo localization (MCL) algorithm 1. MCL is an application of particle fi lter to the problem of robot pose estimation (localization). At each pointin time, both algorithmsmaintain a set of particles rep- resenti

35、ng the posterior ? ? ( 21 ? , denoted ? . Each particle ? ? 8 ? represents a “guess” of the robots path: ? - 4? ? 6 ? - 4? ? ? ? ? ? ? ? ? ? ? 6 ? (5) We use the superscript notation ? ? to refer to the -th par- ticle in the set. The particle set ? is calculated incrementally, from the set ? +* ? at

36、 time$D/, a robot control , and a measurement . First, each particle ? ? in ? +* ? is used to generate a probabilistic guess of the robots pose at time $ : ? ? ? ? ( ? ? ? +* ? ? (6) obtained by sampling from the probabilistic motion model. This estimate is then added to a temporary set of parti- cl

37、es, along with the path ? +*? . Under the assumption that the set of particles in ? +* ? is distributed according to ? ? +* ? ( +* ? +* ? 1 +* ? ? (which is an asymptotically cor- rect approximation), the new particle is distributed accord- ing to ? ( +* ? 21 +* ? ? . This distribution is commonly r

38、eferred to as the proposal distribution of particle fi ltering. After generating particles in this way, the new set ? is obtained by sampling from the temporary particle set. Each particle ? ? is drawn(with replacement)witha probability proportionalto a so-called importance factor ? ? , which is cal

39、culated as follows 10: ? ? - target distribution proposal distribution - ? ? ( 21 ? ? ? ( +* ? 21 +* ? ? (7) The exact calculation of (7) will be discussed further below. The resultingsampleset ? is distributedaccordingto anap- proximation to the desired pose posterior ? ( 21 ? , an approximationwhi

40、chis correctas the numberof particles goes to infi nity. We also notice that only the most recent robot pose estimate ? ? ? +* ? is used when generating the parti- cle set ? . This will allows us to silently “forget” all other pose estimates, rendering the size of each particle indepen- dent of the

41、time index $ . Landmark Location Estimation FastSLAM represents the conditional landmark estimates ? ( ? 1 ? in (4) by Kalman fi lters. Since this estimate is conditioned on the robot pose, the Kalman fi lters are attached to individual pose particles in ? . More specifi - cally, the full posterior

42、over paths and landmark positions in the FastSLAM algorithm is represented by the sample set C-4? ? ? ? ? ? ? ? ? ? ? ? 6?(8) Here ? ? and ? ? are mean and covariance of the Gaus- sian representing the , -th landmark , attached to the -th particle. In the planar robot navigation scenario, each mean

43、? ? is a two-element vector, and ? ? is a 2 by 2 matrix. The posterior over the , -th landmarkpose is easily ob- tained. Its computation depends on whether or not 1 -?, thatis, whetherornot was observedattime $ . For 1 - , , we obtain ? (? 21 ? (9) ?)(? ? +* ? 1 ? ? ( ? +* ? 1 ? !#%$# every time a p

44、article is added to , its has to be copied. Since each particle contains ? landmark esti- mates, this copyingprocedurerequires ? ? time. How- ever, most of this copying can be avoided. Our approach makes it possible to execute a FastSLAM iteration in?! # ? ? time. The basic idea is that the set ofGa

45、ussians in eachparticleis representedbya balancedbi- nary tree. Figure 2 shows such a tree for a single particle, in the case of 8 landmarks. The Gaussian parameters ? ? and 8,87,7 k? 3? FT 6,65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 8,87,7 k? 3? FT 6,

46、65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 3,3 k? 3? FT k? 2? F T k? 4? F T mm new?particle old?particle 8,87,7 k? 3? FT 6,65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 8,87,7 k? 3? FT 6,65,5 k? 1? FT 4,43,3 k

47、? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 8,87,7 k? 3? FT 6,65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 8,87,7 k? 3? FT 6,65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 8,87,7 k? 3? FT

48、6,65,5 k? 1? FT 4,43,3 k? 3? FT 2,21,1 k? 1? FT k? 6? FT k? 2? FT k? 4? FT mmmmmmmmmmmmmmmm 3,3 k? 3? FT k? 2? F T k? 4? F T mm new?particle old?particle Figure 3: Generating a new particle from an old one, while modi- fying only a single Gaussian. The new particle receives only a par- tial tree, co

49、nsisting of a path to the modifi ed Gaussian. All other pointers are copied from the generating tree. ? ? are located at the leaves of the tree. Clearly, accessing each Gaussian requires time logarithmic in ? . Suppose FastSLAM incorporates a new control and a new measurement . Each new particle in

50、will differ from the corresponding one in +* ? in two ways: First, it will possess a different path estimate obtained via (6), and second, the Gaussian with index 1 will be different in ac- cordance with (9). All other Gaussians will be equivalent to the generating particle. When copying the particl

51、e, thus, only a single path has to be modifi ed in the tree representing all Gaussians. An example is shown in Figure 3: Here we assume 1 - * , that is, only the Gaussian parameters ? ? and ? ? are updated. Instead of generatingan entirely new tree, only a single path is created, leading to the Gaus

52、sian 1 - * . 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 (unmodifi ed) subtree as that of the generating tree. Cl

53、early, generating such an incompletetree takesonlytimelogarithmicin ? . Moreover, accessing a Gaussian also takes time logarithmicin ? , 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 defi nition logarithmic). Thus, both gene

54、rating 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 ?! # ? . Data Association In many real-world problems, landmarks are not identifi - able, and the total number of landmarks ? cannot be ob- tained

55、triviallyas was the case above. In such situations, the robot has to solve a data association problem between momentary landmarks sightingsand the set of landmarks in the map . It also has to determine if a measurement cor- responds to a new, previously unseen landmark, in which (a)(b)(c) Figure 4:

56、(a) Physical robot mapping rocks, in a testbed developed for Mars Rover research. (b) Raw range and path data. (c) Map generated using FastSLAM (dots), and locations of rocks determined manually (circles). case the map should be augmented accordingly. In most existing SLAM solutions based on EKFs, t

57、hese problems are solved via maximum likelihood. More specif- ically, the probability of a data association 1 is given by ? 1 )( ? - ? ? 1 ( ? ? ? ( ? ? ? ? ? ? 1 )( ? ? ? !#%$#& - ? ? ? 1 )( ? ? ? ? # ? ? ? ( ? ? ? 1 ? (12) The step labeled “PF” uses the particle fi lter approxima- tion to the post

58、erior ? ? ( ? . The fi nal step assumes a uniform prior ? 1 ( ? ? , which is commonly used 2. The maximum likelihood data association is simply the in- dex 1 that maximizes (12).If the maximum value of ? 1 D( ? with careful consideration of all constants in (12)is below a threshold ? , the landmark is considered previously unseen and the map is augmented accordingly. In FastSLAM, the data association is estimat