Probabilistic Robotics SLAM 1 Based on slides from the book's website.
-
Upload
evan-wilkerson -
Category
Documents
-
view
217 -
download
0
Transcript of Probabilistic Robotics SLAM 1 Based on slides from the book's website.
Probabilistic Robotics
SLAM
1Based on slides from the
book's website
2
Given:
• The robot’s control signals.
• Observations of nearby features.
Estimate:
• Map of features.
• Path (sequence of poses) of the robot.
The SLAM Problem
A robot is exploring an unknown, static environment.
3
Structure of the Landmark-based SLAM Problem
4
Mapping with Raw Odometry
5
SLAM Applications
Indoors
Space
Undersea
Underground
6
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;…
7
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown!
Robot path error correlates errors in the map!
8
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
SLAM: Simultaneous Localization and Mapping
• Full SLAM:
• Online SLAM:
Integrations typically done one at a time.
• Aspects:• Continuous: Robot pose, object locations.
• Discrete: feature correspondence i.e. relationship with previously seen objects.
),|,( :1:1:1 ttt uzmxp
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
Estimate most recent pose and map!
Estimate entire path and map!
10
Graphical Model of Online SLAM
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
11
Graphical Model of Full SLAM:
),|,( :1:1:1 ttt uzmxp
12
Techniques for Consistent Maps
• Scan matching: online.
• EKF SLAM: online and incremental.
• Graph-SLAM: offline with stored information!
• Sparse Extended Information Filters (SEIFs): online with stored knowledge
• Fast-SLAM: Rao-Blackwellized Particle Filters
13
Scan Matching
• Maximize the likelihood of the ith pose and map relative to the (i-1)th pose.
• Calculate the map according to “mapping with known poses” based on the poses and observations.
)ˆ,|( )ˆ ,|( maxargˆ 11]1[
tttt
ttx
t xuxpmxzpxt
robot motioncurrent measurementmap constructed so far
][ˆ tm
14
Scan Matching Example
15
(E)Kalman Filter Algorithm
1. Algorithm (E)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 )(
),( 1 ttt ug
tTtttt RGG 1
1)( tTttt
Tttt QHHHK
))(( ttttt hzK
tttt HKI )(
16
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.• Approximately known initial pose.
(E)KF-SLAM
17
Classical Solution – The EKF
• Approximate the SLAM posterior with a high-dimensional Gaussian [Smith & Cheeseman, 1986].
• Single hypothesis data association!
Known/Unknown Correspondences
• Known correspondence:• Easier to solve • Features associated with signatures – visual landmarks.• 3N + 3 dimensions.
• Unknown correspondence:• Harder to solve • Features not associated with unique signatures – range
information.
18
1 2 1
121:1:1:1:1:1:1 ...),|,,(),|,,(c c c
ttttttttt
t
dxdxdxuzcmxpuzcmxp
),|,,( :1:1 tttt uzcmxp
19
EKF-SLAM (Known correspondences)
Map Correlation matrix
20
EKF-SLAM (Known correspondences)
Map Correlation matrix
21
EKF-SLAM (Known correspondences)
Map Correlation matrix
22
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]
Some Observations…
• Over time, the x-y coordinate estimates become fully correlated!
• Implications:• Absolute map coordinates relative to coordinate system
defined by initial robot pose is approximately known.• Map coordinates relative to robot pose known with
certainty asymptotically.
• Local accuracy much better than global accuracy.
23
24
Victoria Park Data Set
[courtesy by E. Nebot]
25
Victoria Park Data Set Vehicle
[courtesy by E. Nebot]
26
SLAM
[courtesy by E. Nebot]
27
Map and Trajectory
Landmarks
Covariance
[courtesy by E. Nebot]
28
Landmark Covariance
[courtesy by E. Nebot]
29
Estimated Trajectory
[courtesy by E. Nebot]
30
EKF SLAM Application (MIT B21)
[courtesy by John Leonard]
31
EKF SLAM Application (MIT B21)
odometry estimated trajectory
[courtesy by John Leonard]
Unknown Correspondences
• Algorithm similar to the case of known correspondences.
• Incremental Maximum Likelihood (ML) estimation of correspondences.
• Strategy:• Propose new landmark and correspondence.• Accept if Mahalanobis distance to previous landmarks
more than a threshold.
32
),,|,,( 1:1:1 ttttt NuzNmxp
33
EKF-SLAM Summary
• Have been applied successfully in real-world environments.
• Convergence results for the linear case.• Can diverge if nonlinearities are large!
• Quadratic in the number of landmarks: O(n2). Not suitable for more than a few 1000 landmarks.
• Additional features:• Map management required to overcome errors due to Gaussian
assumption and spurious landmark creation.• Landmark existence probabilities: landmarks on “probation” • Numerical instability for large sparse matrices.
• Approximations reduce the computational complexity.
34
• 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