slam seminar
TRANSCRIPT
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
Simultaneous Localization And Mapping : Part 1
August , 2016
JunHyuk Shin
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
Contents
1. Introduction
2. Formulation of SLAM problem
3. Structure of Probabilistic SLAM
4. Solutions to the SLAM Problem- EKF SLAM- Rao-Blackwellized Filter (FastSLAM)
5. Conclusion & Limitation
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
1. Introduction
SLAM Problem (Simultaneous localization and mapping)- Mobile robot to be placed at an unknown location in an unknown environment : the robot to build a map of environment while simultaneously determining its location within the map Implement domain
1) From indoor to outdoor2) Underwater3) Airborne systems
Elements of SLAM Localization : determine pose given a priori map Mapping : generate map when pose is accurately known from auxiliary source- SLAM allows robots to operate in an environment without a priori knowledge of a map and without access to independent position information
Page 1
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
2. Formulation of SLAM problem
A. Preliminaries Defined Vector- : state vector describing the location and orientation of the vehicle- : control vector applied at time k-1 to drive the vehicle to a state at time - : location vector of the th landmark whose location is assumed time invariant- : observation taken from the vehicle at the location of the th landmarks at time k. with no specific landmark, Defined Set- - - -
Page 2
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
2. Formulation of SLAM problem
B. Probabilistic SLAM SLAM probability distribution
: the joint posterior density of the landmark locations and vehicle state given the recorded observation and control inputs up with initial state of the vehicle
Page 3
Hidden Markov Model (HMM): true state is assumed to be unob-served Markov process, and the measurements are the observed states - each state has a probability distri-bution over the possible output token- conditional probability distribution at future steps depends only on the current state of the system, and not additionally previous steps
(1)
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
2. Formulation of SLAM problem
Recursive Bayes filters: general probabilistic approach for estimating an unknown pdf recursively over time for given system model, but no analytical solution in general - Observation model - Motion model Prediction step (a-priori)
: use system model to predict forward (Chapman-Kolmogorov identity + Markov property)
Update step (a-posteriori): update the prediction with observed data
where
Page 4
𝑃 (𝒁 𝑘|𝒙𝑘 ,𝒎 )𝑃 (𝒙𝑘|𝒙𝑘− 1 ,𝒖𝑘 )
Motion model Previous Posterior
Observation model Current prior
Normalization constant
(2) (3)
(4)
(5)
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
3. Structure of Probabilistic SLAM
Page 6
Nearly independent measurement Correlation between landmark- Error in landmark location is highly correlated : relative location between any two landmark, may be known with high accuracy- Joint probability density(and correlation) on all landmarks becomes monotonically more peaked as more observation are made- This convergence occurs because the relative loca-tion of observed two landmark is nearly independent of the coordinate frame of the vehicle Spring network analogy: Network of spring connect all landmarks together with correlation spring- As observations move through environment, springs of network become increasingly stiffer- Localization accuracy is bounded only by relative location accuracy
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem
No Analytic solution on Recursive Bayes filters EKF(Extended Kalman Filter) SLAM
- Representation of (4) and (5) in the form of a state-space model- Additive Gaussian noise- Linearize non-linear system model
Fast SLAM- Describe the vehicle motion model in (3) as a set of samples - General non-Gaussian probability distribution- Using Rao-Blackwellized particle filter- Directly represent the nonlinear process model (still linearize the observation model)
Page 7
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM
A. Basis for EKF-SLAM method System model definition
Vehicle motion model : additive, zero mean uncorrelated Gaussian disturbances with covariance
Observation model : additive, zero mean uncorrelated Gaussian errors with covariance
Joint posterior distribution Mean
Covariance
Page 8
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM
B. Linearization: linearize to approximate the Gaussian, using Taylor expansion- : Jacobian of that
- : Jacobian of that
C. Prediction Step (Time-update): predict the robot motion from the control signal - Generally no need to perform a time-update for stationary landmarks
Page 9
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM
D. Feature Extraction: extract stable salient features from environment Performance
- Stable features- Outlier rejection- Accuracy- Speed
Feature extraction algorithm- Expectation Maximization- RANSAC- Split and Merge- Hough Transform- Incremental Line Fitting (line tracking)
E. Data Association: find matches between features and landmarks to make general landmark observation For each feature
- calculate the predicted feature for each landmark (Observation model)- compute the Mahalanobis Distance- choose the feature/landmark with the lowest distance (Maximum Likelihood) below some threshold
Page 10
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM
F. Update Step (observation-update): For each data association, update state and covariance estimates Where (Kalman Gain)G. Augment: state vector and covariance matrix grow as new landmarks are observed- If a feature has no matches, add it to the state vector as a new landmark- Linearize the new landmark equations
Page 11
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM issue
Convergence
Computational effort- Computation grows quadratically with the number of landmarks , : update step requires that all landmarks and the joint covariance matrix be updated every time an observation is made- Parameter memory of mean and covariance matrix grows quadratically : mean factors, covariance matrix factors for 2-dimensional vehicle
Data Association: EKF SLAM is fragile to incorrect association of observations(feature) to landmarks- If many observations are incorporated into the EKF with wrong data associations which never be removed, the EKF will diverge
Nonlinearity: Non-linearity can be a significant problem in EKF-SLAM (Convergence, Consistency)
Page 12
- Convergence of map : monotonic convergence of the determinant of the map covariance matrix toward zero- The individual landmark variances converge toward a lower bound determined by initial uncertainties in robot position and observations
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : EKF-SLAM issue
Page 13
Data Association on Loop-closure Before closing the loop : landmark uncertainty increases as robot pose uncertainty increases. Then distributions of ob-servations from nearby landmarks will begin to overlap substantially. This overlap leads to am-biguity in the identity of the landmarks After closing the loop : revisiting a known landmark decreases both the robot pose uncertainty, and the uncertainty of landmarks previously observed Loop-closure problem : association problem is compounded in envi-ronments where landmarks are not simple points and indeed look different from different view-points
Robot Uncertainty Landmark Uncertainty
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM
A. Basis for FastSLAM method : FastSLAM decomposes the SLAM problem into particle filtering for one robot localiza-tion problem, and the EKF for a collection of landmark estimation problems Particle Filtering : represent the posterior pdf by a set of randomly chosen (Monte Carlo) weighted sam-ples Reduce sample space by Rao-Blackwellization
- Joint state is partitioned to - only need be sampled - marginal can be obtained
Rao-Blackwellized state: trajectory is represented by weighted samples and the map is computed analytically- The joint SLAM state
- Joint distribution at time - When conditioned on the trajectory, the map landmarks become independent
Page 14
Conditional Map Component Path Posterior Component
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM
B. Sampling a New Pose- Cannot draw samples directly from the SLAM poste-rior at time : draw samples from a simpler distribution called the proposal distribution, and correct for the difference us-ing a technique called IS(importance sampling) Proposal Distribution : generate new particle at time given each particle
Samples drawn from the probabilistic motion model
C. Updating the Landmark Estimates: implements the update equation of each conditional landmark estimation using EKF
Page 15
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM
D. Calculating Importance Weights- Samples from the proposal distribution are distributed according to , therefore do not match the desired posterior(target distribution) : this difference is corrected through IS (importance sampling) Importance Sampling
- In regions where the target distribution is larger than the proposal distribution, the samples receive higher weights: As a result, samples in this region will be picked more
Page 16
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM
The degeneracy phenomenon : after a few iteration with sequential IS, most particles have negligible weights Measure of degeneracy
- Uniform weight : - Severe degeneracy :
F. Importance Resampling: When degeneracy is above some threshold, eliminate particles with low importance weights and multiply particles with high importance weight - The new set is generated by sampling with replacement such that - This step is necessary to insure that the particles are distributed according to the true posterior- Complexity :
Page 17
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM
FastSLAM 1.0 The proposal distribution is the motion model
The samples are weighted according to the marginalized observation model
FastSLAM 2.0 The proposal distribution includes the current observation
=
Importance weight according to ( : normalization constant)
The advantage of FastSLAM 2.0 is that its proposal distribution is locally optimal: for each particle, it give the smallest possible variance in importance weight condi-tioned upon available information, , ,
Page 18
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM issue
Convergence- Theorem : Linear-Gaussian FastSLAM converges in expectation to the correct map with particle if all features are observed infinitely often, and the location of one feature is known in ad-vance
Page 19
Computational Complexity- Computation grows logarithmic increase with the number of landmarks , - Binary tree of landmark filters for a collection of landmark estimation𝑀 : In updating new particle process, instead of dupli-cate the entire tree, a single path is duplicated
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
4. Solutions to the SLAM Problem : FastSLAM issue
Data Association Motion ambiguity
- By Per-Particle Data Association - FastSLAM takes a Multi-Hypothesis approach to the data association problem due to particles- Particles that pick the correct data association will receive high weights : because they explain the observations well
Measurement ambiguity- Monte Carlo Data Association: following other methods..- Individual Compatibility- Joint Compatibility Branch and Bound - Combined Constrained Data Association- Random Joint Compatibility
Page 20
Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University
RISL | Robotics & Intelligent Systems Lab.Seoul National University
5. Conclusion
SLAM problem- Simultaneously determine localization and mapping- Probabilistic SLAM is recursive Bayesian filter problem- In this paper, two solution of SLAM which are EKF-SLAM and FastSLAM is intro-duced
Pros & Cons of SLAM solution
Page 21
EKF-SLAM FastSLAM
Pros
-Straightforward application of the EKF-Works reasonably well for small # of features and distinct landmarks-Retain covariance
-Complexity logarithmic with landmark M-Multi-hypothesis data association(robust)-No linearization of non-linear motion models-Solves both full SLAM and online SLAM
Cons
-Complexity quadratic with landmark M-No guarantee of convergence in non-linear case-Gaussian assumption-Can’t correct for data association errors
-Underestimated covariance cause lower map accuracy given data association-Harder to close the loop (by above)