1 Simultaneous Localisation and Mapping (SLAM): Part I The ...

1 1 Simultaneous Localisation and Mapping ( slam ):Part I The Essential AlgorithmsHugh Durrant-Whyte,Fellow, IEEE, and Tim BaileyAbstract This tutorial provides an introduction to Simul-taneous Localisation and Mapping ( slam ) and the exten-sive research on slam that has been undertaken over thepast decade. slam is the process by which a mobile robotcan build a map of an environment and at the same timeuse this map to compute it s own location. The past decadehas seen rapid and exciting progress in solving the slam problem together with many compelling implementations ofSLAM methods. Part I of this tutorial (this paper), de-scribes the probabilistic form of the slam problem, essen-tial solution methods and significant implementations. PartII of this tutorial will be concerned with recent advances incomputational methods and new formulations of the slam problem for large scale and complex IntroductionThe Simultaneous Localisation and Mapping ( slam )problem asks if it is possible for a mobile robot to be placedat an unknown location in an unknown environment andfor the robot to incrementally build a consistent map ofthis environment while simultaneously determining its lo-cation within this map.

2 A solution to the slam problemhas been seen as a holy grail for the mobile robotics com-munity as it would provide the means to make a robot solution of the slam problem has been one ofthe notable successes of the robotics community over thepast decade. slam has been formulated and solved as atheoretical problem in a number of different forms. SLAMhas also been implemented in a number of different domainsfrom indoor robots, to outdoor, underwater and airbornesystems. At a theoretical and conceptual level, slam cannow be considered a solved problem. However, substantialissues remain in practically realizing more general slam solutions and notably in building and using perceptuallyrich maps as part of a slam two-part tutorial and survey of slam aims to pro-vide a broad introduction to this rapidly growing I (this paper) begins by providing a brief history ofearly developments in slam . Section III introduces thestructure the slam problem in now standard Bayesianform, and explains the evolution of the slam IV describes the two key computational solutionsto the slam problem; through the use of the extendedKalman filter (EKF- slam ) and through the use of Rao-Blackwellised particle filters (FastSLAM).

3 Other recent so-lutions to the slam problem are discussed in Part II ofHugh Durrant-Whyte, and Tim Bailey are with theAustralian Centre for Field Robotics (ACFR) J04, TheUniversity of Sydney, Sydney NSW 2006, tutorial. Section V describes a number of importantreal-world implementations of slam and also highlightsimplementations where the sensor data and software arefreely down-loadable for other researchers to study. PartII of this tutorial describes major issues in computation,convergence and data association in slam . These are sub-jects that have been the main focus of the slam researchcommunity over the past five History of the slam ProblemThe genesis of the probabilistic slam problem occurredat the 1986 IEEE Robotics and Automation Conferenceheld in San Francisco. This was a time when probabilis-tic methods were only just beginning to be introducedinto both robotics and AI.

4 A number of researchers hadbeen looking at applying estimation-theoretic methods tomapping and Localisation problems; these included PeterCheeseman, Jim Crowley, and Hugh Durrant-Whyte. Overthe course of the conference many paper table cloths andnapkins were filled with long discussions about consistentmapping. Along the way, Raja Chatila, Oliver Faugeras,Randal Smith and others also made useful contributions tothe result of this conversation was a recognition thatconsistent probabilistic Mapping was a fundamental prob-lem in robotics with major conceptual and computationalissues that needed to be addressed. Over the next few yearsa number of key papers were produced. Work by Smithand Cheesman [39] and Durrant-Whyte [17] established astatistical basis for describing relationships between land-marks and manipulating geometric uncertainty. A key el-ement of this work was to show that there must be a highdegree of correlation between estimates of the location ofdifferent landmarks in a map and that indeed these corre-lations would grow with successive the same time Ayache and Faugeras [1] were under-taking early work in visual navigation, Crowley [9] andChatila and Laumond [6] in sonar-based navigation of mo-bile robots using Kalman filter-type algorithms.

5 These twostrands of research had much in common and resulted soonafter in the landmark paper by Smith, Self and Cheese-man [40]. This paper showed that as a mobile robot movesthrough an unknown environment taking relative observa-tions of landmarks, the estimates of these landmarks areall necessarily correlated with each other because of thecommon error in estimated vehicle location [27]. The im-plication of this was profound: A consistent full solutionto the combined Localisation and Mapping problem would2require a joint state composed of the vehicle pose and everylandmark position, to be updated following each landmarkobservation. In turn, this would require the estimator toemploy a huge state vector (of order the number of land-marks maintained in the map) with computation scaling asthe square of the number of , this work did not look at the convergence prop-erties of the map or its steady-state behavior.

