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
