Lecture Notes - Robot Vision Group

Transcripción

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
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.
5
The General Problem of Mapping
What does the
environment look like?
6
The General Problem of Mapping
•
Formally, mapping involves, given the sensor data,
to calculate the most likely map
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.
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.
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!
10
Updating Occupancy Grid Maps
•
Idea: Update each individual cell using a binary Bayes filter.
•
Additional assumption: Map is static.
11
•
•
11
•
•
11
Sensor Model
OGM: sensor model
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))
13
Incremental Updating
of Occupancy Grids (Example)
14
Resulting Map Obtained with
Ultrasound Sensors
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
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}
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
19
Grid Localization Example
Grid Localization: example
20
Grid
Localization:
example
Grid
Localization Example
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
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]
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.
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
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.
27
MCL:
example
Montecarlo Example
28
Another Example
29
Importance Sampling with Resampling:
Landmark Detection Example
30
Distributions
31
Distributions
31
Distributions
31
Distributions
31
Distributions
31
Distributions
Wanted: samples distributed according to p(x|
z1, z2, z3)
32
This is Easy!
We can draw samples from p(x|zl) by adding
noise to the detection parameters.
33
Importance Sampling with
Resampling
Weighted samples
After resampling
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’.
35
Resampling
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
36
Resampling
wn
Wn-1
w1
w2
w3
• Roulette wheel
• Binary search, n log n
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
36
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
w1
w2
w3
w3
• Roulette wheel
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
36
Using Ceiling Maps for Localization
37
Vision-based Localization
38
z
h(x)
38
P(z|x)
z
h(x)
38
Under a Light
Measurement z:
P(z|x):
39
Next to a Light
Measurement z:
P(z|x):
40
Elsewhere
Measurement z:
P(z|x):
41
Localization for AIBO robots
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)?
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).
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.
45
Assignment
•
Basic part: implements the mapping grid algorithm using the
framework provided
46

Lecture Notes - Robot Vision Group

Transcripción

Documentos relacionados

Your Title - IEARobotics

Ph.D. Thesis Extended Abstract