Upload
faunus
View
74
Download
0
Tags:
Embed Size (px)
DESCRIPTION
Probabilistic Robotics. SLAM and FastSLAM. The SLAM Problem. SLAM stands for simultaneous localization and mapping originally developed by Hugh Durrant -Whyte and John J. Leonard “Mobile robot localization by tracking geometric beacons” - 1991 - PowerPoint PPT Presentation
Citation preview
Probabilistic Robotics
SLAM and FastSLAM
2
SLAM stands for simultaneous localization and mapping originally developed by Hugh Durrant-Whyte and John J. Leonard “Mobile robot localization by tracking geometric beacons” - 1991
The task of building a map while estimating the pose of the robot relative to this map
Why is SLAM hard? Chicken and egg problem: A map is needed to localize the robot and
a pose estimate is needed to build a map Errors correlate!
The SLAM Problem
3
SLAM ApplicationsIndoors
Space
Undersea
Underground
4
Typical Robot Equipment Mobile robot
Typically want error under 2 cm per meter moved 2° per 45° degrees turned
5
Typical Robot Equipment Range finding
sensors Sonar
10 mm accuracy 4 m range
SICK laser 10 mm accuracy 80 m range
IR range finder 1.5 m range
Vision Increasingly viable
6
Outline of typical SLAM Process
7
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;…
8
Given: The robot’s
controls Observations of
nearby featuresEstimate: Map of features Path of the
robot
The SLAM ProblemA robot moving though an unknown, static environment
9
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown!
Robot path error increases errors in the map
10
Structure of the Landmark-based SLAM-Problem
11
12
13
14
15
16
17
18
19
20
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
Robot poseuncertainty
21
Detecting Landmarks Look for spikes in range
data Red dots show table legs
Fit geometric primitives RANSAC line fitting
algorithm Don’t want to fit to all data Pick a random subset Fit a primitive
Classify remaining points as outlier or not Keep if enough points
classify well Keep a full occupancy map
Each cell is a landmark
22
(E)KF SLAM Represent robot state and landmark
position all in one big state ( rx, ry, l1x, l1y, l2x, l2y,…) Apply Kalman filter to state update and
observations Uncertainty represented in large
covariance matrix
23
For many applications, the time update and measurement equations are NOT linear The KF is not directly applicable
Linearize around the non-linearities Use of the KF is extended to more
realistic situations
Kalman Filter and Linearity
24
The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm
The EKF allows for estimation of non-linear processes or measurement relationships
The Extended Kalman Filter (EKF)
),(),,(1
kkk
kkkk
vxhzwuxfx
kkk
kkkk
vxHzwuBxAx
1
Kalman Filter Extended Kalman Filter
25
Linearity Assumption Revisited
26
Non-linear Function
27
EKF Linearization (1)
28
EKF Linearization (2)
29
EKF Linearization (3)
30
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
ll
yx
mxBel
Map with N landmarks:(3+2N)-dimensional Gaussian
Can handle hundreds of dimensions
(E)KF-SLAM
31
EKF-SLAM
Map Correlation matrix
32
EKF-SLAM
Map Correlation matrix
33
EKF-SLAM
Map Correlation matrix
34
EKF-SLAM Can be quadratic in number of
landmarks Non-linearities a problem
Has led to other approaches
35
Is there a dependency between the dimensions of the state space?
If so, can we use the dependency to solve the problem more efficiently?
In the SLAM context The map depends on the poses of the robot. We know how to build a map if the position of the
sensor is known.
Dependencies
36
FastSLAM Rao-Blackwellized particle filtering based on
landmarks [Montemerlo et al., 2002] Each landmark is represented by a 2x2
Extended Kalman Filter (EKF) Each particle therefore has to maintain M EKFs
Landmark 1 Landmark 2 Landmark M…x, y,
Landmark 1 Landmark 2 Landmark M…x, y, Particle#1
Landmark 1 Landmark 2 Landmark M…x, y, Particle#2
ParticleN
…
37
FastSLAM – Action Update
Particle #1
Particle #2
Particle #3
Landmark #1Filter
Landmark #2Filter
38
FastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Landmark #1Filter
Landmark #2Filter
39
FastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Weight = 0.8
Weight = 0.4
Weight = 0.1
40
Multi-Hypothesis Data Association
Data association is done on a per-particle basis
Robot pose error is factored out of data association decisions
41
Improved Proposal The proposal adapts to the structure
of the environment
42
Per-Particle Data Association
Was the observationgenerated by the redor the blue landmark?
P(observation|red) = 0.3 P(observation|blue) = 0.7
Two options for per-particle data association Pick the most probable match Pick an random association weighted by
the observation likelihoods If the probability is too low, generate a new
landmark
43
Results – Victoria Park
4 km traverse < 5 m RMS
position error 100 particles
Dataset courtesy of University of Sydney
Blue = GPSYellow = FastSLAM
44
Selective Re-sampling Re-sampling is dangerous, since
important samples might get lost(particle depletion problem)
Key question: When should we re-sample?
45
Typical Evolution of particle number
visiting new areas closing the
first loop
second loop closure
visiting known areas
46
Intel Lab 15 particles four times faster
than real-timeP4, 2.8GHz
5cm resolution during scan matching
1cm resolution in final map
47
More Details on FastSLAM M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A
factored solution to simultaneous localization and mapping, AAAI02
D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03
M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003
G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05
A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03