Click here to load reader

Simultaneous Localization and Mapping

  • View

  • Download

Embed Size (px)


Simultaneous Localization and Mapping. Brian Clipp Comp 790-072 Robotics. The SLAM Problem. Given Robot controls Nearby measurements Estimate Robot state (position, orientation) Map of world features. Indoors. Undersea. Underground. Space. SLAM Applications. - PowerPoint PPT Presentation

Text of Simultaneous Localization and Mapping

  • Simultaneous Localization and MappingBrian ClippComp 790-072 Robotics

  • The SLAM ProblemGiven Robot controlsNearby measurementsEstimateRobot state (position, orientation)Map of world features

  • SLAM ApplicationsImages Probabilistic Robotics

  • OutlineSensorsProbabilistic SLAMFull vs. Online SLAMExample AlgorithmsExtended Kalman Filter (EKF) SLAMFastSLAM (particle filter)

  • Types of SensorsOdometryLaser Ranging and Detection (LIDAR)Acoustic (sonar, ultrasonic)RadarVision (monocular, stereo etc.)GPSGyroscopes, Accelerometers (Inertial Navigation)Etc.

  • Sensor CharacteristicsNoiseDimensionality of OutputLIDAR- 3D pointVision- Bearing only (2D ray in space)RangeFrame of ReferenceMost in robot frame (Vision, LIDAR, etc.)GPS earth centered coordinate frameAccelerometers/Gyros in inertial coordinate frame

  • A Probabilistic ApproachThe following algorithms take a probabilistic approach

  • Full vs. Online SLAMFull SLAM calculates the robot state over all time up to time t

    Online SLAM calculates the robot state for the current time t

  • Full vs. Online SLAMFull SLAMOnline SLAM

  • Two Example SLAM AlgorithmsExtended Kalman Filter (EKF) SLAMSolves online SLAM problemUses a linearized Gaussian probability distribution modelFastSLAMSolves full SLAM problemUses a sampled particle filter distribution model

  • Extended Kalman Filter SLAMSolves the Online SLAM problem using a linearized Kalman filterOne of the first probabilistic SLAM algorithmsNot used frequently today but mainly shown for its explanatory value

  • Process and Measurement ModelsNon-linear Dynamic ModelDescribes change of robot state with timeNon-Linear Measurement ModelPredicts measurement value given robot state

  • EKF EquationsPredictCorrect

  • EKF Examplet=0Initial State and UncertaintyUsing Range Measurements

  • EKF Examplet=1Predict Robot Pose and Uncertainty at time 1

  • EKF Examplet=1Correct pose and pose uncertaintyEstimate new feature uncertainties

  • EKF Examplet=2Predict pose and uncertainty of pose at time 2Predict feature measurements and their uncertainties

  • EKF Examplet=2Correct pose and mapped featuresUpdate uncertainties for mapped featuresEstimate uncertainty of new features

  • Application from Probabilistic Robotics[courtesy by John Leonard]

  • Application from Probabilistic Roboticsodometryestimated trajectory[courtesy by John Leonard]

  • Correlation Between Measurement Association and State Errors

    Association between measurements and features is unknownErrors in pose and measurement associations are correlatedRobot poseuncertaintyCorrect AssociationsRobots Associations

  • Measurement AssociationsMeasurements must be associated with particular featuresIf the feature is new add it to the mapOtherwise update the feature in the mapDiscrete decision must be made for each feature association, ct

  • Problems With EKF SLAMUses uni-modal Gaussians to model non-Gaussian probability density function



  • Problems With EKF SLAMOnly one set of measurement to feature associations consideredUses maximum likelihood associationLittle chance of recovery from bad associationsO(N3) matrix inversion required

  • FastSLAMSolves the Full SLAM problem using a particle filter

  • Particle FiltersRepresent probability distribution as a set of discrete particles which occupy the state space

  • Particle Filter Update CycleGenerate new particle distribution given motion model and controls appliedFor each particleCompare particles prediction of measurements with actual measurementsParticles whose predictions match the measurements are given a high weightResample particles based on weight

  • ResamplingAssign each particle a weight depending on how well its estimate of the state agrees with the measurementsRandomly draw particles from previous distribution based on weights creating a new distribution

  • Particle Filter AdvantagesCan represent multi-modal distributions



  • Particle Filter DisadvantagesNumber of particles grows exponentially with the dimensionality of the state space1D n particles2D n2 particlesmD nm particles

  • FastSLAM FormulationDecouple map of features from poseEach particle represents a robot poseFeature measurements are correlated thought the robot poseIf the robot pose was known all of the features would be uncorrelatedTreat each pose particle as if it is the true pose, processing all of the feature measurements independently

  • Factored Posterior (Landmarks)SLAM posteriorRobot path posterior landmark positionsposesmapobservations & movementsFactorization first introduced by Murphy in 1999

  • Factored PosteriorRobot path posterior (localization problem)Conditionally independent landmark positions

  • Rao-BlackwellizationDimension of state space is drastically reduced by factorization making particle filtering possible

  • FastSLAMRao-Blackwellized particle filtering based on landmarks [Montemerlo et al., 2002]Each landmark is represented by a 2x2 Extended Kalman Filter (EKF)Each particle therefore has to maintain M EKFsLandmark 1Landmark 2Landmark Mx, y, Particle#1Landmark 1Landmark 2Landmark Mx, y, Particle#2ParticleN

  • FastSLAM Action UpdateParticle #1Particle #2Particle #3Landmark #1FilterLandmark #2Filter

  • FastSLAM Sensor UpdateParticle #1Particle #2Particle #3Landmark #1FilterLandmark #2Filter

  • FastSLAM Sensor UpdateParticle #1Particle #2Particle #3

  • FastSLAM ComplexityUpdate robot particles based on control ut-1Incorporate observation zt into Kalman filtersResample particle setN = Number of particlesM = Number of map features

  • Multi-Hypothesis Data AssociationData association is done on a per-particle basis Robot pose error is factored out of data association decisions

  • Per-Particle Data AssociationWas the observationgenerated by the redor the blue landmark?P(observation|red) = 0.3P(observation|blue) = 0.7Two options for per-particle data associationPick the most probable matchPick an random association weighted by the observation likelihoodsIf the probability is too low, generate a new landmark

  • MIT Killian CourtThe infinite-corridor-dataset at MIT

  • MIT Killian Court

  • ConclusionSLAM is a hard problem which is not yet fully solved Probabilistic methods which take account of sensor and process model error tend to work bestEffective algorithms must be robust to bad data associations which EKF SLAM is notReal time operation limits complexity of algorithms which can be applied

  • References on EKF SLAMP. Moutarlier, R. Chatila, "Stochastic Multisensory Data Fusion for Mobile Robot Localization and Environment Modelling",In Proc. of the International Symposium on Robotics Research, Tokyo, 1989.

    R. Smith, M. Self, P. Cheeseman, "Estimating Uncertain Spatial Relationships in Robotics", In Autonomous Robot Vehicles, I. J. Cox and G. T. Wilfong, editors, pp. 167-193, Springer-Verlag, 1990.

    Ali Azarbayejani, Alex P. Pentland, "Recursive Estimation of Motion, Structure, and Focal Length,"IEEE Transactions on Pattern Analysis and Machine Intelligence,vol.17,no.6,pp. 562-575,June,1995.

  • References on FastSLAMM. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored solution to simultaneous localization and mapping, AAAI02 D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03 M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003

    G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05 A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03

  • Additional ReferenceMany of the slides for this presentation are from the book Probabilistic Robotics website

Search related