Post on 29-Dec-2015
transcript
9-1SA-1
Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping
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
Sebastian Thrun & Alex Teichman
Stanford Artificial Intelligence Lab
9-2
Given:
• The robot’s controls
• Observations of nearby features
Estimate:
• Map of features
• Path of the robot
The SLAM Problem
A robot is exploring an unknown, static environment.
9-3
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-4
Structure of the Landmark-based SLAM-Problem
9-5
SLAM Applications
Indoors
Space
Undersea
Underground
9-6
Map Representations
Examples:City map, subway map, landmark-based map
9-7
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-8
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown
Robot path error correlates errors in the map
9-9
Why is SLAM a hard problem?
• 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
Robot poseuncertainty
9-10
SLAM: Simultaneous Localization and Mapping
•Full SLAM:
•Online SLAM:
Integrations typically done one at a time
),|,( :1:1:1 ttt uzmxp
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
Estimates most recent pose and map!
Estimates entire path and map!
9-11
Graphical Model of Full SLAM:
),|,( :1:1:1 ttt uzmxp
9-12
Graphical Model of Online SLAM:
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
9-13
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-14
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
ttttt uBA 1
tTtttt RAA 1
1)( tTttt
Tttt QCCCK
)( tttttt CzK
tttt CKI )(
9-15
2
2
2
2
2
2
2
1
21
2221222
1211111
21
21
21
,),(
NNNNNN
N
N
N
N
N
llllllylxl
llllllylxl
llllllylxl
lllyx
ylylylyyxy
xlxlxlxxyx
N
tt
l
l
l
y
x
mxBel
• Map with N landmarks:(3+2N)-dimensional Gaussian
• Can handle hundreds of dimensions
(E)KF-SLAM
9-16
EKF-SLAM
Map Correlation matrix
9-17
EKF-SLAM
Map Correlation matrix
9-18
EKF-SLAM
Map Correlation matrix
9-19
Properties of KF-SLAM (Linear Case)
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
[Dissanayake et al., 2001]
9-20
Victoria Park Data Set
[courtesy by E. Nebot]
9-21
Victoria Park Data Set Vehicle
[courtesy by E. Nebot]
9-22
Data Acquisition
[courtesy by E. Nebot]
9-23
Raw Odometry (no SLAM)
Odometry
GPS (for reference)
9-24
Estimated Trajectory
[courtesy by E. Nebot]
9-25
EKF SLAM Application
[courtesy by J. Leonard]
9-26
EKF SLAM Application
odometry estimated trajectory
[courtesy by John Leonard]
9-27
• 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]
Approximations for SLAM
9-28
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-SLAMO(n^1.5) / O(n2.5) / O(n)
9-29
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-30
Data Association for SLAM
Interpretation tree
9-31
Data Association for SLAM
Env. Dyn.
9-32
Data Association for SLAM
Geometric Constraints
Location independent constraints
Unary constraint:intrinsic property of featuree.g. type, color, size
Binary constraint:relative measure between featurese.g. relative position, angle
Location dependent constraints
Rigidity constraint:"is the feature where I expect it givenmy 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-33
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-34
Data Association for EKF SLAM
Pygmalion
a = 0.95 , p = 2
9-35
Data Association for EKF SLAM
a = 0.95 , p = 3
Pygmalion
9-36
Data Association for EKF SLAM
a = 0.95 , p = 4a = 0.95 , p = 5
texe: 633 ms
PowerPC at 300 MHz
Pygmalion
9-37
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