Probabilistic Robotics
SLAM
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
2
Structure of the Landmarkbased SLAM-Problem
3
SLAM Applications
Indoors
Undersea
Space
Underground
4
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;…
5
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown
Robot path error correlates errors in the map
6
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
7
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 ) dx1dx2 ...dxt 1
Integrations typically done one at a time
Estimates most recent pose and map!
8
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
Graphical Model of Full SLAM:
p( x1:t , m | z1:t , u1:t )
10
Scan Matching
Maximize the likelihood of the i-th pose and
map relative to the (i-1)-th pose and map.

[ t 1]
xˆt  arg max p( zt | xt , mˆ

)  p( xt | ut 1 , xˆt 1 )
xt
robot motion
current measurement
map constructed so far
ˆ according to “mapping
Calculate the map m
with known poses” based on the poses and
observations.
[t ]
11
Kalman Filter Algorithm
1.
Algorithm Kalman_filter( mt-1, St-1, ut, zt):
2.
3.
4.
Prediction:
m t  At mt 1  Bt ut
5.
6.
7.
8.
Correction:
9.
Return mt, St
St  At St 1 AtT  Rt
Kt  St CtT (Ct St CtT  Qt )1
mt  m t  Kt ( zt  Ct m t )
St  ( I  Kt Ct )St
13
Kalman
Filter
Algorithm
Known
correspondences
14
Kalman
Filter
Algorithm
Unknown
correspondences
15
Kalman Filter Algorithm
Unknown
Correspondences
(cont’d)
16
(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

 yl N
 x
 y
 2
 l1
 l2

 lN
 xl1
 yl1
 l1
 l21
 l1l2

 l1lN
 xl2
 yl2
 l2
 l1l2
 l22

 l2l N
  xl N 

  ylN 

  lN 
  l1lN 

  l2l N 
  

2 
  lN 
• Can handle hundreds of dimensions
17
Classical Solution – The EKF
• Approximate the SLAM posterior with a high•
dimensional Gaussian [Smith & Cheesman, 1986] …
Single hypothesis data association
18
EKF-SLAM
Map
Correlation matrix
19
EKF-SLAM
Map
Correlation matrix
20
EKF-SLAM
Map
Correlation matrix
21
Properties of KF-SLAM
(Linear Case) [Dissanayake et al., 2001]
Theorem:
In the limit the landmark estimates
become fully correlated
22
Victoria Park Data Set
[courtesy by E. Nebot]
23
Victoria Park Data Set Vehicle
[courtesy by E. Nebot]
24
Data Acquisition
[courtesy by E. Nebot]
25
SLAM
[courtesy by E. Nebot]
26
Map and Trajectory
Landmarks
Covariance
[courtesy by E. Nebot]
27
Landmark Covariance
[courtesy by E. Nebot]
28
Estimated Trajectory
[courtesy by E. Nebot]
29
EKF SLAM Application
[courtesy by John Leonard]
30
EKF SLAM Application
odometry
estimated trajectory
[courtesy by John Leonard]
31
EKF-SLAM Summary
• Quadratic in the number of
landmarks: O(n2)
• Convergence results for the linear
case.
• Can diverge if nonlinearities are large!
• Has been applied successfully in
large-scale environments.
• Approximations reduce the
computational complexity.
32