slam techniques and algorithms

7/30/2019 SLAM Techniques and Algorithms

1/73

Defence Research andDevelopment Canada

Recherche et dveloppementpour la dfense Canada Canada

SLAM Techniques and Algorithms

Jack Collier
http://find/http://goback/


2/73

Defence R&D Canada R & D pour la dfense Canada

Goals

What will we learnGain an appreciation for what SLAM is and canaccomplish

Understand the underlying theory behind SLAM

Understand the terminology and fundamentalbuilding blocks of SLAM algorithms

Appreciate the deficiencies of SLAM and SLAMalgorithms

Wont focus on the math, but the conceptOnline non-linear feature based SLAM withunknown data association
http://find/


3/73


Outline

What is SLAM?

Probabilistic basis of SLAM

EKF SLAM

Data AssociationClosing the Loop

FastSLAM

Sub-Mapping SLAM

ResourcesQuestions?
http://find/


4/73


What is SLAM?

Simultaneous Localization and Mapping

Given an unknown environment and vehicle pose:

Move through the environment

Estimate the robot poseGenerate a map of environmental features

Use the robot pose estimate to improve the maplandmark position estimates

Use the landmark estimates to improve the robotpose estimate


5/73


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 wecant accurately predict the robot pose then how can weproduce an accurate map.

GPS can help but is not always available or reliable.


6/73


What is SLAM?



7/73


What is SLAM?



8/73


What is SLAM?

The SLAM problem

Given:Robot control signal uk (or measurement)

A set of feature observations zK (sensormeasurements)

Estimate:Map of landmarks Mk+1

Robot Pose Vk+1

Sources of Error:

Control signal

Mapping sensor

Motion model

Observation model
http://find/


9/73


Probabilistic Basis of SLAM

Probability Theory

Probability Theory gives us a framework for dealing

with these sources of errorThe 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)


11/73



SLAM as a Bayesian filter

p(Vk+1, Mk+1|zk+1, uk+1) =
http://find/


12/73




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)
http://find/


15/73





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



Prior probability:Prior state and covariance estimates from the last filter iteration

S
http://find/


16/73





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



Motion Model:Probabilistic motion model estimates the new vehicle pose covari-

ance estimates from the prior estimate and the control

P b bili i B i f SLAM
http://find/


17/73





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



Measurement Model:Measurement model gives the expected value of the feature obser-

vations.

http://find/


18/73



Gaussian PDF

http://find/


19/73



Gaussian PDF

EKF SLAM
http://find/


20/73


EKF SLAM

Simultaneous Localization and Mapping:

Joint estimate both robot pose and position ofunique landmarks

Landmark estimates improve robot pose estimatesand vice versa

Use standard Extended Kalman Filtering techniques

Assumes Gaussian noise and error.

Mk =

xm1ym1

...xmn

ymn

, Vk = x

rk

yrkrk

EKF SLAM
http://find/


21/73


EKF SLAM

Algorithm

Xk = VkMk

, Pk = Pv PvmP

Tvm Pm

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

EKF SLAM
http://find/


22/73


EKF SLAM

Algorithm

Xk = VkMk

, Pk = Pv PvmP

Tvm Pm

(1)Data Association - Find matches between the currentlandmarks Mk and the new set of observed features zk.

EKF SLAM
http://find/


23/73


EKF SLAM

Algorithm

Xk = VkMk

, Pk = Pv PvmP

Tvm Pm

(1)Measurement Update - Calculate the Kalman gain foreach observed landmark and update the state and covari-ance values based on that Kalman gain and the measure-

ment innovation.

EKF SLAM


24/73


EKF SLAM

Algorithm

Xk = VkMk

, Pk = Pv PvmP

Tvm Pm

(1)Augmentation - Integrate newly observed landmarks intothe state vector and covariance matrix.

EKF SLAM
http://goforward/http://find/http://goback/


25/73


EKF SLAM

Linearization

Kalman Filters work under a linear Gaussianassumption

Robot motion and measurement functions arenon-linear

Use Taylor Expansion to linearize (Approximate aGaussian)g g + g

g is Jacobian of the function with respect to itsvariables

EKF SLAM
http://find/


26/73


EKF SLAM

Linearization

EKF SLAM


27/73


EKF SLAM

Prediction

Predict the robot motion from the control signalLinearize to approximate the Gaussian

Vk+1 = Vk + V

EKF SLAM
http://find/


28/73


EKF SLAM

Prediction


Vk+1 =

xrkyrkrk

+

vkk

sin(rk) +vkk

sin(rk + kt)vkk

