Probabilistic Robotics
SLAM
The SLAM Problem
A robot is exploring an
unknown, static environment.
Given:
• The robot’s controls (U1:t)
• Observations of nearby features (Z1:t)
Estimate:
• Map of features (m)
• Pose / Path of the robot (xt)
2
Why is SLAM a hard problem?
Robot pose
uncertainty
• In the real world, the mapping between
•
observations and landmarks is unknown
Picking wrong data associations can have
catastrophic consequences
3
Nature of the SLAM Problem
 Continuous
component
Location of objects in
the map
Robot’s own pose
 Discrete
component
Correspondence
Object is the same or not
4
SLAM:
Simultaneous Localization and Mapping
• Full SLAM:
Estimates entire path and map!
p( x1:t , m | z1:t , u1:t )
Estimates
Given
Entire pose (x1:t) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
5
Graphical Model of Full SLAM:
p( x1:t , m | z1:t , u1:t )
6
SLAM:
Simultaneous Localization and Mapping
• Online SLAM:
p( xt , m | z1:t , u1:t )
Estimates
Given
Most recent pose (xt) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
Estimates most recent pose and map!
7
Graphical Model of Online SLAM:
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1 dx2 ...dxt 1
Integrations typically done one at a time
8
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
9
EKF-SLAM with known correspondences
Correspondence
Data association problem
Landmarks can’t be
uniquely identified
Correspondence variable
(Cit)
True identity of
observed feature
between feature (fit) and real landmark

f t  rt
i
i

i
t
S

i T
t
10
EKF-SLAM with known correspondences
Signature
Numerical value (average color)
Characterize type of landmark
(integer)
Multidimensional vector
(height and color)
11
EKF-SLAM with known correspondences
Similar development to EKF localization
Diff  robot pose + coordinates of all landmarks
Combined state vector
 xt 
yt     x
m
y 
m1, x
p( yt | z1:t , u1:t )
m1, y
S1 ... mN , x
(3N + 3)
mN , y
SN 
T
Online posterior
12
Mean
Motion update
Covariance
Test for new landmarks
Initialization
of elements
Expected measurement
Iteration through
measurements
Filter is updated
13
EKF-SLAM with known correspondences
Observing a landmark
improves robot pose estimate
eliminates some uncertainty
of other landmarks
Improves position estimates of
the landmark + other landmarks
We don’t need to model past
poses explicitly
14
EKF-SLAM with known correspondences
Example:
15
EKF-SLAM with known correspondences
Example:
• Uncertainty of landmarks are mainly due to
robot’s pose uncertainty (persist over time)
Estimated location of landmarks are correlated
16
EKF-SLAM with unknown correspondences
• No correspondences for landmarks
• Uses an incremental maximum likelihood (ML) estimator
Determines most likely value of the correspondence variable
Takes this value for granted later on
17
EKF-SLAM with unknown correspondences
Mean
Motion update
Covariance
Hypotheses of
new landmark
18
19
General Problem
Gaussian noise assumption
Unrealistic
Spurious measurements
Fake landmarks
Outliers
Affect robot’s localization
20
Solutions to General Problem
Provisional landmark list
New landmarks do not augment the map
Not considered to adjust robot’s pose
Consistent observation
regular map
21
Solutions to General Problem
Landmark Existence Probability
Landmark is observed
Observable variable (o) increased by fixed value
Landmark is NOT observed when it should
Observable variable decreased
Removed from map when (o) drops below
threshold
22
Problem with Maximum Likelihood (ML)
Once ML estimator determines likelihood of correspondence, it
takes value for granted
always correct
Makes EKF susceptible to landmark confusion
Wrong results
23
Solutions to ML Problem
Spatial arrangement
Greater distance between landmarks
Less likely confusion will exist
Trade off:
few landmarks
harder to localize
Little is known about optimal density of
landmarks
Signatures
Give landmarks a very perceptual distinctiveness
(e,g, color, shape, …)
24
EKF-SLAM Limitations
• Selection of appropriate landmarks
• Reduces sensor reading utilization to presence or absence of
those landmarks
Lots of sensor data is discarded
• Quadratic update time
Limits algorithm to scarce maps (< 1000
features)
• Low dimensionality of maps
harder data association
problem
25
EKF-SLAM Limitations
• Fundamental Dilemma of EKF-SLAM
It might work well with dense maps (millions of features)
It is brittle with scarce maps
BUT
It needs scarce maps because of complexity of the
algorithm (update process)
26
SLAM Techniques
• EKF SLAM  (chapter 10)
• Graph-SLAM  (chapter 11)
• SEIF  (chapter 12)
• Fast-SLAM  (chapter 13)
27
Graph-SLAM
• Solves full SLAM problem
• Represents info as a graph of soft constraints
• Accumulates information into its graph without
resolving it (lazy SLAM)
• Computationally cheap
• At the other end of EKF-SLAM
Process information right
away (proactive SLAM)
Computationally expensive
28
Graph-SLAM
• Calculates posteriors over robot path (not
incremental)
• Has access to the full data
• Uses inference to create map using stored data
Offline algorithm
29
Sparse Extended Information
Filter (SEIF)
• Implements a solution to online SLAM problem
• Calculates current pose and map (as EKF)
• Stores information representation of all knowledge (as
Graph-SLAM)
Runs Online and is computationally efficient
• Applicable to multi-robot SLAM problem
30
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
31
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
32
Approximations for SLAM Problem
• Local submaps
[Leonard et al.99, Bosse et al. 02, Newman et al. 03]
• Sparse links (correlations)
[Lu & Milios 97, Guivant & Nebot 01]
• Sparse extended information filters
[Frese et al. 01, Thrun et al. 02]
• Thin junction tree filters
[Paskin 03]
• Rao-Blackwellisation (FastSLAM)
[Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]
33
Thanks !!
34