Introduction to robotics

Occupancy Grid Mapping

FROM SENSING TO MAPPING
Proximity Sensors

• Task: determine P(z|x), the probability of a measurement z given that the

robot is at position x
• Where do the probabilities come from?
• Let’s look at an example

2
Why Mapping?

• Sensors give instantaneous spatial information, maps store that

information

• Robotic systems rely on maps for

– Localization
– Path planning
– Activity planning, etc.

3
Sensing vs. Mapping

Gazebo

4 rviz
Sensing vs. Mapping

Gazebo

5 rviz
Sensing vs. Mapping

Gazebo

6 rviz
Sensing vs. Mapping

7
Why Mapping?

8
The General Problem of Mapping

Based on my location and sensor measurements, can I figure out what the environment
looks like?

9
The General Problem of Mapping

• Formally, given the sensor and robot pose data,

d1:t = {x1, z1, x2 , z2 ,…, xt , zt }

calculate the most likely map

m* = argmax p(m | d1:t )

m

10
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

11
Mapping: Naïve Case

• Known pose

• Perfect sensor

• Use {0,1} probabilities (ok because sensor is perfect)

• Do volume carving: start with grid full of 1’s. Every grid cell that the
sensor passes through will be set to 0

This doesn’t work well because we don’t actually know our pose, and we
don’t have a perfect sensor.

12
Mapping: Real Case

• Noisy sensors
– Need sensor models to characterize nature of noise
– We covered this in previous lectures

• Do not know pose

– Need localization

• Use probability to deal with uncertainty

13
Mapping as a Chicken and Egg Problem

• We can find the pose (position and orientation) of a mobile robot if we

have a map
• We can construct a map of an environment if we have the robot’s pose

• The general problem is denoted as the Simultaneous Localization And

Mapping problem (SLAM)

• Throughout this lecture we will describe how to calculate a map given we

know the pose of the vehicle

14
Simple Mapping Example

• Suppose we discretize the environment in a grid, and always know the

pose of the robot.
• Initially, we assign a value of 5 to each grid cell in the environment
• After each successive sensor scan:
– Each grid cell that is not occupied has its value decreased by 1
– Each grid cell that is occupied has its value increased by 1
– We bound grid values between 0 and 9
• After multiple scans, grid cells with values of 9 are assumed to be
obstacles, and grid cells with values of 0 are assumed to be empty

15
Simple Mapping Example

5 5 5 5
5 5 5 5
5 5 5 5
5 5 5 5

Initially, robot has no knowledge about environment

16
Simple Mapping Example

5 5 5 5
5 5 5 5
5 5 5 5
5 5 5 5

Robot takes scans of environment

17
Simple Mapping Example

5 4 5 5
4 4 6 5
6 4 6 5
4 4 6 5

Update map based on scan data

18
Simple Mapping Example

5 0 5 5
0 0 9 5
9 0 9 5
0 0 9 5

After 4 more scans

19
Simple Mapping Example

5 0 5 5
0 0 9 5
9 0 9 5
0 0 9 5

Robot moves to upper right corner

20
Simple Mapping Example

5 0 5 5
0 0 9 5
9 0 9 5
0 0 9 5

Robot moves to upper right corner

21
Simple Mapping Example

4 0 4 4
0 0 9 4
9 0 9 4
0 0 9 4

Update map based on scan data

22
Simple Mapping Example

0 0 0 0
0 0 9 0
9 0 9 0
0 0 9 0

After 4 more scans

23
Simple Mapping Example: Summary

• Take multiple range scans:

– Scans showing obstacles increase the belief that obstacles exist
– Scans not showing obstacles increase the belief that obstacles aren’t present

24
Question

• Why multiple scans? Why not just say a cell is occupied or unoccupied?

Don’t want to rely on one scan, it could be wrong!

Let’s look more closely at grid mapping…

25
Representing occupancy:
HIMM (quick and dirty)
Occupancy mapping (mathematically rigorous)

METHODS
Representation of World

We represent maps as a grid:

• Each cell is a set of positions in the world

• Every (x,y) coordinate in a single cell maps to that cell

• If something is in both cells, then we assume both cells are full

• Cell size determined by sensor resolution, robot specs, obstacles size, etc.

27
Building Maps from Sensor Data