cos(rk)vkk

cos(rk + kt)

kt

EKF SLAM
http://find/


29/73


EKF SLAM

Prediction


Pv(k+1) = GkPv(k)GTkPvm(k+1) = RkQR

Tk

Pk+1 =

Pv(k+1) Pvm(k+1)

PTvm(k+1) Pm

G linearizes the state transition functionR maps additional motion noise into the state space

EKF SLAM


30/73


EKF SLAM

Feature Extraction Algorithms

Extract stable salient features from environment):

EKF SLAM
http://find/


31/73


EKF SLAM


What do we want in feature extraction algorithm?

EKF SLAM
http://find/


32/73


EKF SLAM



Stable featuresOutlier rejection

Accuracy

Speed

EKF SLAM


33/73


EKF SLAM



Expectation Maximization

EKF SLAM
http://find/


34/73


K S



RANSAC

EKF SLAM
http://find/


35/73




Split and Merge

EKF SLAM
http://find/


36/73




Hough Transform

EKF SLAM
http://find/


37/73




Incremental Line Fitting

EKF SLAM
http://find/


38/73


Laser Feature Extraction

Extract features from Raw laserdata

Incremental algorithm (linetracking)

Fit a line to a set of pointsCompute the residualIf below error threshold add

another point and repeatIf above the last point addedis a feature

EKF SLAM
http://find/


39/73


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 lowestdistance (Maximum Likelihood) below some

thresholdEnd for

EKF SLAM
http://find/


40/73


Data Association

Measurement Model:

zmj =

rmjmj

= y2 + x2

arctan (y, x)

EKF SLAM
http://find/


41/73


Data Association

Measurement Model:

zmj =

rmjmj

= y2 + x2

arctan (y, x)

Mahalanobis Distance normalizes feature/landmarkdistance based on their covariances:

dij = (zi zmj )T(Hj Pk+1HjT + R)1(zi zmj )

EKF SLAM


42/73


Why Mahalanobis Distance?

Why Mahalanobis Distance:

EKF SLAM
http://find/


43/73



EKF SLAM
http://find/


44/73



Points which lie on the same covariance ellipse have areequidistant

EKF SLAM
http://find/


45/73


Update

For each data associationCalculate Kalman Gain:Kj = Pk+1Hj

T(HjPk+1HjT + R)1

H linearizes the measurement modelUpdate state and covariance estimates:

Xk+1 = Xk+1 + Kj(zi zmj )Pk+1 = Pk+1 KjHjPk+1

End for

Innovation vector - Larger bigger correction

Kalman Gain - Weights the innovation vector.Smaller measurement error higher gain (trustthe measurement)Smaller covariance smaller gain (trust the

prediction)

EKF SLAM
http://find/


46/73


Augment

State Vector and Covariance matrix grow as newlandmarks are observed

If a feature has no matches, add it to the statevector as a new landmark

EKF SLAM
http://find/


47/73


Augment

New Landmark Locations:xmi = x

rk + ri cos(

rk + i)

ymi = yrk + ri sin(

rk + i)

EKF SLAM


48/73


Augment

Append to state vector:

X = X

xmi

ymi

EKF SLAM

A
http://find/


49/73


Augment

Original Covariance Matrix:

P = Pv PvmPvm

T

Pm

EKF SLAM

A


50/73


Augment

Augmented Covariance Matrix:

Pa = Pv Pvm Pv

TFiT

PvmT

Pm PvmT

FiT

FiPv FiPvm FiPvFiT + MiRMi

T

F and M are the Jacobians which linearize the new land-mark equations with respect to vehicle pose and measure-ment variables respectively

EKF SLAM

Oth I
http://find/


51/73


Other Issues

Can use provisional landmark list to reject spuriousfeatures

Pruning (removing old/irrelevant landmarks)Landmark Signatures (reflectivity/color/shape/etc.)

Landmark minimum distance

Intelligent update (Only update relevant landmarks)

Sub-Mapping (more on this later)

EKF SLAM

Ad t
http://find/


52/73


Advantages

Straightforward application of the EKF

Large body of research to pull from

Works reasonably well for small number of featuresand distinct landmarks

EKF SLAM

Di d t
http://find/


53/73


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 sufficiently distinct landmarks (low clutter)

Gaussian Assumption

EKF SLAM

Variants
http://find/


54/73


Variants

Unscented KF SLAM

Linearization using Sigma points

Preserves the true mean and covariance of theposterior

Extended Information Filter SLAM

Dual representation of EKFLess complex measurement update

Data AssociationErroneous data association WILL cause SLAM to fail!
http://find/


55/73


Erroneous data association WILL cause SLAM to fail!

1 feature to 2 landmarks

2 features to 1 landmarkclutter

Symmetry

Reduce the max

Mahalanobis distance?

Data Association
http://find/


56/73


How do determine the correspondence between a featureand landmark?

Individual Compatibility

Joint Compatibility Branch and BoundCombined Constrained Data Association

Randomized Joint Compatibility

Multi hypothesis Data Association

Data Association



57/73



Calculate Mahalanobis distance from each featureto landmark

Choose feature with smallest distance to landmarkwithin threshold (validation gate)

Disregard ambiguous associations

Advantage: Simple

Disadvantage: Can be error prone especially inclutter

Data Association

Joint Compatibility Branch and
http://find/


58/73



Bound

Consider multiple associations simultaneously

Find the largest set of matches which correspond

Joint Mahalanobis distance measurement with jointvalidation gate

Branch and bound interpretation tree search

Advantage: Robust

Disadvantage: Tree search computation/May beAmbiguous sets

Data Association

http://find/


59/73



Bound

PotentialMatchesz1 L1 or L2z2 L2 or L3

z3 L4Null

Data Association

http://find/


60/73



Bound

Data Association

Combined Constrained Data
http://find/


61/73


Combined Constrained Data

Association

Correspondence Graph technique

Nodes are individually compatible associations

Edges are compatible associationsFind the maximum clique of associations that aremutually compatible

Consider both relative and absolute constraints

Advantage: Can work with no pose info (relativeconstraints)

Disadvantage: May be ambiguous sets

Data Association

http://find/


62/73



JCBB with RANSAC

Uses relative constraintsRandomly select m of the n feature measurements

Advantage: Reduced complexity, no pose necessary

Disadvantage: Dont always know Pfail and Pgood

Closing the Loop

What is loop closing?
http://find/


63/73


What is loop closing?

Even with the best SLAM algorithm poseuncertainty will increase as the vehicle moves.

This pose uncertainty means that landmarklocations further from the map origin have a higher

uncertaintyRevisiting a previously observed landmarkssignificantly reduces uncertainty in robot andlandmark pose estimates

Errors in landmark estimates are correlated withrobot pose

New pose info necessarily improved landmarkestimates

Closing the Loop

How to Close the loop
http://find/


64/73


How to Close the loop

Map Matching Techniques (feature matching)

Can use pose estimate to constrain the searchExpectation Maximization AlgorithmNon-linear Optimization

FastSLAMRepresent probability distributions by set of particles
http://find/


65/73


Rao-Blackwellized particle filter (low dimensionalEKFs)Each particle maintains its own pose and mapestimates (multi-hypothesis)Each landmark has its own EKFN landmarks and M particles we have MxN + 1

filters

FastSLAM

Algorithm
http://find/


66/73


go t

Do M times:

Retrieve pose from particle k

Predict a new pose (motion model)

Data Association

Measurement UpdateImportance Weight

ENDRe-sample with Replacement using the importance

weight

Raw Odometry

FastSLAM

FastSLAM

Advantages
http://pics/rawodom.avihttp://pics/fastslam.avihttp://pics/fastslam.avihttp://pics/rawodom.avihttp://find/


67/73


g

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

FastSLAM

Disadvantages


68/73


g

Throws away correlations

Over optimistic pose estimate

Harder to close the loop

Sub-Mapping SLAM

Partition SLAM into sub-maps
http://find/


69/73


pOptimally Fuse at a global level periodically

Reduces EKF linearization errorsReduces computational complexityClosing the loop techniques for sub-maps

Sub-Mapping SLAM

Algorithms
http://find/


70/73


g

ATLAS

Hybrid-SLAM

Decoupled Stochastic MappingHierarchical SLAM

Conditionally In dependant SLAM

Divide and Conquer

EKF SLAM vs. D&C SLAM

Other Topics
http://pics/dcslam.avihttp://pics/dcslam.avihttp://find/


71/73


Visual SLAM

SIFT, Harris Corners, SURF, etc. for featuresNo range to featuresInverse Depth

Muti-robot SLAM

Teams map building

Information sharing/cooperation

Learning for SLAM

Adaptive SLAMLearnt sensor, observation, motion models

6-DOF SLAM/Mesh Models


Scan matching

Resources
http://find/


72/73


Probabilistic Robotics

SLAM Summer School 2009http://www.acfr.usyd.edu.au/education/summerschool.shtml

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

Questions?
http://find/


73/73

http://find/

slam techniques and algorithms

Documents