Probabilistic Robotics: SLAM
= Simultaneous Localization and Mapping
Sebastian Thrun & Alex Teichman
Stanford Artificial Intelligence Lab
Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio
Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike
Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others
SA-1
9-1
The SLAM Problem
A robot is exploring an
unknown, static environment.
Given:
• The robot’s controls
• Observations of nearby features
Estimate:
• Map of features
• Path of the robot
9-2
Chicken-or-Egg
• SLAM is a chicken-or-egg problem
• A map is needed for localizing a robot
• A good pose estimate is needed to build a map
• Thus, SLAM is regarded as a hard problem
•
•
in robotics
A variety of different approaches to
address the SLAM problem have been
presented
Probabilistic methods outperform most
other techniques
9-3
Structure of the Landmarkbased SLAM-Problem
9-4
SLAM Applications
Indoors
Undersea
Space
Underground
9-5
Map Representations
Examples:
City map, subway map, landmark-based map
9-6
Map Representations
• Grid maps or scans
[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras,
99; Haehnel, 01;…]
• Landmark-based
[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…
9-7
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown
Robot path error correlates errors in the map
9-8
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
Pose error correlates data associations
9-9
SLAM:
Simultaneous Localization and Mapping
• Full SLAM: Estimates entire path and map!
p( x1:t , m | z1:t , u1:t )
• Online SLAM:
p ( xt , m | z1:t , u1:t )      p ( x1:t , m | z1:t , u1:t ) dx1dx 2 ...dxt 1
Integrations typically done one at a time
Estimates most recent pose and map!
9-10
Graphical Model of Full SLAM:
p( x1:t , m | z1:t , u1:t )
9-11
Graphical Model of Online SLAM:
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1 dx2 ...dxt 1
9-12
Techniques for Generating
Consistent Maps
• Scan matching
• EKF SLAM
• FastSLAM
• Probabilistic mapping with a single
map and a posterior about poses
Mapping + Localization
• GraphSLAM, SEIF
9-13
Kalman Filter Algorithm
1.
Algorithm Kalman_filter( mt-1, St-1, ut, zt):
2.
3.
4.
Prediction:
m t  At mt 1  Bt ut
St  At St 1 AtT  Rt
5.
6.
7.
8.
Correction:
Kt  St CtT (Ct St CtT  Qt )1
mt  m t  Kt ( zt  Ct m t )
St  (I  Kt Ct )St
9.
Return mt, St
9-14
(E)KF-SLAM
• Map with N landmarks:(3+2N)-dimensional
Gaussian
Bel( xt , mt ) 
x
 
 y
 
 
 l1  ,
l 
 2

 
 lN 
  x2

  xy

  x
  xl1

  xl2
 

 xl
 N
 xy
 y2
 y
 yl1
 yl2

 ylN
 x
 y
 2
 l1
 l2

 lN
 xl1
 yl1
 l1
 l21
 l1l2

 l1lN
 xl2
 yl2
 l2
 l1l2
 l22

 l2l N







 xlN 

 ylN 
 lN 
 l1lN 

 l2l N 
 

2 
 lN 
• Can handle hundreds of dimensions
9-15
EKF-SLAM
Map
Correlation matrix
9-16
EKF-SLAM
Map
Correlation matrix
9-17
EKF-SLAM
Map
Correlation matrix
9-18
Properties of KF-SLAM
(Linear Case) [Dissanayake et al., 2001]
Theorem:
The determinant of any sub-matrix of the
map covariance matrix decreases
monotonically as successive observations
are made.
Theorem:
In the limit the landmark estimates become
fully correlated
9-19
Victoria Park Data Set
[courtesy by E. Nebot]
9-20
Victoria Park Data Set Vehicle
[courtesy by E. Nebot]
9-21
Data Acquisition
[courtesy by E. Nebot]
9-22
Raw Odometry (no SLAM)
GPS (for reference)
Odometry
9-23
Estimated Trajectory
[courtesy by E. Nebot]
9-24
EKF SLAM Application
[courtesy by J. Leonard]
9-25
EKF SLAM Application
odometry
estimated trajectory
[courtesy by John Leonard]
9-26
Approximations for SLAM
• 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]
9-27
EKF-SLAM: Complexity
• Cost per step: O(n2), quadratic in the
number of landmarks: O(n2)
• Total cost to build a map with n
landmarks: O(n3)
• Memory: O(n2)
Approaches exist that make EKF-SLAM
O(n^1.5) / O(n2.5) / O(n)
9-28
EKF-SLAM: Summary
• Convergence for linear case!
• Can diverge if nonlinearities are large
• Has been applied successfully in
large-scale environments
• Approximations reduce the
computational complexity
9-29
Data Association for SLAM
Interpretation tree
9-30
Data Association for SLAM
Env. Dyn.
9-31
Data Association for SLAM
Geometric Constraints
Location independent constraints
Unary constraint:
intrinsic property of feature
e.g. type, color, size
Binary constraint:
relative measure between features
e.g. relative position, angle
Location dependent constraints
Rigidity constraint:
"is the feature where I expect it given
my position?"
Visibility constraint:
"is the feature visible from my position?"
Extension constraint:
"do the features overlap at my position?"
All decisions on a significance level 
9-32
Data Association for SLAM
Interpretation Tree
[Grimson 1987], [Drumheller 1987],
[Castellanos 1996], [Lim 2000]
Algorithm
• backtracking
• depth-first
• recursive
• uses geometric constraints
• exponential complexity
• absence of feature: no info.
• presence of feature: info.
perhaps
9-33
Data Association for EKF SLAM
Pygmalion
a = 0.95 , p = 2
9-34
Data Association for EKF SLAM
Pygmalion
a = 0.95 , p = 3
9-35
Data Association for EKF SLAM
Pygmalion
texe: 633 ms
PowerPC at 300
MHz
a = 0.95 , p = 4
a = 0.95 , p = 5
9-36
Summary: EKF SLAM
• Extends EKF localization by additional
state variables (landmark locations)
• Converges in linear-Gaussian world
• Data association problem: which
measurement corresponds to which
landmark?
• Data association solved by tree
search
9-37