SLAM Techniques and Algorithms Jack Collier

DefenceResearchand DevelopmentCanada

Rechercheetdveloppement pourladfenseCanada

Canada

Goals
What will we learn Gain an appreciation for what SLAM is and can accomplish Understand the underlying theory behind SLAM Understand the terminology and fundamental building blocks of SLAM algorithms Appreciate the deciencies of SLAM and SLAM algorithms Wont focus on the math, but the concept Online non-linear feature based SLAM with unknown data association
DefenceR&DCanadaR&DpourladfenseCanada

Outline
What is SLAM? Probabilistic basis of SLAM EKF SLAM Data Association Closing the Loop FastSLAM Sub-Mapping SLAM Resources Questions?

DefenceR&DCanadaR&DpourladfenseCanada

What is SLAM?

Simultaneous Localization and Mapping Given an unknown environment and vehicle pose:
Move through the environment Estimate the robot pose Generate a map of environmental features

Use the robot pose estimate to improve the map landmark position estimates Use the landmark estimates to improve the robot pose estimate

DefenceR&DCanadaR&DpourladfenseCanada

What is SLAM? and why do we need it?

Robot motion models arent accurate Wheel odometry error is cumulative IMU sensors errors Maps are generated from sensors on the vehicle. If we cant accurately predict the robot pose then how can we produce an accurate map. GPS can help but is not always available or reliable.
DefenceR&DCanadaR&DpourladfenseCanada

What is SLAM? and why do we need it?

DefenceR&DCanadaR&DpourladfenseCanada

What is SLAM? and why do we need it?

DefenceR&DCanadaR&DpourladfenseCanada

What is SLAM? The SLAM problem

Given: Robot control signal uk (or measurement) A set of feature observations zK (sensor measurements) Estimate: Map of landmarks Mk +1 Robot Pose Vk +1 Sources of Error: Control signal Mapping sensor Motion model Observation model

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM Probability Theory

Probability Theory gives us a framework for dealing with these sources of error The online SLAM theory is: p (Vk +1 , Mk +1 |zk , uk ) Estimate the Joint Probability of Vk +1 and Mk +1 conditioned on zk , uk (Joint Conditional PDF)

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM Probability toolkit

Conditional Probability p (x |y ) =

p ( x ,y ) p (y )

Product Rule p (x , y ) = p (x |y )p (y )
|x )p (x ) Bayes Rule p (x |y ) = p(yp or (y ) p (x |y ) = p (y |x )p (x ) Gaussian PDF 1 p (x ) = det (2 ) 2 exp 1 (x x )T 1 (x x ) 2

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) =

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter)

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter) = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 , Mk +1 |Vk , zk , uk +1 )p (Vk |zk , uk +1 )dvk

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter) = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 , Mk +1 |Vk , zk , uk +1 )p (Vk |zk , uk +1 )dvk = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 |Vk , uk )p (Vk , Mk |zk , uk )dvk No closed form solution Approximate the solution using an Extended Kalman Filter

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter) = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 , Mk +1 |Vk , zk , uk +1 )p (Vk |zk , uk +1 )dvk = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 |Vk , uk )p(Vk , Mk |zk , uk )dvk Prior probability: Prior state and covariance estimates from the last lter iteration

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter) = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 , Mk +1 |Vk , zk , uk +1 )p (Vk |zk , uk +1 )dvk = p (zk +1 |Vk +1 , Mk +1 ) p(Vk+1 |Vk , uk )p (Vk , Mk |zk , uk )dvk Motion Model: Probabilistic motion model estimates the new vehicle pose covariance estimates from the prior estimate and the control

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM SLAM as a Bayesian lter

p (Vk +1 , Mk +1 |zk +1 , uk +1 ) = p (zk +1 |Vk +1 , Mk +1 )p (Vk +1 , Mk +1 |zk , uk +1 ) (Bayes Filter) = p (zk +1 |Vk +1 , Mk +1 ) p (Vk +1 , Mk +1 |Vk , zk , uk +1 )p (Vk |zk , uk +1 )dvk = p(zk+1 |Vk+1 , Mk+1 ) p (Vk +1 |Vk , uk )p (Vk , Mk |zk , uk )dvk Measurement Model: Measurement model gives the expected value of the feature observations.

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM Gaussian PDF

DefenceR&DCanadaR&DpourladfenseCanada

Probabilistic Basis of SLAM Gaussian PDF

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM
Simultaneous Localization and Mapping: Joint estimate both robot pose and position of unique landmarks Landmark estimates improve robot pose estimates and vice versa Use standard Extended Kalman Filtering techniques Assumes Gaussian noise and error. m x1 r ym xk 1 . r y Mk = . , V = k k .m r k xn m yn
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Algorithm

Xk =

Vk Mk

, Pk =

Pv Pvm T Pvm Pm

(1)

Prediction - A prediction of the new state vector and covariance matrix is calculated from the previous state and covariance, and the new control uk .

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Algorithm

Xk =

Vk Mk

, Pk =