6 Indeed, itwas widely assumed at the time that the estimated maperrors would not converge and would instead exhibit a ran-dom walk behavior with unbounded error growth. Thus,given the computational complexity of the Mapping prob-lem and without knowledge of the convergence behavior ofthe map, researchers instead focused on a series of approxi-mations to the consistent Mapping problem solution whichassumed or even forced the correlations between landmarksto be minimized or eliminated so reducing the full filter toa series of decoupled landmark to vehicle filters ([28], [38]for example). Also for these reasons, theoretical work onthe combined Localisation and Mapping problem came to atemporary halt, with work often focused on either mappingor Localisation as separate conceptual break-through came with the realisationthat the combined Mapping and Localisation problem, onceformulated as a single estimation problem, was actuallyconvergent.

7 Most importantly, it was recognised that thecorrelations between landmarks, that most researchers hadtried to minimize, were actually the critical part of theproblem and that, on the contrary, the more these corre-lations grew, the better the solution. The structure of theSLAM problem, the convergence result and the coining ofthe acronym slam was first presented in a mobile robot-ics survey paper presented at the 1995 International Sym-posium on Robotics Research [16]. The essential theory onconvergence and many of the initial results were developedby Csorba [11], [10]. Several groups already working onmapping and Localisation , notably at MIT [29], Zaragoza[5], [4], the ACFR at Sydney [20], [45] and others [7], [13],began working in earnest on SLAM1applications in indoor,outdoor and sub-sea this time, work focused on improving computationalefficiency and addressing issues in data association or loopclosure.

8 The 1999 International Symposium on Robot-ics Research (ISRR 99) [23] was an important meetingpoint where the first slam session was held and wherea degree of convergence between the Kalman-filter basedSLAM methods and the probabilistic Localisation and map-ping methods introduced by Thrun [42] was achieved. The2000 IEEE ICRA Workshop on slam attracted fifteen re-searchers and focused on issues such as algorithmic com-plexity, data association and implementation following slam workshop at the 2002 ICRA attracted150 researchers with a broad range of interests and appli-cations. The 2002 slam summer school hosted by Hen-rik Christiansen at KTH in Stockholm attracted all the1 Also called Concurrent Mapping and Localisation (CML) at researchers together with some 50 PhD students fromaround the world and was a tremendous success in build-ing the field. Interest in slam has grown exponentiallyin recent years, and workshops continue to be held at bothICRA and IROS.

9 The slam summer school ran in 2004in Tolouse and will run at Oxford in Formulation and Structure ofthe slam problemSLAM is a process by which a mobile robot can builda map of an environment and at the same time use thismap to deduce it s location. In slam both the trajectoryof the platform and the location of all landmarks are esti-mated on-line without the need for anya prioriknowledgeof Preliminariesx k x k+2 m j x k x k-1 x k+1 m i z k-1,i z k,j u k u k+1 u k+2 Robot Landmark Estimated True Fig. essential slam problem. A Simultaneous estimate ofboth robot and landmark locations is required. The true locations arenever known or measured directly. Observations are made betweentrue robot and landmark locations. See text for a mobile robot moving through an environmenttaking relative observations of a number of unknown land-marks using a sensor located on the robot as shown inFigure 1. At a time instantk, the following quantities aredefined: xk: The state vector describing the location and orien-tation of the vehicle.

10 Uk: The control vector, applied at timek 1 to drive thevehicle to a statexkat timek. mi: A vector describing the location of theithlandmarkwhose true location is assumed time invariant. zik: An observation taken from the vehicle of the locationof theithlandmark at timek. When there are multiplelandmark observations at any one time or when the specificlandmark is not relevant to the discussion, the observationwill be written simply addition, the following sets are also defined: X0:k={x0,x1, ,xk}={X0:k 1,xk}: The history ofvehicle locations. U0:k={u1,u2, ,uk}={U0:k 1,uk}: The historyof control inputs. m={m1,m2, ,mn}The set of all landmarks. Z0:k={z1,z2, ,zk}={Z0:k 1,zk}: The set of alllandmark Probabilistic SLAMIn probabilistic form, the Simultaneous Localisation andMap Building ( slam ) problem requires that the probabil-ity distributionP(xk,m|Z0:k,U0:k,x0)(1)be computed for all timesk.