雙足步行機(jī)器人頭部及身體結(jié)構(gòu)的設(shè)計(jì)-3 外文原文.pdf

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 University 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 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 robot 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 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, 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- sarilyinducesarobotlocalizationproblemhencethename 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.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 documents. 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 quadratic 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 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 probabilistic 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 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 oracle 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 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, this 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 path 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 problem: 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 ? 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 structure 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 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 han- dle orders of magnitude more landmarks than present day approaches. We also fi nd that in certain situations, an in- creased number 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, is 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 ?, 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 correspondence. 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 spaced 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 location 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: ? 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 Correspondences 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 follows: ? 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 always 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 even 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 estimates. 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 posterior ? ( 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- resenting 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 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- cles, 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 referred 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 calculated 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 approximationwhichis 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 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 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 ? ? 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 particle 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 ofGaussians 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,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? 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 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, consisting 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 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 particle, 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 Gaussian 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. Clearly, 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 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 ?! #“ ? . Data Association In many real-world problems, landmarks are not identifi - able, and the total number of landmarks ? cannot be ob- tained triviallyas 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 cor- responds to a new, previously unseen landmark, in which (a)(b)(c) Figure 4: (a) Physical robot mapping rocks, in a testbed developed for Mars Rover research. (b) Raw range and path data. (c) Map gene