Introduction to

Mobile Robotics

Welcome
Wolfram Burgard
Organization

§ Web page:
§ www.informatik.uni-freiburg.de/~ais/
§ http://ais.informatik.uni-freiburg.de/teaching/
ss18/robotics/
People
Teaching:
 Wolfram Burgard

Teaching assistants:
 Marina Kollmitz
 Chau Do
 Lukas Luft

3
Goal of this course

 Provide an overview of problems /

approaches in mobile robotics

 Probabilistic reasoning: Dealing with

noisy data

 Hands-on experience
AI View on Mobile Robotics
Sensor data

Control system

World model
Actions
Robotics Yesterday
Current Trends in Robotics
Robots are moving away from factory floors
to
 Entertainment, toys
 Personal services
 Medical, surgery
 Industrial automation
(mining, harvesting, …)
 Hazardous environments
(space, underwater)
Robotics Today
Shakey the Robot
Shakey the Robot
The Helpmate System
DARPA Grand Challenge

[Courtesy by Sebastian Thrun]

RoboCup-99, Stockholm, Sweden
Emotional Robots: Cog & Kismet

[Brooks et al., MIT AI Lab, 1993]

PR2 Robot
PR2 Robot

pr2-drawing.m4v
General Background
 Autonomous, automaton
 self-willed (Greek, auto+matos)

 Robot
 Karel Capek in 1923 play R.U.R.
(Rossum’s Universal Robots)
 labor (Czech or Polish, robota)
 workman (Czech or Polish, robotnik)
Asimov’s Three Laws of Robotics
1. A robot may not injure a human being, or,
through inaction, allow a human being to come to
harm.

2. A robot must obey the orders given it by human

beings except when such orders would conflict
with the first law.

3. A robot must protect its own existence as long as

such protection does not conflict with the first or
second law.

[Runaround, 1942]
Wiener, Cybernetics
 Studied regulatory systems and their application to
control (antiaircraft gun)

 “it has long been clear to me that the modern ultra-rapid

computing machine was in principle an ideal central nervous
system to an apparatus for automatic control; and its input
and output need not be in the form of numbers or diagrams,
but might very well be, respectively, the readings of artificial
sensors such as photoelectric cells or thermometers, and the
performance of motors or solenoids”.

[Electronics, 1949]
Trends in Robotics Research
Classical Robotics (mid-70’s)
• exact models
• no sensing necessary

Reactive Paradigm (mid-80’s)

• no models
Hybrids (since 90’s) • relies heavily on good sensing
• model-based at higher levels
• reactive at lower levels
Learning-based (since 2015)
• data driven
• deep learning

Probabilistic Robotics (since mid-90’s)

• seamless integration of models and sensing
• inaccurate models, inaccurate sensors
Brief Case Study:
Museum Tour-Guide Robots

Rhino, 1997 Minerva, 1998

Rhino (Univ. Bonn + CMU, 1997)
Minerva (CMU + Univ. Bonn, 1998)
Components of Typical Robots

cameras

sensors
sonars

laser

base actuators

25
26
Architecture of a Typical Control
System
Foundations of Artificial Intelligence

• Action Planning: Theory and Practice

 Fast planning systems (intern. competitions!)
 Applications at airports and for lift systems
 Theoretical results (see new Russell/Norvig)
 SFB AVACS
• Qualitative Temporal-Spatial Reasoning
 Theory and reasoning algorithms
 Application in qualitative layout description
 SFB “Spatial Cognition”
• RoboCup
 World champion three times
 Autonomous table soccer
 RoboCup Rescue
(Multi-Agent-System for disaster relief)
Autonomous Intelligent Systems

 Mobile robots

 State estimation and models

 Adaptive techniques and learning

 Multi-robot systems

 Applications of mobile robots

 Robots and embedded systems

 Interaction and Web interfaces

 Probabilistic robotics
Machine Learning Lab
 Reinforcement Learning

 Supervised Learning

 Efficient Learning Algorithms

 Learning in Multi-Agent systems

 Self-learning robots

 Neural Forecasting Systems

 Neural Controllers

 Learning soccer robots in RoboCup

 Industrial Applications
Humanoid Robots
 Development of techniques for robots with
 human-like body plan
 human-like senses
 Navigation in complex indoor environments
 3D environment modeling
 Path planning
 Classification and learning
 Natural human-robot interaction
 State estimation and modeling of people
 Speech, gestures, facial expression, etc.
Social Robotics Lab
 Towards socially compatible robots

 Social learning, learning by observation

 People detection and tracking

 Motion planning

 Robot navigation

 Spatio-temporal models of
human social behavior

 Human–robot interaction

"Free robots from their social isolation"

Robotics in Freiburg
 Foundations of Artificial Intelligence
 Machine Learning and Data Mining
 Knowledge Representation
 Autonomous Mobile Systems
 AI Planning
 Logic
 Game Theory
 Robotics II
 ...
Introduction to
Mobile Robotics
Compact Course on
Linear Algebra
Wolfram Burgard, Bastian Steder
Reference Book
Thrun, Burgard, and Fox:
“Probabilistic Robotics”
Vectors
 Arrays of numbers
 Vectors represent a point in a n dimensional
space
Vectors: Scalar Product
 Scalar-Vector Product
 Changes the length of the vector, but not
its direction
Vectors: Sum
 Sum of vectors (is commutative)

 Can be visualized as “chaining” the vectors.

Vectors: Dot Product
 Inner product of vectors (is a scalar)

 If one of the two vectors, e.g. , has ,

the inner product returns the length of
the projection of along the direction of

 If , the
two vectors are
orthogonal
Vectors: Linear (In)Dependence

 A vector is linearly dependent from

if
 In other words, if can be obtained by
summing up the properly scaled
 If there exist no such that
then is independent from
Vectors: Linear (In)Dependence

 A vector is linearly dependent from

if
 In other words, if can be obtained by
summing up the properly scaled
 If there exist no such that
then is independent from
Matrices
 A matrix is written as a table of values

rows columns

 1st index refers to the row

 2nd index refers to the column
 Note: a d-dimensional vector is equivalent
to a dx1 matrix
Matrices as Collections of
Vectors
 Column vectors
Matrices as Collections of
Vectors
 Row vectors
Important Matrices Operations
 Multiplication by a scalar
 Sum (commutative, associative)
 Multiplication by a vector
 Product (not commutative)
 Inversion (square, full rank)
 Transposition
Scalar Multiplication & Sum
 In the scalar multiplication, every element
of the vector or matrix is multiplied with the
scalar
 The sum of two vectors is a vector
consisting of the pair-wise sums of the
individual entries
 The sum of two matrices is a matrix
consisting of the pair-wise sums of the
individual entries
Matrix Vector Product
 The ith component of is the dot product
.
 The vector is linearly dependent from
the column vectors with coefficients

row vectors column vectors

Matrix Vector Product
 If the column vectors of represent a
reference system, the product
computes the global transformation of the
vector according to

column vectors
Matrix Matrix Product
 Can be defined through
 the dot product of row and column vectors
 the linear combination of the columns of A
scaled by the coefficients of the columns of B

column vectors
Matrix Matrix Product
 If we consider the second interpretation,
we see that the columns of C are the
“transformations” of the columns of B
through A
 All the interpretations made for the matrix
vector product hold

column vectors
Rank
 Maximum number of linearly independent rows (columns)
 Dimension of the image of the transformation

 When is we have
 and the equality holds iff is the null matrix


 Computation of the rank is done by

 Gaussian elimination on the matrix
 Counting the number of non-zero rows
Inverse

 If A is a square matrix of full rank, then

there is a unique matrix B=A-1 such that
AB=I holds
 The ith row of A is and the jth column of A-1
are:
 orthogonal (if i  j)
 or their dot product is 1 (if i = j)
Matrix Inversion

 The ith column of A-1 can be found by

solving the following linear system:

This is the ith column

of the identity matrix
Determinant (det)
 Only defined for square matrices
 The inverse of exists if and only if
 For matrices:
Let and , then

 For matrices the Sarrus rule holds:

Determinant
 For general matrices?
Let be the submatrix obtained from
by deleting the i-th row and the j-th column

Rewrite determinant for matrices:

Determinant
 For general matrices?

Let be the (i,j)-cofactor, then

This is called the cofactor expansion across the first row

Determinant
 Problem: Take a 25 x 25 matrix (which is considered small).
The cofactor expansion method requires n! multiplications.
For n = 25, this is 1.5 x 10^25 multiplications for which a
today supercomputer would take 500,000 years.

 There are much faster methods, namely using Gauss

elimination to bring the matrix into triangular form.

Because for triangular matrices the determinant is the

product of diagonal elements
Determinant: Properties
 Row operations ( is still a square matrix)
 If results from by interchanging two rows,
then
 If results from by multiplying one row with a number ,
then
 If results from by adding a multiple of one row to another
row, then

 Transpose:

 Multiplication:

 Does not apply to addition!

Determinant: Applications
 Compute Eigenvalues:
Solve the characteristic polynomial

 Area and Volume:

( is i-th row)
Orthogonal Matrix
 A matrix is orthogonal iff its column (row)
vectors represent an orthonormal basis

 As linear transformation, it is norm preserving

 Some properties:
 The transpose is the inverse
 Determinant has unity norm (±1)
Rotation Matrix
 A Rotation matrix is an orthonormal matrix with det =+1

 2D Rotations

 3D Rotations along the main axes

 IMPORTANT: Rotations are not commutative

Matrices to Represent Affine
Transformations
 A general and easy way to describe a 3D
transformation is via matrices
Translation Vector

Rotation Matrix

 Takes naturally into account the non-

commutativity of the transformations
 Homogeneous coordinates
Combining Transformations
 A simple interpretation: chaining of transformations
(represented as homogeneous matrices)
 Matrix A represents the pose of a robot in the space
 Matrix B represents the position of a sensor on the robot
 The sensor perceives an object at a given location p, in
its own frame [the sensor has no clue on where it is in the
world]
 Where is the object in the global frame?

p
Combining Transformations
 A simple interpretation: chaining of transformations
(represented as homogeneous matrices)
 Matrix A represents the pose of a robot in the space
 Matrix B represents the position of a sensor on the robot
 The sensor perceives an object at a given location p, in
its own frame [the sensor has no clue on where it is in the
world]
 Where is the object in the global frame?
Bp gives the pose of the
object wrt the robot

B
Combining Transformations
 A simple interpretation: chaining of transformations
(represented as homogeneous matrices)
 Matrix A represents the pose of a robot in the space
 Matrix B represents the position of a sensor on the robot
 The sensor perceives an object at a given location p, in
its own frame [the sensor has no clue on where it is in the
world]
 Where is the object in the global frame?

Bp gives the pose of the

object wrt the robot

ABp gives the pose of the

object wrt the world

A
Positive Definite Matrix
 The analogous of positive number

 Definition

 Example


Positive Definite Matrix
 Properties
 Invertible, with positive definite inverse
 All real eigenvalues > 0
 Trace is > 0
 Cholesky decomposition
Linear Systems (1)

Interpretations:
 A set of linear equations
 A way to find the coordinates x in the
reference system of A such that b is the
result of the transformation of Ax
 Solvable by Gaussian elimination
Gaussian Elimination
A method to solve systems of linear equations.

Example for three variables:

We want to transform this to

Gaussian Elimination
Written as an extended coefficient matrix:

To reach this form we only need two

elementary row operations:
 Add to one row a scalar multiple of another.
 Swap the positions of two rows.

Another commonly used term for Gaussian

Elimination is row reduction.
Linear Systems (2)

Notes:
 Many efficient solvers exist, e.g., conjugate
gradients, sparse Cholesky decomposition
 One can obtain a reduced system (A’, b’) by
considering the matrix (A, b) and suppressing all
the rows which are linearly dependent
 Let A'x=b' the reduced system with A':n'xm and
b':n'x1 and rank A' = min(n',m) rows columns
 The system might be either over-constrained
(n’>m) or under-constrained (n’<m)
 Over-Constrained Systems
 “More (ind.) equations than variables”
 An over-constrained system does not
admit an exact solution
 However, if rank A’ = cols(A) one often
computes a minimum norm solution

Note: rank = Maximum number of linearly independent rows/columns

Under-Constrained Systems
 “More variables than (ind.) equations”
 The system is under-constrained if the
number of linearly independent rows of A’
is smaller than the dimension of b’
 An under-constrained system admits infinite
solutions
 The degree of these infinite solutions is
cols(A’) - rows(A’)
Jacobian Matrix
 It is a non-square matrix in general

 Given a vector-valued function

 Then, the Jacobian matrix is defined as

Jacobian Matrix
 It is the orientation of the tangent
plane to the vector-valued function at a
given point

 Generalizes the gradient of a scalar

valued function
Further Reading
 A “quick and dirty” guide to matrices is the
Matrix Cookbook.
Just google for ‘matrix cook book’ to find
the pdf version.
Introduction to
Mobile Robotics

Robot Control Paradigms

Wolfram Burgard

1
Classical / Hierarchical Paradigm

Sense Plan Act

 70s
 Focus on automated reasoning and knowledge
representation
 STRIPS (Stanford Research Institute Problem
Solver): Perfect world model, closed world
assumption
 Find boxes and move them to the designated
position
2
Stanford CART 1973

Stanford AI Laboratory / CMU (Moravec)

3
Classical Paradigm
Stanford Cart

1. Take nine images of the environment, identify

interesting points in one image, and use other
images to obtain depth estimates.
2. Integrate information into global world model.
3. Correlate images with previous image set to
estimate robot motion.
4. On basis of desired motion, estimated motion,
and current estimate of environment, determine
direction in which to move.
5. Execute the motion.
4
Classical Paradigm as
Horizontal/Functional Decomposition

Motor Control
Perception

Execute
Model

Plan
Sense Plan Act

Sensing Action

Environment

5
Reactive / Behavior-based Paradigm

Sense Act

 No models: “The world is its own, best

model”
 Early successes, but also limitations
 Investigate biological systems

6
Reactive Paradigm as
Vertical Decomposition

Explore

Wander

Avoid obstacles

Sensing Action
Environment
7
Characteristics of Reactive Paradigm
 Situated agent, robot is integral part of its
environment.
 No memory, controlled by what is
happening in the world.
 Tight coupling between perception and
action via behaviors.
 Only local, behavior-specific sensing is
permitted (ego-centric representation).

8
Behaviors
 … are a direct mapping of sensory
inputs to a pattern of motor actions
that are then used to achieve a task.
 … serve as the basic building blocks
for robot actions, and the overall
behavior of the robot is emergent.
 … support good software design
principles due to modularity.

9
Subsumption Architecture
 Introduced by Rodney Brooks ‘86.
 Behaviors are networks of sensing and
acting modules (augmented finite
state machines AFSM).
 Modules are grouped into layers of
competence.
 Layers can subsume lower layers.
 No internal state!

10
Level 0: Avoid

Polar plot of sonars

Feel force Run away Turn

force heading

polar heading
Sonar
plot encoders

Collide Forward
halt

11
Level 1: Wander

heading

Wander Avoid
force modified
heading

Feel force Run away s Turn

force
heading
polar heading
Sonar
plot encoders

Collide Forward
halt

12
 Level 2: Follow Corridor
Stay in distance, direction traveled
Integrate
Look middle heading
corridor to middle

s
Wander Avoid
force modified
heading

Feel force
force
Run away s Turn
heading
polar heading
Sonar
plot encoders

Collide Forward
halt

13
Potential Field Methods
 Treat robot as particle acting under the
influence of a potential field
 Robot travels along the derivative of the
potential
 Field depends on obstacles, desired travel
directions and targets
 Resulting field (vector) is given by the
summation of primitive fields
 Strength of field may change with distance
to obstacle/target
14
Primitive Potential Fields

Uniform Perpendicular

Attractive Repulsive Tangential

15
Corridor Following with
Potential Fields
 Level 0 (collision avoidance)
is done by the repulsive fields of detected
obstacles.
 Level 1 (wander)
adds a uniform field.
 Level 2 (corridor following)
replaces the wander field by three fields
(two perpendicular, one uniform).

16
Characteristics of Potential Fields

 Suffer from local minima

Goal

 Backtracking
 Random motion to escape local minimum
 Procedural planner s.a. wall following
 Increase potential of visited regions
 Avoid local minima by harmonic functions

17
Characteristics of Potential Fields

 No preference among layers

 Easy to visualize
 Easy to combine different fields
 High update rates necessary
 Parameter tuning important

18
Reactive Paradigm
 Representations?
 Good software engineering principles?
 Easy to program?
 Robustness?
 Scalability?

19
Hybrid Deliberative/Reactive
Paradigm

Plan

Sense Act

 Combines advantages of previous paradigms

 World model used for planning
 Closed loop, reactive control

20
Discussion
 Imagine you want your robot to perform
navigation tasks, which approach would you
choose?
 What are the benefits and drawbacks of the
behavior based paradigm?
 What are drawbacks of the subsumption
architecture?

21
Introduction to
Mobile Robotics

Wheeled Locomotion
Wolfram Burgard

1
Locomotion of Wheeled Robots
Locomotion (Oxford Dict.): Power of motion from
place to place

 Differential drive (AmigoBot, Pioneer 2-DX)

 Car drive (Ackerman steering)
 Synchronous drive (B21)
 XR4000
 Mecanum wheels

roll
x
y y

z m otion

we also allow wheels to rotate around the z axis

2
Instantaneous Center of Curvature

ICC

 For rolling motion to occur, each wheel has to

move along its y-axis

3
 Differential Drive

y ICC  [ x  R sin q , y  R cos q ]

w
ICC
vl

R q
(x,y) x

vr
l/2
Differential Drive: Forward Kinematics

 x '   cos( wd t )  sin( wd t ) 0  x  ICC x   ICC x 

ICC        
y '  sin( wd t ) cos( wd t ) 0 y  ICC y  ICC y
       
q '   0 0 1   q   wd t 

t
R x (t )   v (t ' ) cos[ q ( t ' )] dt '
P(t+dt) 0
t

y (t )   v (t ' ) sin[ q ( t ' )] dt '

0
t
P(t)
q ( t )   w ( t ' ) dt '
0

5
Differential Drive: Forward Kinematics

 x '   cos( wd t )  sin( wd t ) 0  x  ICC x   ICC x 

ICC        
y '  sin( wd t ) cos( wd t ) 0 y  ICC y  ICC y
       
q '   0 0 1   q   wd t 

R 1
t

x (t ) 
2
 [ v ( t ' )  v ( t ' )]
r l cos[ q ( t ' )] dt '
P(t+dt) 0
t
1
y (t ) 
2
 [ v ( t ' )  v ( t ' )]
r l sin[ q ( t ' )] dt '
0
t
1
P(t)
q (t ) 
l
 [ v ( t ' )  v ( t ' )] dt '
r l

6
 Ackermann Drive
ICC  [ x  R sin q , y  R cos q ]
d
R 
y tan 
w
ICC   w ( R  l / 2)  vr
vl
w ( R  l / 2)  vl
l (vl  vr )
d R 
R q 2 (vr  vl)
(x,y) x
vr  vl
w 
vr l
l/2
7
Synchronous Drive

y t

x (t )   v (t ' ) cos[ q ( t ' )] dt '

0
t

v(t)
y (t )   v (t ' ) sin[ q ( t ' )] dt '
0
q t
x
q ( t )   w ( t ' ) dt '
w(t ) 0

8
 t
XR4000 Drive x (t )   v (t ' ) cos[ q ( t ' )] dt '
0
t
y
y (t )   v (t ' ) sin[ q ( t ' )] dt '
0
t

q ( t )   w ( t ' ) dt '
0
vi(t)
q
wi(t) x

ICC

9
Mecanum Wheels
v y  ( v 0  v1  v 2  v 3 ) / 4
v x  ( v 0  v1  v 2  v 3 ) / 4
v q  ( v 0  v1  v 2  v 3 ) / 4
v error  ( v 0  v1  v 2  v 3 ) / 4

11
The Kuka OmniRob Platform
Example: KUKA youBot

15
Tracked Vehicles

17
Other Robots: OmniTread

[courtesy by Johann Borenstein]

18
Non-Holonomic Constraints
 Non-holonomic constraints limit the possible
incremental movements within the
configuration space of the robot.
 Robots with differential drive or synchro-
drive move on a circular trajectory and
cannot move sideways.
 Mecanum-wheeled robots can move
sideways (they have no non-holonomic
constraints).

20
Holonomic vs. Non-Holonomic
 Non-holonomic constraints reduce the
control space with respect to the current
configuration
 E.g., moving sideways is impossible.

 Holonomic constraints reduce the

configuration space.
 E.g., a train on tracks (not all positions and
orientations are possible)

21
Drives with Non-Holonomic
Constraints
 Synchro-drive
 Differential drive
 Ackermann drive

22
Drives without Non-Holonomic
Constraints
 Mecanum wheels

23
Dead Reckoning and Odometry
 Estimating the motion based on the issued
controls/wheel encoder readings
 Integrated over time

24
Summary
 Introduced different types of drives for
wheeled robots
 Math to describe the motion of the basic
drives given the speed of the wheels
 Non-holonomic constraints
 Odometry and dead reckoning

25
Introduction to
Mobile Robotics

Proximity Sensors
Wolfram Burgard

1
Sensors of Wheeled Robots
Perception of the environment

Active: Time of flight

 Ultrasound
 Laser range finder
 Infrared Phase shift

Intensity-based
Passive:
 Cameras
 Tactiles

2
Tactile Sensors
Measure contact with objects

Touch sensor

Spring
Bumper sensor
Contact

3
Ultrasound Sensors
 Emit an ultrasound signal
 Wait until they receive
the echo
 Time of flight sensor

Polaroyd 6500
4
Time of Flight Sensors

emitter

object

d  vt /2

v: speed of the signal

t: time elapsed between broadcast of signal and
reception of the echo.

5
Properties of Ultrasounds
 Signal profile [Polaroid]

6
Sources of Error
 Opening angle
 Crosstalk
 Specular reflection

7
Typical Ultrasound Scan

8
Parallel Operation
 Given a 15 degrees opening angle, 24 sensors are
needed to cover the whole 360 degrees area
around the robot.
 Let the maximum range we are interested in be
10m.
 The time of flight then is 2*10m divided by the
speed of sound (330m/sec) which leads to 0.06sec
 A complete scan thus requires 24*0.06=1.45sec
 To allow frequent updates (necessary for high
speed) the sensors have to be fired in parallel.
 This increases the risk of crosstalk

9
Laser Range Scanner

10
Properties
 High precision
 Wide field of view
 Some laser scanners are security approved
for emergency stops (collision detection)

11
Computing the End Points
 Laser data comes as an array or range
readings, e.g. [1; 1.2; 1.5; 0.1; 81.9; …]
 Assume an field of view of 180 deg
 First beams starts at -½ of the fov
 Maximum range: ~80 m (SICK LMS)

12
Computing the End Points
 Laser data comes as an array or range
readings, e.g. [1; 1.2; 1.5; 0.1; 91.9; …]
 Assume an field of view of 180 deg
 First beams starts at -½ of the fov

Typical problems to be solved:

 Where are the end points relative to the
sensor location?
 Where are the end points in an external
coordinate system?

13
Robots Equipped with Laser
Scanners

14
Typical Scans

15
Another Range Sensor (Kinect)

16
Wolfram in 3D

17
Introduction to
Mobile Robotics

Probabilistic Robotics
Wolfram Burgard

1
Probabilistic Robotics
Key idea:

Explicit representation of uncertainty

(using the calculus of probability theory)

 Perception = state estimation

 Action = utility optimization

2
Axioms of Probability Theory
P(A) denotes probability that proposition A is true.

3
A Closer Look at Axiom 3

True
A A∧ B B

4
Using the Axioms

5
Discrete Random Variables

 X denotes a random variable

 X can take on a countable number of values
in {x1, x2, …, xn}
 P(X=xi) or P(xi) is the probability that the
random variable X takes on value xi
 P(.) is called probability mass function

 E.g.

6
Continuous Random Variables

 X takes on values in the continuum.

 p(X=x) or p(x) is a probability density
function

 E.g. p(x)

7
“Probability Sums up to One”

Discrete case Continuous case

8
Joint and Conditional Probability
 P(X=x and Y=y) = P(x,y)

 If X and Y are independent then

P(x,y) = P(x) P(y)
 P(x | y) is the probability of x given y
P(x | y) = P(x,y) / P(y)
P(x,y) = P(x | y) P(y)
 If X and Y are independent then
P(x | y) = P(x)

9
Law of Total Probability

Discrete case Continuous case

10
Marginalization

Discrete case Continuous case

11
Bayes Formula

12
Normalization

Algorithm:

13
Bayes Rule
with Background Knowledge

14
Conditional Independence

P ( x, y z ) = P ( x | z ) P ( y | z )

 Equivalent to P ( x z ) = P ( x | z , y )

and P( y z ) = P( y | z , x)

 But this does not necessarily mean

P ( x, y ) = P ( x ) P ( y )
(independence/marginal independence)
15
Simple Example of State Estimation

 Suppose a robot obtains measurement z

 What is P(open | z)?

16
Causal vs. Diagnostic Reasoning
 P(open|z) is diagnostic
 P(z|open) is causal
 In some situations, causal knowledge
is easier to obtain count frequencies!
 Bayes rule allows us to use causal
knowledge:
P( z | open) P(open)
P(open | z ) =
P( z )

17
Example
 P(z|open) = 0.6 P(z|¬open) = 0.3
 P(open) = P(¬open) = 0.5

P( z | open) P(open)
P(open | z ) =
P( z | open) p (open) + P( z | ¬open) p (¬open)
0.6 ⋅ 0.5 0 .3
P(open | z ) = = = 0.67
0.6 ⋅ 0.5 + 0.3 ⋅ 0.5 0.3 + 0.15

 z raises the probability that the door is open

18
Combining Evidence
 Suppose our robot obtains another
observation z2

 How can we integrate this new information?

 More generally, how can we estimate

P(x | z1, ..., zn )?

19
Recursive Bayesian Updating

Markov assumption:
zn is independent of z1,...,zn-1 if we know x

20
Example: Second Measurement

 P(z2|open) = 0.25 P(z2|¬open) = 0.3

 P(open|z1)=2/3

• z2 lowers the probability that the door is open

21
Actions
 Often the world is dynamic since
 actions carried out by the robot,
 actions carried out by other agents,
 or just the time passing by
change the world

 How can we incorporate such actions?

23
Typical Actions
 The robot turns its wheels to move
 The robot uses its manipulator to grasp
an object
 Plants grow over time …

 Actions are never carried out with

absolute certainty
 In contrast to measurements, actions
generally increase the uncertainty

24
Modeling Actions
 To incorporate the outcome of an
action u into the current “belief”, we
use the conditional pdf

P(x | u, x’)

 This term specifies the pdf that

executing u changes the state
from x’ to x.

25
Example: Closing the door

26
State Transitions
P(x | u, x’) for u = “close door”:

0.9
0.1 open closed 1
0
If the door is open, the action “close door”
succeeds in 90% of all cases

27
Integrating the Outcome of Actions
Continuous case:

Discrete case:

We will make an independence assumption to

get rid of the u in the second factor in the
sum.
28
Example: The Resulting Belief

29
Bayes Filters: Framework
 Given:
 Stream of observations z and action data u:

 Sensor model P(z | x)

 Action model P(x | u, x’)
 Prior probability of the system state P(x)
 Wanted:
 Estimate of the state X of a dynamical system
 The posterior of the state is also called Belief:

30
Markov Assumption

Underlying Assumptions
 Static world
 Independent noise
 Perfect model, no approximation errors
31
 z = observation
u = action
Bayes Filters x = state

Bayes

Markov

Total prob.

Markov

Markov

32
Bayes
Bel ( xt ) = ηFilter
P( zt | xt ) ∫Algorithm
P( xt | ut , xt −1 )Bel ( xt −1 )dxt −1

1. Algorithm Bayes_filter(Bel(x), d):

2. η=0
3. If d is a perceptual data item z then
4. For all x do
5.
6.
7. For all x do
8.
9. Else if d is an action data item u then
10. For all x do
11.
12. Return Bel’(x)

33
Bayes Filters are Familiar!
Bel ( xt ) = η P ( zt | xt ) ∫ P( xt | ut , xt −1 )Bel ( xt −1 )dxt −1

 Kalman filters
 Particle filters
 Hidden Markov models
 Dynamic Bayesian networks
 Partially Observable Markov Decision
Processes (POMDPs)

34
Probabilistic Localization
Probabilistic Localization
Summary
 Bayes rule allows us to compute
probabilities that are hard to assess
otherwise.
 Under the Markov assumption,
recursive Bayesian updating can be
used to efficiently combine evidence.
 Bayes filters are a probabilistic tool
for estimating the state of dynamic
systems.
37
Introduction to
Mobile Robotics

Probabilistic Motion Models

Wolfram Burgard

1
Robot Motion
 Robot motion is inherently uncertain.
 How can we model this uncertainty?

2
Dynamic Bayesian Network for
Controls, States, and Sensations

3
Probabilistic Motion Models
 To implement the Bayes Filter, we need the
transition model .

 The term specifies a posterior

probability, that action ut carries the robot
from xt-1 to xt.

 In this section we will discuss, how

can be modeled based on the
motion equations and the uncertain
outcome of the movements.

4
Coordinate Systems
 The configuration of a typical wheeled robot in 3D
can be described by six parameters.
 This are the three-dimensional Cartesian
coordinates plus the three Euler angles for roll,
pitch, and yaw.
 For simplicity, throughout this section we consider
robots operating on a planar surface.

 The state space of such

systems is three-
dimensional (x,y,).

5
Typical Motion Models
 In practice, one often finds two types of
motion models:
 Odometry-based
 Velocity-based (dead reckoning)
 Odometry-based models are used when
systems are equipped with wheel encoders.
 Velocity-based models have to be applied
when no wheel encoders are given.
 They calculate the new pose based on the
velocities and the time elapsed.
6
Example Wheel Encoders
These modules provide
+5V output when they
"see" white, and a 0V
output when they "see"
black.

These disks are

manufactured out of high
quality laminated color
plastic to offer a very crisp
black to white transition.
This enables a wheel
encoder sensor to easily
see the transitions.

Source: http://www.active-robots.com/ 7
Dead Reckoning
 Derived from “deduced reckoning.”
 Mathematical procedure for determining the
present location of a vehicle.
 Achieved by calculating the current pose of
the vehicle based on its velocities and the
time elapsed.
 Historically used to log the position of ships.

[Image source:
Wikipedia, LoKiLeCh] 8
Reasons for Motion Errors of
Wheeled Robots

ideal case different wheel

diameters

bump
carpet
and many more …
9
Odometry Model
• Robot moves from x , y ,  to x ' , y ' ,  ' .
• Odometry information u   rot 1 ,  rot 2 ,  trans .

 trans  ( x ' x )  ( y ' y )

2 2

 rot 1  atan2 ( y ' y , x ' x )  

 rot 2   '    rot 1  rot 2

x ' , y ' , '

x , y ,  trans
 rot 1

10
The atan2 Function
 Extends the inverse tangent and correctly
copes with the signs of x and y.

11
Noise Model for Odometry
 The measured motion is given by the true
motion corrupted with noise.

ˆrot 1   rot 1    1 | rot 1 |   2 | trans |

ˆtrans   trans    3 | trans |   4 (| rot 1 |  | rot 2 |)

ˆrot 2   rot 2    1 | rot 2 |   2 | trans |

12
Typical Distributions for
Probabilistic Motion Models
Normal distribution Triangular distribution

2  0 if | x | 6 2


1 x
1 
 (x)  2
 2 (x)  
2

6  | x |
2
2 e
2  
2

 6
2

13
Calculating the Probability
Density (zero-centered)
 For a normal distribution query point

1. Algorithm prob_normal_distribution(a,b):

std. deviation
2. return

 For a triangular distribution

1. Algorithm prob_triangular_distribution(a,b):

2. return

14
Calculating the Posterior
Given x, x’, and Odometry
hypotheses odometry
1. Algorithm motion_model_odometry(x, x’,u)
2.  trans  ( x ' x )  ( y ' y )
2 2

3.  rot 1  atan2 ( y ' y , x ' x )   odometry params (u)

4.  rot 2   '    rot 1
5. ˆtrans  ( x ' x )  ( y ' y )
2 2

6. ˆrot 1  atan2 ( y ' y , x ' x )   values of interest (x,x’)

7. ˆrot 2   '   ˆrot 1
8. p 1  prob ( rot 1  ˆrot 1 ,  1 |  rot 1 |   2  trans )
9. p 2  prob ( trans  ˆtrans ,  3 trans   4 (|  rot1 |  |  rot2 |))
10. p 3  prob ( rot 2  ˆrot 2 ,  1 |  rot 2 |   2  trans )
11. return p1 · p2 · p3

15
Application
 Repeated application of the motion model
for short movements.
 Typical banana-shaped distributions
obtained for the 2d-projection of the 3d
posterior.

x x
u
u
16
Sample-Based Density Representation

17
Sample-Based Density Representation

18
How to Sample from a Normal
Distribution?
 Sampling from a normal distribution
1. Algorithm sample_normal_distribution(b):

2. return

19
Normally Distributed Samples

106 samples
20
How to Sample from Normal or
Triangular Distributions?
 Sampling from a normal distribution
1. Algorithm sample_normal_distribution(b):

2. return

 Sampling from a triangular distribution

1. Algorithm sample_triangular_distribution(b):

2. return

21
For Triangular Distribution

103 samples 104 samples

105 samples 106 samples 22

How to Obtain Samples from
Arbitrary Functions?

23
Rejection Sampling
 Sampling from arbitrary distributions
 Sample x from a uniform distribution from [-b,b]
 Sample y from [0, max f]
 if f(x) > y keep the sample x
otherwise reject it

f(x’)
y y’
OK
f(x)
x x’

24
Rejection Sampling

Sampling from arbitrary distributions

1. Algorithm sample_distribution(f,b):
2. repeat
3.
4.
5. until ( )
6. return

25
Example
 Sampling from

26
Sample Odometry Motion Model
1. Algorithm sample_motion_model(u, x):
u   rot 1 ,  rot 2 ,  trans , x  x , y , 
1. ˆrot 1   rot 1  sample(  1 |  rot 1 |   2  trans )
2. ˆtrans   trans  sample(  3  trans   4 (|  rot 1 |  |  rot 2 |))
3. ˆrot 2   rot 2  sample(  1 |  rot 2 |   2  trans )

4. x '  x  ˆtrans cos(   ˆrot 1 )

5. y '  y  ˆtrans sin(   ˆrot 1 ) sample_normal_distribution
6.  '    ˆrot 1  ˆrot 2

7. Return x ' , y ' , '

27
Examples (Odometry-Based)

28
Sampling from Our Motion
Model

Start

29
Velocity-Based Model

-90

30
Noise Model for the Velocity-
Based Model
 The measured motion is given by the true
motion corrupted with noise.

vˆ  v    1 |v |   2 | |

ˆ      3 | v |   4 | |

 Discussion: What is the disadvantage of this

noise model?
31
Noise Model for the Velocity-
Based Model
 The ( vˆ , ˆ )-circle constrains the final
orientation (2D manifold in a 3D space)
 Better approach:

vˆ  v    1 |v |   2 | |

ˆ      3 | v |   4 | |

ˆ    5 | v |   6 | |

Term to account for the final rotation

32
Motion Including 3rd Parameter

Term to account for the final rotation

33
Equation for the Velocity Model

Center of circle:

some constant (distance to ICC)

(center of circle is orthogonal
to the initial heading)

34
Equation for the Velocity Model

some constant

Center of circle:

some constant (the center of the circle lies

on a ray half way between x and x’ and is
orthogonal to the line between x and x’)
35
Equation for the Velocity Model

some constant

Center of circle:

Allows us to solve the equations to:

36
Equation for the Velocity Model

and

37
Equation for the Velocity Model
 The parameters of the circle:

 allow for computing the velocities as

38
Posterior Probability for
Velocity Model

39
Sampling from Velocity Model

40
Examples (Velocity-Based)

41
Map-Consistent Motion Model

p ( x '| u , x )  p ( x '| u , x, m )

Approximation: p ( x '| u , x, m )   p ( x '| m ) p ( x '| u , x )

42
Summary
 We discussed motion models for odometry-based
and velocity-based systems
 We discussed ways to calculate the posterior
probability p(x’| x, u).
 We also described how to sample from p(x’| x, u).
 Typically the calculations are done in fixed time
intervals t.
 In practice, the parameters of the models have to
be learned.
 We also discussed how to improve this motion
model to take the map into account.

43
Introduction to
Mobile Robotics

Probabilistic Sensor Models

Marina Kollmitz, Wolfram Burgard

1
Bayes Filters are Familiar!
Bel ( x t )   P ( z t | x t )  P ( x t | u t , x t 1 ) Bel ( x t 1 ) dx t 1

 Kalman filters
 Particle filters
 Hidden Markov models
 Dynamic Bayesian networks
 Partially Observable Markov Decision
Processes (POMDPs)

2
Sensors for Mobile Robots
 Contact sensors: Bumpers
 Proprioceptive sensors
 Accelerometers (spring-mounted masses)
 Gyroscopes (spinning mass, laser light)
 Compasses, inclinometers (earth magnetic field, gravity)
 Proximity sensors
 Sonar (time of flight)
 Radar (phase and frequency)
 Laser range-finders (triangulation, tof, phase)
 Infrared (intensity)
 Visual sensors: Cameras
 Satellite-based sensors: GPS

3
Proximity Sensors

 The central task is to determine P(z|x), i.e., the

probability of a measurement z given that the robot
is at position x.
 Question: Where do the probabilities come from?
 Approach: Let’s try to explain a measurement.

4
Beam-based Sensor Model
 Scan z consists of K measurements.

z  { z 1 , z 2 ,..., z K }

 Individual measurements are independent

given the robot position.

P ( z | x, m )   P ( zk | x, m )
k 1

5
Beam-based Sensor Model

P ( z | x, m )   P ( zk | x, m )
k 1

6
 Typical Measurement Errors of
an Range Measurements

1. Beams reflected by
obstacles
2. Beams reflected by
persons / caused
by crosstalk
3. Random
measurements
4. Maximum range
measurements

7
Proximity Measurement
 Measurement can be caused by …
 a known obstacle.
 cross-talk.
 an unexpected obstacle (people, furniture, …).
 missing all obstacles (total reflection, glass, …).
 Noise is due to uncertainty …
 in measuring distance to known obstacle.
 in position of known obstacles.
 in position of additional obstacles.
 whether obstacle is missed.

8
Beam-based Proximity Model
Measurement noise Unexpected obstacles

0 zexp zmax 0 zexp zmax

  e   z z  z exp 
2
1 ( z  z exp )
1 
Phit ( z | x , m )   e 2 b Punexp ( z | x, m )   
2 b  0 otherwise 

9
Beam-based Proximity Model
Random measurement Max range

0 zexp zmax 0 zexp zmax

1 1 𝑧 = 𝑧max
Prand ( z | x , m )   𝑃max 𝑧 𝑥, 𝑚 =
z max 0 otherwise

10
Resulting Mixture Density

T
  hit   Phit ( z | x , m ) 
   
  unexp   Punexp ( z | x , m ) 
P ( z | x, m )      P ( z | x, m ) 

 max   max 
   P ( z | x, m ) 
 rand   rand 

How can we determine the model parameters?

11
Raw Sensor Data
Measured distances for expected distance of 300 cm.

Sonar Laser

12
Approximation
 Maximize log likelihood of the data

P ( z | z exp )

 Search space of n-1 parameters.

 Hill climbing
 Gradient descent
 Genetic algorithms
 …
 Deterministically compute the n-th
parameter to satisfy normalization
constraint.
13
Approximation Results

Laser

Sonar

300cm 400cm
Approximation Results

Laser

Sonar

15
Example

z P(z|x,m)

16
Influence of Angle to Obstacle
"sonar-0"

0.25
0.2
0.15
0.1
0.05
0
70
60
50
0 40
10 20 30
30 40 20
50 10
60 70 0

19
Influence of Angle to Obstacle
"sonar-1"

0.3
0.25
0.2
0.15
0.1
0.05
0
70
60
50
0 40
10 20 30
30 40 20
50 10
60 70 0

20
Influence of Angle to Obstacle
"sonar-2"

0.3
0.25
0.2
0.15
0.1
0.05
0
70
60
50
0 40
10 20 30
30 40 20
50 10
60 70 0

21
Influence of Angle to Obstacle
"sonar-3"

0.25
0.2
0.15
0.1
0.05
0
70
60
50
0 40
10 20 30
30 40 20
50 10
60 70 0

22
Summary Beam-based Model
 Assumes independence between beams.
 Justification?
 Overconfident!
 Models physical causes for measurements.
 Mixture of densities for these causes.
 Assumes independence between causes. Problem?
 Implementation
 Learn parameters based on real data.
 Different models should be learned for different angles at
which the sensor beam hits the obstacle.
 Determine expected distances by ray-tracing.
 Expected distances can be pre-processed.

23
Scan-based Model
 Beam-based model is …
 not smooth for small obstacles and at edges.
 not very efficient.

 Idea: Instead of following along the beam,

just check the end point.

24
Scan-based Model
 Probability is a mixture of …
 a Gaussian distribution with mean at distance to
closest obstacle,
 a uniform distribution for random
measurements, and
 a small uniform distribution for max range
measurements.
 Again, independence between different
components is assumed.

25
Example

Likelihood field

Map m

P(z|x,m)

26
San Jose Tech Museum

Occupancy grid map Likelihood field

27
Scan Matching
 Extract likelihood field from scan and use it
to match different scan.

28
Properties of Scan-based Model
 Highly efficient, uses 2D tables only.
 Distance grid is smooth w.r.t. to small
changes in robot position.
 Allows gradient descent, scan matching.
 Ignores physical properties of beams.

30
Additional Models of Proximity
Sensors
 Map matching (sonar, laser): generate
small, local maps from sensor data and
match local maps against global model.
 Scan matching (laser): map is represented
by scan endpoints, match scan into this
map.
 Features (sonar, laser, vision): Extract
features such as doors, hallways from
sensor data.

31
Landmarks
 Active beacons (e.g., radio, GPS)
 Passive (e.g., visual, retro-reflective)
 Standard approach is triangulation

 Sensor provides
 distance, or
 bearing, or
 distance and bearing.

32
Vision-Based Localization

33
Distance and Bearing

34
Probabilistic Model
1. Algorithm landmark_detection_model(z,x,m):
z  i, d ,  , x  x, y ,

2. dˆ  ( m x (i )  x )  ( m y (i )  y )
2 2

3. ˆ  atan2( m y ( i )  y , m x ( i )  x )  

4. p det  prob( dˆ  d ,  d )  prob( ˆ   ,   )

5. Return p det

35
Distributions

36
Distances Only x  (a  d1  d 2 ) / 2a
2 2 2

No Uncertainty y   (d  x )
1
2 2

X’

a
P1 P2
d1 d2

x P1=(0,0)
P2=(a,0)

37
 Bearings Only
P3
No Uncertainty
D z3
2 b
P2  P2 D
z2 3 
D1
D1
z2
z1
P1  P1
z1

D 1  z 1  z 2  2 z 1 z 2 cos(  )
2 2 2

Law of cosine
D 2  z 2  z 3  2 z 1 z 2 cos( b )
2 2 2

D 1  z 1  z 2  2 z 1 z 2 cos 
2 2 2

D 3  z 1  z 3  2 z 1 z 2 cos(   b )
2 2 2

38
Bearings Only With Uncertainty

P3
P2

P2

P1

P1

Most approaches attempt to find estimation mean.

39
Summary of Sensor Models
 Explicitly modeling uncertainty in sensing is key to
robustness.
 In many cases, good models can be found by the
following approach:
1. Determine parametric model of noise free measurement.
2. Analyze sources of noise.
3. Add adequate noise to parameters (eventually mix in
densities for noise).
4. Learn (and verify) parameters by fitting model to data.
5. Likelihood of measurement is given by “probabilistically
comparing” the actual with the expected measurement.
 This holds for motion models as well.
 It is extremely important to be aware of the
underlying assumptions!
40
Introduction to
Mobile Robotics

Bayes Filter – Discrete Filters

Wolfram Burgard

1
Probabilistic Localization

2
Piecewise
Constant

3
Discrete Bayes Filter Algorithm
1. Algorithm Discrete_Bayes_filter( Bel(x),d ):
2. h=0
3. If d is a perceptual data item z then
4. For all x do
5. Bel ' ( x ) = P ( z | x ) Bel ( x )
6. h = h  Bel ' ( x )
7. For all x do
1
8. Bel ' ( x ) = h Bel ' ( x )
9. Else if d is an action data item u then
10. For all x do
11. Bel ' ( x ) =  P ( x | u , x ' ) Bel ( x ' )
12. Return Bel’(x)
x'

4
Piecewise Constant
Representation
Bel ( x t = x , y ,   )

5
Implementation (1)
 To update the belief upon sensory input and to
carry out the normalization one has to iterate
over all cells of the grid.
 Especially when the belief is peaked (which is
generally the case during position tracking), one
wants to avoid updating irrelevant aspects of the
state space.
 One approach is not to update entire sub-spaces
of the state space.
 This, however, requires to monitor whether the
robot is de-localized or not.
 To achieve this, one can consider the likelihood
of the observations given the active components
of the state space. 6
Implementation (2)
 To efficiently update the belief upon robot motions, one typically
assumes a bounded Gaussian model for the motion uncertainty.
 This reduces the update cost from O(n2) to O(n), where n is the
number of states.
 The update can also be realized by shifting the data in the grid
according to the measured motion.
 In a second step, the grid is then convolved using a separable
Gaussian Kernel.
 Two-dimensional example:

1/16 1/8 1/16 1/4

1/8 1/4 1/8  1/2 + 1/4 1/2 1/4

1/16 1/8 1/16 1/4

 Fewer arithmetic operations

 Easier to implement
7
Grid-based Localization

8
Sonars and
Occupancy Grid Map

9
Tree-based Representation

Idea: Represent density using a variant of octrees

10
Tree-based Representations

• Efficient in space and time

• Multi-resolution

11
Summary
 Discrete filters are an alternative way for
implementing Bayes Filters
 They are based on histograms for representing
the density.
 They have huge memory and processing
requirements
 Can easily recover from localization errors
 Their accuracy depends on the resolution of
the grid.
 Special approximations need to be made to
make this approach having dynamic memory
and computational requirements.
12
Introduction to
Mobile Robotics
Bayes Filter – Particle Filter
and Monte Carlo Localization
Wolfram Burgard

1
Motivation
 Recall: Discrete filter
 Discretize the continuous state space
 High memory complexity
 Fixed resolution (does not adapt to the belief)

 Particle filters are a way to efficiently represent

non-Gaussian distribution

 Basic principle
 Set of state hypotheses (“particles”)
 Survival-of-the-fittest
2
Sample-based Localization (sonar)
Mathematical Description

 Set of weighted samples

State hypothesis Importance weight

 The samples represent the posterior

4
Function Approximation

 Particle sets can be used to approximate functions

 The more particles fall into an interval, the higher

the probability of that interval

 How to draw samples from a function/distribution?

5
Rejection Sampling

 Let us assume that f(x)<1 for all x

 Sample x from a uniform distribution
 Sample c from [0,1]
 if f(x) > c keep the sample
otherwise reject the sample

f(x’)
c c
OK
f(x)
x x’

6
Importance Sampling Principle

 We can even use a different distribution g to

generate samples from f
 By introducing an importance weight w, we can
account for the “differences between g and f ”
 w=f/g
 f is called target
 g is called proposal
 Pre-condition:
f(x)>0  g(x)>0
 Derivation: See
webpage
7
Importance Sampling with Resampling:
Landmark Detection Example
Distributions
Distributions

Wanted: samples distributed according to

p(x| z1, z2, z3)

10
This is Easy!
We can draw samples from p(x|zl) by adding
noise to the detection parameters.
Importance Sampling
Importance Sampling with
Resampling

Weighted samples After resampling

Particle Filters
Sensor Information: Importance Sampling
Bel ( x)   p( z | x) Bel  ( x)
 p( z | x) Bel  ( x)
w  
  p ( z | x)
Bel ( x)
Robot Motion
Sensor Information: Importance Sampling
Robot Motion
Particle Filter Algorithm

 Sample the next generation for particles using the

proposal distribution

 Compute the importance weights :

weight = target distribution / proposal distribution

 Resampling: “Replace unlikely samples by more

likely ones”

19
Particle Filter Algorithm

20
Particle Filter Algorithm
Resampling
 Given: Set S of weighted samples.

 Wanted : Random sample, where the

probability of drawing xi is given by wi.

 Typically done n times with replacement to

generate new sample set S’.
Resampling
w1 wn w1
wn
w2 w2
Wn-1 Wn-1

w3 w3

 Stochastic universal sampling

 Roulette wheel  Systematic resampling
 Binary search, n log n  Linear time complexity
 Easy to implement, low variance
Resampling Algorithm
1. Algorithm systematic_resampling(S,n):

2. S '  , c1  w1
3. For i  2n Generate cdf
4. ci  ci 1  w i

5. u1 ~ U ]0, n 1 ], i  1 Initialize threshold

6. For j  1n Draw samples …

7. While ( u j  ci ) Skip until next threshold reached
8. i  i 1
9. S '  S ' xi , n 1  Insert
10. u j 1  u j  n 1 Increment threshold

11. Return S’
Also called stochastic universal sampling
Mobile Robot Localization

 Each particle is a potential pose of the robot

 Proposal distribution is the motion model of

the robot (prediction step)

 The observation model is used to compute

the importance weight (correction step)

[For details, see PDF file on the lecture web page]

25
Motion Model Reminder
end pose

start pose

According to the estimated motion

Motion Model Reminder

rotation

translation

rotation

 Decompose the motion into

 Traveled distance
 Start rotation
 End rotation
Motion Model Reminder

 Uncertainty in the translation of the robot:

Gaussian over the traveled distance
 Uncertainty in the rotation of the robot:
Gaussians over start and end rotation
 For each particle, draw a new pose by sampling
from these three individual normal distributions
Motion Model Reminder

Start
Proximity Sensor Model Reminder

Laser sensor Sonar sensor

Mobile Robot Localization Using
Particle Filters (1)
 Each particle is a potential pose of the robot

 The set of weighted particles approximates

the posterior belief about the robot’s pose
(target distribution)

31
Mobile Robot Localization Using
Particle Filters (2)
 Particles are drawn from the motion model
(proposal distribution)
 Particles are weighted according to the
observation model (sensor model)
 Particles are resampled according to the
particle weights

32
Mobile Robot Localization Using
Particle Filters (3)
Why is resampling needed?
 We only have a finite number of particles
 Without resampling: The filter is likely to
loose track of the “good” hypotheses
 Resampling ensures that particles stay in
the meaningful area of the state space

33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
Sample-based Localization (sonar)

51
Initial Distribution

52
After Incorporating Ten
Ultrasound Scans

53
After Incorporating 65 Ultrasound
Scans

54
Estimated Path

55
Using Ceiling Maps for Localization

[Dellaert et al. 99]

Vision-based Localization

P(z|x)
z
h(x)
Under a Light

Measurement z: P(z|x):
Next to a Light

Measurement z: P(z|x):
Elsewhere

Measurement z: P(z|x):
Global Localization Using Vision
Vision-based Localization
Limitations
 The approach described so far is able
 to track the pose of a mobile robot and
 to globally localize the robot

 How can we deal with localization errors

(i.e., the kidnapped robot problem)?

64
Approaches
 Randomly insert a fixed number of
samples with randomly chosen poses
 This corresponds to the assumption that
the robot can be teleported at any point in
time to an arbitrary location
 Alternatively, insert such samples inverse
proportional to the average likelihood of
the observations (the lower this likelihood
the higher the probability that the current
estimate is wrong).

65
Summary – Particle Filters
 Particle filters are an implementation of
recursive Bayesian filtering
 They represent the posterior by a set of
weighted samples
 They can model arbitrary and thus also
non-Gaussian distributions
 Proposal to draw new samples
 Weights are computed to account for the
difference between the proposal and the
target
 Monte Carlo filter, Survival of the fittest,
Condensation, Bootstrap filter
66
Summary – PF Localization

 In the context of localization, the particles

are propagated according to the motion
model.
 They are then weighted according to the
likelihood model (likelihood of the
observations).
 In a re-sampling step, new particles are
drawn with a probability proportional to the
likelihood of the observation.
 This leads to one of the most popular
approaches to mobile robot localization
67
 Bayes Filter Reminder

Bel(xt )   p(zt | xt )  p(x | u , x

t t t1 ) Bel(xt1 ) dxt1

 Prediction

Bel(xt )   p(x | u , x
t t t1 ) Bel(xt1 ) dxt1
 Correction

Bel(xt )   p(zt | xt ) Bel(xt )

 Bayes Filter Reminder

bel(xt )   p(zt | xt )  p(x | u , x

t t t1 ) bel(xt1 ) dxt1

 Prediction

bel(xt )   p(x | u , x
t t t1 ) bel(xt1 ) dxt1
 Correction

bel(xt )   p(zt | xt )bel(xt )

Kalman Filter
 Bayes filter with Gaussians
 Developed in the late 1950's
 Most relevant Bayes filter variant in practice
 Applications range from economics, weather
forecasting, satellite navigation to robotics
and many more.

 The Kalman filter “algorithm” is

a couple of matrix multiplications!

5
Gaussians
p( x) ~ N (  ,  2 ) :

2
1 ( x  )
1 
p( x)  e2 2
2 
Univariate - 

p(x) ~ N( ,) :

1 
1  (x )t 1 (x )
p(x)  1/2
e 2

(2 ) d/2


Multivariate
Properties of Gaussians
 Univariate case

X ~ N (  ,  2 )
  Y ~ N (a  b, a2 2 )
Y  aX  b 

X1 ~ N ( 1 ,  1 )     
2 2 2
1
2 
 p( X ) p( X ) ~ N  2
  1
 , 
1 2  1 2 2 2 
 1   2 1   2 1   2 
2 2 2 2
X 2 ~ N (  2 ,  2 )
Properties of Gaussians
 Multivariate case
X ~ N (  , ) 
  Y ~ N ( A  B, AAT )
Y  AX  B 

X1 ~ N ( 1 , 1 )   2 1 1 
  p( X1 ) p( X 2 ) ~ N 1  2 , 1 

X 2 ~ N (  2 ,  2 )  1   2 1   2 1
1   2 
(where division "–" denotes matrix inversion)

 We stay Gaussian as long as we start with

Gaussians and perform only linear
transformations
Discrete Kalman Filter
Estimates the state x of a discrete-time
controlled process that is governed by the
linear stochastic difference equation

xt  At xt1  Btut   t

with a measurement

zt  Ct xt   t

10
Components of a Kalman Filter

At Matrix (n×n) that describes how the state

evolves from t-1 to t without controls or
noise.
B Matrix (n×l) that describes how the control ut
t
changes the state from t-1 to t.

C Matrix (k×n) that describes how to map the

t state xt to an observation zt.

t Random variables representing the process

and measurement noise that are assumed to
t be independent and normally distributed
with covariance Qt and Rt respectively.
11
Kalman Filter Updates in 1D

How to get the blue one?

Kalman correction step

 t  t  K t (zt  t )  t2
bel(xt )   with K t  2
  t2  (1 K t ) t2  t   obs,t
2

  t  t  K t (zt  Ctt )

bel(xt )    with K t   tCtT (Ct  tCtT  Rt )1
   t  (I  K tCt ) t
14
Kalman Filter Updates in 1D

 t  att1  btut

bel(xt )    2 How to get the

  t  a 
2 2
t t   2
act,t magenta one?
  t  Att1  Btut State prediction step
bel(xt )   

  t  A t t1 t  Qt
 A T
Kalman Filter Updates

prediction

measurement correction
Linear Gaussian Systems: Initialization

Initial belief is normally distributed:

bel ( x0 )  Nx0 ; 0 , 0 
Linear Gaussian Systems: Dynamics

Dynamics are linear functions of the state

and the control plus additive noise:

xt  At xt1  Btut   t
p(xt | ut, xt1 )  N  xt; At xt1  Btut,Qt 

bel(xt )   p(x | u , x
t t t1 ) bel(xt1 ) dxt1
 
~ N  xt; At xt1  Btut,Qt  ~ N  xt1; t1, t1 
Linear Gaussian Systems: Dynamics
bel(xt )   p(x | u , x
t t t1 ) bel(xt1 ) dxt1
 
~ N xt;At xt1  Btut,Qt  ~ N xt1;t1, t1 

  1  
bel(xt )    exp  T 1
 (xt  At xt1  Btut ) Qt (xt  At xt1  Btut ) 
  2  
  1  
exp   (xt1  t1 )  t1 (xt1  t1 ) dxt1
T 1

  2  
  t  Att1  Btut
bel(xt )   

  t  A t t1 t  Qt
 AT
Linear Gaussian Systems: Observations

Observations are a linear function of the state

plus additive noise:

zt  Ct xt   t
p(zt | xt )  N zt;Ct xt,Rt 

bel(xt )   p(zt | xt ) bel(xt )

 
~ N zt;Ct xt,Rt  
~ N xt; t, t 
Linear Gaussian Systems: Observations

bel(xt )   p(zt | xt ) bel(xt )

 
~ N zt;Ct xt,Rt  
~ N xt; t, t 

  1     1  
bel(xt )   exp   (zt  Ct xt ) Rt (zt  Ct xt ) exp  (xt  t ) t (xt  t ) 
T 1 T 1

  2     2  

  t  t  K t (zt  Ctt )

bel(xt )    with K t   tCtT (Ct  tCtT  Rt )1
   t  (I  K tCt ) t
 Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:
3.  t  Att1  Btut
4.  t  Att1 AtT  Qt

  5. Correction:
K t   tCtT (Ct  tCtT  Rt )1
  6.
7. t   t  K t (zt  Ct  t )
8.  t  (I  K tCt ) t
  9. Return t, t
 
 
The Prediction-Correction-Cycle

Prediction

 t  att1  btut

bel(xt )    2
 
 t  at  t   act,t
2 2 2

  t  Att1  Btut

bel(xt )   
  t  At t1 At  Qt
T

 

 

25
The Prediction-Correction-Cycle

 t  t  K t (zt  t ) t2

bel(xt )    2 , Kt  2
   t  (1 K t )t t  obs,t
2 2

     K t (zt  Ctt )
bel(xt )    t t ,K t   tCtT (Ct  tCtT  Rt )1
   t  (I  K tCt ) t

Correction

26
 The Prediction-Correction-Cycle

Prediction

  t  t  K t (zt  t ) t2  t  att1  btut

bel(xt )    2 , Kt  2 bel(xt )    2
   t  (1 K t )t t  obs,t
2 2
 
 t  at  t   act,t
2 2 2

     K t (zt  Ctt )   t  Att1  Btut

bel(xt )    t t ,K t   tCtT (Ct tCtT  Rt )1 bel(xt )   
    t  (I  K tCt ) t   t  At t1 At  Qt
T

 

Correction
 
 

27
Kalman Filter Summary
 Only two parameters describe belief about
the state of the system
 Highly efficient: Polynomial in the
measurement dimensionality k and
state dimensionality n:
O(k2.376 + n2)

 Optimal for linear Gaussian systems!

 However: Most robotics systems are
nonlinear!
 Can only model unimodal beliefs
Bayes Filter Reminder

bel(xt )   p(zt | xt )  p(x | u , x

t t t1 ) bel(xt1 ) dxt1

 Prediction
bel(xt )   p(x | u , x
t t t1 ) bel(xt1 ) dxt1

 Correction

bel ( xt )   p( zt | xt ) bel ( xt )
Discrete Kalman Filter
Estimates the state x of a discrete-time
controlled process

xt  At xt1  Btut   t

with a measurement

zt  Ct xt   t

3
Components of a Kalman Filter
Matrix (nxn) that describes how the state
At evolves from t-1 to t without controls or
noise.

B Matrix (nxl) that describes how the control ut

t
changes the state from t-1 to t.

C Matrix (kxn) that describes how to map the

t state xt to an observation zt.

t Random variables representing the process

and measurement noise that are assumed to
t be independent and normally distributed
with covariance Qt and Rt respectively.
4
Kalman Filter Update Example

prediction

measurement correction
 Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:
3.  t  Att1  Btut
4.  t  At t1 AtT  Qt

5. Correction:
  6. K t   tCtT (Ct  tCtT  Rt )1
  7. t   t  K t (zt  Ct  t )
8.  t  (I  K tCt ) t
  9. Return t, t
 
 
Nonlinear Dynamic Systems
 Most realistic robotic problems involve
nonlinear functions

xt  At xt1  Btut   t zt  Ct xt   t

xt  g(ut , xt1 ) zt  h( xt )
Linearity Assumption Revisited
 Non-Linear Function

Non-Gaussian!
Non-Gaussian Distributions
 The non-linear functions lead to non-
Gaussian distributions
 Kalman filter is not applicable anymore!

What can be done to resolve this?

Non-Gaussian Distributions
 The non-linear functions lead to non-
Gaussian distributions
 Kalman filter is not applicable anymore!

What can be done to resolve this?

Local linearization!
EKF Linearization: First Order
Taylor Expansion
 Prediction:
g(ut , t1 )
g(ut , xt1 )  g(ut , t1 )  ( xt1  t1 )
xt1
g(ut , xt1 )  g(ut , t1 )  Gt ( xt1  t1 )

 Correction:
h( t ) Jacobian matrices
h( xt )  h(  t )  ( xt  t )
xt
h( xt )  h(  t )  H t ( xt   t )
Reminder: Jacobian Matrix
 It is a non-square matrix in general

 Given a vector-valued function

 The Jacobian matrix is defined as

Reminder: Jacobian Matrix
 It is the orientation of the tangent plane to
the vector-valued function at a given point

 Generalizes the gradient of a scalar valued

function
EKF Linearization: First Order
Taylor Expansion
 Prediction:
g(ut , t1 )
g(ut , xt1 )  g(ut , t1 )  ( xt1  t1 )
xt1
g(ut , xt1 )  g(ut , t1 )  Gt ( xt1  t1 )

 Correction:
h( t ) Linear function!
h( xt )  h(  t )  ( xt  t )
xt
h( xt )  h(  t )  H t ( xt   t )
Linearity Assumption Revisited
Non-Linear Function
EKF Linearization (1)
EKF Linearization (2)
EKF Linearization (3)
EKF Algorithm
1. Extended_Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:
3. t  g(ut,  t1 ) t  Att1  Btut
4. t  Gtt1GtT  Qt t  Att1 AtT  Qt

5. Correction:
6. K t  tHtT (H t tH tT  Rt )1 K t  tCtT (Ct tCtT  Rt )1
7. t  t  K t (zt  h(t )) t  t  K t (zt  Ctt )
8. t  (I  K tHt )t t  (I  K tCt )t
9. Return t, t g(ut, t1 )
h(t ) Gt 
Ht 
xt xt1
Example: EKF Localization
 EKF localization with landmarks (point features)
 1. EKF_localization ( t-1, t-1, ut, zt, m):
 
Prediction:  x' x' x' 
 t1, x t1,y t1, 
 
g(ut, t1 ) y' y' y' 
Gt    
t1

t1, x t1,y Jacobian
t1,

of g w.r.t location
  '  '  ' 
 t1, x t1,y t1,  
 
 
 x' x' 
 vt  t 
 
 
g(ut, t1 ) y' y'
Vt    
ut  vt  t  Jacobian of g w.r.t control
 
  '  ' 
 
 vt  t 
 
   | v |  |  | 2
0 
2. Qt    1 t 2 t
Motion noise
2 

 0   3 | vt |  4 |  t |  
3.  t  g(ut ,  t1 ) Predicted mean
  4. t  Gtt1GtT VtQtVtT Predicted covariance (V
maps Q into state space)
1. EKF_localization ( t-1, t-1, ut, zt, m):
Correction:
 
 mx  t,x    my  t,y
2 2
 
ẑt    Predicted measurement mean
 atan 2  my  t, y, mx   t,x   t, 
  (depends on observation type)
 
 rt rt rt 
h(t, m)  t,x t,y t, 
Jacobian of h w.r.t location
Ht   
xt  t t  t 
 t,x t,y t, 
 
  
  2
r 0 
Rt 
2.  0  r2 
 
3. St  HttHt  Rt
T
Innovation covariance

4. K t  tHtT St1 Kalman gain

5. t  t  K t (zt  ẑt ) Updated mean

6. t   I  K tHt  t Updated covariance

EKF Prediction Step Examples
VtQtVtT VtQtVtT

VtQtVtT VtQtVtT
EKF Observation Prediction Step
Rt Rt

Rt
Rt
EKF Correction Step
Rt

Rt
Estimation Sequence (1)
Estimation Sequence (2)
Extended Kalman Filter
Summary
 The EKF is an ad-hoc solution to deal with non-
linearities
 It performs local linearization in each step
 It works well in practice for moderate non-linearities
(example: landmark localization)
 There exist better ways for dealing with non-linearities
such as the unscented Kalman filter, called UKF
 Unlike the KF, the EKF in general is not an optimal
estimator
 It is optimal if the measurement and the motion
model are both linear, in which case the EKF reduces
to the KF.

31
Introduction to
Mobile Robotics
SLAM: Simultaneous
Localization and Mapping
Lukas Luft, Wolfram Burgard
What is SLAM?
 Estimate the pose of a robot and the map of
the environment at the same time
 SLAM is hard, because
 a map is needed for localization and
 a good pose estimate is needed for mapping

 Localization: inferring location given a

map
 Mapping: inferring a map given locations
 SLAM: learning a map and locating the
robot simultaneously
The SLAM Problem
 SLAM has long been regarded as a
chicken-or-egg problem:
→ a map is needed for localization and
→ a pose estimate is needed for mapping

3
SLAM Applications
 SLAM is central to a range of indoor,
outdoor, in-air and underwater applications
for both manned and autonomous vehicles.

Examples:
 At home: vacuum cleaner, lawn mower
 Air: surveillance with unmanned air vehicles
 Underwater: reef monitoring
 Underground: exploration of mines
 Space: terrain mapping for localization
 Every application that requires a map
4
SLAM Applications
Indoors Undersea

Space Underground

5
Map Representations
Examples: Subway map, city map,
landmark-based map

Maps are topological and/or metric

models of the environment
6
Map Representations in Robotics
 Grid maps or scans, 2d, 3d

[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99;
Haehnel, 01; Grisetti et al., 05; …]

 Landmark-based

[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…
7
The SLAM Problem
 SLAM is considered a fundamental
problem for robots to become truly
autonomous
 Large variety of different SLAM
approaches have been developed
 The majority uses probabilistic
concepts
 History of SLAM dates back to the
mid-eighties
8
Feature-Based SLAM
Given:
 The robot’s controls

 Relative observations

Wanted:
 Map of features

 Path of the robot

9
Feature-Based SLAM
 Absolute
robot poses
 Absolute
landmark
positions
 But only
relative
measurements
of landmarks

10
Why is SLAM a Hard Problem?
1. Robot path and map are both unknown

2. Errors in map and pose estimates correlated

11
Why is SLAM a Hard Problem?
 The mapping between observations and
landmarks is unknown
 Picking wrong data associations can have
catastrophic consequences (divergence)

Robot pose
uncertainty

12
SLAM: Simultaneous
Localization And Mapping
 Full SLAM:
p ( x 0 :t , m | z 1:t , u 1:t )

Estimates entire path and map!

 Online SLAM:
p ( x t , m | z 1:t , u 1:t )     p(x 1:t
, m | z 1:t , u 1:t ) dx 1 dx 2 ... dx t 1

Estimates most recent pose and map!

 Integrations (marginalization) typically
done recursively, one at a time
13
Graphical Model of Full SLAM

p ( x1:t  1 , m | z 1:t  1 , u 1:t  1 )

14
 Graphical Model of Online SLAM

p ( x t  1 , m | z 1:t  1 , u 1:t  1 )     p(x 1:t  1

, m | z 1:t  1 , u 1:t  1 ) dx 1 dx 2  dx t
15
Motion and Observation Model

"Motion model"

"Observation model"
16
Remember the KF Algorithm
1. Algorithm Kalman_filter(mt-1, St-1, ut, zt):
2. Prediction:
3.
4.

5. Correction:
6.
7.
8.
9. Return mt, St

17
EKF SLAM: State representation
 Localization
3x1 pose vector
3x3 cov. matrix

 SLAM
Landmarks simply extend the state.
Growing state vector and covariance matrix!

18
EKF SLAM: State representation
 Map with n landmarks: (3+2n)-dimensional
Gaussian

 Can handle hundreds of dimensions

19
EKF SLAM: Filter Cycle
1. State prediction (odometry)
2. Measurement prediction
3. Measurement
4. Data association
5. Update
6. Integration of new landmarks
EKF SLAM: Filter Cycle
1. State prediction (odometry)
2. Measurement prediction
3. Measurement
4. Data association
5. Update
6. Integration of new landmarks
EKF SLAM: State Prediction
Odometry:

Robot-landmark cross-
covariance prediction:
EKF SLAM: Measurement
Prediction

Global-to-local
frame transform h
EKF SLAM: Obtained
Measurement
(x,y)-point landmarks
EKF SLAM: Data Association

Associates predicted
measurements
with observation
?
EKF SLAM: Update Step

The usual Kalman

filter expressions
EKF SLAM: New Landmarks

State augmented by

Cross-covariances:
EKF SLAM

Map Correlation matrix

28
EKF SLAM

Map Correlation matrix

29
EKF SLAM

Map Correlation matrix

30
EKF SLAM: Correlations Matter
 What if we neglected cross-correlations?

31
EKF SLAM: Correlations Matter
 What if we neglected cross-correlations?

 Landmark and robot uncertainties would

become overly optimistic
 Data association would fail
 Multiple map entries of the same landmark
 Inconsistent map

32
SLAM: Loop Closure
 Recognizing an already mapped area,
typically after a long exploration path (the
robot “closes a loop”)
 Structurally identical to data association,
but
 high levels of ambiguity
 possibly useless validation gates
 environment symmetries
 Uncertainties collapse after a loop closure
(whether the closure was correct or not)

37
SLAM: Loop Closure
 Before loop closure

38
SLAM: Loop Closure
 After loop closure

39
SLAM: Loop Closure
 By revisiting already mapped areas,
uncertainties in robot and landmark
estimates can be reduced

 This can be exploited when exploring an

environment for the sake of better (e.g.
more accurate) maps

 Exploration: the problem of where to

acquire new information

→ See separate chapter on exploration

40
KF-SLAM Properties
(Linear Case)
 The determinant of any sub-matrix of the map
covariance matrix decreases monotonically as
successive observations are made

 When a new land-

mark is initialized,
its uncertainty is
maximal

 Landmark
uncertainty
decreases
monotonically
with each new
observation
[Dissanayake et al., 2001] 41
KF-SLAM Properties
(Linear Case)
 In the limit, the landmark estimates
become fully correlated

[Dissanayake et al., 2001] 42

KF-SLAM Properties
(Linear Case)
 In the limit, the covariance associated with
any single landmark location estimate is
determined only by the initial covariance
in the vehicle location estimate.

[Dissanayake et al., 2001] 43

EKF SLAM Example:
Victoria Park Dataset

44
Victoria Park: Data Acquisition

[courtesy by E. Nebot] 45
Victoria Park: Estimated
Trajectory

[courtesy by E. Nebot] 46
Victoria Park: Landmarks

[courtesy by E. Nebot] 47
EKF SLAM Example: Tennis
Court

[courtesy by J. Leonard] 48
EKF SLAM Example: Tennis
Court

odometry estimated trajectory

[courtesy by John Leonard] 49

EKF SLAM Example: Line
Features
 KTH Bakery Data Set

[Wulf et al., ICRA 04] 50

EKF-SLAM: Complexity
 Cost per step: quadratic in n, the
number of landmarks: O(n2)
 Total cost to build a map with n
landmarks: O(n3)
 Memory consumption: O(n2)
 Problem: becomes computationally
intractable for large maps!
 There exists variants to circumvent
these problems
51
SLAM Techniques
 EKF SLAM
 FastSLAM
 Graph-based SLAM
 Topological SLAM
(mainly place recognition)
 Scan Matching / Visual Odometry
(only locally consistent maps)
 Approximations for SLAM: Local submaps,
Sparse extended information filters, Sparse
links, Thin junction tree filters, etc.
 …
52
EKF-SLAM: Summary
 The first SLAM solution
 Convergence proof for linear Gaussian
case
 Can diverge if nonlinearities are large
(and the real world is nonlinear ...)
 Can deal only with a single mode
 Successful in medium-scale scenes
 Approximations exists to reduce the
computational complexity
53
Introduction to
Mobile Robotics
SLAM –
Landmark-based FastSLAM
Marina Kollmitz, Wolfram Burgard

Partial slide courtesy of Mike Montemerlo

1
The SLAM Problem

 SLAM stands for simultaneous localization

and mapping
 The task of building a map while estimating
the pose of the robot relative to this map

 Why is SLAM hard?

Chicken-or-egg problem:
 A map is needed to localize the robot
 A pose estimate is needed to build a map
2
The SLAM Problem
A robot moving through an unknown, static environment

Given:
 The robot’s
controls
 Observations of
nearby features
Estimate:
 Map of features
 Path of the robot

3
Map Representations
Typical models are:
 Feature maps
 Grid maps (occupancy or reflection
probability maps)

4
Why is SLAM a Hard Problem?

SLAM: robot path and map are both unknown!

Robot path error correlates errors in the map

5
Why is SLAM a Hard Problem?

Robot pose
uncertainty

 In the real world, the mapping between

observations and landmarks is unknown
 Picking wrong data associations can have
catastrophic consequences
 Pose error correlates data associations
7
Data Association Problem

 A data association is an assignment of

observations to landmarks
 In general there are more than
(n observations, m landmarks) possible
associations
 Also called “assignment problem”
9
Particle Filters
 Represent belief by random samples
 Estimation of non-Gaussian, nonlinear
processes

 Sampling Importance Resampling (SIR)

principle
 Draw the new generation of particles
 Assign an importance weight to each particle
 Resample

 Typical application scenarios are tracking,

localization, … 10
Recap: Particle Filter Localization

1. initialize particles
2. apply motion
model
3. weight particles
(sensor model)
4. resample
according to
weight

11
Recap: Particle Filter Localization

1. initialize particles
2. apply motion
model
3. weight particles
(sensor model)
4. resample
according to
weight

12
Recap: Particle Filter Localization

1. initialize particles
2. apply motion
model
3. weight particles
(sensor model)
4. resample
according to
weight

13
Recap: Particle Filter Localization

1. initialize particles
2. apply motion
model
3. weight particles
(sensor model)
4. resample
according to
weight
Actual measurement:

14
Recap: Particle Filter Localization

2
1 1. initialize particles
2. apply motion
model
1 1
3. weight particles
(sensor model)
2 4. resample
according to
weight

15
Recap: Particle Filter Localization

2
1 1. initialize particles
2. apply motion
model
1 1
3. weight particles
(sensor model)
2 4. resample
according to
weight

16
Localization vs. SLAM
 A particle filter can be used to solve both
problems
 Localization: state space <x, y, θ>
 SLAM: state space <x, y, θ, map>
 for landmark maps = <l1, l2, …, lm>
 for grid maps = <c11, c12, …, c1n, c21, …, cnm>

 Problem: The number of particles needed to

represent a posterior grows exponentially with
the dimension of the state space!

17
Dependencies
 Is there a dependency between certain
dimensions of the state space?
 If so, can we use the dependency to solve
the problem more efficiently?

 In the SLAM context

 The map depends on the poses of the robot.
 We know how to build a map given the position
of the sensor is known.

18
Rao-Blackwellization
 Factorization to exploit dependencies
between variables:

 If can be computed in closed form,

represent only with samples
and compute for every sample

 It comes from the Rao-Blackwell theorem

 Factored Posterior (Landmarks)
poses map observations & movements

Factorization first introduced by Murphy in 1999 20

 Factored Posterior (Landmarks)
poses map observations & movements

Factorization first introduced by Murphy in 1999 21

 Factored Posterior (Landmarks)
poses map observations & movements

SLAM posterior
Robot path posterior
landmark positions
Does this help to solve the problem?
Factorization first introduced by Murphy in 1999 22
Revisit the Graphical Model

Courtesy: Thrun, Burgard, Fox

Revisit the Graphical Model
known

Courtesy: Thrun, Burgard, Fox

Landmarks are Conditionally
Independent Given the Poses

Landmark variables are all disconnected

(i.e. independent) given the robot’s path
Remember: Landmarks Correlated

SLAM: robot path and map are both unknown!

Robot path error correlates errors in the map

26
After Factorization

For estimating landmarks: robot path known!

Landmarks are not correlated

27
Factored Posterior

Robot path posterior

(localization problem) Conditionally
independent
landmark positions
31
Rao-Blackwellization for SLAM

 Given that the second term can be computed

efficiently, particle filtering becomes possible!

32
 FastSLAM
 Rao-Blackwellized particle filtering based on
landmarks [Montemerlo et al., 2002]
 Each landmark is represented by a 2x2
Extended Kalman Filter (EKF)
 Each particle therefore has to maintain M EKFs

Particle
#1
x, y, θ Landmark 1 Landmark 2 … Landmark M

Particle
#2
x, y, θ Landmark 1 Landmark 2 … Landmark M
…

Particle
N
x, y, θ Landmark 1 Landmark 2 … Landmark M
33
 FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter

Particle #2

Particle #3

34
 FastSLAM – Sensor Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter

Particle #2

Particle #3

35
 FastSLAM – Sensor Update

Particle #1 Weight = 0.8

Particle #2 Weight = 0.4

Particle #3 Weight = 0.1

36
 FastSLAM – Sensor Update

Update map
Particle #1
of particle #1

Update map
Particle #2
of particle #2

Particle #3 Update map

of particle #3

37
FastSLAM - Video

38
FastSLAM Complexity – Naive
 Update robot particles O(N)
based on the control

 Incorporate an observation O(N)

into the Kalman filters
(given the data association)

 Resample particle set O(N M)

N = Number of particles
M = Number of map features
A Better Data Structure for
FastSLAM

Courtesy: M. Montemerlo
A Better Data Structure for
FastSLAM

5 7
FastSLAM Complexity
 Update robot particles
based on the control

 Incorporate an observation
into the Kalman filters
(given the data association)

 Resample particle set

N = Number of particles
M = Number of map features
O(N log(M))
Data Association Problem
 Which observation belongs to which
landmark?

 A robust SLAM solution must consider

possible data associations
 Potential data associations depend also
on the pose of the robot
43
Multi-Hypothesis Data Association

 Data association is done

on a per-particle basis

 Robot pose error is

factored out of data
association decisions

44
 Per-Particle Data Association

Was the observation

generated by the red
or the brown landmark?

P(observation|red) = 0.3 P(observation|brown) = 0.7

 Two options for per-particle data association

 Pick the most probable match
 Pick a random association weighted by
the observation likelihoods
 If the probability is too low, generate a new
landmark
45
Results – Victoria Park

 4 km traverse
 < 5 m RMS
position error
 100 particles

Blue = GPS
Yellow = FastSLAM
Dataset courtesy of University of Sydney 46
Results – Victoria Park (Video)

Dataset courtesy of University of Sydney 47

Results – Data Association

48
FastSLAM Summary
 FastSLAM factors the SLAM posterior into
low-dimensional estimation problems
 Scales to problems with over 1 million features
 FastSLAM factors robot pose uncertainty
out of the data association problem
 Robust to significant ambiguity in data
association
 Allows data association decisions to be delayed
until unambiguous evidence is collected
 Advantages compared to the classical EKF
approach (especially with non-linearities)
 Complexity of O(N log M)
49
Introduction to
Mobile Robotics
SLAM – Grid-based
FastSLAM
Lukas Luft, Wolfram Burgard

1
The SLAM Problem
 SLAM stands for simultaneous localization
and mapping
 The task of building a map while estimating
the pose of the robot relative to this map

 SLAM has for a long time considered being

a chicken and egg problem:
• a map is needed to localize the robot and
• a pose estimate is needed to build a map

2
Mapping using Raw Odometry

3
Grid-based SLAM

 Can we solve the SLAM problem if no pre-

defined landmarks are available?
 Can we use the ideas of FastSLAM to build
grid maps?
 As with landmarks, the map depends on
the poses of the robot during data
acquisition
 If the poses are known, grid-based
mapping is easy (“mapping with known
poses”)

4
 Rao-Blackwellization
poses map observations & movements

Factorization first introduced by Murphy in 1999 5

 Rao-Blackwellization
poses map observations & movements

SLAM posterior

Robot path posterior

Mapping with known poses

Factorization first introduced by Murphy in 1999 6

Rao-Blackwellization

This is localization, use MCL

Use the pose estimate

from the MCL and apply
mapping with known poses
7
A Graphical Model of Mapping
with Rao-Blackwellized PFs

u0 u1 ut-1

x0 x1 x2 ... xt

z1 z2 zt
8
Mapping with Rao-
Blackwellized Particle Filters
 Each particle represents a possible
trajectory of the robot

 Each particle
 maintains its own map and
 updates it upon “mapping with known
poses”

 Each particle survives with a probability

proportional to the likelihood of the
observations relative to its own map
9
 Particle Filter Example

3 particles

map of particle 1 map of particle 3

map of particle 2 10
Problem
 Each map is quite big in case of grid
maps
 Each particle maintains its own map,
therefore, one needs to keep the
number of particles small
 Solution:
Compute better proposal
distributions!
 Idea:
Improve the pose estimate before
applying the particle filter 11
Pose Correction Using Scan
Matching
Maximize the likelihood of the i-th pose and
map relative to the (i-1)-th pose and map

current measurement robot motion

map constructed so far

12
Scan-Matching Example

14
Motion Model for Scan Matching

Raw Odometry
Scan Matching

15
Mapping using Scan Matching

16
FastSLAM with Improved
Odometry
 Scan-matching provides a locally
consistent pose correction

 Pre-correct short odometry sequences

using scan-matching and use them as
input to FastSLAM

 Fewer particles are needed, since the

error in the input is smaller

17
Graphical Model for Mapping
with Improved Odometry
u0 ... uk-1 uk ... u2k-1 un·k ... u(n+1)·k-1
z1 ... z k-1 zk+1...z2k-1 ... z n·k+1... z(n+1)·k-1

u'1 u'2 ... u'n

x0 xk x2k ... x n·k

zk z2k ... zn·k

18
FastSLAM with Scan-Matching

19
FastSLAM with Scan-Matching

Loop Closure

20
 FastSLAM with Scan-Matching

21

Map: Intel Research Lab Seattle

Comparison to Standard FastSLAM
 Same model for observations
 Odometry instead of scan matching as input
 Number of particles varying from 500 to 2,000
 Typical result:

22
Conclusion (thus far …)

 The presented approach is a highly

efficient algorithm for SLAM combining
ideas of scan matching and FastSLAM
 Scan matching is used to transform
sequences of laser measurements into
odometry measurements
 This version of grid-based FastSLAM can
handle larger environments than before in
“real time”

23
What’s Next?

 Further reduce the number of

particles
 Improved proposals will lead to
more accurate maps
 Use the properties of our sensor
when drawing the next generation
of particles

24
The Optimal Proposal
Distribution
observation motion
Probability for pose model model
given collected data

[Arulampalam et al., 01]

normalization

25
The Optimal Proposal
Distribution

For lasers is extremely

peaked and dominates the product.
We can safely approximate
by a constant:
26
 Resulting Proposal Distribution

Approximate this equation by a Gaussian:

maximum reported
by a scan matcher
Gaussian
approximation

Draw next
generation of
Sampled points around samples
the maximum 29
Estimating the Parameters of
the Gaussian for each Particle

 xj are a set of sample points around

the point x* the scan matching has
converged to.
  is a normalizing constant
30
Computing the Importance
Weight

Sampled points around the

maximum of the observation
likelihood
31
Improved Proposal
 The proposal adapts to the structure of
the environment

32
Resampling
 Sampling from an improved proposal reduces the
effects of resampling
 However, resampling at each step limits the
“memory” of our filter
 Supposed we loose at each frame 25% of the
particles, in the worst case we have a memory of
only 4 steps.

Goal: reduce the number of resampling actions

33
Selective Re-sampling
 Re-sampling is dangerous, since important
samples might get lost
(particle depletion problem)

 In case of suboptimal proposal distributions

re-sampling is necessary to achieve
convergence.

 Key question: When should we re-sample?

34
Number of Effective Particles

 Assuming normalized particle weights that

sum up to 1.0:
 Empirical measure of how well the goal distribution
is approximated by samples drawn from the
proposal
 It describes “the variance of the particle weights”
 It is maximal for equal weights. In this case
the distribution is close to the proposal

35
Resampling with neff
 If our approximation is close to the
proposal, no resampling is needed

 We only re-sample when drops

below a given threshold, typically

 See [Doucet, ’98; Arulampalam, ’01]

36
Typical Evolution of neff
visiting new
areas closing the
first loop

visiting
known areas

second loop closure

37
Intel Lab
 15 particles
 four times faster
than real-time
P4, 2.8GHz
 5cm resolution
during scan
matching
 1cm resolution in
final map

38
Intel Lab
 15 particles
 Compared to
FastSLAM with
Scan-Matching,
the particles are
propagated
closer to the true
distribution

39
Outdoor Campus Map
 30 particles
 250x250m2
 1.75
1.088km
miles
(odometry)
 20cm resolution
during scan
matching
 30cm resolution
in final map

40
Outdoor Campus Map - Video

41
MIT Killian Court

 The “infinite-corridor-dataset” at MIT

42
MIT Killian Court

43
MIT Killian Court - Video

44
Conclusion
 The ideas of FastSLAM can also be applied in the
context of grid maps
 Utilizing accurate sensor observation leads to good
proposals and highly efficient filters
 It is similar to scan-matching on a per-particle base
 The number of necessary particles and
re-sampling steps can seriously be reduced
 Improved versions of grid-based FastSLAM can
handle larger environments than naïve
implementations in “real time” since they need one
order of magnitude fewer samples

45
More Details on FastSLAM
 M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM:
A factored solution to simultaneous localization and mapping, AAAI02
(The classic FastSLAM paper with landmarks)

 D. Haehnel, W. Burgard, D. Fox, and S. Thrun.

An efficient FastSLAM algorithm for generating maps of large-scale
cyclic environments from raw laser range measurements, IROS03
(FastSLAM on grid-maps using scan-matched input)

 G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based

SLAM with Rao-Blackwellized particle filters by adaptive proposals
and selective resampling, ICRA05
(Proposal using laser observation, adaptive resampling)

 A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultaneous

localization and mapping without predetermined landmarks, IJCAI03
(An approach to handle big particle sets)

46
Introduction to
Mobile Robotics
Graph-Based SLAM

Wolfram Burgard

1
Particle Filter: Campus Map
 30 particles
 250x250m2
 1.75 km
(odometry)
 20cm resolution
during scan
matching
 30cm resolution
in final map

2
Graph-Based SLAM
 Constraints connect the poses of the
robot while it is moving
 Constraints are inherently uncertain

Robot pose Constraint

3
Graph-Based SLAM
 Observing previously seen areas
generates constraints between non-
successive poses

Robot pose Constraint Constraint

(odometry) (loop closure) 4
Example: Odometry Map

Hanover2 dataset (Courtesy of Oliver Wulf) 5

Example: Loop Closures

Hanover2 dataset (Courtesy of Oliver Wulf) 6

How to correct the trajectory?

Imagine this to be a system of masses and springs!

7
Idea of Graph-Based SLAM
 Use a graph to represent the problem
 Every node in the graph corresponds
to a pose of the robot during mapping
 Every edge between two nodes
corresponds to a spatial constraint
between them
 Graph-Based SLAM: Build the graph
and find a node configuration that
minimize the error introduced by the
constraints
8
Graph-Based SLAM in a Nutshell
 Every node in the
graph corresponds
to a robot position
and a laser
measurement
 An edge between
two nodes
represents a spatial
constraint between
the nodes

KUKA Halle 22, courtesy of P. Pfaff 9

Graph-Based SLAM in a Nutshell
 Every node in the
graph corresponds
to a robot position
and a laser
measurement
 An edge between
two nodes
represents a spatial
constraint between
the nodes

KUKA Halle 22, courtesy of P. Pfaff 10

Graph-Based SLAM in a Nutshell
 Once we have the
graph, we determine
the most likely map
by correcting the
nodes

11
Graph-Based SLAM in a Nutshell
 Once we have the
graph, we determine
the most likely map
by correcting the
nodes
… like this

12
Graph-Based SLAM in a Nutshell
 Once we have the
graph, we determine
the most likely map
by correcting the
nodes
… like this
 Then, we can render a
map based on the
known poses

13
The Overall SLAM System
 Interplay of front-end and back-end
 A consistent map helps to determine new
constraints by reducing the search space
 This lecture focuses only on the optimization

node positions

Graph Graph
raw Construction Optimization
data
(Front-End) graph (Back-End)
(nodes & edges)

today 14
Least Squares in General
 Approach for computing a solution for
an overdetermined system
 “More equations than unknowns”
 Minimizes the sum of the squared
errors in the equations
 Standard approach to a large set of
problems

15
Problem
 Given a system described by a set of n
observation functions
 Let
 be the state vector
 be a measurement of the state x
 be a function which maps to a
predicted measurement
 Given n noisy measurements about
the state
 Goal: Estimate the state which bests
explains the measurements
16
Graphical Explanation

state predicted real

(unknown) measurements measurements

17
Error Function
 Error is typically the difference between
the predicted and actual measurement

 We assume that the error has zero mean

and is normally distributed
 Gaussian error with information matrix
 The squared error of a measurement
depends only on the state and is a scalar

18
Least Squares for SLAM
 Overdetermined system for estimating
the robot’s poses given observations
 “More observations than states”
 Minimizes the sum of the squared
errors

Today: Application to SLAM

19
The Graph
 It consists of n nodes
 Each is a 2D or 3D transformation
(the pose of the robot at time ti)
 A constraint/edge exists between the
nodes and if…

20
Create an Edge If… (1)
 …the robot moves from to
 Edge corresponds to odometry

The edge represents the

odometry measurement

21
Create an Edge If… (2)
 …the robot observes the same part of
the environment from and from
 Construct a virtual measurement
about the position of seen from

xi
xj

Measurement from i Measurement from

22
Create an Edge If… (2)
 …the robot observes the same part of
the environment from and from
 Construct a virtual measurement
about the position of seen from

Edge represents the position of seen

from based on the observation
23
Pose Graph

observation edge
of from

error
nodes
according to
the graph

24
Pose Graph

observation edge
of from

error
nodes
according to
the graph

Goal:
25
Gauss-Newton: The Overall
Error Minimization Procedure
 Define the error function
 Linearize the error function
 Compute its derivative
 Set the derivative to zero
 Solve the linear system
 Iterate this procedure until
convergence

26
Sparse Pose Adjustment

27
Example: CS Campus Freiburg

28
There are Variants for 3D
 Highly connected
graph
 Poor initial guess
 LU & variants fail
 2200 nodes
 8600 constraints

29
Hanover2: 3D SLAM Map

31
Campus : Scan Matching Map

32
Campus : Graph Optimization

34
Campus : SLAM Map

35
Freiburg Campus Octomap

36
Example: Stanford Garage

38
3D Map of the Stanford Parking
Garage

approx. 260MB 39
Application: Navigation with
the Autonomous Car Junior
 Task: reach a parking spot on the
upper level of the garage.

40
Autonomous Parking

42
 Graph-SLAM with more Sensors

Graph SLAM is flexible regarding additional information

(GPS, IMU, road network matches, …) 43
Conclusions
 The back-end part of the SLAM
problem can be effectively solved with
Gauss-Newton error minimization
 Error functions compute the
mismatch between the state and
the observations
 Currently one of the state-of-the-art
solutions for SLAM

44
Introduction to
Mobile Robotics
Path and Motion Planning

Wolfram Burgard

1
Motion Planning

Latombe (1991):

“… eminently necessary since, by

definition, a robot accomplishes tasks by
moving in the real world.”

Goals:
 Collision-free trajectories.
 Robot should reach the goal location as
quickly as possible.

2
… in Dynamic Environments
 How to react to unforeseen obstacles?
 efficiency
 reliability

 Dynamic Window Approaches

[Simmons, 96], [Fox et al., 97], [Brock & Khatib, 99]

 Grid-map-based planning
[Konolige, 00]

 Nearness-Diagram-Navigation
[Minguez at al., 2001, 2002]

 Vector-Field-Histogram+
[Ulrich & Borenstein, 98]

 A*, D*, D* Lite, ARA*, …

3
Two Challenges
 Calculate the optimal path taking potential
uncertainties in the actions into account

 Quickly generate actions in the case of

unforeseen objects

4
Classic Two-layered Architecture

Planning low frequency

map sub-goal

Collision
high frequency
Avoidance
motion command
sensor data

robot
5
Dynamic Window Approach

 Collision avoidance: Determine collision-

free trajectories using geometric operations
 Here: Robot moves on circular arcs
 Motion commands (v,ω)
 Which (v,ω) are admissible and reachable?

6
Admissible Velocities
 Speeds are admissible if the robot would be
able to stop before reaching the obstacle

7
Reachable Velocities
 Speeds that are reachable by acceleration

8
DWA Search Space

 Vs = all possible speeds of the robot.

 Va = obstacle free area.
 Vd = speeds reachable within a certain time frame based on
possible accelerations.

9
Dynamic Window Approach

 How to choose <v,ω>?

 Steering commands are chosen by a
heuristic navigation function.
 This function tries to minimize the travel-
time by “driving fast into the right
direction.”

10
Dynamic Window Approach
 Heuristic navigation function.
 Planning restricted to <x,y>-space.
 No planning in the velocity space.

Navigation Function: [Brock & Khatib, 99]

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

11
Dynamic Window Approach
 Heuristic navigation function.
 Planning restricted to <x,y>-space.
 No planning in the velocity space.

Navigation Function: [Brock & Khatib, 99]

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

Maximizes
velocity.
12
Dynamic Window Approach
 Heuristic navigation function.
 Planning restricted to <x,y>-space.
 No planning in the velocity space.

Navigation Function: [Brock & Khatib, 99]

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

Maximizes Considers cost to
velocity. reach the goal. 13
Dynamic Window Approach
 Heuristic navigation function.
 Planning restricted to <x,y>-space.
 No planning in the velocity space.

Navigation Function: [Brock & Khatib, 99]

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

Maximizes Considers cost to Follows grid based path
velocity. reach the goal. computed by A*. 14
Dynamic Window Approach
 Heuristic navigation function.
 Planning restricted to <x,y>-space.
 No planning in the velocity space.

Navigation Function: Goal

[Brock nearness
& Khatib, 99]

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

Maximizes Considers cost to Follows grid based path
velocity reach the goal computed by A* 15
Dynamic Window Approach
 Reacts quickly.
 Low computational requirements.
 Guides a robot along a collision-free path.
 Successfully used in a lot of real-world
scenarios.
 Resulting trajectories sometimes sub-
optimal.
 Local minima might prevent the robot from
reaching the goal location.

16
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

17
 Robot’s
Problems of DWAs velocity.

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

18
 Robot’s
Problems of DWAs velocity.

Preferred
direction of NF.

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

19
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

20
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

21
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

The robot drives too fast at c0 to enter

corridor facing south.
22
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

23
Problems of DWAs

NF = α ⋅ vel + β ⋅ nf + γ ⋅ ∆nf + δ ⋅ goal

24
Problems of DWAs

Same situation as in the beginning.

 DWAs have problems to reach the goal.

25
Problems of DWAs
 Typical problem in a real world situation:

 Robot does not slow down early enough to

enter the doorway.
26
Motion Planning Formulation
 The problem of motion planning can be
stated as follows. Given:
 A start pose of the robot
 A desired goal pose
 A geometric description of the robot
 A geometric representation of the
environment

 Find a path that moves the robot

gradually from start to goal while
never touching any obstacle
27
Configuration Space
 Although the motion planning problem is
defined in the regular world, it lives in
another space: the configuration space

 A robot configuration q is a specification of

the positions of all robot points relative to
a fixed coordinate system

 Usually a configuration is expressed as a

vector of positions and orientations

28
Configuration Space
 Free space and obstacle region
 With being the work space,
the set of obstacles, the robot in
configuration

 We further define
 : start configuration
 : goal configuration

29
Configuration Space
Then, motion planning amounts to
 Finding a continuous path

with

 Given this setting,

we can do planning
with the robot being
a point in C-space!

30
C-Space Discretizations
 Continuous terrain needs to be discretized
for path planning
 There are two general approaches to
discretize C-spaces:
 Combinatorial planning
Characterizes Cfree explicitly by capturing the
connectivity of Cfree into a graph and finds
solutions using search
 Sampling-based planning
Uses collision-detection to probe and
incrementally search the C-space for a solution
31
Search
The problem of search: finding a sequence
of actions (a path) that leads to desirable
states (a goal)

 Uninformed search: besides the problem

definition, no further information about the
domain (“blind search”)
 The only thing one can do is to expand
nodes differently
 Example algorithms: breadth-first,
uniform-cost, depth-first, bidirectional, etc.

32
Search
The problem of search: finding a sequence
of actions (a path) that leads to desirable
states (a goal)

 Informed search: further information

about the domain through heuristics
 Capability to say that a node is “more
promising” than another node
 Example algorithms: greedy best-first
search, A*, many variants of A*, D*, etc.

33
Search
The performance of a search algorithm is
measured in four different ways:

 Completeness: does the algorithm find a

solution when there is one?
 Optimality: is the solution the best one of
all possible solutions in terms of path cost?
 Time complexity: how long does it take
to find a solution?
 Space complexity: how much memory is
needed to perform the search?
34
Discretized Configuration Space

35
Uninformed Search
 Breadth-first
 Complete
 Optimal if action costs equal
 Time and space: O(bd)

 Depth-first
 Not complete in infinite spaces
 Not optimal
 Time: O(bm)
 Space: O(bm) (can forget
explored subtrees)
(b: branching factor, d: goal depth, m: max. tree depth)
36
Informed Search: A*
 What about using A* to plan
the path of a robot?
 Finds the shortest path
 Requires a graph structure
 Limited number of edges
 In robotics: planning on a 2d
occupancy grid map

37
A*: Minimize the Estimated
Path Costs
 g(n) = actual cost from the initial state to n.
 h(n) = estimated cost from n to the next goal.
 f(n) = g(n) + h(n), the estimated cost of the
cheapest solution through n.
 Let h*(n) be the actual cost of the optimal path
from n to the next goal.
 h is admissible if the following holds for all n :
h(n) ≤ h*(n)
 We require that for A*, h is admissible (the
straight-line distance is admissible in the
Euclidean Space).
38
Example: PathPlanning for
Robots in a Grid-World

39
Deterministic Value Iteration
 To compute the shortest path from
every state to one goal state, use
(deterministic) value iteration.
 Very similar to Dijkstra’s Algorithm.
 Such a cost distribution is the optimal
heuristic for A*.

40
Typical Assumption in Robotics
for A* Path Planning
1. The robot is assumed to be localized.
2. The robot computes its path based on
an occupancy grid.
3. The correct motion commands are
executed.

Are 1. and 3. always true?

41
Problems

 What if the robot is (slightly) delocalized?

 Moving on the shortest path often guides

the robot along a trajectory close to
obstacles.

 Trajectory aligned to the grid structure.

42
Convolution of the Grid Map

 Convolution blurs the map.

 Obstacles are assumed to be bigger
than in reality.
 Perform an A* search in such a
convolved map (using occupancy as
traversal cost).
 Robot increases distance to obstacles
and moves on a short path!

43
Example: Map Convolution
 one-dimensional environment, cells
c0, …, c5

 Cells before and after 2 convolution runs.

44
Convolution
 Consider an occupancy map. Than the
convolution is defined as:

 This is done for each row and each

column of the map.
 “Gaussian blur”

45
A* in Convolved Maps
 The costs are a product of path length
and occupancy probability of the cells.

 Cells with higher probability (e.g.,

caused by convolution) are avoided by
the robot.

 Thus, it keeps distance to obstacles.

 This technique is fast and quite reliable.

46
5D-Planning – an Alternative to
the Two-layered Architecture
 Plans in the full <x,y,θ,v,ω>-configuration
space using A*.
 Considers the robot's kinematic constraints.

 Generates a sequence of steering

commands to reach the goal location.

 Maximizes trade-off between driving time

and distance to obstacles.

47
The Search Space (1)
 What is a state in this space?
<x,y,θ,v,ω> = current position and
speed of the robot

 How does a state transition look like?

<x1,y1,θ1,v1,ω1> <x2,y2,θ2,v2,ω2>
 with motion command (v2,ω2) and
|v1-v2| < av, |ω1-ω2| < aω.
 The new pose of the Robot <x2,y2,θ2> is a
result of the motion equations.

48
The Search Space (2)
Idea: search in the discretized
<x,y,θ,v,ω>-space.

Problem: the search space is too huge to be

explored within the time constraints
(5+ Hz for online motion planning).

Solution: restrict the full search space.

49
The Main Steps of the Algorithm
1. Update (static) grid map based on sensory
input.

2. Use A* to find a trajectory in the <x,y>-

space using the updated grid map.

3. Determine a restricted 5d-configuration

space based on step 2.

4. Find a trajectory by planning in the

restricted <x,y,θ,v,ω>-space.

50
Updating the Grid Map
 The environment is represented as a 2d-
occupency grid map.
 Convolution of the map increases security
distance.
 Detected obstacles are added.
 Cells discovered free are cleared.

update 51
Find a Path in the 2d-Space
 Use A* to search for the optimal path in
the 2d-grid map.

 Use heuristic based on a deterministic

value iteration within the static map.

52
Restricting the Search Space

Assumption: the projection of the 5d-path

onto the <x,y>-space lies close to the
optimal 2d-path.

Therefore: construct a restricted search

space (channel) based on the 2d-path.

53
Space Restriction
 Resulting search space =
<x, y, θ, v, ω> with (x,y) Є channel.
 Choose a sub-goal lying on the 2d-path
within the channel.

54
Find a Path in the 5d-Space

 Use A* in the restricted 5d-space to find a

sequence of steering commands to reach
the sub-goal.

 To estimate cell costs: perform a

deterministic 2d-value iteration within the
channel.

55
Examples

56
Timeouts
 Steering a robot online requires to set new
steering commands frequently.
E.g., every 0.2 secs.

 Abort search after 0.2 secs.

How to find an admissible steering

command?

57
Alternative Steering Command
 Previous trajectory still admissible?
 OK

 If not, drive on the 2d-path or use DWA to

find new command.

58
Timeout Avoidance

 Reduce the size of the channel if the 2d-

path has high cost.
59
Example

Robot Albert Planning state

60
Comparison to the DWA (1)
 DWAs often have problems entering narrow
passages.

DWA planned path. 5D approach.

61
Comparison to the DWA (2)

The 5D approach results in significantly faster

motion when driving through narrow passages!

62
Comparison to the Optimum

Channel: with length=5m, width=1.1m

Resulting actions are close to the optimal solution.
63
Rapidly Exploring Random Trees
 Idea: aggressively probe and explore the
C-space by expanding incrementally
from an initial configuration q0
 The explored territory is marked by a
tree rooted at q0

45 2,345
iterations iterations 64
RRTs
The algorithm: Given C and q0

Sample from a
bounded region
centered around q0

E.g. an axis-aligned
relative random
translation or random
rotation

65
RRTs
 The algorithm

Finds closest vertex in G

using a distance
function

formally a metric
defined on C

66
RRTs
 The algorithm

Several stategies to find

qnear given the closest
vertex on G:
• Take closest vertex
• Check intermediate
points at regular
intervals and split edge
at qnear

67
RRTs
 The algorithm

Connect nearest point

with random point
using a local planner
that travels from qnear
to qrand

• No collision: add
edge

68
RRTs
 The algorithm

Connect nearest point

with random point
using a local planner
that travels from qnear
to qrand

• No collision: add
edge
• Collision: new vertex
is qs, as close as
possible to Cobs
69
RRTs
 How to perform path planning with RRTs?
1. Start RRT at qI
2. At every, say, 100th iteration, force qrand = qG
3. If qG is reached, problem is solved

 Why not picking qG every time?

 This will fail and waste much effort in
running into CObs instead of exploring the
space

70
RRTs
 However, some problems require more
effective methods: bidirectional search
 Grow two RRTs, one from qI, one from qG
 In every other
step, try to
extend each
tree towards
the newest
vertex of the
other tree
Filling a A bug
well trap 71
RRTs
 RRTs are popular, many extensions exist:
real-time RRTs, anytime
RRTs, for dynamic
environments etc.
 Pros:
 Balance between greedy
search and exploration
 Easy to implement
Alpha 1.0
 Cons:
puzzle.
 Metric sensivity Solved with
 Unknown rate of convergence bidirectional
RRT
72
Road Map Planning
 A road map is a graph in Cfree in which each
vertex is a configuration in Cfree and each
edge is a collision-free path through Cfree

 Several planning techniques

 Visibility graphs
 Voronoi diagrams
 Exact cell decomposition
 Approximate cell decomposition
 Randomized road maps

73
Road Map Planning
 A road map is a graph in Cfree in which each
vertex is a configuration in Cfree and each
edge is a collision-free path through Cfree

 Several planning techniques

 Visibility graphs
 Voronoi diagrams
 Exact cell decomposition
 Approximate cell decomposition
 Randomized road maps

74
Generalized Voronoi Diagram
 Defined to be the set of points q whose
cardinality of the set of boundary points of
Cobs with the same distance to q is greater
than 1
 Let us decipher
qI'
this definition... qI
qG'
qG
 Informally:
the place with the
same maximal
clearance from
all nearest obstacles
75
Generalized Voronoi Diagram
 Formally:
Let be the boundary of Cfree, and d(p,q)
the Euclidian distance between p and q. Then, for
all q in Cfree, let

be the clearance of q, and

the set of "base" points on β with the same

clearance to q. The Voronoi diagram is then the
set of q's with more than one base point p

76
Generalized Voronoi Diagram
 Geometrically:

two closest points p

p
one closest point
q q
clearance(q) q
p p p

 For a polygonal Cobs, the Voronoi diagram

consists of (n) lines and parabolic segments
 Naive algorithm: O(n4), best: O(n log n)
77
Voronoi Diagram
 Voronoi diagrams have been well studied
for (reactive) mobile robot path planning
 Fast methods exist to compute and
update the diagram in real-time for low-
dim. C's
 Pros: maximize clear-
ance is a good idea for
an uncertain robot
 Cons: unnatural at-
traction to open space,
suboptimal paths

 Needs extensions 78
Randomized Road Maps
Also called Probabilistic Road Maps

 Idea: Take random samples from C,

declare them as vertices if in Cfree, try to
connect nearby vertices with local planner
 The local planner checks if line-of-sight is
collision-free (powerful or simple methods)
 Options for nearby: k-nearest neighbors
or all neighbors within specified radius
 Configurations and connections are added
to graph until roadmap is dense enough
79
Randomized Road Maps
 Example

specified
radius
What does “nearby” mean
Example local on a manifold? Defining a
planner good metric on C is crucial 80
Randomized Road Maps
 Pros:
 Probabilistically complete
qG
 Do not construct C-space
Cobs
 Apply easily to high
dimensional C-spaces Cobs
 Randomized road maps have Cobs
solved previously unsolved qI
problems qG
 Cons: Cobs Cobs
 Do not work well for some
problems, narrow passages
Cobs Cobs
 Not optimal, not complete qI
81
Randomized Road Maps
 How to uniformly sample C ? This is not at all
trivial given its topology
 For example over spaces of rotations: Sampling
Euler angles gives more samples near poles, not
uniform over SO(3). Use quaternions!
 However, Randomized Road Maps are powerful,
popular and many extensions exist: advanced
sampling strategies (e.g. near obstacles), PRMs
for deformable objects, closed-chain systems,
etc.

82
From Road Maps to Paths
 All methods discussed so far construct a
road map (without considering the query
pair qI and qG)

 Once the investment is made, the same

road map can be reused for all queries
(provided world and robot do not change)
1. Find the cell/vertex that contain/is close to qI
and qG (not needed for visibility graphs)
2. Connect qI and qG to the road map
3. Search the road map for a path from qI to qG
83
Markov Decision Process
 Consider an agent acting in this
environment

 Its mission is to reach the goal marked

by +1 avoiding the cell labelled -1
84
Markov Decision Process
 Consider an agent acting in this
environment

 Its mission is to reach the goal marked

by +1 avoiding the cell labelled -1
85
Markov Decision Process
 Easy! Use a search algorithm such as A*

 Best solution (shortest path) is the action

sequence [Right, Up, Up, Right]
86
What is the problem?
 Consider a non-perfect system
in which actions are performed with a
probability less than 1
 What are the best actions for an agent
under this constraint?

 Example: a mobile robot does not

exactly perform a desired motion
 Example: human navigation

Uncertainty about performing actions!

87
MDP Example
 Consider the non-deterministic
transition model (N / E / S / W):
desired action

p=0.8

p=0.1 p=0.1

 Intended action is executed with p=0.8

 With p=0.1, the agent moves left or right
 Bumping into a wall “reflects” the robot
88
MDP Example
 Executing the A* plan in this environment

89
MDP Example
 Executing the A* plan in this environment

But: transitions are non-deterministic! 90

MDP Example
 Executing the A* plan in this environment

This will happen sooner or later... 91

MDP Example
 Use a longer path with lower probability
to end up in cell labelled -1

 This path has the highest overall utility

 Probability 0.86 = 0.2621
92
Transition Model
 The probability to reach the next state s'
from state s by choosing action a

is called transition model

Markov Property:
The transition probabilities from s to s'
depend only on the current state s
and not on the history of earlier states
93
Reward
 In each state s, the agent receives a
reward R(s)

 The reward may be positive or negative

but must be bounded

 This can be generalized to be a function

R(s,a,s').
Here: considering only R(s), does not
change the problem

94
Reward
 In our example, the reward is -0.04 in all
states (e.g. the cost of motion) except the
terminal states (that have rewards +1/-1)

 A negative reward
gives agents an in-
centive to reach
the goal quickly

 Or: “living in this

environment is
not enjoyable”
95
MDP Definition
 Given a sequential decision problem in
a fully observable, stochastic environment
with a known Markovian transition model

 Then a Markov Decision Process is

defined by the components
• Set of states:
• Set of actions:
• Initial state:
• Transition model:
• Reward funciton:

96
Policy
 An MDP solution is called policy π
 A policy is a mapping from states to actions

 In each state, a policy tells the agent

what to do next
 Let π (s) be the action that π specifies for s
 Among the many policies that solve an
MDP, the optimal policy π* is what we
seek. We'll see later what optimal means
97
Policy
 The optimal policy for our example

Conservative choice
Take long way around
as the cost per step of
-0.04 is small compared
with the penality to fall
down the stairs and
receive a -1 reward

98
Policy
 When the balance of risk and reward
changes, other policies are optimal
R = -2 R = -0.2

Leave as soon as possible Take shortcut, minor risks

R = -0.01 R>0

No risks are taken Never leave (inf. #policies)

100
Utility of a State
 The utility of a state U(s) quantifies the
benefit of a state for the overall task
 We first define Uπ(s) to be the expected
utility of all state sequences that start
in s given π

 U(s) evaluates (and encapsulates) all

possible futures from s onwards

101
Utility of a State
 With this definition, we can express Uπ(s)
as a function of its next state s'

102
Optimal Policy
 The utility of a state allows us to apply the
Maximum Expected Utility principle to
define the optimal policy π*

 The optimal policy π* in s chooses the

action a that maximizes the expected
utility of s (and of s')

 Expectation taken over all policies

103
Optimal Policy
 Substituting Uπ(s)

 Recall that E[X] is the weighted average of

all possible values that X can take on
104
Utility of a State
 The true utility of a state U(s) is then
obtained by application of the optimal
policy, i.e. . We find

105
Utility of a State
 This result is noteworthy:

We have found a direct relationship

between the utility of a state and the
utility of its neighbors
 The utility of a state is the immediate
reward for that state plus the expected
utility of the next state, provided the
agent chooses the optimal action
106
Bellman Equation

 For each state there is a Bellman equation

to compute its utility
 There are n states and n unknowns
 Solve the system using Linear Algebra?
 No! The max-operator that chooses the
optimal action makes the system nonlinear
 We must go for an iterative approach
107
Discounting
We have made a simplification on the way:
 The utility of a state sequence is often
defined as the sum of discounted rewards

with 0 δ γ δ 1 being the discount factor

 Discounting says that future rewards are
less significant than current rewards.
This is a natural model for many domains
 The other expressions change accordingly
108
Separability
We have made an assumption on the way:
 Not all utility functions (for state
sequences) can be used
 The utility function must have the
property of separability (a.k.a. station-
arity), e.g. additive utility functions:

 Loosely speaking: the preference between

two state sequences is unchanged over
different start states
109
Utility of a State
 The state utilities for our example

 Note that utilities are higher closer to the

goal as fewer steps are needed to reach it
110
Iterative Computation
Idea:

 The utility is computed iteratively:

 Optimal utility:
 Abort, if change in utility is below a
threshold
111
Value Iteration Example
 Calculate utility of the center cell

desired action = Up u=10

p=0.8
u=5 r=1 u=-8

p=0.1 p=0.1
u=1

Transition Model State space

(u=utility, r=reward)
114
Value Iteration Example

p=0.8

p=0.1 p=0.1

u=10

u=5 r=1 u=-8

u=1
115
Value Iteration Example
 In our example

(1,1) nr. of iterations →

 States far from the goal first accumulate

negative rewards until a path is found to
the goal
116
Convergence
 The condition in the
algorithm can be formulated by

 Different ways to detect convergence:

 RMS error: root mean square error
 Max error:
 Policy loss
117
Value Iteration
 Value Iteration finds the optimal solution
to the Markov Decision Problem!
 Converges to the unique solution of
the Bellman equation system
 Initial values for U' are arbitrary
 Proof involves the concept of contraction.
with B being
the Bellman operator (see textbook)
 VI propagates information through the
state space by means of local updates
119
Optimal Policy
 How to finally compute the optimal
policy? Can be easily extracted along
the way by

 Note: U(s) and R(s) are quite different

quantities. R(s) is the short-term reward
for being in s, whereas U(s) is the long-
term reward from s onwards

120
Summary
 Robust navigation requires combined path
planning & collision avoidance.
 Approaches need to consider robot's kinematic
constraints and plans in the velocity space.
 Combination of search and reactive techniques
show better results than the pure DWA in a
variety of situations.
 Using the 5D-approach the quality of the
trajectory scales with the performance of the
underlying hardware.
 The resulting paths are often close to the
optimal ones.
121
Summary
 Planning is a complex problem.
 Focus on subset of the configuration space:
 road maps,
 grids.
 Sampling algorithms are faster and have a
trade-off between optimality and speed.
 Uncertainty in motion leads to the need of
Markov Decision Problems.

122
What’s Missing?

 More complex vehicles (e.g., cars, legged

robots, manipulators, …).
 Moving obstacles, motion prediction.
 High dimensional spaces.
 Heuristics for improved performances.
 Learning.

123
Exploration

 The approaches seen so far are purely

passive

 Given an unknown environment, how can

we control multiple robots to efficiently
learn a map?

 By reasoning about control, the mapping

process can be made much more effective
Decision-Theoretic Formulation of
Exploration

reward cost
(expected information gain) (path length)
Single Robot Exploration
 Frontiers between free space and unknown
areas are potential target locations
 Going to frontiers will gain information

 Select the target that minimizes a cost

function (e.g. travel time / distance /…)
Multiple Robots
Multiple robots: how to control them
to optimize the performance of the
whole team?

 Exploration
 Path planning
 Action planning …

6
Exploration: The Problem

Given:
 Unknown environment
 Team of robots
Task:
 Coordinate the robots to
efficiently learn a complete
map of the environment
Complexity:
 Exponential in the number of robots

7
Example

Robot 1: Robot 2:

8
Levels of Coordination
 No exchange of information

 Implicit coordination (uncoordinated):

Sharing a joint map [Yamauchi et.al, 98]
 Communication of the individual maps and poses
 Central mapping system

 Explicit coordination: Improve

assignment of robots to target points
 Communication of the individual maps and poses
 Central mapping system
 Central planner for target point assignment

9
Realizing Explicit Coordination
for Multi-Robot Exploration
 Robots share a common map
 Frontiers between free space and unknown
areas are potential target locations
 Find a good assignment of frontier locations
to robots to minimize exploration time and
maximize information gain
Key Ideas
1. Choose target locations at the frontier to
the unexplored area by trading off the
expected information gain and travel
costs.
2. Reduce utility of target locations whenever
they are expected to be covered by
another robot.
3. Use on-line mapping and localization to
compute the joint map.

11
The Coordination Algorithm
(informal)

1. Determine the frontier cells.

2. Compute for each robot the cost for reaching each
frontier cell.
3. Choose the robot with the optimal overall
evaluation and assign the corresponding target
point to it.
4. Reduce the utility of the frontier cells visible from
that target point.
5. If there is one robot left go to 3.

12
The Coordination Algorithm

13
Estimating the Visible Area
Distances measured
during exploration:

Resulting probability
of measuring at least
distance d:

14
Application Example

First robot: Second robot:

15
Multi-Robot Exploration and
Mapping of Large Environments

16
Resulting Map

43m

62m 17
Further Application

18
Typical Trajectories in an Office
Environment
Implicit coordination: Explicit coordination:

19
Exploration Time

20
Simulation Experiments

Implicitly coordinated: Explicitly coordinated:

21
Optimizing Assignments
 The current system performs a greedy
assignment of robots to target locations

 What if we optimize the assignment?

22
Optimizing Assignment Algorithm

One approach: randomized optimization

of assignments.
23
General Idea for Optimization
1. Start with an initial assignment
2. Select a pair of robot/target point
assignments
3. If the evaluation improves we swap the
assignments

. Variants:
 Accept lower evaluations with a certain but
over time decreasing probability
 Perform random restarts
Other Coordination Techniques
 Hungarian Method:
 Optimal assignment of jobs to machines given a
fixed cost matrix.
 Similar results that the presented coordination
technique.

 Market economy-guided approaches:

 Robots trade with targets.
 Computational load is shared between the robots

25
Exploration Time

26
Summary on Exploration
 Efficient coordination leads to reduced exploration
times
 In general exponential in the team size
 Efficient polynomial approximations
 Distributing the robots over the environment is
key to efficiency
 Methods trade off the cost of an action and the
expected utility of reaching the corresponding
frontier (target location)

27
Other Problems
 Unknown starting locations
 Exploration under position uncertainty
 Limited communication abilities
 Efficient exchange of information
 …

28
 Tasks of Mobile Robots
SLAM
mapping localization

integrated
approaches

active
localization
exploration

path planning
2
Exploration and SLAM
 SLAM is typically passive, because it
consumes incoming sensor data
 Exploration actively guides the robot to
cover the environment with its sensors
 Exploration in combination with SLAM:
Acting under pose and map uncertainty
 Uncertainty should/needs to be taken into
account when selecting an action

3
Mapping with Rao-Blackwellized

Particle Filter (Brief Summary)

 Each particle represents a possible
trajectory of the robot
 Each particle
 maintains its own map and
 updates it upon “mapping with known
poses”
 Each particle survives with a probability
proportional to the likelihood of the
observations relative to its own map

4
 Factorization Underlying
Rao-Blackwellized Mapping
poses map observations & odometry

Mapping with known poses

Particle filter representing trajectory hypotheses

5
Example: Particle Filter for Mapping

3 particles

map of particle 1 map of particle 2

map of particle 3 6
Combining Exploration and SLAM

SLAM
mapping localization

integrated
approaches

active
localization
exploration

path planning
8
Exploration

 SLAM approaches seen so far are

purely passive
 By reasoning about control, the
mapping process can be made
much more effective
 Question: Where to move next?

9
Where to Move Next?

10
Decision-Theoretic Approach

 Learn the map using a Rao-Blackwellized

particle filter
 Consider a set of potential actions
 Apply an exploration approach that
minimizes the overall uncertainty

Utility = uncertainty reduction - cost

11
The Uncertainty of a Posterior
 Entropy is a general measure for the
uncertainty of a posterior

 Conditional Entropy

13
Mutual Information
 Expected Information Gain or Mutual
Information = Expected Uncertainty
Reduction

14
Entropy Computation

15
The Uncertainty of the Robot
 The uncertainty of the RBPF:

trajectory particle map

uncertainty weights uncertainty
Computing the Entropy of the
Map Posterior
Occupancy Grid map m:

map
grid cells probability that the
uncertainty
cell is occupied
17
Map Entropy

probability
Low entropy

occupied free

probability
Low entropy

occupied free

probability

High entropy

occupied free

The overall entropy is the sum of the individual entropy values

18
Computing the Entropy of the
Trajectory Posterior

1. High-dimensional Gaussian

reduced rank for sparse particle sets

2. Grid-based approximation

for sparse particle clouds

19
Approximation of the
Trajectory Posterior Entropy
Average pose entropy over time:

20
Mutual Information
 The mutual information I is given by the
expected reduction of entropy in the belief

action to be carried out

“uncertainty of the filter” –

“uncertainty of the filter
after carrying out action a”
Integrating Over Observations
 Computing the mutual information requires
to integrate over potential observations

potential observation
sequences
Approximating the Integral
 The particle filter represents a posterior
about possible maps

map of particle 1 map of particle 2 map of particle 3

 Approximating the Integral
 The particle filter represents a posterior
about possible maps
 Simulate laser measurements in the maps
of the particles

measurement sequences likelihood

simulated in the maps (particle weight)
Simulating Observations
 Ray-casting in the map of each particle
to generate observation sequences

map of particle i
The Utility
 We take into account the cost of an action:
mutual information utility U

 Select the action with the highest utility

Focusing on Specific Actions
To efficiently sample actions we consider
 exploratory actions (1-3)
 loop closing actions (4) and
 place revisiting actions (5)

27
Dual Representation
for Loop Detection

 Trajectory graph (“topological map”)

stores the path traversed by the robot
 Occupancy grid map represents the space
covered by the sensors
 Loops correspond to long paths in the
trajectory graph and short paths in the grid
map

28
Example: Trajectory Graph

29
Application Example

high pose uncertainty

30
Example: Possible Targets

31
Example: Evaluate Targets

32
Example: Move Robot to Target

33
Example: Evaluate Targets

34
Example: Move Robot

… continue …
35
Example: Entropy Evolution

36
Comparison
Map uncertainty only:

After loop closing action:

37
Real Exploration Example

Selected
target
location

38
Corridor Exploration

 The decision-theoretic approach leads to intuitive

behaviors: “re-localize before getting lost”
 Some animals show a similar behavior
(dogs marooned in the tundra of north Russia)

39
Summary
 A decision-theoretic approach to
exploration in the context of RBPF-SLAM
 The approach utilizes the factorization of
the Rao-Blackwellization to efficiently
calculate the expected information gain
 Reasons about measurements obtained
along the path of the robot
 Considers a reduced action set consisting
of exploration, loop-closing, and place-
revisiting actions

40