Laboratory of Autonomous Robots

and Intelligent Systems

1001501 - INTRODUÇÃO À
PROGRAMAÇÃO DE ROBÔS MÓVEIS
CCO728 - ROBÔS MÓVEIS AUTÔNOMOS

LOCALIZATION AND NAVIGATION

Departamento de Computação
Prof. Dr. Kelen Cristiane Teixeira Vivaldini
CLARIS*

Slides adapted from:

Roland Siegwart and Illah R. Nourbakhsh (2004). Introduction to
Autonomous Mobile Robots.
Roland Siegwart, Margarita Chli, Juan Nieto and Nicholas Lawrance,
Localization I and Localization II, Autonomous Mobile Robots - Spring
2019
CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS? Where am I?
a:*'
• Odometry, Dead reckoning
• Localization bases in external sensors, beacons or
landmarks
• Probabilistic Map Based Localization

PROF. DR. KELEN TEIXEIRA VIVALDINI 5

CLARIS* Do we need to localize or not?

• To go from A to B, does the robot need to know

where it is?

PROF. DR. KELEN TEIXEIRA VIVALDINI 6

^LWSs Do we need to localize or not?
DC^
• How to navigate between A and B
– navigation without hitting obstacles
– detection of goal location
• Possible by following always the left wall
– However, how to detect that the goal is reached

PROF. DR. KELEN TEIXEIRA VIVALDINI 7

CLARIS? Do we need to localize or not?

• Following the left wall is an example of “behavior

based navigation”
– It can work in some environments but not in all
– With which accuracy and reliability do we reach the
goal?

coordination I fusion
e.g. fusion via vector summation

Figure 5.7 8
An architecture for behavior-based navigation.
 Do we need to localize or not?

• Map based navigation: Assuming that the map is known, at

every time step the robot has to know where it is. How?
• If we know the start position, we can use wheel odometry or dead
reckoning. Is this enough? What else can we use?
• But how do we represent the map for the robot?
• And how do we represent the position of the robot in
the map?

Figure 5.8
9
An architecture for map-based (or model-based) navigation.
 Localization Problem

• Given: Map of the environment

• Want: Pose of the robot
• Problem: Estimate p(x|M, {z0, z1, . . .})

PROF. DR. KELEN TEIXEIRA VIVALDINI 10

^LARISf Definitions

Global localization
– The robot is not told its initial position
– Its position must be estimated from scratch
Position Tracking
– A robot knows its initial position and “only” has to
accommodate small errors in its odometry as it moves
Kidnapped problem
– The robot can be moved to any position in the environment
– Requires robust solution, capable of recovering from
failures

PROF. DR. KELEN TEIXEIRA VIVALDINI 11

CLARIS* Definitions

Global localization
– Find the pose of the robot without any prior knowledge
– Data can typically be explained in several ways ⇒
ambiguities
– Can typically not be done without integrating information
over time

PROF. DR. KELEN TEIXEIRA VIVALDINI 12

CLARIS* Definitions

Position Tracking
– Robot knows where it was and just has to keep localized
– Simpler problem

PROF. DR. KELEN TEIXEIRA VIVALDINI 13

DC Definitions

Kidnapped problem
– Robot is abducted in tracking phase
and put down somewhere else
– Hard problem
– Must first realize that it is lost
– Then perform global localization
– Example: Cleaning robot that is
carried away to another room
Y. Seow, R. 2017 I"Detecting and solving the
kidnapped robot problem using laser range
finder and wifi signal," , Okinawa, Japan,
2017, pp. 303-308, doi:
10.1109/RCAR.2017.8311878.

PROF. DR. KELEN TEIXEIRA VIVALDINI 14

CLARIS* Definitions

Global Localization Position Tracking Kidnapped problem

