Sensor-Based Simultaneous Localization and Mapping - Part ...

1 Sensor-Based Simultaneous Localization and Mapping Part I:GAS Robocentric FilterBruno J. Guerreiro, Pedro Batista, Carlos Silvestre, and Paulo OliveiraAbstract This paper presents the design, analysis, and ex-perimental validation of a Sensor-Based globally asymptoticallystable (GAS) filter for Simultaneous Localization and Mapping (SLAM) with application to uninhabited aerial vehicles (UAVs).The SLAM problem is first formulated in a sensor -basedframework, without any type of vehicle pose information, andmodified in such a way that the underlying system structurecan be regarded as linear time varying for observability, filterdesign, and convergence analysis purposes.

2 Thus, a Kalmanfilter follows naturally with GAS error dynamics that esti-mates, in a robocentric coordinate frame, the positions of thelandmarks, the velocity of the vehicle, and the bias of theangular velocity measurement. The online inertial map andtrajectory estimation is detailed in a companion paper andfollows from the estimation solution provided by the SLAM filter herein presented. The performance and consistency ofthe proposed method are successfully validated experimentallyin a structured real world environment using a quadrotorinstrumented INTRODUCTIONR eliable navigation and positioning of uninhabited aerialvehicles (UAV) are fundamental for any autonomous mis-sion, particularly in unknown environments where absolutepositioning systems are absent or unreliable.

3 The motiva-tion for this work arises from the usage of autonomousrotorcraft for automatic inspection of critical infrastructuresand buildings, such as bridges, electric power lines, dams,construction areas, etc. Near these structures, the ubiquitousglobal positioning system (GPS) signal may be unreliableor completely unavailable, whereas the electromagnetic in-terference may degrade any magnetometer measurement tothe point of becoming unusable. Therefore, aided navigationstrategies have to be devised in such a way that these sensorsare made paper presents a globally asymptotically stable (GAS) Sensor-Based filter for Simultaneous Localization and map-ping (SLAM) with application to uninhabited aerial vehicles(UAVs) in GPS-denied environments, using acceleration andangular rate inertial measurements, a LASER scanning de-vice and an altitude sensor .

4 Over the past decades the re-Thisworkwaspartiallysupportedbyprojec tFCT[PEst-OE/EEI/LA0009/2011],byprojectF CTAMMAIA(PTDC/HIS-ARQ/103227/2008), and by project AIRTICI from AdI through the POSC onhecimento Program that includes FEDER funds. The work of BrunoGuerreiro was supported by the PhD Student Grant SFRH/BD/21781/2005from the Portuguese FCT POCTI J. Guerreiro, P. Batista, C. Silvestre, and Paulo Oliveira are withthe Institute for Systems and Robotics, Instituto Superior T ecnico, at theTechnical University of Lisbon, Av. Rovisco Pais 1, 1049-001 Lisboa,Portugal.

5 C. Silvestre is also with the Faculty of Science and Technology,University of Macau, Taipa, community has devoted tremendous effort in the fieldof probabilistic SLAM. The seminal works that establishedthe statistical foundations for describing the relationshipsbetween landmarks and their correlations include [1], and[2]. For a detailed survey and tutorial on SLAM please referto [3], [4], and references therein, which include a significantnumber of successful implementations of SLAM algorithmsin real world scenarios. A general proof of global conver-gence for the extended Kalman filter (EKF), to the best ofthe authors knowledge, is yet to be found.

6 Nonetheless, thereare some notable EKF- based SLAM convergence results [5],[6], which usually assume that the linearized system matricesare evaluated at the ideal values of the state linearization may lead to statistical inconsistency as itwas first pointed out in [7] and subsequently acknowledgedand discussed in [8] and [9]. In order to minimize theinconsistency problems induced by the linearization, somenew algorithms have been proposed, such as the robocentricmap joining algorithm [10], where the filtering is made inthe sensor space, or the first-estimates Jacobian EKF [11].

7 The main contribution of this paper is the design, analysis,and experimental validation of a novel Sensor-Based SLAM filter for structured 3-dimensional (3-D) environments, 2-Dmapping and altitude, as part of an integrated SLAM al-gorithm, that: (i) has globally asymptotically stable errordynamics; (ii) allows for online Simultaneous estimation ofthe inertial frame map, position and motion of the vehicle,as detailed in the companion paper [12]; (iii) resorts tothe linear and angular motion kinematics, which are exact;(iv) can easily be generalized into full 3-D SLAM, if 3-Dlandmarks are available; (v) builds on the well-establishedlinear time-varying (LTV) Kalman filtering theory; and (vi)estimates explicitly the rate gyro bias, merging low band-width landmark observations with high bandwidth rate gyromeasurements.

8 The result of this filter is the solution ofthe SLAM problem in the vehicle frame, where the vehiclepose is deterministic, as it corresponds to that of the vehicleframe, and the positions of the landmarks rotate and translateaccording to the vehicle motion, in a similar fashion to whathappens in [10]. An essential feature of the filter design is themodification of the nominal nonlinear Sensor-Based systemdynamics such that it can be regarded as LTV for observabil-ity and convergence analysis purposes, even though it stillis intrinsically nonlinear. Nevertheless, it must be stressedthat the resulting system dynamics used in the design andanalysis of the Kalman filter are exact and no approximationsor linearizations are performed whatsoever.

9 The proposedsolution builds on previous Sensor-Based navigation filters2012 American Control ConferenceFairmont Queen Elizabeth, Montr al, CanadaJune 27-June 29, 2012978-1-4577-1094-0/12/$ 2012 AACC6352proposed by the authors in [13], [14], and [15].The second part of the proposed SLAM algorithm, whichis detailed in the companion paper [12], focus on the onlineoptimal transformation of the estimated Sensor-Based mapinto the inertial frame, by estimating the translation androtation between any two consecutive instants, provided atleast two landmark paper is organized as follows.

10 Section II introducesthe Sensor-Based paradigm, the nominal system dynamics,and the problem at hand. A constructive observability anal-ysis is presented in Section III and Section IV details thedefinition of the SLAM filter. Experimental results using aninstrumented quadrotor platform are shown and discussedin Section V and, finally, in Section VI, some concludingremarks and directions of future work are the paper the symbol0n mdenotes ann mmatrix of zeros,Inan identity matrix with dimensionn n,anddiag(A1,..,An)a block diagonal matrix. When thedimensions are omitted the matrices are assumed of appro-priate PROBLEM STATEMENTA.