Pv Pvm T Pvm Pm

(1)

Data Association - Find matches between the current landmarks Mk and the new set of observed features zk .

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Algorithm

Xk =

Vk Mk

, Pk =

Pv Pvm T Pvm Pm

(1)

Measurement Update - Calculate the Kalman gain for each observed landmark and update the state and covariance values based on that Kalman gain and the measurement innovation.

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Algorithm

Xk =

Vk Mk

, Pk =

Pv Pvm T Pvm Pm

(1)

Augmentation - Integrate newly observed landmarks into the state vector and covariance matrix.

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Linearization

Kalman Filters work under a linear Gaussian assumption Robot motion and measurement functions are non-linear Use Taylor Expansion to linearize (Approximate a Gaussian) g g +g g is Jacobian of the function with respect to its variables
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Linearization

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Prediction

Predict the robot motion from the control signal Linearize to approximate the Gaussian k +1 = Vk + V V

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Prediction

Predict the robot motion from the control signal Linearize to approximate the Gaussian

Vk +1

vk vk r r r k sin(k )+ sin ( + t ) xk k k k vk r vk r r cos (k cos (k ) + k t ) = yk + k k r k k t

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Prediction

Predict the robot motion from the control signal Linearize to approximate the Gaussian

k +1 P

v (k +1) = Gk Pv (k ) G T P k vm(k +1) = Rk QR T P k v (k +1) P vm(k +1) P = T P Pm vm(k +1)

G linearizes the state transition function R maps additional motion noise into the state space
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

Extract stable salient features from environment):

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm?

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? Stable features Outlier rejection Accuracy Speed

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? Expectation Maximization

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? RANSAC

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? Split and Merge

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? Hough Transform

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Feature Extraction Algorithms

What do we want in feature extraction algorithm? Incremental Line Fitting

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Laser Feature Extraction

Extract features from Raw laser data Incremental algorithm (line tracking)
Fit a line to a set of points Compute the residual If below error threshold add another point and repeat If above the last point added is a feature
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Data Association

Find matches between features and landmarks For each feature
Calculate the predicted feature for each landmark (measurement model) compute the Mahalanobis Distance: choose the feature/landmark with the lowest distance (Maximum Likelihood) below some threshold

End for
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Data Association

Measurement Model: zjm = rjm jm = y 2 + x 2 arctan (y , x )

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Data Association

Measurement Model: zjm = rjm y 2 + x 2 = m j arctan (y , x ) Mahalanobis Distance normalizes feature/landmark distance based on their covariances:
T 1 m dij = (zi zjm )T (Hj Pk +1 Hj + R ) (zi zj )

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Why Mahalanobis Distance?

Why Mahalanobis Distance:

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Why Mahalanobis Distance?

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Why Mahalanobis Distance?

Points which lie on the same covariance ellipse have are equidistant
DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Update

For each data association
Calculate Kalman Gain: k +1 Hj T (Hj P k +1 Hj T + R )1 Kj = P H linearizes the measurement model Update state and covariance estimates: k +1 + Kj (zi z m ) Xk +1 = X j k +1 Kj Hj P k +1 Pk +1 = P

End for Innovation vector - Larger bigger correction Kalman Gain - Weights the innovation vector.
Smaller measurement error higher gain (trust the measurement) Smaller covariance DefenceR&DCanadaR&Dpourlad smaller gain (trust the fenseCanada prediction)

EKF SLAM 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

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Augment

New Landmark Locations:
r r xim = xk + ri cos(k + i ) m r r yi = yk + ri sin(k + i )

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Augment

Append to state vector:

X X = xim yim

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Augment

Original Covariance Matrix:

P=

Pv Pvm T Pvm Pm

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Augment

Augmented Covariance Matrix:

Pv Pvm Pv T Fi T Pa = Pvm T Pm Pvm T Fi T T T Fi Pv Fi Pvm Fi Pv Fi + Mi RMi F and M are the Jacobians which linearize the new landmark equations with respect to vehicle pose and measurement variables respectively

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Other Issues

Can use provisional landmark list to reject spurious features Pruning (removing old/irrelevant landmarks) Landmark Signatures (reectivity/color/shape/etc.) Landmark minimum distance Intelligent update (Only update relevant landmarks) Sub-Mapping (more on this later)

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Advantages

Straightforward application of the EKF Large body of research to pull from Works reasonably well for small number of features and distinct landmarks

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Disadvantages

Complexity Quadratic with number of features No guarantee of convergence in non-linear case Make hard decisions about data associations Cant correct for erroneous data associations Need suciently distinct landmarks (low clutter) Gaussian Assumption

DefenceR&DCanadaR&DpourladfenseCanada

EKF SLAM Variants

Unscented KF SLAM
Linearization using Sigma points Preserves the true mean and covariance of the posterior

Extended Information Filter SLAM

Dual representation of EKF Less complex measurement update

DefenceR&DCanadaR&DpourladfenseCanada

Data Association
Erroneous data association WILL cause SLAM to fail!