• Values in grid cells represent either…

– Likelihood of occupancy
– Likelihood of something being there when you observe it

• Values updated after each new sensor measurement

• Key assumption:
– Occupancy of individual cells is independent

28
Mapping: Example 1

• Start with an empty grid (assume all cells are unoccupied)

• Increase values in each cell with a sensor return

29
Mapping: Example 1

What are some problems with this approach?

• Assumes static environment

• Assumes robot knows where it is

Are there any safety concerns?

• We start with an empty grid…
• This means initially, the robot thinks there are no obstacles around it

30
Mapping: Example 2

• Start with a full grid (assume all cells are occupied)

• Reduce values of cells that don’t have a sensor return

• More risk averse

31
Mapping: Example 3

• Combine both methods

• Decrease occupancy values for empty spots

• Increase occupancy values for full spots

32
Sonar Sensor Model

• For sonar, a 15 degree cone is sent

out, we get a distance corresponding
to closest thing in the cone
• We don’t know where the object is
in the cone, only distance
• Three regions:
– Empty region
– Occupied region
– Unknown region
• For cone sensors, we are most likely
to detect things right in front of us

(Borenstein & Koren, 1991)

33
Histogramic In-Motion Mapping (HIMM)

• Cut world into a grid

– Values in grid are between 0 and 15
– Represent probabilities between 0 and 1

• Move the robot

• Get sensor reading, z
• Update the grid:
– Cells in empty region, subtract 1
– Cells in occupied, add 3
– Truncate at 0 and 15
• Repeat

(Borenstein & Koren, 1991)

34
HIMM Notes

• Why between 0 and 15?

– Main reason: 0-15 (16 total values) can be stored in a “nibble” (half a byte).
§ Any more values would lead to memory inefficiency
§ Any less values would lead to wasted memory (can’t allocate one dataset across
multiple different bytes in memory)
– Second reason: 16 total values allows for a fairly good map: if we don’t see
anything in a cell 16 times, then it’s probably empty

• HIMM keeps track of occupancy probabilities in an indirect manner,

allowing for an easier implementation than mapping with real
probabilities
– Although it is not quite as reliable/accurate

35
Occupancy Mapping vs. HIMM

• Histogram grid values are computed using hand-tuned variables

• Occupancy grid values are computed via Bayesian updates
– Cell values represent probability of occupation

m* = argmax p ( m d1:t )
m

[i ] Grid cell index

m= m { }
p ( m d1:t ) = ∏ p m [ ] d1:t
i
( i
)
( ) = p (m
p m [i ] [i ]
=1 )
36
Why Probabilities?

• Sensors are noisy: if we “see” something, we don’t know it is truly there

• Position estimates are noisy: localization is necessary to create a really

accurate map

• A good framework (mathematically sound framework for thinking about

uncertainty)

37
Building the Map

• The sensor model gives us P(z|m)

• Use Bayes’ rule to get posterior P(m|z)

p (m z) =
[i ] ( ) ( )
p z m[ ] P m[ ]
i i

p ( z)

p (z m ) p (m ) [i ] [i ]
p (m z) =
[i ]
p ( z m ) p ( m ) + p ( z ¬m ) p (¬m )
[i ] [i ] [i ] [i ]

denominator expanded with law

of total probability

38
In Practice

We have a grid with values in each of the cells

• Move robot
• Get sensor reading z
• For each cell the sensor observes:
– Calculate distance to the sensor
– If there is something there, we know what we expect from the sensor reading
– Calculate P(z|m) for that cell
– P(m) is the cell content and represents the prior probability that the cell is
occupied
– Calculate posterior P(m|z)
– Replace cell content with that value

39
Initial Value in the Cells

• Maximally uninformative prior: P(m)=0.5

– Before we know anything, each outcome is equally likely
• Once we sense enough, it doesn’t matter where we started, values will
converge
• If we think the world is “dangerous,” then we set priors higher. This affects
risk aversion as well as speed of convergence
• If prior is zero, it will stay zero no matter what. If prior is one, it will stay
one no matter what.
– If we initialize with priors of zero or one, then we have committed to those
values

40
Updating Occupancy Grid Maps

• Idea: Update each individual cell using a binary Bayes filter

