Lecture Notes - Robot Vision Group

Robots Autónomos
Mapping and Localization
Miguel Cazorla
Robot Vision Group
http://www.rvg.ua.es/master/robots
References
•
Thrun, Burgard, Fox. Probabilistic Robotics. MIT Press,
2005. Cap. 4, 8, 9.
•
Visual object tracking in 3D with color based particle filter
P.Barrera, J.M.Cañas, V.Matellán. Int. Journal of
Information Technology, Vol 2, Num 1, pp 61-65, 2005.
ISSN: 1305-2403
Depto DCCIA - Robots Autónomos
2
Contents
1. Grid Mapping
2. Grid Localization
3. Particle Filter
4. Montecarlo Localization
5. Practical
Depto DCCIA - Robots Autónomos
3
Robots Autónomos
Mapping
Why Mapping?
•
Learning maps is one of the fundamental problems in mobile
robotics
•
Maps allow robots to efficiently carry out their tasks, allow
localization …
•
Successful robot systems rely on maps for localization, path
planning, activity planning etc.
Depto DCCIA - Robots Autónomos
5
The General Problem of Mapping
What does the
environment look like?
Depto DCCIA - Robots Autónomos
6
The General Problem of Mapping
•
Formally, mapping involves, given the sensor data,
to calculate the most likely map
Depto DCCIA - Robots Autónomos
7
Mapping as a Chicken and Egg
Problem
•
•
•
Mapping involves to simultaneously estimate the
pose of the vehicle and the map.
The general problem is therefore denoted as the
simultaneous localization and mapping problem
(SLAM).
Throughout this section we will describe how to
calculate a map given we know the pose of the
vehicle.
Depto DCCIA - Robots Autónomos
8
Problems in Mapping
•
•
Sensor interpretation
•
How do we extract relevant information from raw sensor
data?
•
How do we represent and integrate this information over
time?
Robot locations have to be estimated
•
How can we identify that we are at a previously visited
place?
•
This problem is the so-called data association problem.
Depto DCCIA - Robots Autónomos
9
Occupancy Grid Maps
•
•
•
Introduced by Moravec and Elfes in 1985
•
Key assumptions
Represent environment by a grid.
Estimate the probability that a location is
occupied by an obstacle.
•
Occupancy of individual cells (m[xy]) is independent
•
Robot positions are known!
Depto DCCIA - Robots Autónomos
10
Updating Occupancy Grid Maps
•
Idea: Update each individual cell using a binary Bayes filter.
•
Additional assumption: Map is static.
Depto DCCIA - Robots Autónomos
11
Updating Occupancy Grid Maps
•
Idea: Update each individual cell using a binary Bayes filter.
•
Additional assumption: Map is static.
Depto DCCIA - Robots Autónomos
11
Updating Occupancy Grid Maps
•
Idea: Update each individual cell using a binary Bayes filter.
•
Additional assumption: Map is static.
Depto DCCIA - Robots Autónomos
11
Sensor Model
OGM: sensor model
Depto DCCIA - Robots Autónomos
12
Sensor
OGM:Model
sensor model
•
Let R be the distance to the sonar reading, α the angle from a
cell to the center of the beam, and d the distance of the cell to
the sonar.
•
•
The sensor model provides a value in the range [0 : 1].
•
Region 2: d < R−ε and |α| < θ then P(zt|mt[xy]) = 0,5 ∗ (1− (1−
(α/θ)2)) ∗ (1− (d/(R-ε))2))
Region 1: |d−R| < ε and |α| < θ then P(zt|mt[xy]) = 0,5+(0,5 ∗ (1−
(α/θ)2) * (1− ((d−R)/ε)2))
Depto DCCIA - Robots Autónomos
13
Incremental Updating
of Occupancy Grids (Example)
Depto DCCIA - Robots Autónomos
14
Resulting Map Obtained with
Ultrasound Sensors
Depto DCCIA - Robots Autónomos
15
Resulting Occupancy and
Maximum Likelihood Map
The maximum likelihood map is obtained by
clipping the occupancy grid map at a threshold
of 0.5 Depto DCCIA - Robots Autónomos
16
Tech Museum, San Jose
CAD map
occupancy grid map
Depto DCCIA - Robots Autónomos
17
Grid Localization
•
The key idea is using a grid to approximate the posterior using
a decomposition of the pose space.
•
The space is decomposed into cells of the same size and a
third dimension to include angles.
•
The granularity used is critical: finer representations are
computational expensive and less discretization provides less
accuracy.
•
Mantains as posterior a collection of discrete probability values:
bel(xt) = {pk,t}
Depto DCCIA - Robots Autónomos
18
Grid Localization Algorithm
•
Grid Localization: algorithm
Algorithm Grid localizacion ({pk,t−1}, ut, zt)
•
•
Algorithm Grid localizacion ({pk,t−1}, ut, zt)
for all k do
for all k do
�
p̂k,t = i pi,t−1motion model(ut, mean(xk ), mean(xi))
pk,t = ηmeasurement model(zt, mean(xk ))
endfor
endfor
Doctorado Depto. CCIA - Robótica y Visión
16/29
Depto DCCIA - Robots Autónomos
19
Grid Localization Example
Grid Localization: example
Depto DCCIA - Robots Autónomos
20
Grid
Localization:
example
Grid
Localization Example
Depto DCCIA - Robots Autónomos
21
Robots Autónomos
Montecarlo Localization
Particle Filters
• Represent belief by random samples
• Estimation of non-Gaussian, nonlinear processes
• Monte Carlo filter, Survival of the fittest, Condensation,
Bootstrap filter, Particle filter
• Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
• Computer vision: [Isard and Blake 96, 98]
• Dynamic Bayesian Networks: [Kanazawa et al., 95]d
Depto DCCIA - Robots Autónomos
23
Particle Filter
•
Particle filter is a nonparametric implementation of the Bayes
filter.
•
The key idea is to represent the posterior bel(xt) by a set of
random state samples drawn from this posterior.
•
This representation is approximate, but it is nonparametric: can
represent a much broader space of distribution (non
Gaussians!).
•
The samples of a posterior distribution are called particles: Xt =
xt[1],xt[2],...,xt[M]
Depto DCCIA - Robots Autónomos
24
Particle Filter
•
A particle is a hypothesis as to what the true world state may
be at time t.
•
M is the number of particles (usually 1000), but can be a
function of time.
•
The particle filter algorithm constructs bel(xt) recursively from
bel(xt−1), i.e., the particle set at time t is constructed from the
one at t − 1.
Depto DCCIA - Robots Autónomos
25
Particle Filter: algorithm
Particle filter: algorithm
Algorithm Particle filter (Xt−1, ut, zt)
X̂t = Xt = ∅
for m = 1 to M do
[m]
[m]
sample xt ∼ p(xt|ut, xt−1)
[m]
[m]
wt = p(zt|xt )
[m]
[m]
X̂t = X̂t + �xt , wt �
endfor
for m = 1 to M do (importance sampling)
[m]
draw i with probability ∝ wt
[i]
add xt to Xt.
endfor
Doctorado Depto. CCIA - Robótica y Visión
21/29
Depto DCCIA - Robots Autónomos
26
Montecarlo Localization
•
•
•
It is just the particle filter, substituting the probabilistic motion
and perceptual models (example).
It can solve the kidnapping problem.
It is easy to implement and works fine in a broad range of
localization problems: widely used.
Depto DCCIA - Robots Autónomos
27
MCL:
example
Montecarlo Example
Depto DCCIA - Robots Autónomos
28
Another Example
Depto DCCIA - Robots Autónomos
29
Importance Sampling with Resampling:
Landmark Detection Example
Depto DCCIA - Robots Autónomos
30
Distributions
Depto DCCIA - Robots Autónomos
31
Distributions
Depto DCCIA - Robots Autónomos
31
Distributions
Depto DCCIA - Robots Autónomos
31
Distributions
Depto DCCIA - Robots Autónomos
31
Distributions
Depto DCCIA - Robots Autónomos
31
Distributions
Wanted: samples distributed according to p(x|
z1, z2, z3)
Depto DCCIA - Robots Autónomos
32
This is Easy!
We can draw samples from p(x|zl) by adding
noise to the detection parameters.
Depto DCCIA - Robots Autónomos
33
Importance Sampling with
Resampling
Weighted samples
After resampling
Depto DCCIA - Robots Autónomos
34
Resampling
• Given: Set S of weighted samples.
• Wanted : Random sample, where the
probability of drawing xi is given by wi.
•
Typically done n times with replacement to
generate new sample set S’.
Depto DCCIA - Robots Autónomos
35
Resampling
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
w1
w2
w3
• Roulette wheel
• Binary search, n log n
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
• Binary search, n log n
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
• Binary search, n log n
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
• Binary search, n log n
Depto DCCIA - Robots Autónomos
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
• Binary search, n log n
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
Depto DCCIA - Robots Autónomos
36
Using Ceiling Maps for Localization
Depto DCCIA - Robots Autónomos
37
Vision-based Localization
Depto DCCIA - Robots Autónomos
38
Vision-based Localization
z
h(x)
Depto DCCIA - Robots Autónomos
38
Vision-based Localization
P(z|x)
z
h(x)
Depto DCCIA - Robots Autónomos
38
Under a Light
Measurement z:
P(z|x):
Depto DCCIA - Robots Autónomos
39
Next to a Light
Measurement z:
P(z|x):
Depto DCCIA - Robots Autónomos
40
Elsewhere
Measurement z:
P(z|x):
Depto DCCIA - Robots Autónomos
41
Localization for AIBO robots
Depto DCCIA - Robots Autónomos
42
Limitations
•
•
The approach described so far is able to
•
•
track the pose of a mobile robot and to
globally localize the robot.
How can we deal with localization errors (i.e., the kidnapped
robot problem)?
Depto DCCIA - Robots Autónomos
43
Approaches
•
Randomly insert samples (the robot can be teleported at any
point in time).
•
Insert random samples proportional to the average likelihood of
the particles (the robot has been teleported with higher
probability when the likelihood of its observations drops).
Depto DCCIA - Robots Autónomos
44
Summary
•
Occupancy grid maps are a popular approach to represent the
environment of a mobile robot given known poses. In this approach each
cell is considered independently from all others.
•
It stores the posterior probability that the corresponding area in the
environment is occupied. Occupancy grid maps can be learned efficiently
using a probabilistic approach.
•
We provided a sensor model for computing the likelihood of
measurements and showed that the counting procedure underlying
reflection maps yield the optimal map.
•
Particle filters are an implementation of recursive Bayesian filtering.
They represent the posterior by a set of weighted samples.
•
In the context of localization, the particles are propagated according to the
motion model. They are then weighted according to the likelihood of the
observations.
•
In a re-sampling step, new particles are drawn with a probability
proportional to the likelihood of the observation.
Depto DCCIA - Robots Autónomos
45
Assignment
•
Basic part: implements the mapping grid algorithm using the
framework provided
Depto DCCIA - Robots Autónomos
46