1 feature to 2 landmarks 2 features to 1 landmark clutter Symmetry Reduce the max Mahalanobis distance?

DefenceR&DCanadaR&DpourladfenseCanada

Data Association

How do determine the correspondence between a feature and landmark? Individual Compatibility Joint Compatibility Branch and Bound Combined Constrained Data Association Randomized Joint Compatibility Multi hypothesis Data Association

DefenceR&DCanadaR&DpourladfenseCanada

Data Association Individual Compatibility

Calculate Mahalanobis distance from each feature to landmark Choose feature with smallest distance to landmark within threshold (validation gate) Disregard ambiguous associations Advantage: Simple Disadvantage: Can be error prone especially in clutter

DefenceR&DCanadaR&DpourladfenseCanada

Data Association Joint Compatibility Branch and Bound

Consider multiple associations simultaneously Find the largest set of matches which correspond Joint Mahalanobis distance measurement with joint validation gate Branch and bound interpretation tree search Advantage: Robust Disadvantage: Tree search computation/May be Ambiguous sets
DefenceR&DCanadaR&DpourladfenseCanada

Data Association Joint Compatibility Branch and Bound

Potential Matches z 1 L1 or L2 z 2 L2 or L3 z 3 L4 Null

DefenceR&DCanadaR&DpourladfenseCanada

Data Association Joint Compatibility Branch and Bound

DefenceR&DCanadaR&DpourladfenseCanada

Data Association Combined Constrained Data Association

Correspondence Graph technique Nodes are individually compatible associations Edges are compatible associations Find the maximum clique of associations that are mutually compatible Consider both relative and absolute constraints Advantage: Can work with no pose info (relative constraints) Disadvantage: May be ambiguous sets
DefenceR&DCanadaR&DpourladfenseCanada

Data Association Randomized Joint Compatibility

JCBB with RANSAC Uses relative constraints Randomly select m of the n feature measurements Advantage: Reduced complexity, no pose necessary Disadvantage: Dont always know Pfail and Pgood

DefenceR&DCanadaR&DpourladfenseCanada

Closing the Loop What is loop closing?

Even with the best SLAM algorithm pose uncertainty will increase as the vehicle moves. This pose uncertainty means that landmark locations further from the map origin have a higher uncertainty Revisiting a previously observed landmarks signicantly reduces uncertainty in robot and landmark pose estimates Errors in landmark estimates are correlated with robot pose New pose info necessarily improved landmark estimates DefenceR&DCanadaR&DpourladfenseCanada

Closing the Loop How to Close the loop

Map Matching Techniques (feature matching) Can use pose estimate to constrain the search Expectation Maximization Algorithm Non-linear Optimization

DefenceR&DCanadaR&DpourladfenseCanada

FastSLAM
Represent probability distributions by set of particles Rao-Blackwellized particle lter (low dimensional EKFs) Each particle maintains its own pose and map estimates (multi-hypothesis) Each landmark has its own EKF N landmarks and M particles we have MxN + 1 lters

DefenceR&DCanadaR&DpourladfenseCanada

FastSLAM Algorithm
Do M times: Retrieve pose from particle k Predict a new pose (motion model) Data Association Measurement Update Importance Weight END Re-sample with Replacement using the importance weight Raw Odometry FastSLAM
DefenceR&DCanadaR&DpourladfenseCanada

FastSLAM Advantages

Time logarithmic to number of landmarks Multi-hypothesis data association (robustness) No linearization of non-linear motion models Solves both full SLAM and Online SLAM problems

DefenceR&DCanadaR&DpourladfenseCanada

FastSLAM Disadvantages

Throws away correlations Over optimistic pose estimate Harder to close the loop

DefenceR&DCanadaR&DpourladfenseCanada

Sub-Mapping SLAM
Partition SLAM into sub-maps Optimally Fuse at a global level periodically Reduces EKF linearization errors Reduces computational complexity Closing the loop techniques for sub-maps

DefenceR&DCanadaR&DpourladfenseCanada

Sub-Mapping SLAM Algorithms

ATLAS Hybrid-SLAM Decoupled Stochastic Mapping Hierarchical SLAM Conditionally In dependant SLAM Divide and Conquer EKF SLAM vs. D&C SLAM
DefenceR&DCanadaR&DpourladfenseCanada

Other Topics
Visual SLAM
SIFT, Harris Corners, SURF, etc. for features No range to features Inverse Depth

Muti-robot SLAM
Teams map building Information sharing/cooperation

Learning for SLAM

Adaptive SLAM Learnt sensor, observation, motion models

6-DOF SLAM/Mesh Models Feature Extraction Algorithms Scan matching

DefenceR&DCanadaR&DpourladfenseCanada

Resources
Probabilistic Robotics

SLAM Summer School 2009

http://www.acfr.usyd.edu.au/education/summerschool.shtml

openslam.org (MATLAB, C, Dierent algorithms, data sets, etc.)

DefenceR&DCanadaR&DpourladfenseCanada

Questions?

DefenceR&DCanadaR&DpourladfenseCanada