Tutorial on Probabilistic
Techniques for Robot
Navigation

Wolfram Burgard

1Probabilistic Techniques in
Robotics
 Perception
 Action= state estimation= utility maximization

Key Questions
 Representation
 Maximization
especially in the context of higher
dimensions

Dimensions ofMobile Robot Navigation
SLAM

mapping

localization

integrated
approaches
activelocalization
exploration

motion control

Topics in this Talk
 State estimation
 Localization
 Mapping SLAM
 Exploration
 Applications

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



0 ≤ P(A) ≤ 1

 P(True) = 1

P(False) = 0

 P(A ∨ B) = P(A) + P(B) − P(A ∧ B)

A Closer Look at Axiom 3P(A ∨ B) = P(A) + P(B) − P(A ∧ B)
True

B

A

A∧ B

B

Bayes FormulaP(x, y) = P(x | y)P(y) = P(y | x)P(x)⇒P(y | x) P(x) likelihood ⋅ prior=P(x y) =P(y)evidenceRecursive Bayesian UpdatingP(zn | x, z1,…, zn − 1) P(x | z1,…, zn − 1)
P(x | z1,…, zn ) =P(zn | z1,…, zn − 1)
Markov assumption:zn is independent of z1,...,zn-1 given x

P(zn | x) P(x | z1,…, zn − 1)
P(x | z1,…, zn ) =P(zn | z1,…, zn − 1)
= η P(zn | x) P(x | z1,…, zn − 1)⎡ n⎤
= η1...n ⎢∏ P(zi | x)⎥ P(x)⎣ i=1⎦Modeling Actions
 To incorporate the outcome of anaction u into the current belief, we
use the conditional pdf
P(x|u,x) This term specifies the pdf that
executing u changes the statefrom x to x.

Example: Closing the door

46

State TransitionsP(x|u,x) for u = close door:

0.9
0.1

open

closed1

0
If the door is open, the action closedoor succeeds in 90% of all cases

Integrating the Outcome of Actions
Continuous case:P(x | u) = ∫ P(x | u, x ')P(x ')dx 'Discrete case:P(x | u) = ∑ P(x | u, x ')P(x ')Markov Assumption

p(zt | x0:t , z1:t−1, u1:t ) = p(zt | xt )p(xt | x1:t−1, z1:t , u1:t ) = p(xt | xt−1, ut )Underlying Assumptions
 Static world
 Independent noise Perfect model, no approximation errors

Bayes Filtersz = observationu = action
x = state

Bel(xt ) = P(xt | u1, z1, …, ut , zt )Bayes

= η P(zt | xt , u1, z1, …, ut ) P(xt | u1, z1, …, ut )Markov= η P(zt | xt ) P(xt | u1, z1, …, ut )Total prob.

= η P(zt | xt ) ∫ P(xt | u1, z1, …, ut , xt−1 )P(xt−1 | u1, z1, …, ut ) dxt−1

Markov= η P(zt | xt ) ∫ P(xt | ut , xt−1 ) P(xt−1 | u1, z1, …, ut ) dxt−1

Markov= η P(zt | xt ) ∫ P(xt | ut , xt−1 ) P(xt−1 | u1, z1, …, zt−1 ) dxt−1= η P(zt | xt ) ∫ P(xt | ut , xt−1 ) Bel(xt−1 ) dxt−1

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

Algorithm Bayes_filter( Bel(x),d ):η=03.

If d is a perceptual data item z then

1.

4.
5.
6.
7.
8.
9.

For all x do

Bel ' ( x) = P( z | x) Bel ( x)
η = η + Bel ' ( x)For all x do

Bel '(x) = η −1Bel '(x)Else if d is an action data item u then

10.
11.

For all x do

12.

Return Bel(x)Bel '(x) = ∫ P(x | u, x ') Bel(x ') dx '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)

Dimensions ofMobile Robot Navigation
SLAM

mapping

localization

integrated
approaches
activelocalization
exploration

motion control

Probabilistic Localization

There are Different
Representations
 Kalman filters Multi-hypothesis tracking
 Grid-based representations
 Topological approaches
 Particle filters

Idea of a Particle Filter
Represent the posterior by a set of weighted samples.

State hypothesis

Importance weight Particles are propagated according to the motion
model They are weighted according to the observation
likelihood
 They survive with a probability proportional to theirimportance weightRepresentations: Particle Filter

1. Draw

from

2. Draw

from

3. Importance factor for

4. Re-sample

Mobile Robot Localization
with Particle Filters

Sensor Update

Robot Motion & ResamplingParticle Filter Algorithm
1. Algorithm particle_filter( St-1, ut-1 zt):
2. St = ∅,η =0

3. For i = 1… n

Generate new samples

4.

Sample index j(i) from the discrete distribution given by wt-1

5.

Sample xti from p( xt | xt −1 , ut −1 ) using xtj−(1i ) and ut −16.

wti = p ( zt | xti )Compute importance weight7.

η = η + wti

Update normalization factor

8.

S t = S t ∪ {< xti , wti >}Insert9. For i = 1… n
10.

wti = wti / ηNormalize weights

Resampling
wn
Wn-1

wn

w1
w2

Wn-1

w3

 Roulette wheel
 Binary search, n log n

w1
w2

w3

 Stochastic universal sampling Systematic resampling Linear time complexity
 Easy to implement, low varianceSensor Models for Proximity
Sensors

 The central task is to determine P(z|x), i.e., theprobability of a measurement z given that the
robot is at position x.
 Question: Where do the probabilities come from?
 Approach: Lets try to explain a measurement.

Conditional IndependenceK

P ( z | x, m) = ∏ P ( z k | x, m)
k =1

Beam-based Proximity Model
Measurement noise

0

zexp

Phit (z | x, m) = ηUnexpected obstacles

zmax

1
e
2π b

1 (z−zexp )
−
2
b

2

0

zexp

⎧⎪
η λ e− λz
Punexp (z | x, m) = ⎨
⎪⎩
0

zmax

⎫⎪
⎬
otherwise ⎪⎭z < zexp

Beam-based Proximity Model
Random measurement

0

zexp

Prand (z | x, m) = η

zmax

1
zmax

Max range0zexp

Pmax (z | x, m) = ηzmax

1
zsmallResulting Mixture DensityT⎞ ⎛ P (z | x, m) ⎞
⎛ α
hit
⎟
⎟ ⎜ hit⎜⎜ α unexp ⎟ ⎜ Punexp (z | x, m) ⎟P(z | x, m) = ⎜⎟⎟ ⋅⎜⎜ α max ⎟ ⎜ Pmax (z | x, m) ⎟⎟ ⎜ P (z | x, m) ⎟
⎜ α
rand
⎠
⎠ ⎝ rand⎝How can we determine the model parameters?

Odometry Motion ModelRobot moves from x , y , θ

to x ' , y ' , θ ' .



Odometry information u = δ rot1 , δ rot 2 , δ trans .

δ trans = ( x '− x ) 2 + ( y '− y ) 2

δ rot1 = atan2( y '− y , x '− x ) − θδ rot 2 = θ '−θ − δ rot1

δ rot 2x ' , y ' ,θ 'x , y,θ

δ rot1

δ trans

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

Sampling from Our Motion
Model

Start

MCL: Global Localization(Sonar)Vision-based Localization

Why Navigation is Relevant:Self-Driving Cars

Precise Localization andPositioning for Mobile Robots

Dimensions ofMobile Robot Navigation
SLAM

mapping

localization

integrated
approaches
activelocalization
exploration

motion control

Occupancy Grid Maps
 Introduced by Moravec and Elfes in 1985
 Represent environment by a grid.
 Estimate the probability that a location is
occupied by an obstacle.
 Key assumptions[ xy]m Occupancy of individual cells t is independent

Bel(mt ) = P(mt | u1, z2 …, ut−1, zt )= ∏ Bel(m

[ xy]
t

x,y

 Robot positions are known!

)

Updating Occupancy Grid Maps
 Update the map cells using theinverse sensor model
−1

[ xy]
[ xy]
[ xy]
⎛
⎞
|
z
,
u
P
m
1−
P
m
Bel
m
(
)
(
)
(
)
t
t
t−1
t
t−1
⎟
Bel ( mt[ xy] ) = 1− ⎜1+⋅⋅⎜ 1− P ( m[ xy] | z , u ) P ( m[ xy] ) 1− Bel ( m[ xy] ) ⎟
t
t
t−1
t
t−1 ⎠
⎝

 Or use the log-odds representation[ xy]
B ( mt[ xy] ) = log odds ( mt[ xy] | zt , u t−1 ) − log odds ( mt[ xy] ) + B ( mt−1
)

B (m

[ xy]
0

) := log odds(m )[ xy]0⎛ P ( x) ⎞⎟⎟odds(x) := ⎜⎜⎝ 1− P ( x ) ⎠Typical Sensor Modelfor Ultrasound Sensors
P ( mt[ xy] | zt , u t−1 ) as a mixture of a linear function

and a Gaussian:Incremental Updatingof Occupancy Grids (Example)

Resulting Map Obtained with
Ultrasound Sensors

3D Occupancy Map (OctoMap)
Freiburg computer science campus(292 x 167 x 28 m³, 20 cm resolution)

Dimensions ofMobile Robot Navigation
SLAM

mapping

localization

integrated
approaches
activelocalization
exploration

motion control

Simultaneous Localization and
Mapping (SLAM) To determine its position, the robot
needs a map.
 During mapping, the robot needs toknow its position to learn aconsistent model Simultaneous localization andmapping (SLAM) is a
chicken and egg problemWhy SLAM is Hard: Raw Odometry

Scan Matching
Maximize the likelihood of the t-th poseand map relative to the pose and themap at t-1.

current measurement

robot motion

map constructed so far

Mapping using Scan MatchingProbabilistic Formulationof SLAMn=axb dimensions
three dimensions

map m

A Graphical Model for SLAM
u0
x0

u1

ut-1

x1

x2

z1

z2

...

xt

m

zt
[Murphy et al., 99]

Rao-Blackwellized Particle
Filters for SLAM
Observation:
Given the true trajectory of the robot, we canefficiently compute the map(mapping with known poses).
Idea:
 Use a particle filter to represent potential
trajectories of the robot.
 Each particle carries its own map.
 Each particle survives with a probability that is

proportional to the likelihood of the observation
given that particle and its map.
[Murphy et al., 99]

Factorization Underlying
Rao-Blackwellization

Mapping with known poses
Particle filter representing trajectory hypotheses

Particle Filter Realization
3 particles

map of particle 1

map of particle 2

map of particle 3

Challenge
Reduction of the number of particles.
Approaches:
 Focused proposal distributions(keep the samples in the right place) Adaptive re-sampling(avoid depletion of relevant particles)Graphical Model for Mappingwith Improved Odometry
u0 ... uk-1
z1 ... z k-1

scan matchingx0

uk ... u2k-1
zk+1...z2k-1

u n·k... u(n+1)·k-1
z n·k+1... z(n+1)·k-1

...

u'1

u'2

...

u'n

xk

x2k

...

x n·k

zk

z2k

...

z n·k

m

Motion Model for Scan Matchingmeasured pose

Raw Odometry
Scan Matching

β'

β

d'
initial posedαfinal pose

α'

path

Incorporating the Current
Measurement
End of a corridor:

Free space:

Corridor:Application Example

Loop ClosureApplication Example

Loop ClosureApplication Example

Number of Effective Particles

 Empirical measure of how well the goal distribution
is approximated by samples drawn from the
proposal.

 neff describes the variance of the particle weights neff is maximal for equal weights. In this case, thedistribution is close to the proposal.

 We only re-sample when neff drops below a given
threshold (n/2)

Typical Evolution of neff
visiting newareas

closing thefirst loop

visiting
known areas
second loop closureMap of the Intel Lab 15 particles
 four times fasterthan real-time
P4, 2.8GHz
 5cm resolution
during scanmatching
 1cm resolution in
final map

MIT Killian CourtGraph-Based SLAM Constraints connect the poses of therobot while it is moving
 Constraints are inherently uncertain

Robot pose

Constraint

Graph-Based SLAM Observing previously seen areas
generates constraints between nonsuccessive poses

Robot pose

Constraint

Graph-Based SLAM in a
Nutshell Every node in thegraph corresponds
to a robot position
and a laser
measurement
 An edge between
two nodes
represents a spatialconstraint between
the nodes
KUKA Halle 22, courtesy of P. Pfaff

Graph-Based SLAM in a
Nutshell Every node in thegraph corresponds
to a robot position
and a laser
measurement
 An edge between
two nodes
represents a spatialconstraint between
the nodes

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

Graph-Based SLAM in a
Nutshell Once we have the graph,
we determine the most
likely map by correcting
the nodes… like this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 amap based on the known
posesLeast Squares in General Approach for computing a solution foran overdetermined system
 “More equations than unknowns”
 Minimizes the sum of the squared
errors in the equations
 Standard approach to a large set ofproblems

Graphical Explanation

state(unknown)

predicted
measurements

realmeasurements

Error Function
 Erroris 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

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

Create an Edge If… (1) …the robot moves fromto
 Edge corresponds to odometry

The edge represents theodometry measurement

Create an Edge If… (2) …the robot observes the same part ofthe environment fromand from Construct a virtual measurement
about the position ofseen fromxi
Measurement fromxj
iMeasurement from

Create an Edge If… (2) …the robot observes the same part ofthe environment fromand from Construct a virtual measurement
about the position ofseen from

Edge represents the position of
seen
from
based on the observation

Pose Graphobservation
of
from

nodes
according to
the graph Goal:

edge

error

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 untilconvergenceExample: CS Campus Freiburg

Application: Sparse Pose
Adjustment

The KUKA Production Site

The KUKA Production Site

3D Map of the Stanford Parking
Garageapprox. 260MB

Application: Navigation with
the Autonomous Car Junior Task: reach a parking spot on theupper level of the garage.

Autonomous Parking

The

Robot

Navigation in Urban Scenes

Dimensions ofMobile Robot Navigation
SLAM

mapping

localization

activelocalization
exploration

motion control

Which Action to Take?

Naïve Approach to Combine
Exploration and Mapping
Learn the map using a Rao-Blackwellized
particle filter.
Apply an exploration approach thatminimizes the map uncertainty.

Disadvantage of the Naïve
Approach
Exploration techniques only consider the map
uncertainty for generating controls.
They avoid re-visiting known areas.
Data association becomes harder.
More particles are needed to learn a correctmap.

Effectively Calculating the Map
and Pose Uncertainty

pose uncertainty

map uncertainty

Goal
Integrated approach that considers
 exploratory actions, place revisiting actions, and loop closing actions

to control the robot.

Dual Representation
for Loop Detection
 Trajectory graph stores the pathtraversed by the robot.
 Grid map represents the space coveredby the sensors.
 Loops correspond to long paths in the
trajectory graph and short paths in thegeometric map.

Dual Representation
for Loop Detection

Application Example

Real Exploration Example

Considering Pose Uncertainty
during Exploration

Research Topics
 Large-scale maps
 Resource-constrained systems
 Data association
 Dynamic environments
 Continuous SLAM
 Semantics
 Efficiency of integrated problem
…

Summary Probabilistic methods are a powerfultool for realizing robust autonomous
systems.
 By reasoning about controls, thegiven algorithms can get even more
effective.
 Probabilistic approaches are highly
relevant for building robustnavigation systems