• Initial Pose: Unknown. • Initial Pose: Known. • Robot is abducted in tracking
• Uncertainty: High (entire • Uncertainty: Small (localized, Initial Pose: Initially known —
map must be considered). often modeled with a but suddenly changed.
• Goal: Estimate where the Gaussian). • Uncertainty: Critical — robot is
robot is within the map. • Goal: Track the robot’s relocated unexpectedly.
• Assumption: Robot knows movement over time,
the map but not its position. • Goal: Recover from a false belief
accounting for minor noise. about its current pose.
• Difficulty: Moderate to hard • Assumption: Robot starts in a
— multiple possible • Assumption: Robot thinks it
known location and moves
locations must be gradually with little drift. knows its position but is actually
considered. somewhere else.
• Difficulty: Easiest of the
Example: A robot is placed • Difficulty: Hardest — robot
randomly in a building and must three.
must realize it's wrong and
use sensor data to determine • Example: A robot starts in a re-localize.
where it is on the map known room corner and
moves slowly. Sensors and Example: A robot navigating a corridor
odometry keep track of its is picked up and laced in another
pose. room. It must detect that it’s been
"kidnapped" and correct its belief.

PROF. DR. KELEN TEIXEIRA VIVALDINI 15

CLARIS* How to localize?

• Localization based on external sensors, beacons or landmarks

• Odometry
• Map Based Localization - without external sensors or artificial
landmarks, just use robot onboard sensors
– Example: Probabilistic Map Based Localization

PROF. DR. KELEN TEIXEIRA VIVALDINI 16

 Beacon Based Localization

• Triangulation
– Ex 1: Poles with highly reflective surface and a laser for detecting them
– Ex 2: Coloured beacons and an omnidirectional camera for detecting
them (example: RoboCup or autonomous robots in tennis fields)

PROF. DR. KELEN TEIXEIRA VIVALDINI 17

CLARIS* Beacon Based Localization

• KIVA Systems, Boston (MA) (acquired by Amazon in 2011)

Unique marker with

known absolute 2D
position in the map
Prof. Raff D'Andrea,
ETH

PROF. DR. KELEN TEIXEIRA VIVALDINI 18

CLARIS* Motion Capture Systems

• High resolution (from VGA up to 16 Mpixels)

• Very high frame rate (several hundreds of Hz)
• Good for ground truth reference and multi-robot control
strategies
• Popular brands:
– VICON
– OptiTrack

PROF. DR. KELEN TEIXEIRA VIVALDINI 19

 Map-based localization

• Consider a mobile robot moving in a known environment.

PROF. DR. KELEN TEIXEIRA VIVALDINI 20

CLARIS* Map-based localization

• 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

PROF. DR. KELEN TEIXEIRA VIVALDINI 21

CLARIS* Map-based localization

• 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

PROF. DR. KELEN TEIXEIRA VIVALDINI 22

CLARIS? Map-based localization

• 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

PROF. DR. KELEN TEIXEIRA VIVALDINI 23

CLARIS? Map-based localization

• 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
• The robot makes an observation and updates its position and
uncertainty

PROF. DR. KELEN TEIXEIRA VIVALDINI 24

CLARIS* Probabilistic Map-based localization

• Probability theory → error propagation, sensor fusion

• Belief representation → discrete / continuous (map/position)
• Motion model → odometry model
• Sensing → measurement model

PROF. DR. KELEN TEIXEIRA VIVALDINI 25

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Probabilistic localization
Belief Representation

• The robot must have a representation (a model) of the

environment, or a map.
– What aspects of the environment are contained in this map?
– At what level of fidelity does the map represent the environment?
• The robot must also have a representation of its belief
regarding its position on the map.
– Does the robot identify a single unique position as its current position,
or does it describe its position in terms of a set of possible positions?
– If multiple possible positions are expressed in a single belief, how are
those multiple positions ranked, if at all?

PROF. DR. KELEN TEIXEIRA VIVALDINI 27

^LARIS$ Probabilistic localization
t>Z^ Belief Representation
PM

• Continuous map with single Kalman Filter

Localization
hypothesis probability distribution
p(x)
pW

• Continuous map with multiple

hypotheses probability distribution
p(x)

• Discretized metric map (grid k)

with probability distribution p(k)

