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.

Download Presentation

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.While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server.

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

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?

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.

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

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

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

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”

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!

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!

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

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

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

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

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

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

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 …

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!

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….

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.

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).

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)

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

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 …

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

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

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

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

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

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.

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

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

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

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

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

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.