Mapping with Known Poses
Pieter Abbeel
UC Berkeley EECS
Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
TexPoint fonts used in EMF.
Read the TexPoint manual before you delete this box.:
AAAAAAAAAAAAA
Why Mapping?
n
n
Learning maps is one of the fundamental problems in mobile
robotics
Successful robot systems rely on maps for localization, path
planning, activity planning etc.
2
Page 1!
The General Problem of Mapping
n
Formally, mapping involves, given the control inputs and sensor
data,
d = {u1 , z1 , u2 , z2 ,…, un , zn }
to calculate the most likely map
m* = arg max P(m | d )
m
3
Mapping as a Chicken and Egg Problem
n
n
n
n
So far we learned how to estimate the pose of a
robot given the data and the map.
Mapping, however, 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 set of slides we will describe
how to calculate a map given we know the
pose of the robot.
n  In future lectures we’ll build on top of this to
achieve SLAM.
Page 2!
4
Types of SLAM-Problems
n
This Lecture
Grid maps or scans
[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…]
n
Landmark-based
[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…
5
Grid Maps
n
Occupancy grid maps
n
n
For each grid cell represent whether occupied or not
Reflection maps
n
For each grid cell represent probability of reflecting a
sensor beam
Page 3!
Occupancy Grid Maps
n
n
n
n
n
Introduced by Moravec and Elfes in 1985
Represent environment by a grid. Each cell can be
empty or occupied.
E.g., 10m by 20m space, 5cm resolution à 80,000
cells à 280,000 possible maps.
à Can’t efficiently compute with general posterior
over maps
Key assumption:
n
Occupancy of individual cells (m[xy]) is independent
Bel(mt ) = P(mt | u1, z2 …, ut!1, zt ) = " Bel(mt[ xy] )
x,y
8
Updating Occupancy Grid Maps
n
Idea: Update each individual cell using a binary Bayes filter.
Bel(mt[ xy ] ) = η p( zt | mt[ xy ] )∫ p(mt[ xy ] | mt[−xy1 ] , ut −1 ) Bel(mt[−xy1 ] ) dmt[−xy1 ]
n
Additional assumption: Map is static.
Bel (mt[ xy ] ) = η p( zt | mt[ xy ] ) Bel (mt[−xy1 ] )
9
Page 4!
Updating Occupancy Grid Maps
n
Per grid-cell update:
[ xy]
Bel(mt[ xy] = v) = ! p(zt | mt[ xy] = v)Bel(mt!1
)
n
BUT: how to obtain p(zt | mt[xy] = v) ?
n
=
n
This would require summation over all maps
n
à Heuristic approximation to update that works well in
practice
12
Key Parameters of the Model
13
Page 5!
Occupancy Value Depending on the
Measured Distance
z+d1
z
z+d2
z+d3
z-d1
14
Recursive Update
Page 6!
Assumption: measurements independent
Incremental Updating
of Occupancy Grids (Example)
18
Resulting Map Obtainedwith Ultrasound
Sensors
19
Page 7!
Resulting Occupancy and Maximum
Likelihood Map
The maximum likelihood map is obtained by clipping the
occupancy grid map at a threshold of 0.5
20
Occupancy Grids: From scans to maps
21
Page 8!
Tech Museum, San Jose
CAD map
occupancy grid map
22
Grid Maps
n
Occupancy grid maps
n
n
For each grid cell represent whether occupied or not
Reflection maps
n
For each grid cell represent probability of reflecting a
sensor beam
Page 9!
Reflection Maps: Simple Counting
n
For every cell count
n
n
hits(x,y): number of cases where a beam ended at <x,y>
misses(x,y): number of cases where a beam passed
through <x,y>
Bel(m[ xy ] ) =
n
n
hits( x, y)
hits( x, y) + misses(x, y )
Value of interest: P(reflects(x,y))
Turns out we can give a formal Bayesian justification
for this counting approach
24
The Measurement Model
1.  pose at time t:
xt
2.  beam n of scan t:
zt ,n
3.  maximum range reading:
ς t ,n = 1
ς t ,n = 0
4.  beam reflected by an object:
n
0 1
m f ( xt ,n, zt ,n )
⎧ zt ,n −1
if ς t ,n = 1
⎪ ∏ (1 − m f ( xt ,n ,k ) )
⎪ k =0
p( zt ,n | xt , m) = ⎨
zt ,n −1
⎪m
(1 − m f ( xt ,n ,k ) ) if ς t ,n = 0
⎪⎩ f ( xt ,n , zt ,n ) ∏
k =0
25
Page 10!
Computing the Most Likely Map
n
Compute values for m that maximize
m* = arg max P(m | z1 ,…, zt , x1 ,…, xt )
m
n
Assuming a uniform prior probability for p(m), this is equivalent
to maximizing (applic. of Bayes rule)
m* = arg max P ( z1 , … , zt | m, x1 , … , xt )
m
T
= arg max ∏ P ( zt | m, xt )
m
t =1
T
= arg max ∑ ln P ( zt | m, xt )
m
t =1
26
Computing the Most Likely Map
⎡ J T N
m* = arg max ⎢∑∑∑ (I ( f ( xt , n, zt ,n ) = j ) ⋅ (1 − ς t ,n ) ⋅ ln m j
m
⎣ j =1 t =1 n =1
zt ,n −1
⎤
+ ∑ I ( f ( xt , n, k ) = j ) ⋅ ln (1 − m j ) )⎥
k =0
⎦
Suppose
T
N
α j = ∑∑ I ( f ( xt , n, zt ,n ) = j ) ⋅ (1 − ς t ,n )
t =1 n =1
T
N
⎡ zt ,n −1
⎤
⎣ k =0
⎦
β j = ∑∑ ⎢ ∑ I ( f ( xt , n, k ) = j )⎥
t =1 n =1
27
Page 11!
Meaning of αj and βj
T
N
α j = ∑∑ I ( f ( xt , n, zt ,n ) = j ) ⋅ (1 − ς t ,n )
t =1 n =1
corresponds to the number of times a beam that is
not a maximum range beam ended in cell j (hits(j))
T
N
⎡ zt ,n −1
⎤
⎣ k =0
⎦
β j = ∑∑ ⎢ ∑ I ( f ( xt , n, k ) = j )⎥
t =1 n =1
corresponds to the number of times a beam
intercepted cell j without ending in it (misses(j)).
28
Computing the Most Likely Reflection Map
We assume that all cells mj are independent:
⎛ J
⎞
m* = arg max⎜⎜ ∑α j ln m j + β j ln(1 − m j ) ⎟⎟
m
⎝ j =1
⎠
If we set
we obtain
βj
∂m α j
=
−
=0
∂m j m j 1 − m j
mj =
αj
αj +βj
Computing the most likely map amounts to counting
how often a cell has reflected a measurement and how
often it was intercepted.
29
Page 12!
Difference between Occupancy Grid
Maps and Counting
n
n
n
The counting model determines how often a cell reflects a
beam.
The occupancy model represents whether or not a cell is
occupied by an object.
Although a cell might be occupied by an object, the reflection
probability of this object might be very small.
30
Example Occupancy Map
31
Page 13!
Example Reflection Map
glass panes
32
Example
n
n
n
n
Out of 1000 beams only 60% are reflected from a cell and 40%
intercept it without ending in it.
Accordingly, the reflection probability will be 0.6.
Suppose p(occ | z) = 0.55 when a beam ends in a cell and p(occ |
z) = 0.45 when a cell is intercepted by a beam that does not
end in it.
Accordingly, after n measurements we will have
⎛ 0.55 ⎞
⎜
⎟
⎝ 0.45 ⎠
n
n*0.6
⎛ 0.45 ⎞
* ⎜
⎟
⎝ 0.55 ⎠
n*0.4
⎛ 11 ⎞
= ⎜ ⎟
⎝ 9 ⎠
n*0.6
⎛ 11 ⎞
* ⎜ ⎟
⎝ 9 ⎠
− n*0.4
⎛ 11 ⎞
= ⎜ ⎟
⎝ 9 ⎠
n*0.2
Whereas the reflection map yields a value of 0.6, the occupancy
grid value converges to 1.
33
Page 14!
Summary
n
Grid maps are a popular approach to represent the environment of a
mobile robot given known poses.
n
In this approach each cell is considered independently from all others.
n
Occupancy grid maps
n
n
n
store the posterior probability that the corresponding area in the
environment is occupied.
can be estimated efficiently using a probabilistic approach.
Reflection maps are an alternative representation.
n
n
store in each cell the probability that a beam is reflected by this cell.
the counting procedure underlying reflection maps yield the optimal
reflection map.
34
Inverse_range_sensor_model(mi, xt, zt)
n
®: thickness of obstacles
n
¯: width of the sensor beam
[Probabilistic Robotics, Table 9.2]
Page 15!