兩足行走機器人—身體及頭部結(jié)構(gòu)部分設(shè)計,兩足行走機器人—身體及頭部結(jié)構(gòu)部分設(shè)計,行走,機器人,身體,頭部,結(jié)構(gòu),部分,部份,設(shè)計 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 mmdecs.cmu.edu, thruncs.cmu.edu Daphne Koller and Ben Wegbreit Computer Science Department Stanford University Stanford, CA 94305-9010 kollercs.stanford.edu, 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 filter-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 The problem of simultaneous localization and mapping, also known as SLAM, has attracted immense attention in 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 robot motion is subject to error, the mapping problem neces- sarily induces a robot localization problemhence the name 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 filter (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 field 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. A key limitation of EKF-based approaches is their compu- tational complexity. Sensor updates require time quadratic in the number of landmarks a0 to compute. This complex- ity stems from the fact that the covariance matrix maintained by the Kalman filters has a1a3a2 a0a5a4a7a6 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 mil- 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 a8a10a9a12a11a13a8 a4 a11a15a14a16a14a16a14a15a11a13a8a15a17 , evolve over time as a function of the robot controls, denoted a18a19a9a7a11a16a14a15a14a16a14a20a11a21a18a22a17 . Each of the land- mark measurements, denoted a23a24a9a7a11a15a14a16a14a16a14a15a11a21a23a25a17 , is a function of the position a26a10a27 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 robots path a8 a9 a11a13a8 a4 a11a15a14a16a14a15a14a16a11a13a8 a17 renders the individual landmark measure- ments independent. So for example, if an oracle provided us with the exact path of the robot, the problem of determin- ing the landmark locations could be decoupled into a0 inde- pendent estimation problems, one for each landmark. This observation was made previously by Murphy 12, who de- veloped an efficient particle filtering algorithm for learning grid maps. 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 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 modified particle filter for estimating the posterior over robot paths. Each particle possesses a0 Kalman fil- ters that estimate the a0 landmark locations conditioned on the path estimate. The resulting algorithm is an instance of the Rao-Blackwellized particle filter 5, 13. A naive im- plementation of this idea leads to an algorithm that requires a1a3a2a29a28 a0a5a6 time, where a28 is the number of particles in the . . . Figure 1: The SLAM problem: The robot moves from pose a0a2a1 through a sequence of controls, a3a4a1a6a5a7a3a9a8a10a5a12a11a6a11a6a11a6a5a13a3a15a14 . As it moves, it observes nearby landmarks. At time a16a18a17a20a19 , it observes landmark a21 a1 out of two landmarks, a22 a21 a1 a5 a21 a8a10a23 . The measurement is denoted a24 a1 (range and bearing). At time a16a25a17a26a19 , it observes the other landmark, a21 a8 , and at time a16a27a17a29a28 , it observes a21 a1 again. The SLAM problem is concerned with estimating the locations of the landmarks and the robots path from the controls a3 and the measurements a24 . The gray shading illustrates a conditional independence relation. particle filter and a0 is the number of landmarks. We de- velop a tree-based data structure that reduces the running time of FastSLAM to a1a3a2a29a28a31a30a33a32a35a34 a0 a6 , 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, 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 find that in certain situations, an in- creased number of landmarks a0 leads to a mild reduction of the number of particles a28 needed to generate accurate mapswhereas in others the number of particles required for accurate mapping may be prohibitively large. SLAM Problem Definition The SLAM problem, as defined in the rich body of litera- ture on SLAM, is best described as a probabilistic Markov chain. The robots pose at time a36 will be denoted a8 a17 . For robots operating in the planewhich is the case in all of our experimentsposes are comprised of a robots a37 -a38 coordi- nate in the plane and its heading direction. Poses evolve according to a probabilistic law, often re- ferred to as the motion model: a39 a2a29a8a15a17a41a40a7a18a22a17 a11 a8a16a17a43a42 a9 a6 (1) Thus, a8 a17 is a probabilistic function of the robot control a18 a17 and the previous pose a8 a17a43a42 a9 . In mobile robotics, the motion model is usually a time-invariant probabilistic generalization of robot kinematics 1. The robots environment possesses a0 immobile land- marks. Each landmark is characterized by its location in space, denoted a26 a27 for a44a46a45a48a47a24a11a16a14a15a14a16a14a20a11 a0 . Without loss of gen- erality, 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 local coordinate frame. The mea- surement at timea36 will be denoted a23a7a17 . While robots can often sense more than one landmark at a time, we follow com- monplace notation by assuming that sensor measurements correspond to exactly one landmark 2. This convention is adopted solely for mathematical convenience. It poses no restriction, as multiple landmark sightings at a single time step can be processed sequentially. Sensor measurements are governed by a probabilistic law, often referred to as the measurement model: a39 a2 a23 a17 a40 a8 a17 a11 a26 a11a50a49 a17 a6 (2) Here a26a51a45a53a52a15a26 a9a7a11a15a14a16a14a15a14a20a11 a26a10a27a55a54 is the set of all landmarks, and a49 a17a57a56a58a52a59a47a24a11a16a14a16a14a15a14a20a11 a0 a54 is the index of the landmark perceived at time a36 . For example, in Figure 1, we have a49 a9a60a45a61a47a24a11a50a49 a4 a45a63a62 , and a49a65a64a66a45a67a47 , since the robot first observes landmark a26a24a9 , then landmark a26 a4 , and finally landmark a26a24a9 for a 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 a49 a17 is often referred to as correspondence. Most theoretical work in the literature assumes knowledge of the correspondence or, put differently, that landmarks are uniquely identifiable. Practi- cal 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 iden- tifiable, 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 a26 and robot poses a8a25a17 from measurements a23 a17 a45 a23a10a9a7a11a15a14a16a14a16a14a16a11a21a23a25a17 and controls a18 a17 a45 a18 a9a12a11a16a14a16a14a15a14a16a11a21a18a22a17 . In probabilis- tic terms, this is expressed by the posteriora39 a2 a8 a17 a11a21a26a68a40 a23 a17 a11 a18 a17 a6 , where we use the superscript a17 to refer to a set of variables from time 1 to time a36 . If the correspondences are known, the SLAM problem is simpler: a39 a2a29a8 a17 a11 a26a69a40a12a23 a17 a11a21a18 a17 a11a50a49 a17 a6 (3) As discussed in the introduction, all individual landmark es- timation problems are independent if one knew the robots path a8 a17 and the correspondence variables a49 a17 . 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 a49 a17 a45a61a49a19a9a12a11a16a14a16a14a15a14a20a11a50a49 a17 are known, and so is the number of landmarks a0 observed thus far. Factored Representation The conditional independence property of the SLAM prob- lem implies that the posterior (3) can be factored as follows: a39 a2a29a8 a17 a11 a26a69a40a12a23 a17 a11a21a18 a17 a11a50a49 a17 a6 a45 a39 a2a29a8 a17 a40 a23 a17 a11 a18 a17 a11a50a49 a17 a6a71a70 a27 a39 a2 a26a10a27a72a40 a8 a17 a11a13a23 a17 a11 a18 a17 a11a50a49 a17 a6 (4) Put verbally, the problem can be decomposed into a0a74a73 a47 esti- mation problems, one problem of estimating a posterior over robot paths a8 a17 , and a0 problems of estimating the locations of the a0 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 a39 a2 a8 a17 a40 a23 a17 a11a21a18 a17 a11a50a49 a17 a6 using a modified particle filter 4. As we argue further below, this filter can sample efficiently from this space, providing a good approximation of the poste- rior even under non-linear motion kinematics. The land- mark pose estimators a39 a2 a26 a27 a40 a8 a17 a11a21a23 a17 a11 a18 a17 a11 a49 a17 a6 are realized by Kalman filters, using separate filters for different landmarks. Because the landmark estimates are conditioned on the path estimate, each particle in the particle filter has its own, lo- cal landmark estimates. Thus, for a28 particles and a0 land- marks, there will be a total of a0 a28 Kalman filters, 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 filter for estimating the path posteriora39 a2a29a8 a17 a40a12a23 a17 a11 a18 a17 a11 a49 a17 a6 in (4), using a filter that is similar (but not identical) to the Monte Carlo localization (MCL) algorithm 1. MCL is an application of particle filter to the problem of robot pose estimation (localization). At each point in time, both algorithms maintain a set of particles rep- resenting the posterior a39 a2 a8 a17 a40 a23 a17 a11a21a18 a17 a11a50a49 a17 a6 , denoted a1 a17 . Each particle a8 a17a3a2a5a4a6a8a7 a56 a1 a17 represents a “guess” of the robots path: a1 a17 a45 a52a7a8 a17a3a2a5a4a6a9a7 a54 a6 a45 a52a12a8 a4a6a9a7 a9 a11a13a8 a4a6a8a7 a4 a11a16a14a15a14a16a14a20a11 a8 a4a6a9a7 a17 a54 a6 (5) We use the superscript notation a4a6a8a7 to refer to the a10 -th par- ticle in the set. The particle set a1 a17 is calculated incrementally, from the set a1 a17a43a42 a9 at time a36a12a11a68a47 , a robot control a18 a17 , and a measurement a23 a17 . First, each particle a8 a17a3a2a5a4a6a9a7 in a1 a17a43a42 a9 is used to generate a probabilistic guess of the robots pose at time a36 : a8 a4a6a9a7 a17 a13 a39 a2a29a8 a17 a40a12a18 a17 a11 a8 a4a6a9a7 a17a43a42 a9 a6 a11 (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 a8 a17a43a42 a9a14a2a5a4a6a9a7 . Under the assumption that the set of particles in a1 a17a43a42 a9 is distributed according to a39 a2 a8 a17a43a42 a9 a40a24a23 a17a43a42 a9 a11 a18 a17a43a42 a9 a11 a49 a17a43a42 a9 a6 (which is an asymptotically cor- rect approximation), the new particle is distributed accord- ing to a39 a2a29a8 a17 a40a24a23 a17a43a42 a9 a11a21a18 a17 a11a50a49 a17a43a42 a9 a6 . This distribution is commonly referred to as the proposal distribution of particle filtering. After generating a28 particles in this way, the new set a1 a17 is obtained by sampling from the temporary particle set. Each particle a8 a17a3a2a5a4a6a9a7 is drawn (with replacement) with a probability proportional to a so-called importance factor a15 a4a6a9a7 a17 , which is calculated as follows 10: a15 a4a6a9a7 a17 a45 target distribution proposal distribution a45 a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17 a11a21a18 a17 a11a50a49 a17 a6 a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17a43a42 a9 a11a21a18 a17 a11a50a49 a17a43a42 a9 a6 (7) The exact calculation of (7) will be discussed further below. The resulting sample set a1 a17 is distributed according to an ap- proximation to the desired pose posterior a39 a2a29a8 a17 a40a22a23 a17 a11 a18 a17 a11a50a49 a17 a6 , an approximation which is correct as the number of particles a28 goes to infinity. We also notice that only the most recent robot pose estimate a8 a4a6a8a7 a17a43a42 a9 is used when generating the parti- cle set a1 a17 . This will allows us to silently “forget” all other pose estimates, rendering the size of each particle indepen- dent of the time index a36 . Landmark Location Estimation FastSLAM represents the conditional landmark estimates a39 a2 a26a10a27a20a40 a8 a17 a11a21a23 a17 a11a21a18 a17 a11 a49 a17 a6 in (4) by Kalman filters. Since this estimate is conditioned on the robot pose, the Kalman filters are attached to individual pose particles in a1 a17 . More specifi- cally, the full posterior over paths and landmark positions in the FastSLAM algorithm is represented by the sample set a16 a17a67a45 a52a12a8 a17a3a2a5a4a6a9a7 a11a18a17 a4a6a8a7 a9 a11a14a19 a4a6a9a7 a9 a11a15a14a16a14a15a14a20a11a20a17 a4a6a9a7 a21 a11a22a19 a4a6a8a7 a21 a54a23a6 (8) Here a17 a4a6a9a7 a27 and a19 a4a6a9a7 a27 are mean and covariance of the Gaus- sian representing the a44 -th landmark a26 a27 , attached to the a10 -th particle. In the planar robot navigation scenario, each mean a17 a4a6a9a7 a27 is a two-element vector, and a19 a4a6a9a7 a27 is a 2 by 2 matrix. The posterior over the a44 -th landmark pose a26 a27 is easily ob- tained. Its computation depends on whether or not a49 a17 a45a63a44 , that is, whether or not a26 a27 was observed at time a36 . For a49 a17 a45 a44 , we obtain a39 a2 a26 a27 a40a24a8 a17 a11a13a23 a17 a11 a18 a17 a11a50a49 a17 a6 (9) a24a26a25a28a27a14a29a28a30 a31 a39 a2a29a23a15a17a41a40a7a26a10a27 a11a13a8 a17 a11a21a23 a17a43a42 a9 a11 a18 a17 a11 a49 a17 a6 a39 a2 a26a10a27 a40a10a8 a17 a11a21a23 a17a43a42 a9 a11a21a18 a17 a11 a49 a17 a6 a32a33a25a35a34a37a36a35a38a28a39 a45 a39 a2a29a23a15a17a41a40a7a26a10a27 a11a13a8a15a17 a11a50a49 a17 a6 a39 a2 a26a10a27 a40a10a8 a17a43a42 a9 a11a13a23 a17a43a42 a9 a11 a18 a17a43a42 a9 a11a50a49 a17a43a42 a9 a6 For a49 a17a41a40a45 a44 , we simply leave the Gaussian unchanged: a39 a2 a26a10a27 a40 a8 a17 a11a21a23 a17 a11a21a18 a17 a11 a49 a17 a6 a45 a39 a2 a26a10a27 a40 a8 a17a43a42 a9 a11a21a23 a17a43a42 a9 a11a21a18 a17a43a42 a9 a11 a49 a17a43a42 a9 a6 (10) The FastSLAM algorithm implements the update equation (9) using the extended Kalman filter (EKF). As in existing EKF approaches to SLAM, this filter uses a linearized ver- sion of the perceptual model a39 a2 a23a7a17a26a40 a8a16a17 a11a21a26 a11a50a49 a17 a6 2. Thus, FastSLAMs EKF is similar to the traditional EKF for SLAM 15 in that it approximates the measurement model using a linear Gaussian function. We note that, with a lin- ear Gaussian observation model, the resulting distribution a39 a2 a26 a27 a40 a8 a17 a11a21a23 a17 a11a21a18 a17 a11 a49 a17 a6 is exactly a Gaussian, even if the mo- tion model is not linear. This is a consequence of the use of sampling to approximate the distribution over the robots pose. One significant difference between the FastSLAM algo- rithms use of Kalman filters and that of the traditional SLAM algorithm is that the updates in the FastSLAM algo- rithm involve only a Gaussian of dimension two (for the two landmark location parameters), whereas in the EKF-based SLAM approach a Gaussian of size a62 a0a29a73a43a42 has to be updated (with a0 landmarks and 3 robot pose parameters). This cal- culation can be done in constant time in FastSLAM, whereas it requires time quadratic in a0 in standard SLAM. Calculating the Importance Weights Let us now return to the problem of calculating the impor- tance weights a15 a4a6a9a7 a17 needed for particle filter resampling, as defined in (7): a15 a4a6a8a7 a17 a31 a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17 a11a21a18 a17 a11 a49 a17 a6 a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17a43a42 a9 a11a21a18 a17 a11 a49 a17a43a42 a9 a6 a24a26a25a28a27a14a29a28a30 a45 a39 a2 a23a25a17 a11a50a49 a17a41a40a10a8 a17a3a2a5a4a6a8a7 a11a21a23 a17a43a42 a9 a11 a18 a17 a11 a49 a17a43a42 a9 a6 a39 a2a29a23 a17 a11 a49 a17 a40a12a23 a17a43a42 a9 a11a21a18 a17 a11 a49 a17a43a42 a9 a6 8,87,7 k 7 ? FT 6,65,5 k 5 ? FT 4,43,3 k 3 ? FT 2,21,1 k 1 ? FT k 6 ? FT k 2 ? FT k 4 ? FT mm mm mm mm mm mm mmmm Figure 2: A tree representing a0 a17a2a1 landmark estimates within a single particle. a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17a43a42 a9 a11a21a18 a17 a11a50a49 a17 a6 a39 a2a29a8 a17a3a2a5a4a6a9a7 a40a12a23 a17a43a42 a9 a11a21a18 a17 a11a50a49 a17 a6 a45 a39 a2 a23 a17 a11a50a49 a17 a40 a8 a17a3a2a5a4a6a9a7 a11a13a23 a17a43a42 a9 a11a21a18 a17 a11a50a49 a17a43a42 a9 a6 a39 a2 a23a25a17 a11a50a49 a17 a40a10a23 a17a43a42 a9 a11 a18 a17 a11a50a49 a17a43a42 a9 a6 a31 a39 a2 a23 a17 a11a50a49 a17 a40 a8 a1