• Discretized topological map (nodes

n) with probability distribution p(n)
n„ml
A B C D E F G
PROF. DR. KELEN TEIXEIRA VIVALDINI 28
CLARIS* Probabilistic localization
Belief Representation

Characteristics
• Continuous
– Precision bound by sensor data
– Typically single hypothesis pose estimate
– Lost when diverging (for single hypothesis)
– Compact representation and typically reasonable in processing power.

• Discrete
– Precision bound by resolution of discretization
– Typically multiple hypothesis pose estimate
– Never lost (when diverges converges to another cell)
– Important memory and processing power needed. (not the case for
topological maps)

PROF. DR. KELEN TEIXEIRA VIVALDINI 29

 Probabilistic localization
Belief Representation
Single-hypothesis belief
• Probabilistic localization - Belief Representation expressed as a
single unique point on the map.
PROF. DR. KELEN TEIXEIRA VIVALDINI 30
CLARIS* Probabilistic localization
Belief Representation

Grid size around 20 cm2.

• Clouds represent possible robot locations
• Darker coloring means higher probability

mnm rniilTi iTiriTTi

Path of the robot Belief states at positions 2, 3 and 4

PROF. DR. KELEN TEIXEIRA VIVALDINI 31

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Odometry

• Most robots have motor encoders ⇒ odometry

• Provides information about relative motion
• Typically very accurate at short range
• Will drift over longer ranges
• Error in dead-reckoning unbounded
• Angular error ⇒ large position errors
CLARIS* Odometry

Definition
– Dead reckoning (also deduced reckoning or odometry) is
the process of calculating vehicle's current position by using
a previously determined position and estimated speeds
over the elapsed time
Robot motion is recovered by integrating proprioceptive sensor
velocities readings
– Pros: Straightforward
– Cons: Errors are integrated -> unbound
Heading sensors (e.g., gyroscope) help to reduce the accumulated
errors but drift remains
PROF. DR. KELEN TEIXEIRA VIVALDINI 34
 Odometry

The Differential Drive Robot

=f(x,_i,ul)

PROF. DR. KELEN TEIXEIRA VIVALDINI 35

CLARIS* Odometry

Kinematics

^0 '
Ascos(0+— )
This term comes from the
x,=f(xl.„ul)= &ssm(0+ —) application of the Instantaneous
Center of Rotation
&0

Av = —-
Av,. + Av,
2
-

PROF. DR. KELEN TEIXEIRA VIVALDINI 36

CLARIS* Odometry

Kinematics

The relationship between these

variables can be expressed using the
Instantaneous Center of Rotation.
The equations for the linear and ● R be the radius of the wheels,
angular velocities of the robot's ● L be the distance between the wheels
(wheelbase),
center are as follows: ● ωL and ωR be the angular velocities of the
left and right wheels, respectively,
1. Linear Velocity (vv):
● v(t) be the linear velocity of the robot's
v=R2(ωR+ωL) center, and
● ω(t) be the angular velocity of the robot.
2. Angular Velocity (ωω):
ω=RL(ωR−ωL)

PROF. DR. KELEN TEIXEIRA VIVALDINI 37

CLARIS* Odometry
Error Propagation
Error model ● Δs be the incremental linear distance traveled,
● Δθ be the incremental angular rotation,
Δs′=Δs+ϵs ● vv be the actual linear velocity,
● ω be the actual angular velocity,
Δθ′=Δθ+ϵθ ● Δs′ be the measured linear distance (odometry),
● Δθ′ be the measured angular rotation
r^T (odometry),
q csdt ϵs be the error in linear distance measurement,
Total Error in Position= ●
● ϵθ be the error in angular rotation measurement.
Total Error in Orientation= CT
Jo

These integrals represent the total accumulated error in position and orientation over
the period from t=0 to t=T. The error is integrated over time, reflecting the impact of
any inaccuracies or uncertainties in the odometry measurements.

PROF. DR. KELEN TEIXEIRA VIVALDINI 38

CLARIS* Odometry
Error Propagation
y t y T
– Error model -f
rxt[ + f -F
rAS
/:r|Asr| 0
E
"I 0

1 0 -4ssin(9 + A6/2)
F =WX*»-4= 0 1 Ascos(0 + A0/2)
dv 30
_0 0 1

PROF. DR. KELEN TEIXEIRA VIVALDINI 39

ufk-r^r
 Odometry
CLARIS* Growth of Pose uncertainty for Straight Line
Movement

Note: Errors perpendicular to the direction of movement are

growing much faster!

PROF. DR. KELEN TEIXEIRA VIVALDINI 40

 Odometry
Example of non-Gaussian error model

Note: Errors are not shaped like ellipses!

PROF. DR. KELEN TEIXEIRA VIVALDINI Courtesy AI Lab, Stanford

41
 Odometry
CLARIS* Example of non-Gaussian error model

Note: Errors are not shaped like ellipses!

[Fox, Thrun, Burgard, Dellaert, 2000]

PROF. DR. KELEN TEIXEIRA VIVALDINI 42
 Odometry
Error Sources

Deterministic Non-Deterministic
(Systematic) (Non-Systematic)

• Deterministic errors can be eliminated by proper calibration of the system.

• Non-Deterministic errors are random errors. They have to be described by
error models and will always lead to uncertain position estimate.
• Major Error Sources in Odometry:
– Limited resolution during integration (time increments, measurement
resolution)
– Wheel misalignment (deterministic)
– Unequal wheel diameters (deterministic)
– Variation in the wheel contact point (non deterministic)
– Uneven contact with the ground (slippage, non planar …) (non
deterministic) PROF. DR. KELEN TEIXEIRA VIVALDINI 43
 Odometry
CLARIS* Calibration of systematic errors
t>Z^ [Borenstein 1996]

The unidirectional square path experiment

Reference Wall

Forward

Start
Preprogrammed
square path, 4x4 m.
87° turn instead of 90P turn
(due to uncertainty about
the effective wheel base).
Curved instead of straight path
(due to unequal wheel diameters).

'J : In the example here, thia causes

a 3 orientation error.

PROF. DR. KELEN TEIXEIRA VIVALDINI 44

 Odometry
CLARIS* Calibration of errors II
[Borenstein 1996]

The bi-directional square path experiment

Reference Wall

Curved instead of straight path

(due to unequal wheel diameters).
In the example here, this causes
a 3 orientation error.
93’ turn instead of 90° turn
(due to uncertainty about the
effective wheelbase).

PROF. DR. KELEN TEIXEIRA VIVALDINI 45

CLARIS* Odometry
Odometer error

PosiQSo Inicial
do Robo

PROF. DR. KELEN TEIXEIRA VIVALDINI 46

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Probabilistic Reasoning

Bayesian
• Reasoning in the presence of uncertainties and incomplete
information
• Combining preliminary information and models with learning
from experimental data

p^MpW
p(x|y) =

PROF. DR. KELEN TEIXEIRA VIVALDINI 48

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Map Representation

• Map precision vs. application

– The precision of the map must match the precision with which the
robot needs to achieve its goals.
• Features precision vs. map precision
– The precision of the map and the type of features represented must
match the precision and data types returned by the robot’s sensors.
• Precision vs. computational complexity
– The complexity of the map representation has a direct impact on the
computational complexity of reasoning about mapping, localization, and
navigation
• Two primary map choices:
– Continuous Representation
– Decomposition (Discretization)
PROF. DR. KELEN TEIXEIRA VIVALDINI 50
CLARIS* Map Representation

• Continuous Representation
• Decomposition strategies

Selecting an appropriate representation requires understanding

all of the trade-offs inherent in that choice as well as
understanding the specific context in which a particular mobile
robot implementation must perform localization

PROF. DR. KELEN TEIXEIRA VIVALDINI 51

CLARIS* Map Representation
Continuous Line-Based

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

PROF. DR. KELEN TEIXEIRA VIVALDINI 52

CLARIS* Map Representation
Exact cell decomposition
Fixed cell decomposition (occupancy grid)
Narrow passages disappear
Exact cell decomposition –
Polygons

Fixed cell decomposition

Narrow passages disappear

PROF. DR. KELEN TEIXEIRA VIVALDINI 53

 Map Representation
^LWSs
DC^ Approximate cell decomposition
Occupancy grid example
– 0 indicates that the cell has not been hit by any ranging measurements
(free space)
– 1 indicates that the cell has been hit one or multiple times by ranging
measurements (occupied space)
– Can change over time (e.g. dynamic obstacles)

Courtesy of S. Thrun

PROF. DR. KELEN TEIXEIRA VIVALDINI 54

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Probabilistic Map-Based

• Markov localization allows for localization starting from any

unknown position and can thus recover from ambiguous situations
because the robot can track multiple, completely disparate possible
positions. However, to update the probability of all positions within
the whole state space at any time requires a discrete representation
of the space, such as a geometric grid or a topological graph. The
required memory and computational power can thus limit precision
and map size.
• Kalman filter localization tracks the robot from an initially known
position and is inherently both precise and efficient. It can be used
in continuous world representations. However, if the uncertainty of
the robot becomes too large (e.g., due to a robot collision with an
object) and thus not truly unimodal, the Kalman filter can fail to
capture the multitude of possible robot positions and can become
irrevocably lost

PROF. DR. KELEN TEIXEIRA VIVALDINI 56

CLARIS* Probabilistic Map-Based

• Markov Localization
– Central idea: represent robot’s belief by a probability distribution over
possible positions, and uses Bayes’ rules and convolution to update the belief
whenever the robot senses or moves
– Markov Assumption: past and future data are independent if one knows the
current state
• Kalman Filter
– Central idea: posing localization problem as a sensor fusion problem
– Assumption: Gaussian distribution function
• Particle Filtering
– Monte-Carlo method
• SLAM (Simultaneous Localization and Mapping)
• Multi-robot localization

PROF. DR. KELEN TEIXEIRA VIVALDINI 57

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Probabilistic Map-Based
Markov Localization
• Applying probability theory to robot localization
• Markov localization uses an explicit, discrete representation
for the probability of all position in the state space.
• This is usually done by representing the environment by a grid
or topological graph with a finite number of possible state
(positions).
• During each update, the probability for each state (element)
of the entire space is updated.

PROF. DR. KELEN TEIXEIRA VIVALDINI 59

CLARIS* Probabilistic Map-Based
t>Z^ Markov Localization
Assume the robot position is one-dimensional

PROF. DR. KELEN TEIXEIRA VIVALDINI 60

CLARIS* Probabilistic Map-Based
Markov Localization
Assume the robot position is one-dimensional

1: Algorithm MaikovJocalizatioii(5eZ(Tf_i), ut, Zt, m):

2: for all xt do
3: bel(xt) = J p(xt | xt_i,m) bel(xt-i) dx
4: bel(xt) = i] p(zt | xt , m) bel(xt)
5: end for
6: return bel(xt)

PROF. DR. KELEN TEIXEIRA VIVALDINI 61

^LARISf Probabilistic Map-Based
Markov
Assume the robot position
is one-dimensional

1: Algorithm Mai kovJocalization(M(xt_ j ), utyzt,m):

2: for allxt do
3: bel(xt) = f p(xt |Uf^x^.m) 6d(ze_j) dx
4: bcl(xt) = r] p(zt |it,m) bcl(Tt)
5: endfor

6: return bel(xt)

Autonomous Mobile Robots

Roland Siegwart, Margarita Chli, Martin Rufli
CLARIS* Probabilistic Map-Based
Markov Localization
Basic Notation
f =< x,y,8> robot location
robot s true location at time t

Lt Random variable that expresses the robot's location

Bel(L,) Robot’s position belief at time t

Probability distribution over the space of locations

Bel(Lt=7 ) Is the probability (density) that the robot

assigns to the possibility7 that its location at time t is 1

The belief is updated in response to two different types of events:

• sensor readings (SEE)
• odometry data (ACT)
PROF. DR. KELEN TEIXEIRA VIVALDINI 63
CLARIS* Probabilistic Map-Based
Markov Localization
Notation

_ (
ai odometry readings
si environment sensor readings

Bayes’ Rule:

Goal:
Estimate the posterior distribution over LT conditioned
on all available data

P(LT=/:|d) = P(LT=/|d0 dT)

PROF. DR. KELEN TEIXEIRA VIVALDINI 64

CLARIS* Probabilistic Map-Based
Markov Localization

Markov Assumption

.
If one knows the robot’s location £ future measurements are
independent of past ones (and vice-versa)

P(d,.bdtt2,- 1 Lt = ^A-d,) = P(dtt1,du2>- 1 Lt = 0

• The robot’s location is the only state in the environment

• Knowing the robot state is all one needs to know about the
past to predict future data.

PROF. DR. KELEN TEIXEIRA VIVALDINI 65

CLARIS* Probabilistic Map-Based
Markov Localization

Update Phase (SEE)

a b r c
P(LT = q d) = P(C^|(tw7^
by Bayes' law: Ralbx)^^^*^
_ P(sT|d0,d„--,dT.„LT = QP(Lt = / |dt,d„-,dT.t)
P(ST I l)
by Markov assumption

= P(sj^=^^
fP(sT I do.d^- -,^ 0 1
L. «— Does not
depend on LT

!-►= aTP(sT |Lt = OP(Lt = f | d0,d1( -,dT ,)

PROF. DR. KELEN TEIXEIRA VIVALDINI 66

CLARIS* Probabilistic Map-Based
Markov Localization

Update Phase (SEE)

Defining

Bel(LT=O = P(LT = ddo.d„-.dT)

Bel(LT = 0 = aTP(sT I LT = OBeKL^ = 0
independent of time

Bel(LT=O = aTP(sT 10661(1^ = 0 Incremental

form

PROF. DR. KELEN TEIXEIRA VIVALDINI 67

CLARIS* Probabilistic Map-Based
Markov Localization
Recursive Localization

P(LT = '|d)

- Case 1 (Update Phase)

• the most recent data item is a sensor
measurement
dT = ST
Bel(LT = /) = aTP(sTp)Bel(LT1 = O

- Case 2 (Prediction Phase)

• The most recent data item is an odometry reading

dT - aT
Bel(LT =/) = JP(/ |aT,r)Bel(LT = /')d/' ,
PROF. DR. KELEN TEIXEIRA VIVALDINI 68
CLARIS* Markov Localization
Case Study – Grid Map

Example 2: Museum -> Laser scan 1

PROF. DR. KELEN TEIXEIRA VIVALDINI 69

CLARIS* Markov Localization
Case Study – Grid Map

Example 2: Museum -> Laser scan 2

PROF. DR. KELEN TEIXEIRA VIVALDINI 70

CLARIS? Markov Localization
Case Study – Grid Map

Example 2: Museum -> Laser scan 3

PROF. DR. KELEN TEIXEIRA VIVALDINI 71

 Markov Localization
Case Study – Grid Map

Example 2: Museum -> Laser scan 13

PROF. DR. KELEN TEIXEIRA VIVALDINI 72

 Markov Localization
Case Study – Grid Map

Example 2: Museum -> Laser scan 21

PROF. DR. KELEN TEIXEIRA VIVALDINI 73

 Drawbacks of
Markov localization
• Planar motion case
– is a three-dimensional grid-map array
– cell size must be chosen carefully.
• During each prediction and measurement steps
– all the cells are updated
– the computation can become too heavy for real-time
operations.
• Example
– 30x30 m environment;
cell size of 0.1 m x 0.1 m x 1 deg
→ 300 x 300 x 360 = 32.4 million cells!
→ Important processing power needed
→ Large memory requirement
PROF. DR. KELEN TEIXEIRA VIVALDINI 74
CLARIS* Drawbacks of
Markov localization

• Reducing complexity
– Various approaches have been proposed for reducing
complexity
– One possible solution would be to increase the cell size at
the expense of localization accuracy.
– Another solution is to use an adaptive cell decomposition
instead of a fixed cell decomposition.

PROF. DR. KELEN TEIXEIRA VIVALDINI 75

CLARIS* Drawbacks of
Markov localization

• Randomized Sampling / Particle Filter

– The main goal is to reduce the number of states that are updated in
each step
– Approximated belief state by representing only a ‘representative’
subset of all states (possible locations)
– E.g update only 10% of all possible locations
– The sampling process is typically weighted, e.g. put more samples
around the local peaks in the probability density function
– However, you have to ensure some less likely locations are still
tracked, otherwise the robot might get lost

PROF. DR. KELEN TEIXEIRA VIVALDINI

r
76
 Map Representation
Topological map
• London underground map
PROF. DR. KELEN TEIXEIRA VIVALDINI 77
^LARIS$ Map Representation
t>Z^ Topological map

A topological map represents the environment as a graph with

nodes and edges.
• Nodes correspond to spaces
• Edge correspond to physical connections between nodes
Topological maps lack scale and distances, but topological
relationships (e.g., left, right, etc.) are maintained

PROF. DR. KELEN TEIXEIRA VIVALDINI 78

 State-of-the-Art: Current
CLARIS*
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
CLARIS* Markov Localization
Case Study - Topological Map 1

The Dervish Robot

• Topological Localization with sonar

PROF. DR. KELEN TEIXEIRA VIVALDINI 80

CLARIS* Markov Localization
Case Study - Topological Map 2
Topological map of office-type environment

RI R2

Wall Closed Open Open Foyer

door door hallway

Nothing detected 0.70 0.40 0.05 0.001 0.30

Closed door detected 0.30 0.60 0 0 0.05

Open door detected 0 0 0.90 0.10 0.15

Open hallway detected 0 0 0.001 0.90 0.50

PROF. DR. KELEN TEIXEIRA VIVALDINI 81

CLARIS* Markov Localization
Case Study - Topological Map 3
Topological map of office-type environment

• Update of believe state for position n given the percept¬

pair i
p(n\i) = p(i\n)p(n)
• p(n\i\. new likelihood for being in position n
• p(n): current believe state
p(i\n): probability of seeing / in n (see table)
No action update !
However, the robot is moving and therefore we can apply a combination of
action and perception update

P(nt\it) = jp('h\n\-th)PWt-iWt-i
t-i is used instead of t-1 because the topological distance between n’and n
is very depending on the specific topological map

PROF. DR. KELEN TEIXEIRA VIVALDINI 82

CLARIS* Markov Localization
Case Study - Topological Map 4

The calculation

is realized by multiplying the probability of generating perceptual event / at

position n by the probability of having failed to generate perceptual event s at
all nodes between n’and n.

p(nt = p(it,nt)-p(0, nl_i)p(0,nl_2)- ... p(0,nt_l+i)

PROF. DR. KELEN TEIXEIRA VIVALDINI 83

CLARIS* Markov Localization
Case Study - Topological Map 5
Example calculation
Assume that the robot has two nonzero belief states
p(1-2)=VQ p(2-3) = 0.2*
and that it is facing east with certainty
Perceptual event: open hallway on its left and open door on its right
State 2-3 will progress potentially to 3, 3-4 or 4.
State 3 and 3-4 can be eliminated because the likelihood of detecting an open door is
zero.
The likelihood of reaching state 4 is the product of the initial likelihood p(2-3)= 0.2, (a) the
likelihood of detecting anything at node 3 and the likelihood of detecting a hallway on the
left and a door on the right at node 4 and (b) the likelihood of detecting a hallway on the
left and a door on the right at node 4. (for simplicity we assume that the likelihood of
detecting nothing at node 3-4 is 1.0)
(a) occurs only if Dervish fails to detect the door on its left at node 3 (either closed or
open), [0.6 • 0.4 +(1-0.6) • 0.05] and correctly detects nothing on its right, 0.7.
(b) occurs if Dervish correctly identifies the open hallway on its left at node 4, 0.9C , and
mistakes the right hallway for an open door, 3.10.
This leads to:
. . . .
0.2 [0.6 • 0.4 + 0.4 • 0.05] 0.7 [0.9 0.1] p(4) = 0.003.
Similar calculation for progress from 1-2 p(2) = 0.3.

PROF. DR. KELEN TEIXEIRA VIVALDINI 84

CLARIS? Markov Localization
Case Study - Topological Map 5

Wall Closed Open Open Foyer

door door hallway
Nothing detected 0.70 0.40 0.05 0.001 0.30

Closed door detected 0.30 0.60 0 0 0.05

Open door detected 0 0 0.90 0.10 0.15

Open hallway detected 0 0 0.001 0.90 0.50

PROF. DR. KELEN TEIXEIRA VIVALDINI 85

CLARIS* Outline

• Introduction
• Probabilistic localization
– Belief Representation
• Odometry
• Probabilistic Reasoning
• Map Representation
• Probabilistic Map-Based
– Markov Localization
– Kalman Filter Localization
CLARIS* Probabilistic Map-Based
Kalman Filter Localization
Typical Kalman filter application
System error
source

Control

Optimal estimate
of system state

Measurement Autonomous Mobile Robots

error sources Roland Siegwart, Margarita Chli, Martin Rufli

PROF. DR. KELEN TEIXEIRA VIVALDINI 87

 Probabilistic Map-Based
CLARIS? Kalman Filter Localization
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Martin Rufli

Encoder

Map
(data base)
opbrseerdvaitcitned
Schematic for Kalman filter
mobile robot localization
1. Prediction based on previous estimate and
odometry
2. Observation with on-board sensors
3. Measurement prediction based on prediction and
map
4. Matching of observation and map
5. Estimation -> position update (posteriori position)
PROF. DR. KELEN TEIXEIRA VIVALDINI 88
ufw^r
CLARIS* Probabilistic Map-Based
Kalman Filter Localization
In summery
1. Prediction (ACT) based on previous estimate and odometry
2. Observation (SEE) with on-board sensors
3. Measurement prediction based on prediction and map
4. Matching of observation and map
5. Estimation — position update (posteriori position)

PROF. DR. KELEN TEIXEIRA VIVALDINI 89

CLARIS* Probabilistic Map-Based
Kalman Filter Localization
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Martin Rufli
Probabilistic Position Estimation
Kalman Filter: continuous, recursive and very compact

PROF. DR. KELEN TEIXEIRA VIVALDINI 90

CLARIS* Kalman Filter Localization
ACT
Using motion model and its uncertainties
0 75 bel^X^^ prior belief
0.5

0.25

0.75

0.5 -
bel(xt) = p{xt\ut9xt^beKxt^)
0.25
xt-i

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32

PROF. DR. KELEN TEIXEIRA VIVALDINI 91

CLARIS* Kalman Filter Localization
SEE
Estimation of position based on perception and map

0.75 belfat) prediction update

0.5 -

0.25

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32

0.75 p(zt|xt,M) perception Map

0.5 -
SEE
0.25

measurement Multiplication and normalization (tj)

0.75 bel(Xt)
0.5 -

0.25
bel(xt) = r\p(zt\xtf M^bel^x^
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32

PROF. DR. KELEN TEIXEIRA VIVALDINI 92

 Probabilistic Map-Based
Markov versus Kalman

Markov Kalman
PROS PROS
• localization starting from any • Tracks the robot and is inherently
unknown position very precise and efficient
• recovers from ambiguous situation CONS
CONS • If the uncertainty of the robot
• However, to update the probability of becomes to large (e.g. collision with
all positions within the whole state an object) the Kalman filter will
space at any time requires a discrete
representation of the space (grid). The
required memory and calculation
power can thus become very
important if a fine grid is used.

PROF. DR. KELEN TEIXEIRA VIVALDINI 93