( ) (
bel mt[ ] = p m [ ] z1:t
i i
)
( ) i
(
bel mt[ ] = η p zt mt[ ]
i
)∫ (p mt[ ] mt−1
[]
i i[]
bel mt−1) ( )
[]
dmt−1
i i

−1
η = #$ p ( zt )%&

• Additional assumption: Map is static

( ) = η p ( z m ) bel (m )
bel mt [i ]
t
[i ]
t
[i ]
t−1

41
Updating Occupancy Grid Maps

• Update the map cells using a binary Bayes filter

−1
# &
= 1− %1+
( )
[i ]
bel mt−1 (
p mt[ ] zt
i
) ( )
1− p m0[ ]
i
(
( )
bel mt [i ]
⋅ ⋅
$ ( )
% 1− bel m [i] 1− p m [i] z
t−1 t( t )
P m( )
0
[i ] (
'

See derivation in Appendix A

42
Updating Occupancy Grid Maps

• Or use the log-odds representation

−1
( )
bel mt [i ]
= 1− "#1+ exp (lt )$%
log-odds inverse
sensor model

(
lt = lt−1 + β m [ ], zt − l0
i
)
& p m [i ] ) & p m [i ] z )
( ( )
0
+, β m [i], z = log ( t ( ) +
l0 = log
( 1− p m [i] + ( t )
( 1− p m [i] z +
' ( ) 0 * ' t ( ) *

43
Example 4: Bayes update with simple sensor model

• Simple sensor model:

– Probability that sensor returns 1 if a cell is occupied: 0.8
– Probability that sensor returns 0 if a cell is empty: 0.99

z=1 z=0
m 0.8
¬m 0.99

44
Example 4: Bayes update with simple sensor model

• Simple sensor model:

– Probability that sensor returns 1 if a cell is occupied: 0.8
– Probability that sensor returns 0 if a cell is empty: 0.99

z=1 z=0
m 0.8 0.2 =1

¬m 0.01 0.99 =1

• Sensor model gives us p(z|m) i.e. p(z=1|m) + p(z=0|m) = 1

45
Example 4: Bayes update with simple sensor model

• Inverse sensor model p(m|z):

– Normalize along columns

z=1 z=0
m 0.988 0.168
¬m 0.012 0.832
=1 =1

46
Example 4: Bayes update with simple sensor model

• Uninformative prior

( ) (
p m [ ] = p ¬m [ ] = 0.5
i i
)
• Measurements
z1:t = [1, 0,1]

• Calculate for each t = {1:3}

( )
bel mt [i ]

47
Example 4: Bayes update with simple sensor model
z=1 z=0
• At t = 1:
m 0.988 0.168
¬m 0.012 0.832

" 0.5 % " 0.988 %

l0 = log $ ([i ]
)
' = 0, lt−1 = l0 = 0, β m , zt = log $
# 1− 0.5 &
'
# 0.012 &

! 0.988 $
lt = 0 + log # &− 0
" 0.012 %

−1
( ) = 1− "#1+ exp (l )$%
bel m1[i ]
t = 0.988

48
Example 4: Bayes update with simple sensor model
z=1 z=0
• At t = 2:
m 0.988 0.168
¬m 0.012 0.832

" 0.988 % " 0.168 %

l0 = 0, lt−1 = log $ (
[i ]
)
', β m , zt = log $
# 0.012 &
'
# 0.832 &

! 0.988 $ ! 0.168 $
lt = log # & + log # &− 0
" 0.012 % " 0.832 %

−1
( )
bel m2 [i ]
= 1− "#1+ exp (lt )$% = 0.9417

49
Example 4: Bayes update with simple sensor model
z=1 z=0
• At t = 3:
m 0.988 0.168
¬m 0.012 0.832

" 0.988 % " 0.168 % " 0.988 %

l0 = 0, lt−1 = log $ ' + log $
# 0.012 &
[i ]
(
', β m , zt = log $
# 0.832 &
) '
# 0.012 &

! 0.988 $ ! 0.168 $ ! 0.988 $

lt = log # & + log # & + log # &− 0
" 0.012 % " 0.832 % " 0.012 %

−1
( )
bel m3 [i ]
= 1− "#1+ exp (lt )$% = 0.9992

50
Typical Sonar Sensor Model for Occupancy Grid Maps

Combination of a linear function and a Gaussian

51
Key Parameters of the Model

• Linear in yt
• Gaussian in θ

52
Intensity of the Update

53
Occupancy Value Depending on Measured Distance

“occupied”

no info
prior
“free”

measured
distance

54
Calculating Occupancy Probability

55
Incremental Updating of Occupancy Grids

56
Resulting Map Obtained with Ultrasound Sensors

57
Resulting Occupancy and Maximum Likelihood Map

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

58
Tech Museum, San Jose

CAD map occupancy grid map

59
Occupancy Grid Mapping Example

[Link]

60
Unknown Space in Maps

• The probability of 0.5 in a cell could mean two things….what are they?
– We haven’t seen the cell before
– We have seen the cell and it has a probability of 0.5

• To deal with this, we add a flag to each cell

– Binary flag corresponding to if we have seen the cell before

When we initialize with 0.5, we are giving a probability saying we don’t know.
But this is not the same as saying “I don’t know what is there”

61
PRACTICAL CONSIDERATIONS
Mapping Environment

• Will our mapping algorithm work in a parking garage?

63
Mapping Environment

• Many positions on different levels of parking garage with same (x,y)

coordinates
• One map is insufficient to create map of garage
• Solution: make one map for each level of the garage
• Learn the transition functions between maps
• When the robot goes up a ramp to the next level, switch which map we
are using

64
Environment Size and Dimensionality

• What about 3D mapping?

• What if we want a 3D map of the inside of Kelley Engineering Center?
65
Environment Size and Dimensionality

• Recall we discretize at ~10cm

• How many 10cm cubes in Kelley?
• For 3D mapping, we need a more efficient algorithm than standard
occupancy grid mapping
• How we represent world affects accuracy of map: cubes? Prisms?

66
3D Mapping Example

[Link]
67
Sensor Limitations

• What are some of the differences between a lidar and a sonar?

• Some obstacles are transparent to some sensing modalities

– Laser and IR go through glass
– Reflectivity depends on sensing conditions

68
Simple Counting Alternative

• For every cell, count:

– 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>

hits ( x, y)
( )
bel m [i ]
=
hits ( x, y) + misses ( x, y)

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

69
Difference Between Occupancy Grid Maps and Counting
Reflection Maps
• 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

• Can be shown that computing the most likely map amounts to counting
the number of hits and misses

See Appendix B for derivation and an example

70
Example Occupancy Map

71
Example Reflection Map

glass panes

72
Summary

• Maps store sensor information about the environment

• Occupancy grid maps encode the probability that the corresponding area
in the environment is occupied
– Mathematically sound probability propagation
– Efficient updates using binary Bayes filter
• Key assumptions of occupancy mapping:
– Occupancy of each cell is independent from all others
– Robot poses known
• Alternatives include:
– Reflection maps
– HIMM

73
Next Time

• SLAM

74
Appendix A. Derivation of Binary Bayes Filter Update
(Section 4.2)
• Bayes filter:

( [i ]
p m | z 1:t )=
( )(
p zt m [ ], z 1:t−1 p m [ ] z 1:t−1
i i
)
p ( zt z 1:t−1 )

• Sensor measurements are independent if given m, therefore:

(
p m z1:t[i ]
)=
( )(
p z t m [ ] p m [ ] z1:t−1
i i
)
p ( z t z1:t−1 )

75
Appendix A. Derivation of Binary Bayes Filter Update
(Section 4.2)
• Applying Bayes rule to the measurement model:

(
p m [ ] zt p ( z t )
i
)
(
p zt m [i ]
)= p m[ ] ( ) i

• Substituting to obtain:

( )
p m [ ] zt p ( z t ) p m [ ] z1:t−1
i
( i
)
( [i ]
p m z1:t = )
( )
p m [ ] p ( z t z1:t−1 )
i

• Similarly for the opposite event ¬m

( )
p ¬m [ ] zt p ( z t ) p ¬m [ ] z1:t−1
i
( i
)
( [i ]
p ¬m | z 1:t = )
(
p ¬m [ ] p ( z t z1:t−1 )
i
)
76
Appendix A. Derivation of Binary Bayes Filter Update
(Section 4.2)
• Odds of event m given by the division:

(
p m [ ] zt
=
i
( )) ( )
p m [ ] zt( i
) p m [ ] z1:t−1
i
p ¬m [ ]
i

p (¬m z ) p (¬m [ ] z ) p (¬m [ ] z ) p ( m [ ] )

[] i i i i
1:t t 1:t−1

p (m[ ] z ) p (m[ ] z ) p ( m [ ] z ) 1− p ( m [ ] )
i i i i
1:t t 1:t−1
=
1− p ( m z ) 1− p ( m [ ] z ) 1− p ( m [ ] z ) p ( m [ ] )
[] i i i i
1:t t 1:t−1

• Since each component on LHS is positive, we can write:

(
p m z1:t [i ]
) = exp (lt )
(
1− p m z1:t [i ]
)
77
Appendix A. Derivation of Binary Bayes Filter Update
(Section 4.2)
• Rearranging to give:

1
(
p m z1:t [i ]
) = 1−
1+ exp (lt )
• Where lt is:

" p m [i ] z % " p m [i ] z % " p m [i ] %

lt = log $ ( t ) ' + log $ (
1:t−1 ) ' − log $ ( ) '
$ 1− p m [i] z ' $ 1− p m [i] z ' $ 1− p m [i] '
# ( t ) & # ( 1:t−1 ) & # ( ) &
l = β ( m[ ], z ) + l
i
t t t−1 − l0

78
Appendix B. Simple Counting Alternative

• For every cell, count:

– 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>

hits ( x, y)
( )
bel m [i ]
=
hits ( x, y) + misses ( x, y)

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

79
Appendix B. The Measurement Model for Skipped Object
(Glass)
1. pose at time t: xt
2. beam n of scan t: z t , n
0 1 n
3. maximum range reading: V t , n = 1
4. beam reflected by an object: V t , n = 0

# zt,n −1
(
% ∏ 1− m f ( xt ,n,k )
% k=0 ) if ς t,n = 1
p ( zt,n xt , m ) = $ zt,n −1
% m
f ( xt ,n,zt,n ) ∏ (
1− m f ( xt ,n,k ) ) if ς t,n = 0
%& k=0

80
Appendix B. Computing the Most Likely Map

• Compute values for m that maximize

m* = argmax P ( m z1,…, zt , x1,…, xt )
m

• Assuming a uniform prior probability for p(m), this is equivalent to

maximizing (applic. of Bayes rule)
m* = argmax P ( z1,…, zt m, x1,…, xt )
m
T
= argmax ∏ P ( zt m, xt )
m t=1
T
= argmax ∑ ln P ( zt m, xt )
m t=1

81
 Appendix B. Computing the Most Likely Map

*J T N$ zt,n −1 '-
m = argmax ,∑∑∑&& I ( f ( xt , n, zt,n ) = j ) ⋅ (1− ς t,n ) ⋅ ln m j + ∑ I ( f ( xt , n, k ) = j ) ⋅ ln (1− m j ) ))/
*

m ,+ j=1 t=1 n=1 % k=0 (/.

Suppose
T N
α j = ∑∑ I ( f ( xt , n, zt,n ) = j ) ⋅ (1− ς t,n )
t=1 n=1

T # zt,n −1
N &
β j = ∑∑% ∑ I ( f ( xt , n, k ) = j )(
t=1 n=1 $ k=0 '

82
Appendix B. Meaning of aj and bj

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 &
β j = ∑∑% ∑ I ( f ( xt , n, k ) = j )(
t=1 n=1 $ k=0 '

corresponds to the umber of times a beam intercepted cell j without

ending in it (misses(j)).

83
Appendix B. Computing the Most Likely Map
We assume that all cells mj are independent:
# J &
m = argmax %% ∑α j ln m j + β j ln (1− m j ) ((
*

m $ j=1 '
If we set we obtain

∂m α j βj αj
= − =0 mj =
∂m j m j 1− m 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.

84
Appendix B. Example

• 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
n*0.6 n*0.4 n*0.6 −n*0.4 n*0.2
! 0.55 $ ! 0.45 $ ! 11 $ ! 11 $ ! 11 $
# & *# & =# & *# & =# &
" 0.45 % " 0.55 % "9% "9% "9%

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

85