wolfram burgard university of freiburg department of computer science autonomous intelligent systems...
TRANSCRIPT
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