University of applied Sciences

Bonn-Rhein-Sieg

Mobile Robots
Mapping I
 University of applied Sciences
Bonn-Rhein-Sieg
Last Lecture
Last week's main topics
Kinematic models of whole vehicles
wheel constraints revisited
- only fixed and steered standard wheels impose real constraints
full vehicle description
- all sliding constraints are combined into one matrix equation
- all rolling constraints are combined into one matrix equation
- equations solved either for position change or for wheel speeds
vehicle characteristics
mobility
- immediately achievable velocities
maneuverability
- ability to achieve different poses

2
 University of applied Sciences
Bonn-Rhein-Sieg
Today's Lecture

Today's topics:
Introduction to Mapping building maps from sonar
Types of Maps data
Metric maps using sonars to estimate
- Grid maps obstacles
Topological maps a sonar sensor model
Sensor maps building an occupancy grid
Map representation map from sonar data
Grid based
Tree based
Other methods

3
University of applied Sciences
Bonn-Rhein-Sieg

Types of maps

4
 University of applied Sciences
Bonn-Rhein-Sieg

Types of maps
Metric maps
Describe distances, sizes, shapes
Topological maps
Landmark based
Describe pathways, connectivity of landmarks
Sensor maps
Special kind of landmark-based maps
Tailored to specific perception of a robot
Can be combined with metric or topological maps

Humans usually use combinations

5
 University of applied Sciences
Bonn-Rhein-Sieg

Usefulness of maps
Depend on the user
Depend on the task at hand

Example: City plan

An abstract drawing of the city ground
Partly metric
The drawing encodes precise distances and (absolute) positions
Partly topological
The drawing highlights pathways, outstanding landmarks and
their connections

6
+
 University of applied Sciences
Bonn-Rhein-Sieg

Example: Electronic navigator (GPS) map

Large set of connections between landmarks
Landmarks are junctions of streets
Assigns distances or costs to each connection
Multiple levels of detail
Encoded as kind of tree, machine-readable

Both types serve the same purpose

Find path from address A to address B!
But are mutually useless for the other user
Same holds for different robot tasks

7
+
 University of applied Sciences
Bonn-Rhein-Sieg

Metric maps
Focuses
On the position of objects
On the geometric shape of the world
Enables
To predict measurement
To obtain relations (sizes, distances) of arbitrary objects in the
map
Defined by its content, not the storage of the content
Can be grid based
Can be mathematical description
Can be tree
many more possibilities (drawing )

8
 University of applied Sciences
Bonn-Rhein-Sieg

Topological map
Focuses on
Connection between places
- Which other place is directly reachable?
Sparse representation of the environment
Enables
Efficient path planning using graph-based algorithms
Direct encoding of distances or path costs
Two-fold representation
Connections (arcs or edges)
Description of places or landmarks (nodes)
- Required for localization

9
 University of applied Sciences
Bonn-Rhein-Sieg

Map representation
(Sigwart Chapter 5.5)

Mostly slide set to Siegwart et.al. Autonomous Mobile Robots

Source: http://www.asl.ethz.ch/education/master/mobile_robotics
10
Autonomous Mobile Robots, Chapter 5 5.5

Representation of the Environment

Environment Representation
Continuos Metric x,y,
Discrete Metric metric grid
Discrete Topological topological grid
Environment Modeling
Raw sensor data, e.g. laser range data, grayscale images
o large volume of data, low distinctiveness on the level of individual
values
o makes use of all acquired information
Low level features, e.g. line other geometric features
o medium volume of data, average distinctiveness
o filters out the useful information, still ambiguities
High level features, e.g. doors, a car, the Eiffel tower
o low volume of data, high distinctiveness
o filters out the useful information, few/no ambiguities, not enough
information
R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.1

Map Representation: Continuous Line-Based

a) Architecture map
b) Representation with set of infinite lines

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (1)

Exact cell decomposition

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (2)

Fixed cell decomposition
Narrow passages disappear

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (3)

Adaptive cell decomposition

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (4)

Fixed cell decomposition Example with very small cells

Courtesy of S. Thrun
R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (5)

Topological Decomposition

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (6)

Topological Decomposition

node

Connectivity
(arch)

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.2

Map Representation: Decomposition (7)

Topological Decomposition

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.5.3
State-of-the-Art: Current Challenges in Map
Representation
Real world is dynamic
Perception is still a major challenge

Error prone
Extraction of useful information difficult

Traversal of open space

How to build up topology (boundaries of

nodes)
Sensor fusion

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8

Autonomous Map Building

Starting from an arbitrary initial point,
a mobile robot should be able to autonomously
explore the environment with its on board sensors,
gain knowledge about it,
interpret the scene,
build an appropriate map
and localize itself relative to this map.

SLAM
The Simultaneous Localization and Mapping
Problem
R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8
Map Building:
How to Establish a Map
3. Basic Requirements of a Map:
1. By Hand a way to incorporate newly sensed
information into the existing world
model
information and procedures for
estimating the robots position
information to do path planning and
other navigation task (e.g. obstacle
2. Automatically: Map Building avoidance)

The robot learns its environment Measure of Quality of a map

topological correctness
Motivation: metrical correctness predictability
- by hand: hard and costly
- dynamically changing environment But: Most environments are a mixture of
- different look due to different predictable and unpredictable features
perception hybrid approach

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8
Map Building:
The Problems
1. Map Maintaining: Keeping track 2. Representation and
of changes in the environment Reduction of Uncertainty

e.g. disappearing position of robot -> position of wall

cupboard

position of wall -> position of robot

- e.g. measure of belief of each probability densities for feature positions

environment feature additional exploration strategies

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8.1

General Map Building Schematics

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8.1

Map Representation
M is a set of n probabilistic feature locations z
Each feature is represented by the covariance matrix t and an
associated credibility factor ct

ct is between 0 and 1 and quantifies the belief in the existence of

the feature in the environment

a and b define the learning and forgetting rate and ns and nu are
the number of matched and unobserved predictions up to time k,
respectively.
R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8.1
Autonomous Map Building
Stochastic Map Technique

Stacked system state vector:

State covariance matrix:

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8.2

Cyclic Environments Courtesy of Sebastian Thrun

Small local error accumulate to arbitrary large global

errors!
This is usually irrelevant for navigation
However, when closing loops, global error does matter

R. Siegwart, I. Nourbakhsh
Autonomous Mobile Robots, Chapter 5 5.8.2

Dynamic Environments
Dynamical changes require continuous mapping

If extraction of high-level features would be

possible, the mapping in dynamic
environments would become
significantly more straightforward.
e.g. difference between human and wall
Environment modeling is a key factor
for robustness ?

R. Siegwart, I. Nourbakhsh
University of applied Sciences
Bonn-Rhein-Sieg

That's all
on basic mapping
Next:
Example: sonar maps

29
University of applied Sciences
Bonn-Rhein-Sieg

Building a map from sonar data

30
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps
The task
use low resolution sonar sensors
cone width ~30, ~ +/- 5 cm distance error, 0.3 5 m range
on a moving robot
to build a high resolution map
resolution approximately distance error

The problem
30 at 5 m yields 250 cm uncertainty

31
University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Problem of cone width

Sonar cone hits at lower
(green) point
Robot doesn't know which
part of cone hit an obstacle
Obstacle is sensed at upper
(red) position (center of
cone)

32
+
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Problem of motion
Robot moves
Two measurements of same obstacle are different d'
how to find matching observation d
Obstacles seem to move in local reference
frame
Sensed at t +1 Sensed at t +1
Sensed at t
Sensed at t

t+1
t+1

t t

33
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

d'
Problem of motion d
Robot really moves
Two measurements of same obstacle are different
how to find matching observation
Obstacles seemingly move in local reference
frame
Need to keep track of obstacles
Kalman filter, but now tracks obstacles, not robot
- Here u is inverse robot motion
1st Alternative: transform previous estimate into new frame
- Allows direct update of estimate
2nd Alternative: keep all calculations in global reference frame
- Requires (precise) global localization

34
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Alternative #1: transform previous estimate into new frame

d j , t1= d j , t 2 d j , t u t cos t u t
2 2
dj,t dj,t+1

t 1== = atan
2

2 d j , t u t cost
u t sin t

ut

Alternative #2: keep all calculations in global reference frame

After motion step, convert into local coordinates
i.e. predict sensor data, use prediction as new mean
Update mean and variance based on new measurement
Convert back to global coordinates

35
+
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Common problems
What is measured and estimated?
Direct pose estimate works well only for localizable landmarks
What is the pose of a wall?
Same for averaging raw sensor data.
How to deal with occurring and disappearing landmarks?
Need to match readings at t+1 with landmarks from step t
How to deal with data association (or assignment) problem?
- If in view of more then one sensors, used best-matching sensor
- If no match is found, or new value dj is outside 3 , start new estimation
- Over time more estimations than sensors evolve, requiring match step for
each measurement
- One sensor may update more than one estimation

36
University of applied Sciences
Bonn-Rhein-Sieg

Method so far not very satisfying

Let's look for something better

37
University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Better:
Use full angular range for
occupied estimate (grey)
Whole area between sensor and
range estimate is know to be free

38
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimating high resolution maps

form noisy low resolution sonar
Main problem of previous approach
Only distance has Gaussian error
Angle has large rectangular distribution
Result of multiple (shifted) measurements can neither be
described by Gaussian nor rectangular distribution
Subsequent measurements can not easily matched
Errors in motion estimation have been neglected

39
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimating high resolution sonar maps

Main problem was large angular uncertainty in single

measurement
Approach
Decompose measurement into several (artificial) sub-
measurement with small angular uncertainty
Estimate probably free space and probably occupied space
independently
Update free and occupied space estimation with each new
measurement

40
University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

41
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimating high resolution maps

Each sonar measurement
Has area of high occupation probability
Has area of high free probability
Is stored in grid
In subsequent measurements
Occupied reinforces occupied
Free reinforces free
Free weakens occupied and vice versa
Result is a probability map

42
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps P

Free space Area
Let P = ( x, y ) be a point in the grid. S
Let
R distance measured (mean value) R
mean error
S position of sensor
opening angle of sonar cone
distance S to P
angle between main axis of sensor and line S to P
P is free space if R / 2
with probability p E = f E ,

43
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps P

Occupied space Area

Let P = ( x, y) be a point in the grid. S
Let
R distance measured (mean value) R
mean error
S position of sensor
opening angle of sonar cone
distance S to P
angle between main axis of sensor and line S to P
P is occupied space if [ R , R ] / 2
with probability pO = f O ,

44
 University of applied Sciences
Bonn-Rhein-Sieg

Cells with non-zero probability to be

occupied free

wall wall

45
 University of applied Sciences
Bonn-Rhein-Sieg

Summary I
R distance measured
A point P( x, y) is error of distance
free space if R / 2 opening angle of sonar cone
, polar coordinates of P
with probability p E = f E , wrt. sonar sensor

occupied space if [ R , R ] / 2
with probability p O = f O ,

What is computed here?

the probability of a (random) point P to be free / occupied,
given a single sonar measurement
we are NOT calculating the error of the measurement here!

46
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps
R distance measured
Estimating high resolution maps error of distance
Probability function has two opening angle of sonar cone
, polar coordinates of P
components wrt. sonar sensor

{
2

Ea angular probability E a , =
1
2

if

2
0 else

Er distance probability

{
2

1

R
if R

E r R , , = 2

1

R

if R R

0 else

47
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps
Probability for empty space
p E =E r ( R , ) E a ( , ) if [ R , R+ ]
Probability for occupied space
pO =E r R , E a , if [ R , R ]

Calculated here:
Confidence that a given (random)
point is free or occupied, based
on a single sonar measurement.
Note: pE pO
This is not a real probability.
Neither PE nor PO integrate to 1.

R
48
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps

Combining multiple measurements
Free and occupied space is kept in global coordinates
Each sensor reading is combined with previous estimation
Combination is asymmetric
Areas read as free have higher confidence
- Occupied space has angular uncertainty
- Free space has no angular uncertainty
Free readings are used directly
Occupied readings are first modulated by a priori known free
space

49
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps

Three states are maintained
Free space of current reading
Occupied space of current reading
Combined map
Let be
EmpM(X,Y) a priori probability for cell (X,Y) to be free (old map)
Empk(X,Y) probability for cell (X,Y) to be free in reading k
Emp k X , Y = min p E x , y
x X , yY

OccM(X,Y) a priori probability for cell (X,Y) to be occupied

Occk(X,Y) cell probability to be occupied in reading k
Occ k X ,Y = max pO x , y Sub-sampling:
xX , yY
Sample grid cell to find
min. or max. value.
50
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimating high resolution maps

Set of free space

Computed from current reading Empk() and last value Empt-1()
Emp ( X , Y )=[ 0,1]
Emp t ( X , Y ):=
Emp t1 ( X , Y )+
Emp k ( X , Y )Emp t 1 ( X , Y ) Emp k ( X , Y )
values: 01 values: 01

Emp t ( X , Y ):=Emp t 1 ( X , Y )+(1Emp t 1 ( X , Y )) Emp k ( X , Y )

51
+
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution map

Set of occupied space

Computed from current reading Occk(), last value Occt-1() and
free space Emp()
Occ X ,Y [0,1]
Occ k ,c X , Y :=Occ k X ,Y 1Emp t X ,Y
Occ k ,c X , Y
Occ k , n X ,Y :=
Occ k ,c X ' , Y '
X ' ,Y '
(normalization over all covered cells)

Occ t X , Y :=Occ t 1 X , Y Occ k , n X , Y Occ t1 X , Y Occ k , n X ,Y

52
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution map

Update map
Map computed from free space set and occupied space set
Encodes free, unknown and occupied areas
Map X , Y [1,1]
Map() < 0 free space, probability Map X , Y
Map() > 0 occupied, probability Map ( X, Y )

{
Occ t ( X , Y ) if Occt ( X , Y )> Emp t ( X , Y )
Map t ( X , Y )= Emp t ( X , Y ) if Occt ( X , Y )< Emp t ( X , Y )
0 unknown

53
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps

Shown method
Properties
Increases (angular) resolution by integrating different
viewpoints
Does fairly good free space estimation
Tend to overestimate occupied space
Shortcomings
Assumes precise position estimate
Sonar readings need to be preprocessed to remove outliers

54
 University of applied Sciences
Bonn-Rhein-Sieg
Sonar maps

Estimate high resolution maps

Incorporate motion estimation errors

Motion estimate used for determining S,
For position uncertainty p probability of point P to be free or
occupied become more uncertain
Take all possible values for S into account when calculation pE
and pO
The matching of map cells and reading cell becomes
uncertain
Distribute pE and pO over all possible target cells

55
University of applied Sciences
Bonn-Rhein-Sieg

That's all

56