1 / 84

SLAM/FastSLAM

Advanced Mobile Robotics. Probabilistic Robotics:. SLAM/FastSLAM. Dr. J izhong Xiao Department of Electrical Engineering CUNY City College jxiao@ccny.cuny.edu. Probabilistic Robotics. SLAM/FastSLAM. SLAM Problem : Chicken or Egg. Fundamental problems for localization and mapping.

cira
Télécharger la présentation

SLAM/FastSLAM

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Advanced Mobile Robotics Probabilistic Robotics: SLAM/FastSLAM Dr. Jizhong Xiao Department of Electrical Engineering CUNY City College jxiao@ccny.cuny.edu

  2. Probabilistic Robotics SLAM/FastSLAM

  3. SLAM Problem : Chicken or Egg Fundamental problems for localization and mapping • The task of SLAM is to build a map while estimating the pose of the robot relative to this map. • Without a map, robot cannot localize itself • Without knowing its location, robot cannot build a map • Which needed to be done first? Localization or mapping?

  4. SLAM Simultaneous Localization and Mapping Given: • The robot’s controls (U1:t) • Observations of nearby features (Z1:t) Estimate: • Map of features (m) • Pose / Path of the robot (xt) A robot is exploring an unknown, static environment.

  5. Why is SLAM a hard problem? Uncertanties • Error in pose • Error in observation • Error in mapping • Error accumulated

  6. Why is SLAM a hard problem? SLAM: robot path and map are both unknown! Robot path error correlates errors in the map

  7. Why is SLAM a hard problem? • In the real world, the mapping between observations and landmarks is unknown • Picking wrong data associations can have catastrophic consequences • Pose error correlates data associations Robot pose uncertainty

  8. Data Association Problem • A data association is an assignment of observations to landmarks • In general there are more than (n observations, m landmarks) possible associations • Also called “assignment problem”

  9. SLAM: Simultaneous Localization and Mapping • Full SLAM: Estimates Entire pose (x1:t) and map (m) Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut) Estimates entire path and map!

  10. Graphical Model of Full SLAM:

  11. SLAM: Simultaneous Localization and Mapping • Online SLAM: Estimates Most recent pose (xt) and map (m) Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut) Estimates most recent pose and map!

  12. Graphical Model of Online SLAM: Integrations typically done one at a time

  13. SLAM with Extended Kalman Filter • Pre-requisites • Maps are feature-based (landmarks) small number (< 1000) • Assumption - Gaussian Noise • Process only positive sightings No landmark = negative Landmark = positive

  14. EKF-SLAM with known correspondences Correspondence Data association problem Landmarks can’t be uniquely identified True identity of observed feature Correspondence variable (Cit) between feature (fit) and real landmark

  15. EKF-SLAM with known correspondences Signature Numerical value (average color) Characterize type of landmark (integer) Multidimensional vector (height and color)

  16. EKF-SLAM with known correspondences Similar development to EKF localization Diff  robot pose + coordinates of all landmarks Combined state vector (3N + 3) Online posterior

  17. Mean Motion update Covariance Test for new landmarks Initialization of elements Expected measurement Iteration through measurements Filter is updated

  18. FastSLAM Jizhong Xiao Department of Electrical Engineering The City College of New York

  19. Particle Filters • Represent belief by random samples • Estimation of non-Gaussian, nonlinear processes • Sampling Importance Resampling (SIR) principle • Draw the new generation of particles • Assign an importance weight to each particle • Resampling • Typical application scenarios are tracking, localization, mapping …

  20. Localization vs. SLAM • A particle filter can be used to solve both problems • Monte Carlo Localization: state space < x, y, > • SLAM: state space < x, y, , map> • for landmark maps = < l1, l2, …, lm> • for grid maps = < c11, c12, …, c1n, c21, …, cnm> • Disadvantage: The number of particles needed to represent a posterior grows exponentially with the dimension of the state space!

  21. Some considerations of particle filter • Scale exponentially with dimension • Large variables involved to mapping • Convergence of particles • Choice of Re-sampling • Conditionally independent mapping error • And more….

  22. Dependencies • Is there a dependency between the dimensions of the state space? • If so, can we use the dependency to solve the problem more efficiently? • In the SLAM context • The map depends on the poses of the robot. • We know how to build a map given the position of the sensor is known.

  23. l1 Landmark 1 z1 z3 observations . . . x1 x2 x3 xt x0 Robot poses u1 ut-1 u1 u0 controls z2 zt l2 Landmark 2 Mapping using Landmarks Knowledge of the robot’s true path renders landmark positions conditionally independent

  24. FastSLAM Principle Features are conditionally independent given the robot path Figure 13.3 The SLAM problem depicted as Bayes network graph. The robot moves from pose xt-1 to pose xt+2, driven by a sequence of controls. At each pose xt , it observes a nearby feature in the map m={m1, m2, m3}. This graphical network illustrates that the pose variables “separate” the individual features in the map from each other. If the poses are known, there remains no other path involving variables whose value is not known, between any two features in the map. This lack of a path renders the posterior of any two features in the map conditionally independent (given the poses).

  25. Factored Posterior (Landmarks) poses map observations & movements SLAM posterior Robot path posterior landmark positions Does this help to solve the problem? (Factorizationfirst introduced by Murphy in 1999)

  26. Factored Posterior Robot path posterior(localization problem) Conditionally independent landmark positions

  27. Rao-Blackwellization • This factorization is also called Rao-Blackwellization • Given that the second term can be computed efficiently, particle filtering becomes possible!

  28. Fast SLAM • Rao-Blackwellized particle filtering based on landmarks [Montemerlo et al., 2002] • Each landmark is represented by a Extended Kalman Filter • Each particle therefore has to maintain M EKFs Particle #1 x, y,  Landmark 1 Landmark 2 Landmark M … Particle #2 x, y,  Landmark 1 Landmark 2 Landmark M … … Particle N x, y,  Landmark 1 Landmark 2 Landmark M …

  29. FastSLAM Algorithm setup

  30. Basic Step of FastSLAM

  31. Rao-Blackwellization LandmarksGrid-based map poses map observations movements

  32. Factored SLAM Posterior The mathematic expression of full SLAM posterior: After a long derivation (on page 442), it becomes

  33. Advantage of factorization • FastSLAM estimates the path posterior using particle filter, estimate map feature using EKF • Because of factorization: • Maintain MN+1 filters (M – particles, N features) • All filters are low-dimensional • Separate EFK for each map features which makes update more efficient than EKF-SLAM • Each particle possess its own sets of EKF’s • Easy for data association

  34. FastSLAM 1.0 Algorithm: N-th feature location (mean, variance) relative to the k-th particle Robot path estimation K-th Particle is denoted as: Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Resembling Uses control ut to sample new robot pose xt Added into a temporary set of particle, along with the path of previous poses

  35. FastSLAM 1.0 Algorithm: • Step 1: Extend the path posterior by sampling new poses • Step 2: Updating the observed feature estimate For n≠ct do not update For n=ct Perceptual model, Gaussian Previous belief, Gaussian Using EKF, approximate the measurement model g by Taylor expansion New mean Kalman Gain New covariance

  36. FastSLAM 1.0 Algorithm: Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Resembling • Necessity: particles in the temporary set are not distributed according to the desired posterior, (i.e., poses xt driven by most recent control ut, without paying attention to measurement zt). • Importance factor = Target distribution / Proposal distribution

  37. Re-sampling by Importance Sampler Target distribution, takes into account of measurement zt, along with the correspondence ct Samples cannot be drawn conveniently from the target distribution f. Instead, the importance sampler draws samples from the proposal distribution g, which has a simpler form. Proposal distribution of the path particles in the temporary set A sample of f is obtained by attaching the weight f/g to each sample x.

  38. Re-sampling by Importance Sampler Importance factor Observed feature Measurement model, Gaussian With covariance:

  39. FastSLAM 1.0 Algorithm with known correspondence • See table 13.1 of text book on page 450

  40. Weighting particle to Approximate Target Distribution

  41. Resampling mismatch FastSLAM 1.0 problem: samples poses based on control ut only, but the accuracy of control is low. Proposal generates a large spectrum of samples, but only a small subset has high likelihood . After re-sampling, only particles within the ellipsoid “survive” with reasonably high likelihood

  42. FastSLAM 2.0 Algorithm: FastSLAM 2.0 avoids the problem by sampling poses based on the measurement Zt in addition to control ut Step 1: Extend the path posterior by sampling new poses Step 2: Updating the observed feature estimate Step 3: Re-sampling See math on page 452~453

  43. FastSLAM 2.0 Algorithm: FastSLAM 2.0 avoids the problem by sampling poses based on the measurement Zt in addition to control ut • Extend the path posterior by sampling new poses • Updating the observed feature estimate • Re-sampling

  44. FastSLAM Algorithm • Particle filter approach to the SLAM problem • Maintain a set of particles • Particles contain a sampled robot path and a map • The features of the map are represented by own local Gaussian • Map is created as a set of separate Gaussians Map features are conditionally independent given the path Factoring out the path (1 per particle) Map feature become independent Eliminates the need to maintain correlation among them

  45. FastSLAM Algorithm • Updating in FastSLAM Sample new pose update the observed features • Update can be performed online • Solves both online and offline SLAM problem • Instances Feature-based maps Grid-based algorithm

  46. Features of Fast-SLAM • Each particle can rely on its own, local data association decisions • More robust data association decision on per-particle basis. • Solves both full SLAM & on-line SLAM problems. • FastSLAM 1.0 – less efficient in generating samples • FastSLAM 2.0 – more efficient by improved proposal distribution, at the cost of mathematic complexity.

  47. Unknown Data Association Correspondence: maximizing the likelihood of the measurement zt

  48. FastSLAM – Action Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

  49. FastSLAM – Sensor Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

  50. Weight = 0.8 Weight = 0.4 Weight = 0.1 FastSLAM – Sensor Update Particle #1 Particle #2 Particle #3

More Related