Autonomous Mobile Robots
"Position"
Localization Cognition
Global Map
Environment Model Path
Local Map
Perception Real World Motion Control
Environment
Probabilistic Map
Based Localization
Zürich Autonomous Systems Lab
5 - Localization and Mapping
5
2
Outline of today’s lecture
Definition and illustration of probabilistic map-based localization
Two solutions
Markov localization (today)
Kalman filter localization (next lecture)
Further references:
Thrun, Fox, Burgard, “Probabilistic Robotics,” MIT Press, 2005.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
3
The problem
Consider a mobile robot moving in a known environment.
As it starts to move (say from a precisely known location) it can keep track
of its location using wheel odometry.
However, after a certain movement the robot will get very uncertain about
its position => Thus, it should update its position by making “observations”
of the environment
Observations lead to an estimate of the robot position which can then be
fused with the odometric estimation to get the best possible update of the
actual robot’s position.
?
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5 Illustration of probabilistic bap-based localization:
4 Improving belief state by moving
The robot is placed somewhere in the
environment but is not told its location
x
The robot queries its sensors and finds it is
next to a pillar
The robot moves one meter forward.
To account for inherent noise in robot motion
the new belief is smoother
The robot queries its sensors and again it finds
itself next to a pillar
Finally, it updates its belief by combining this
information with its previous belief
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
5
Why a probabilistic approach for mobile robot localization?
Because the data coming from the robot sensors are affected by
measurement errors, we can only compute the probability that the robot is
in a given configuration.
The key idea in probabilistic robotics is to represent uncertainty using
probability theory: instead of giving a single best estimate of the current
robot configuration, probabilistic robotics represents the robot configuration
as a probability distribution over the all possible robot poses. This
probability is called belief.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 Example probability distributions for representing robot
6
configurations
In the following we consider a robot which is constrained to move along a
straight rail. The problem is one dimensional.
The robot configuration is the position x along the rail.
0 x
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
7
Example 1: Uniform distribution
This is used when the robot does not know where it is. It means that the
information about the robot configuration is null.
1
N
N
x
Remember that in order to be a probability distribution a function p(x) must
always satisfy the following constraint:
p( x)dx 1
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
8
Example 2: Multimodal distribution
The robot is in the region around “a” or “b”.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
9
Example 3: Dirac distribution
The robot has a probability of 1.0 (i.e. 100%) to be at position “a”.
p ( x) ( x a )
x
Note, the Dirac distribution is usually represented with an arrow.
The Dirac function (x) is defined as:
if x 0
( x)
0 otherwise
with ( x)dx 1
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
10
Example 4: Gaussian distribution
The Gaussian distribution has the shape of a bell and is defined by the
following formula:
( x )2
1
p ( x) e 2 2
2
where μ is the mean value and σ is the standard deviation.
The Gaussian distribution is also called normal distribution and is usually
abbreviated with N(μ ,σ ).
x
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
11
Description of the probabilistic localization problem
Consider a mobile robot moving in a known environment.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
12
Description of the probabilistic localization problem
Consider a mobile robot moving in a known environment.
As it starts to move, say from a precisely known location, it can keep track of its motion using
odometry.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
13
Description of the probabilistic localization problem
Consider a mobile robot moving in a known environment.
As it starts to move, say from a precisely known location, it can keep track of its motion using
odometry.
Due to odometry uncertainty, after some movement the robot will become very uncertain
about its position.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
14
Description of the probabilistic localization problem
Consider a mobile robot moving in a known environment.
As it starts to move, say from a precisely known location, it can keep track of its motion using
odometry.
Due to odometry uncertainty, after some movement the robot will become very uncertain
about its position.
To keep position uncertainty from growing unbounded, the robot must localize itself in relation
to its environment map. To localize, the robot uses its on-board exteroceptive sensors (e.g.
ultrasonic, laser, vision sensors) to make observations of its environment
Sensor reference frame
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
15
Description of the probabilistic localization problem
Consider a mobile robot moving in a known environment.
As it starts to move, say from a precisely known location, it can keep track of its motion using
odometry.
Due to odometry uncertainty, after some movement the robot will become very uncertain
about its position.
To keep position uncertainty from growing unbounded, the robot must localize itself in relation
to its environment map. To localize, the robot uses its on-board exteroceptive sensors (e.g.
ultrasonic, laser, vision sensors) to make observations of its environment
The robot updates its position based on the observation. Its uncertainty shrinks.
Sensor reference frame
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
16
Action and perception updates
In robot localization, we distinguish two update steps:
1. ACTION (or prediction) update:
• the robot moves and estimates its position through its proprioceptive sensors.
During this step, the robot uncertainty grows.
2. PERCEPTION (or meausurement) update:
• the robot makes an observation using its exteroceptive sensors and correct its position by
opportunely combining its belief before the observation with the probability of making exactly that
observation.
During this step, the robot uncertainty shrinks.
Probability of
making this
observation
Robot belief
before the
observation
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
17
The solution to the probabilistic localization problem
Solving the probabilistic localization problem
consists in solving separately the Action and Perception updates
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
18
The solution to the probabilistic localization problem
A probabilistic approach to the mobile robot
localization problem is a method able to compute the
probability distribution of the robot configuration
during each Action and Perception step.
The ingredients are:
1. The initial probability distribution p( x) t 0
2. The statistical error model of the proprioceptive
sensors (e.g. wheel encoders)
3. The statistical error model of the exteroceptive
sensors (e.g. laser, sonar, camera)
4. Map of the environment
(If the map is not known a priori then the robot needs to build a
map of the environment and then localize in it. This is called
SLAM, Simultaneous Localization And Mapping)
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
19
The solution to the probabilistic localization problem
How do we solve the Action and Perception updates?
Action update uses the Theorem of Total probability
Perception update uses the Bayes rule
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
20
Action update
In this phase the robot estimates its current position based on the
knowledge of the previous position and the odometric input
(The hypothesis that the current robot position depends only on the
previous position and the odometric input is called Markov assumption)
Using the Theorem of Total probability, we compute the robot’s belief after
the motion as
which can be written as a cross-correlation (sliding inner product)
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
21
Perception update
In this phase the robot corrects its previous position (i.e. its former belief)
by opportunely combining it with the information from its exteroceptive
sensors
p( zt | xt ) p( xt )
p( xt | zt )
p ( zt )
bel ( xt ) p( zt | xt )bel ( xt )
where η is a normalization factor which makes the probability integrate to 1.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
22
The probabilistic map-based localization algorithm
This localization algorithm is also called Bayes filter
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
23
Perception update
Explanation of bel ( xt ) p( zt | xt )bel ( xt )
p( zt | xt ) p( xt )
p( xt | zt )
p ( zt )
p( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
Illustration of probabilistic bap based localization
5 - Localization and Mapping
5
24
Initial probability distribution p( x) t 0
x
p( zt | xt )
Perception update
bel ( xt ) p( zt | xt )bel ( xt )
Action update
p( zt | xt )
Perception update bel ( xt ) p( zt | xt )bel ( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
Illustration of probabilistic bap based localization
5 - Localization and Mapping
5
25
Initial probability distribution p( x) t 0
x
p( zt | xt )
Perception update
bel ( xt ) p( zt | xt )bel ( xt )
Action update
p( zt | xt )
Perception update bel ( xt ) p( zt | xt )bel ( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
Illustration of probabilistic bap based localization
5 - Localization and Mapping
5
26
Initial probability distribution p( x) t 0
x
p( zt | xt )
Perception update
bel ( xt ) p( zt | xt )bel ( xt )
Action update
p( zt | xt )
Perception update bel ( xt ) p( zt | xt )bel ( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
Illustration of probabilistic bap based localization
5 - Localization and Mapping
5
27
Initial probability distribution p( x) t 0
x
p( zt | xt )
Perception update
bel ( xt ) p( zt | xt )bel ( xt )
Action update
p( zt | xt )
Perception update bel ( xt ) p( zt | xt )bel ( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
28
Markov versus Kalman localization
Two approaches exist to represent the probability distribution and to compute the Total Probability
and Bayes Rule during the Action and Perception phases
Markov Kalman
• The configuration space is divided into • The probability distribution of both the
many cells. The configuration space of a robot configuration and the sensor model is
robot moving on a plane is 3D dimensional assumed to be continuous and Gaussian!
(x,y,θ). Each cell contains the probability of
the robot to be in that cell. • Since a Gaussian distribution is only
described by its mean value μ and
• The probability distribution of the sensors variance Σ2, we need only to update μ and
model is also discrete. Σ2. Therefore the computational cost is
very low!
• During Action and Perception, all the cells
are updated. Therefore, the computational
cost is very high
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
Autonomous Mobile Robots
"Position"
Localization Cognition
Global Map
Environment Model Path
Local Map
Perception Real World Motion Control
Environment
Probabilistic Map Based
Localization:
Markov Localization
Zürich Autonomous Systems Lab
5 - Localization and Mapping
5
30
Markov localization
Markov localization uses a grid space representation of the robot configuration.
For sake of simplicity let us consider a robot moving in a one dimensional environment (e.g.
moving on a rail). Therefore the robot configuration space can be represented through the sole
variable x.
Let us discretize the configuration space into 10 cells
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
31
Markov localization
Let us discretize the configuration space into 10 cells
Suppose that the robot’s initial belief is a uniform distribution from 0 to 3. Observe that all the
elements were normalized so that their sum is 1.
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
32
Markov localization
Initial belief distribution
Action phase:
Let us assume that the robot moves forward with the following statistical model
This means that we have 50% probability that the robot moved 2 or 3 cells forward.
Considering what the probability was before moving, what will the probability be after the motion?
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
33
Markov localization: Action update
The solution is given by the cross correlation of the two distributions
*
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
34
Markov localization: Action update - Explanation
Here, we would like to explain where this formula actually comes from. In order to compute
the probability of position x1 in the new belief state, one must sum over all the possible ways
in which the robot may reach x1 according to the potential positions expressed in the former
belief state x0 and the potential input expressed by u1. Observe that because is limited
between 0 and 3, the robot can only reach the states between 2 to 6, therefore
You can now verify that these equations are computing nothing but the cross correlation in
the Action update or the Theorem of Total probability
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
35
Markov localization: Perception update
Let us now assume that the robot uses its onboard range finder and measures the distance
from the origin. Assume that the statistical error model of the sensors is
This plot tells us that the distance of the robot from the origin can be equally 5 or 6 units.
What will the final robot belief be after this measurement?
The answer is again given by the Bayes rule:
bel ( xt ) p( zt | xt )bel ( xt )
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
36 Markov Localization: Case Study 2 – Grid Map (5)
Example 2: Museum Courtesy of
Laser scan 1 W. Burgard
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
37 Markov Localization: Case Study 2 – Grid Map (6)
Example 2: Museum Courtesy of
Laser scan 2 W. Burgard
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
38 Markov Localization: Case Study 2 – Grid Map (7)
Example 2: Museum Courtesy of
Laser scan 3 W. Burgard
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
39 Markov Localization: Case Study 2 – Grid Map (8)
Example 2: Museum Courtesy of
Laser scan 13 W. Burgard
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
40 Markov Localization: Case Study 2 – Grid Map (9)
Example 2: Museum Courtesy of
Laser scan 21 W. Burgard
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
41
Drawbacks of Markov localization
In the more general planar motion case the grid-map is a three-dimensional array where each cell contains the
probability of the robot to be in that cell. In this case, the cell size must be chosen carefully.
During each prediction and measurement steps, all the cells are updated. If the number of cells in the map is
too large, the computation can become too heavy for real-time operations.
As an example, consider a 30x30 m environment and a cell size of 0.1 m x 0.1 m x 1 deg. In this case, the
number of cells which need to be updated at each step would be 30x30x100x360=32.4 million cells!
Fine fixed decomposition grids result in a huge state space
Very important processing power needed
Large memory requirement
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
5 - Localization and Mapping
5
42
Next lecture: Kalman filter localization
© R. Siegwart, D. Scaramuzza ETH Zurich - ASL
