A Solution to the Simultaneous Localization and Map Building (SLAM) Problem

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 3, JUNE 2001 229 A Solution to the Simultaneous Localization and Map Building (SLAM) Problem M. W. M. Gamini Dissanayake, Member, IEEE, Paul Newman, Member, IEEE, Steven Clark, Hugh F. Durrant-Whyte, Member, IEEE, and M. Csorba Abstract—The simultaneous localization and map building (SLAM) problem asks if it is possible for an autonomous vehicle to start in an unknown location in an unknown environment and then to incrementally build
of 13
All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.
Related Documents
  IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 3, JUNE 2001 229 A Solution to the Simultaneous Localization andMap Building (SLAM) Problem M. W. M. Gamini Dissanayake  , Member, IEEE  , Paul Newman  , Member, IEEE  , Steven Clark,Hugh F. Durrant-Whyte  , Member, IEEE  , and M. Csorba  Abstract— The simultaneous localization and map building(SLAM) problem asks if it is possible for an autonomous vehicleto start in an unknown location in an unknown environment andthen to incrementally build a map of this environment while si-multaneously using this map to compute absolute vehicle location.Starting from the estimation-theoretic foundations of this problemdeveloped in [1]–[3], this paper proves that a solution to theSLAM problem is indeed possible. The underlying structure of theSLAM problem is first elucidated. A proof that the estimated mapconverges monotonically to a relative map with zero uncertaintyis then developed. It is then shown that the absolute accuracy of the map and the vehicle location reach a lower bound definedonly by the initial vehicle uncertainty. Together, these resultsshow that it is possible for an autonomous vehicle to start inan unknown location in an unknown environment and, usingrelative observations only, incrementally build a perfect map of the world and to compute simultaneously a bounded estimate of vehicle location. This paper also describes a substantial imple-mentation of the SLAM algorithm on a vehicle operating in anoutdoor environment using millimeter-wave (MMW) radar toprovide relative map observations. This implementation is used todemonstrate how some key issues such as map management anddata association can be handled in a practical environment. Theresults obtained are cross-compared with absolute locations of themap landmarks obtained by surveying. In conclusion, this paperdiscusses a number of key issues raised by the solution to theSLAM problem including suboptimal map-building algorithmsand map management.  Index Terms— Autonomous navigation, millimeter wave radar,simultaneous localization and map building. I. I NTRODUCTION T HE solution to the simultaneous localization and mapbuilding (SLAM) problem is, in many respects, a “HolyGrail” of the autonomous vehicle research community. Theability to place an autonomous vehicle at an unknown locationin an unknown environment and then have it build a map, usingonly relative observations of the environment, and then to usethis map simultaneously to navigate would indeed make sucha robot “autonomous”. Thus the main advantage of SLAMis that it eliminates the need for artificial infrastructures or a priori topological knowledge of the environment. A solutionto the SLAM problem would be of inestimable value in a Manuscript received March 22, 1999; revised September 15, 2000. Thispaper was recommended for publication by Associate Editor N. Xi and EditorV. Lumelsky upon evaluation of the reviewers’ comments.The authors are with the Department of Mechanical and Mechatronic Engi-neering, Australian Centre for Field Robotics, The University of Sydney, NSW2006, Australia (e-mail: Item Identifier S 1042-296X(01)06731-3. range of applications where absolute position or precise mapinformation is unobtainable, including, amongst others, au-tonomous planetary exploration, subsea autonomous vehicles,autonomous air-borne vehicles, and autonomous all-terrainvehicles in tasks such as mining and construction.The general SLAM problem has been the subject of sub-stantial research since the inception of a robotics researchcommunity and indeed before this in areas such as mannedvehicle navigation systems and geophysical surveying. Anumber of approaches have been proposed to address both theSLAM problem and also more simplified navigation problemswhere additional map or vehicle location information is madeavailable. Broadly, these approaches adopt one of three mainphilosophies. The most popular of these is the estimation-the-oretic or Kalman filter based approach. The popularity of thisapproach is due to two main factors. First, it directly providesboth a recursive solution to the navigation problem and ameans of computing consistent estimates for the uncertainty invehicle and map landmark locations on the basis of statisticalmodels for vehicle motion and relative landmark observations.Second, a substantial corpus of method and experience hasbeen developed in aerospace, maritime and other navigationapplications, from which the autonomous vehicle communitycan draw. A second philosophy is to eschew the need forabsolute position estimates and for precise measures of uncer-tainty and instead to employ more qualitative knowledge of therelative location of landmarks and vehicle to build maps andguide motion. This general philosophy has been developed bya number of different groups in a number of different ways; see[4]–[6]. The qualitative approach to navigation and the generalSLAM problem has many potential advantages over the esti-mation-theoretic methodology in terms of limiting the need foraccurate models and the resulting computational requirements,and in its significant “anthropomorphic appeal”. The third,very broad philosophy does away with the rigorous Kalmanfilter or statistical formalism while retaining an essentiallynumerical or computational approach to the navigation andSLAM problem. Such approaches include the use of iconiclandmark matching [7], global map registration [8], boundedregions [9] and other measures to describe uncertainty. Notableare the work by Thrun et al. [10] and Yamauchi et al. [11].Thrun et al. use a bayesian approach to map building thatdoes not assume Gaussian probability distributions as requiredby the Kalman filter. This technique, while very effective forlocalization with respect to maps, does not lend itself to providean incremental solution to SLAM where a map is graduallybuilt as information is received from sensors. Yamauchi et al. 1042–296X/01$10.00 © 2001 IEEE AlultIXDoM1a1UfIX Ra  230 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 3, JUNE 2001 use a evidence grid approach that requires that the environmentis decomposed to a number of cells.An estimation-theoretic or Kalman filter based approach tothe SLAM problem is adopted in this paper. A major advantageof thisapproach is thatit is possible todevelop acompleteproof ofthevariouspropertiesoftheSLAMproblemandtostudysys-tematically the evolution of the map and the uncertainty in themap and vehicle location. A proof of existenceand convergencefor a solution of the SLAM problem within a formal estima-tion-theoretic framework also encompasses the widest possiblerange of navigation problems and implies that solutions to theproblem using other approaches are possible.The study of estimation-theoretic solutions to the SLAMproblem within the robotics community has an interestinghistory. Initial work by Smith et al. [12] and Durrant-Whyte[13] established a statistical basis for describing relationshipsbetween landmarks and manipulating geometric uncertainty.A key element of this work was to show that there must be ahigh degree of correlation between estimates of the location of different landmarks in a map and that indeed these correlationswould grow to unity following successive observations. At thesame time Ayache and Faugeras [14] and Chatila and Laumond[15] were undertakingearlywork invisual navigationofmobilerobots using Kalman filter-type algorithms. These two strandsof research had much in common and resulted soon after inthe key paper by Smith, Self and Cheeseman [1]. This papershowed that as a mobile robot moves through an unknownenvironment taking relative observations of landmarks, theestimates of these landmarks are all necessarily correlatedwith each other because of the common error in estimatedvehicle location. This paper was followed by a series of relatedwork developing a number of aspects of the essential SLAMproblem ([2] and [3], for example). The main conclusion of thiswork was twofold. First, accounting for correlations betweenlandmarks in a map is important if filter consistency is to bemaintained. Second, that a full SLAM solution requires that astate vector consisting of all states in the vehicle model and  allstates of every landmark in the map needs to be maintained andupdated following each observation if a complete solution tothe SLAM problem is required. The consequence of this in anyreal application is that the Kalman filter needs to employ a hugestatevector(oforderthenumberoflandmarksmaintainedinthemap) and is in general, computationally intractable. Crucially,this work did not look at the convergence properties of the mapor its steady-state behavior. Indeed, it was widely assumed atthe time that the estimated map errors would not converge andwould instead execute a random walk behavior with unboundederrorgrowth.GiventhecomputationalcomplexityoftheSLAMproblem and without knowledge of the convergence behaviorof the map, a series of approximations to the full SLAMsolution were proposed which assumed that the correlationsbetween landmarks could be minimized or eliminated thusreducing the full filter to a series of decoupled landmark tovehicle filters (see Renken [16], Leonard and Durrant-Whyte[3] for example). Also for these reasons, theoretical work onthe full estimation-theoretic SLAM problem largely ceased,with effort instead being expended in map-based navigationand alternative theoretical approaches to the SLAM problem.This paper starts from the srcinal estimation-theoretic work of Smith, Self and Cheeseman. It assumes an autonomous ve-hicle (mobile robot) equipped with a sensor capable of makingmeasurements of the location of landmarks relative to the ve-hicle. The landmarks may be artificial or natural and it is as-sumed that the signal processing algorithms are available to de-tect these landmarks. The vehicle starts at an unknown locationwith no knowledge of the location of landmarks in the environ-ment. As the vehicle moves through the environment (in a sto-chastic manner) it makes relative observations of the locationof individual landmarks. This paper then proves the followingthree results.1) The determinant of any submatrix of the map covariancematrix decreases monotonically as observations are suc-cessively made.2) In the limit as the number of observations increases, thelandmark estimates become fully correlated.3) In the limit, the covariance associated with any singlelandmark location estimate is determined only by the ini-tial covariance in the vehicle location estimate.These three results describe, in full, the convergence proper-ties of the map and its steady state behavior. In particular theyshow the following.ã The entire structure of the SLAM problem criticallydepends on maintaining complete knowledge of the crosscorrelation between landmark estimates. Minimizing orignoring cross correlations is precisely contrary to thestructure of the problem.ã As the vehicle progresses through the environment the er-rorsintheestimatesofanypairoflandmarksbecomemoreand more correlated, and indeed never become less corre-lated.ã In the limit, the errors in the estimates of any pair of land-marks becomes fully correlated. This means that giventheexact location of any one landmark, the location of anyother landmark in the map can also be determined withabsolute certainty.ã As the vehicle moves through the environment taking ob-servations of individual landmarks, the error in the esti-mates of the relative location between different landmarksreduces monotonically to the point where the map of rel-ative locations is known with absolute precision.ã As themapconvergesin theabovemanner, theerror intheabsolute location of every landmark (and thus the wholemap) reaches a lower bound determined only by the errorthat existed when the first observation was made.Thus a solution to the general SLAM problem exists and it isindeedpossibletoconstructaperfectlyaccuratemapand simul-taneously compute vehicle position estimates without any priorknowledge of vehicle or landmark locations.This papermakes three principal contributions tothe solutionof the SLAM problem. First, it proves three key convergenceproperties of the full SLAM filter. Second, it elucidates the truestructure of the SLAM problem and shows how this can be usedin developing consistent SLAM algorithms. Finally, it demon-strates and evaluates the implementation of the full SLAM al- AlultIXDoM1a1UfIX Ra  DISSANAYAKE et al. : A SOLUTION TO THE SLAM PROBLEM 231 Fig. 1. A vehicle taking relative measurements to environmental landmarks. gorithm in an outdoor environment using a millimeter wave(MMW) radar sensor.Section II of this paper introduces the mathematical struc-tureoftheestimation-theoreticSLAMproblem.SectionIIIthenproves and explains the three convergence results. Section IVprovides a practical demonstration of an implementation of thefull SLAM algorithm in an outdoor environment using MMWradar to provide relative observations of landmarks. An algo-rithm addressing pertinent issues of map initialization and man-agement is also presented. The algorithm outputs are shown toexhibit the convergent properties derived in Section III. Sec-tion V discusses the many remaining problems with obtaining apractical, large scale solution to the SLAM problem includingthedevelopmentofsuboptimalsolutions,mapmanagement,anddata association.II. T HE E STIMATION -T HEORETIC SLAM P ROBLEM This section establishes the mathematical framework em-ployed in the study of the SLAM problem. This framework is identical in all respects to that used in Smith et al. [1]and uses the same notation as that adopted in Leonard andDurrant-Whyte [3].  A. Vehicle and Landmark Models The setting for the SLAM problem is that of a vehicle witha known kinematic model, starting at an unknown location,moving through an environment containing a population of features or landmarks. The terms feature and landmark will beused synonymously. The vehicle is equipped with a sensor thatcan take measurements of the relative location between any in-dividual landmark and the vehicle itself as shown in Fig. 1. Theabsolute locations of the landmarks are not available. Withoutprejudice, a linear (synchronous) discrete-time model of theevolution of the vehicle and the observations of landmarksis adopted. Although vehicle motion and the observation of landmarks is almost always nonlinear and asynchronous in anyreal navigation problem, the use of linear synchronous modelsdoes not affect the validity of the proofs in Section III otherthan to require the same linearization assumptions as thosenormally employed in the development of an extended Kalmanfilter. Indeed, the implementation of the SLAM algorithmdescribed in Section IV uses nonlinear vehicle models andnonlinear asynchronous observation models. The state of thesystem of interest consists of the position and orientation of thevehicle together with the position of all landmarks. The state of the vehicle at a time step is denoted . The motion of thevehicle through the environment is modeled by a conventionallinear discrete-time state transition equation or process modelof the form(1)wherestate transition matrix;vector of control inputs; andvectoroftemporallyuncorrelatedprocessnoiseerrorswith zero mean and covariance (see [17] and[18] for further details).The location of the th landmark is denoted . The “state tran-sition equation” for the th landmark is(2)since landmarks are assumed stationary. Without loss of gener-ality the number of landmarks in the environment is arbitrarilyset at . The vector of all landmarks is denoted(3)where denotes the transpose and is used both inside and out-side the brackets to conserve space. The augmented state vectorcontaining both the state of the vehicle and the state of all land-mark locations is denoted(4)The augmented state transition model for the complete systemmay now be written as.....................(5)(6)where is the identity matrix and isthe null vector.The case in which landmarks are in stochastic motion mayeasily be accommodated in this framework. However, doing soofferslittleinsightinto theproblem andfurthermoretheconver-gence properties presented by this paper do not hold. AlultIXDoM1a1UfIX Ra  232 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 3, JUNE 2001  B. The Observation Model The vehicle is equipped with a sensor that can obtain obser-vations of the relative location of landmarks with respect to thevehicle. Again, without prejudice, observations are assumed tobe linear and synchronous. The observation model for the thlandmark is written in the form(7)(8)where is a vector of temporally uncorrelated observationerrors with zero mean and variance . The term is theobservation matrix and relates the output of the sensor tothe state vector when observing the th landmark. It is im-portant to note that the observation model for the th landmark is written in the form(9)This structure reflects the fact that the observations are “rela-tive” between the vehicle and the landmark, often in the form of relative location, or relative range and bearing (see Section IV). C. The Estimation Process In the estimation-theoretic formulation of the SLAMproblem, the Kalman filter is used to provide estimates of vehicle and landmark location. We briefly summarize thenotation and main stages of this process as a necessary preludeto the developments in Section III and IV of this paper. Detaileddescriptions may be found in [17], [18] and [3]. The Kalmanfilter recursively computes estimates for a state whichis evolving according to the process model in (5) and whichis being observed according to the observation model in (7).The Kalman filter computes an estimate which is equivalentto the conditional mean , whereis the sequence of observations taken up until time . Theerror in the estimate is denoted .The Kalman filter also provides a recursive estimate of thecovariance in the estimate. The Kalman filter algorithm now proceeds recursivelyin three stages:ã Prediction: Given that the models described in (5) and (7)hold, and that an estimate of the state at timetogetherwithanestimateofthecovariance exist,the algorithm first generates a prediction for the state es-timate, the observation (relative to the th landmark) andthe state estimate covariance at time according to(10)(11)(12)respectively.ã Observation: Following the prediction, an observationof the th landmark of the true stateis made according to (7). Assuming correct landmark association, an innovation is calculated as follows:(13)together with an associated innovation covariance matrixgiven by(14)ã Update: The state estimate and corresponding state esti-mate covariance are then updated according to(15)(16)where the gain matrix is given by(17)The update of the state estimate covariance matrixis of paramount importance to the SLAM problem.Understanding the structure and evolution of the statecovariance matrix is the key component to this solutionof the SLAM problem.III. S TRUCTURE OF THE SLAM P ROBLEM In this section proofs for the three key results underlying thestructure of the SLAM problem are provided. The implicationsof these results will also be examined in detail. The appendixprovides a summary of the key properties of positive semidefi-nite matrices that are invoked implicitly in the following proofs.  A. Convergence of the Map Covariance Matrix The state covariance matrix may be written in block form aswhereerror covariance matrix associated with the ve-hicle state estimate;map covariance matrix associated with the land-mark state estimates; andcross-covariance matrix between vehicle andlandmark states. Theorem 1: The determinant of any submatrix of the mapcovariancematrixdecreasesmonotonicallyassuccessiveobser-vations are made.Thealgorithmisinitializedusingapositivesemidefinite(  psd  )state covariance matrix . The matrices and areboth psd  , and consequently the matrices , ,and are all  psd  . From (16), and for any landmark (18) AlultIXDoM1a1UfIX Ra
Related Search
We Need Your Support
Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks