Wolfram Burgard University of Freiburg Department of Computer Science Autonomous Intelligent Systems burgard/ Probabilistic.

Wolfram Burgard

University of Freiburg

Department of Computer Science

Autonomous Intelligent Systems

http://www.informatik.uni-freiburg.de/~burgard/

Probabilistic Techniquesfor

Mobile Robot Navigation

Special Thanks to Frank Dellaert, Dieter Fox, Giorgio Grisetti, Dirk Haehnel, Cyrill Stachniss, Sebastian Thrun, …

Probabilistic Robotics

Robotics Today

“Humanoids”

Overcoming the uncanny valley

[Courtesy by Hiroshi Ishiguro]

Humanoid Robots

[Courtesy by Sven Behnke]

DARPA Grand Challenge

[Courtesy by Sebastian Thrun]

DCG 2007

Robot Projects: Interactive Tour-guides

Rhino: Albert:

Minerva:

Minerva

Robot Projects: Acting in the Three-dimensional World

Herbert:

Zora: Groundhog:

Nature of Data

Odometry Data Range Data

Probabilistic Techniques in Robotics

Perception = state estimation Action = utility maximization

Key Question How to scale to higher-dimensional

spaces

Pr(A) denotes probability that proposition A is true.

Axioms of Probability Theory

1)Pr(0 A

1)Pr( True

)Pr()Pr()Pr()Pr( BABABA

0)Pr( False

Bayes Formula

evidence

prior likelihood

)(

)()|()(

)()|()()|(),(

yP

xPxyPyxP

xPxyPyPyxPyxP

Normalization

)()|(

1)(

)()|()(

)()|()(

1

xPxyPyP

xPxyPyP

xPxyPyxP

x

yx

xyx

yx

yxPx

xPxyPx

|

|

|

aux)|(:

aux

1

)()|(aux:

Algorithm:

Simple Example of State Estimation

Suppose a robot obtains measurement z What is P(open|z)?

Causal vs. Diagnostic Reasoning

P(open|z) is diagnostic. P(z|open) is causal. Often causal knowledge is easier to

obtain. Bayes rule allows us to use causal

knowledge:

)()()|(

)|(zP

openPopenzPzopenP

count frequencies!

Example

P(z|open) = 0.6 P(z|open) = 0.3 P(open) = P(open) = 0.5

67.03

2

5.03.05.06.0

5.06.0)|(

)()|()()|(

)()|()|(

zopenP

openpopenzPopenpopenzP

openPopenzPzopenP

• z raises the probability that the door is open.

29

Combining Evidence

Suppose our robot obtains another observation z2.

How can we integrate this new information?

More generally, how can we estimateP(x| z1...zn )?

Recursive Bayesian Updating

),,|(

),,|(),,,|(),,|(

11

11111

nn

nnnn

zzzP

zzxPzzxzPzzxP

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

)()|(

),,|()|(

),,|(

),,|()|(),,|(

...1...1

11

11

111

xPxzP

zzxPxzP

zzzP

zzxPxzPzzxP

ni

in

nn

nn

nnn

Example: Second Measurement

P(z2|open) = 0.5 P(z2|open) = 0.6

P(open|z1)=2/3

625.08

5

31

53

32

21

32

21

)|()|()|()|(

)|()|(),|(

1212

1212

zopenPopenzPzopenPopenzP

zopenPopenzPzzopenP

• z2 lowers the probability that the door is open.

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?

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.

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.

Example: Closing the door

State Transitions

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

If the door is open, the action “close door” succeeds in 90% of all cases.

open closed0.1 1

0.9

0

Integrating the Outcome of Actions

')'()',|()|( dxxPxuxPuxP

)'()',|()|( xPxuxPuxP

Continuous case:

Discrete case:

Example: The Resulting Belief

)|(1161

83

10

85

101

)(),|(

)(),|(

)'()',|()|(

1615

83

11

85

109

)(),|(

)(),|(

)'()',|()|(

uclosedP

closedPcloseduopenP

openPopenuopenP

xPxuopenPuopenP

closedPcloseduclosedP

openPopenuclosedP

xPxuclosedPuclosedP

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:

),,,|()( 11 tttt zuzuxPxBel

},,,{ 11 ttt zuzud

Markov Assumption

Underlying Assumptions Static world Independent noise Perfect model, no approximation errors

),|(),,|( 1:11:11:1 ttttttt uxxpuzxxp )|(),,|( :11:1:0 tttttt xzpuzxzp

111 )(),|()|( ttttttt dxxBelxuxPxzP

Bayes Filters

),,,,|(),,,,,|( 111111 ttttttt uzzuxPuzzuxzP Bayes

z = observationu = actionx = state

),,,|()( 11 tttt zuzuxPxBel

Markov ),,,,|()|( 111 ttttt uzzuxPxzP

Markov11111 ),,,|(),|()|( ttttttttt dxuzuxPxuxPxzP

11111

1111

),,,,|(

),,,,,|()|(

tttt

tttttt

dxuzzuxP

xuzzuxPxzP

Total prob.

Markov111111 ),,,|(),|()|( tttttttt dxzzuxPxuxPxzP

Bayes Filter Algorithm

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

3. If d is a perceptual data item z then4. For all x do5. 6. 7. For all x do8.

9. Else if d is an action data item u then10. For all x do11.

12. Return Bel’(x)

)()|()(' xBelxzPxBel )(' xBel

)(')(' 1 xBelxBel

')'()',|()(' dxxBelxuxPxBel

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Bayes Filters are Familiar!

Kalman filters Discrete filters Particle filters Hidden Markov models Dynamic Bayesian networks Partially Observable Markov Decision Processes

(POMDPs)

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

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.

Dimensions of Mobile Robot Navigation

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

Probabilistic Localization

s’a

p(x|u,x’)

a

s’

laser data p(o|s,m)p(z|x)observation x

Localization with Bayes Filters

Localization with Sonars in an Occupancy Grid Map

Resulting Beliefs

What is the Right Representation?

Kalman filters

Multi-hypothesis tracking

Grid-based representations

Topological approaches

Particle filters

Set of N samples {<x1,w1>, … <xN,wN>} containing a state x and an importance weight w

Initialize sample set according to prior knowledge

For each motion u do: Sampling: Generate from each sample a new pose

according to the motion model

For each observation z do: Importance sampling: weigh each sample with the

likelihood

Re-sampling: Draw N new samples from the sample set according to the importance weights

Monte-Carlo Localization

Represent density by random samples

Estimation of non-Gaussian, nonlinear processes

Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter

Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]

Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al., 95]

Particle Filters

Mobile Robot Localization with Particle Filters

MCL: Sensor Update

PF: Robot Motion

Particle Filter Algorithm

1. Draw from

2. Draw from

3. Importance factor for

4. Re-sample

Beam-based Sensor Model

K

kk mxzPmxzP

1

),|(),|(

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

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.

Beam-based Proximity Model

Measurement noise

zexp zmax0

b

zz

hit eb

mxzP

2exp )(

2

1

2

1),|(

otherwise

zzmxzP

z

0

e),|( exp

unexp

Unexpected obstacles

zexp zmax0

Beam-based Proximity Model

Random measurement Max range

max

1),|(

zmxzPrand

smallzmxzP

1),|(max

zexp zmax0zexp zmax

0

Resulting Mixture Density

),|(

),|(

),|(

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzP

mxzP

mxzP

mxzP

mxzP

T

How can we determine the model parameters?

Raw Sensor DataMeasured distances for expected distance of 300 cm.

Sonar Laser

Approximation Maximize log likelihood of the data

Search space of n-1 parameters. Hill climbing Gradient descent Genetic algorithms …

Deterministically compute the n-th parameter to satisfy normalization constraint.

)|( expzzP

Approximation Results

Sonar

Laser

300cm 400cm

Example

z P(z|x,m)

Odometry Model

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

• Robot moves from to .

• Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

Sampling from a Motion Model

Start

MCL: Global Localization (Sonar)

Using Ceiling Maps for Localization

[Dellaert et al. 99]

Vision-based Localization

P(s|x)

h(x)s

MCL: Global Localization Using Vision

Vision-based Localization

Adaptive Sampling

• Idea: • Assume we know the true belief.

• Represent this belief as a multinomial distribution.

• Determine number of samples such that we can guarantee that, with probability (1- ), the KL-distance between the true posterior and the sample-based approximation is less than .

• Observation: • For fixed and , number of samples only depends on

number k of bins with support:

KLD-sampling

3

12

)1(9

2

)1(9

21

2

1)1,1(

2

1

zkk

kkn

MCL: Adaptive Sampling (Sonar)

Dimensions of Mobile Robot Navigation

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

Occupancy Grid Maps

Store in each cell of a discrete grid the probability that the corresponding area is occupied.

Approximative

Do not require features

All cells are considered independent

Their probabilities are updated using the binary Bayes filter

Updating Occupancy Grid Maps

Update the map cells using the inverse sensor model

Or use the log-odds representation

1

][1

][1

][

][

1][

1][

][

1

1

,|1

,|11

xy

t

xyt

xyt

xyt

ttxy

t

ttxy

txyt mBel

mBel

mP

mP

uzmP

uzmPmBel

1][][ ,|log tt

xyt

xyt uzmoddsmB )(log: ][][ xy

txy

t moddsmB

xP

xPxodds

1:)(

][log xytmodds

][1

xytmB

Typical Sensor Model for Occupancy Grid Maps

Combination of a linear function and a Gaussian:

Incremental Updating of Occupancy Grids (Example)

Resulting Map Obtained with Ultrasound Sensors

Resulting Occupancy and Maximum Likelihood Map

The maximum likelihood map is obtained by clipping the occupancy grid map at a threshold of 0.5

Occupancy Grids: From scans to maps

Tech Museum, San Jose

CAD map occupancy grid map

Dimensions of Mobile Robot Navigation

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

Simultaneous Localization and Mapping (SLAM)

To determine its position, the robot needs a map.

During mapping, the robot needs to know its position to learn a consistent model

Simultaneous localization and mapping (SLAM) is a “chicken and egg problem”

Why SLAM is Hard: Raw Odometry

Why SLAM is Hard: Ambiguity

Start

End

Same position

[Courtesy of Eliazar & Parr]

Probabilistic Formulation of SLAM

n=a£b dimensions

map m

three dimensions

Key Question/Problem

How to maintain multiple map and pose hypotheses during mapping?

Ambiguity caused by the data association problem.

A Graphical Model for SLAM

m

x

z

u

x

z

u

2

2

x

z

u

... t

t

x 1

1

0

10 t-1

Techniques for Generating Consistent Maps

Scan matching (online)

Probabilistic mapping with a single map and a posterior about poses Mapping + Localization (online)

EKF SLAM (online, mostly landmarks or features only)

EM techniques (offline)

Lu and Milios (offline)

Scan Matching

Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map.

)ˆ,|( )ˆ ,|( maxargˆ 11]1[

ttt

ttt

xt xuxpmxzpx

t

robot motioncurrent measurement

map constructed so far

Scan Matching Example

Rao-Blackwellized Particle Filters for SLAMObservation:

Given the true trajectory of the robot, we can efficiently 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

Particle filter representing trajectory hypotheses

Mapping with known poses

Example

map of particle 1 map of particle 3

map of particle 2

3 particles

Limitations

A huge number of particles is required.

This introduces enormous memory and computational requirements.

It prevents the application of the approach in realistic scenarios.

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)

Motion Model for Scan Matching

'

'

d'

final pose

d

measured pose

initial pose

path

Raw OdometryScan Matching

Graphical Model for Mapping with Improved Odometry

m

z

kx

1u'

0uzk-1

...1z ...

uk-1 ...k+1z

ukzu2k-1

2k-1...

x0

k

x2k

z2k

...

u'2u' n

...

xn·k

zu u(n+1)·k-1n·k

n·k+1

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

n·kz

...

...

Application Example

The Optimal Proposal Distribution

For lasers is extremely peaked and dominates the product.

[Doucet, 98]

We can safely approximate by a constant:

Resulting Proposal Distribution

Gaussian approximation:

Resulting Proposal Distribution

Approximate this equation by a Gaussian:

Sampled points around the maximum

maximum reported by a scan matcher

Gaussian approximation

Draw next generation of samples

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

Computing the Importance Weight

Sampled points around the maximum of the observation likelihood

Improved Proposal

The proposal adapts to the structure of the environment

Selective Resampling

Resampling is dangerous, since important samples might get lost(particle depletion problem)

In case of suboptimal proposal distributions resampling is necessary to achieve convergence.

Key question: When should we resample?

Effective Number of 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, the

distribution is close to the proposal

Resampling with Neff

If our approximation is close to the proposal, no resampling is needed

We only resample when Neff drops below a given threshold (N/2)

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

Typical Evolution of Neff

visiting new areas closing the

first loop

second loop closure

visiting known areas

Map of the Intel Lab

15 particles

four times faster than real-time(P4, 2.8GHz)

5cm resolution during scan matching

1cm resolution in final map

Outdoor Campus Map

30 particles

250x250m2

1.75 km (odometry)

20cm resolution during scan matching

30cm resolution in final map

128

MIT Kilian Court

129

MIT Kilian Court

Dimensions of Mobile Robot Navigation

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

The approaches seen so far are purely passive.

By reasoning about control, the mapping process can be made much more effective.

Exploration

Decision-Theoretic Formulation of Exploration

reward (expected information gain)

cost (path length)

Exploration with Known Poses

Move to the place that provides you with the best tradeoff between information gathered and cost of reaching that point.

Exploration With Known Poses

Real robot, ultrasounds only:

Dimensions of Mobile Robot Navigation

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

Where to Move Next?

Naïve Approach to Combine Exploration and Mapping

Learn the map using a Rao-Blackwellized particle filter.

Apply an exploration approach that minimizes 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 correct map.

Application Example

Path estimated by the particle filter

True map and trajectory

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

Application Example

Real Exploration Example

Comparison

Map and pose uncertainty:

Map uncertainty only:

Example: Entropy Evolution

Quantitative Results

Localization error:

map and pose uncertainty

avg.

loca

lizati

on e

rror

[m]

map uncertainty

Corridor Exploration

Summary

Probabilistic techniques are a powerful tool to deal with a variety of problems in mobile robot navigation.

By actively controlling mobile robots one can more effectively solve high-dimensional state estimation problems.

Questions?No Arrows Left …

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches