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

2

Why Mapping?

n  Learning maps is one of the fundamental problems in mobile robotics

n  Successful robot systems rely on maps for localization, path planning, activity planning etc.

3

The General Problem of Mapping

n  Formally, mapping involves, given the control inputs and sensor data,

to calculate the most likely map

},,,,,,{ 2211 nn zuzuzud …=

)|(maxarg* dmPmm

=

4

Mapping as a Chicken and Egg Problem

n  So far we learned how to estimate the pose of a robot given the data and the map.

n  Mapping, however, involves to simultaneously estimate the pose of the vehicle and the map.

n  The general problem is therefore denoted as the simultaneous localization and mapping problem (SLAM).

n  Throughout this set of slides we will describe how to calculate a map given we know the pose of the robot. n  We’ll build on top of this to achieve SLAM.

5

Types of SLAM-Problems

n  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;…

This Lecture

n  Occupancy grid maps

n  For each grid cell represent whether occupied or not

n  Reflection maps

n  For each grid cell represent probability of reflecting a sensor beam

Grid Maps

8

Occupancy Grid Maps

n  Introduced by Moravec and Elfes in 1985

n  Represent environment by a grid. Each cell can be empty or occupied.

n  E.g., 10m by 20m space, 5cm resolution à 80,000 cells à 280,000 possible maps.

n  à Can’t efficiently compute with general posterior over maps

n  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"

9

Updating Occupancy Grid Maps

][1

][11

][1

][][][ )(),|()|()( xyt

xytt

xyt

xyt

xytt

xyt dmmBelummpmzpmBel −−−−∫=η

n  Idea: Update each individual cell using a binary Bayes filter.

n  Additional assumption: Map is static.

)()|()( ][1

][][ xyt

xytt

xyt mBelmzpmBel −=η

12

Updating Occupancy Grid Maps

n  Per grid-cell update:

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

Bel(mt[ xy] = v) =! p(zt |mt

[ xy] = v)Bel(mt!1[ xy] )

13

Key Parameters of the Model

14

z+d1 z+d2

z+d3 z

z-d1

Occupancy Value Depending on the Measured Distance

Recursive Update Assumption: measurements independent

18

Incremental Updating of Occupancy Grids (Example)

19

Resulting Map Obtainedwith Ultrasound Sensors

20

Resulting Occupancy and Maximum Likelihood Map

The maximum likelihood map is obtained by clipping the occupancy grid map at a threshold of 0.5

21

Occupancy Grids: From scans to maps

22

Tech Museum, San Jose

CAD map occupancy grid map

n  Occupancy grid maps

n  For each grid cell represent whether occupied or not

n  Reflection maps

n  For each grid cell represent probability of reflecting a sensor beam

Grid Maps

24

Reflection Maps: Simple Counting

n  For every cell count n  hits(x,y): number of cases where a beam ended at <x,y>

n  misses(x,y): number of cases where a beam passed through <x,y>

n  Value of interest: P(reflects(x,y))

n  Turns out we can give a formal Bayesian justification for this counting approach

),misses(),hits(),hits()( ][

yxyxyxmBel xy

+=

25

The Measurement Model

⎪⎪⎩

⎪⎪⎨

⎧

=−

=−=

∏

∏−

=

−

=1

0,),,(),,(

1

0,),,(

, ,

,

,

0 if)1(

1 if)1(),|(

nt

tntt

nt

t

z

kntknxfznxf

z

kntknxf

tnt

mm

mmxzp

ς

ς

1, =ntς

1.  pose at time t:

2.  beam n of scan t:

3.  maximum range reading:

4.  beam reflected by an object: 0, =ntς

ntz ,tx 0 n 1

),,( ,ntt znxfm

26

Computing the Most Likely Map n  Compute values for m that maximize

n  Assuming a uniform prior probability for p(m), this is equivalent to maximizing (applic. of Bayes rule)

),,,,,|(maxarg 11*

ttm

xxzzmPm ……=

∑

∏

=

=

=

=

=

T

ttt

m

T

ttt

m

ttm

xmzP

xmzP

xxmzzPm

1

1

11*

),|(lnmaxarg

),|(maxarg

),,,|,,(maxarg ……

27

Computing the Most Likely Map

(

)⎥⎦

⎤−⋅=+

⋅−⋅=⎢⎣

⎡=

∑

∑∑∑−

=

= = =

1

0

,1 1 1

,*

,

)1(ln)),,((

ln)1()),,((maxarg

ntz

kjt

jnt

J

j

T

t

N

nntt

m

mjknxfI

mjznxfIm ς

Suppose

∑∑= =

−⋅==T

t

N

nntnttj jznxfI

1 1,, )1()),,(( ςα

∑∑ ∑= =

−

=⎥⎦

⎤⎢⎣

⎡==

T

t

N

n

z

ktj

nt

jknxfI1 1

1

0

,

)),,((β

28

Meaning of αj and βj

∑∑= =

−⋅==T

t

N

nntnttj jznxfI

1 1,, )1()),,(( ςα

∑∑ ∑= =

−

=⎥⎦

⎤⎢⎣

⎡==

T

t

N

n

z

ktj

nt

jknxfI1 1

1

0

,

)),,((β

corresponds to the number of times a beam that is not a maximum range beam ended in cell j (hits(j))

corresponds to the number of times a beam intercepted cell j without ending in it (misses(j)).

29

Computing the Most Likely Reflection Map

⎟⎟⎠

⎞⎜⎜⎝

⎛−+= ∑

=

J

jjjjj

mmmm

1

* )1ln(lnmaxarg βα

We assume that all cells mj are independent:

01

=−

−=∂

∂

j

j

j

j

j mmmm βα

If we set

Computing the most likely map amounts to counting how often a cell has reflected a measurement and how often it was intercepted.

jj

jjm βα

α

+=

we obtain

30

Difference between Occupancy Grid Maps and Counting

n  The counting model determines how often a cell reflects a beam.

n  The occupancy model represents whether or not a cell is occupied by an object.

n  Although a cell might be occupied by an object, the reflection probability of this object might be very small.

31

Example Occupancy Map

32

Example Reflection Map

glass panes

33

Example n  Out of 1000 beams only 60% are reflected from a cell and 40%

intercept it without ending in it.

n  Accordingly, the reflection probability will be 0.6.

n  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.

n  Accordingly, after n measurements we will have

n  Whereas the reflection map yields a value of 0.6, the occupancy grid value converges to 1.

2.0*4.0*6.0*4.0*6.0*

911

911*

911

55.045.0*

45.055.0 nnnnn

⎟⎠

⎞⎜⎝

⎛=⎟⎠

⎞⎜⎝

⎛⎟⎠

⎞⎜⎝

⎛=⎟⎠

⎞⎜⎝

⎛⎟⎠

⎞⎜⎝

⎛−

34

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  store the posterior probability that the corresponding area in the environment is occupied.

n  can be estimated efficiently using a probabilistic approach.

n  Reflection maps are an alternative representation.

n  store in each cell the probability that a beam is reflected by this cell.

n  the counting procedure underlying reflection maps yield the optimal reflection map.

Inverse_range_sensor_model(mi, xt, zt)

n  ®: thickness of obstacles

n  ¯: width of the sensor beam [Probabilistic Robotics, Table 9.2]