Sebastian ThrunCarnegie Mellon & Stanford
Wolfram BurgardUniversity of Freiburg
and Dieter FoxUniversity of Washington
Probabilistic Algorithms forMobile Robot Mapping
LEP: Adapted, combining partially with Thrun’s Tutorial
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Based on the paper
A Real-Time Algorithm for Mobile Robot MappingWith Applications to Multi-Robot and 3D Mapping
Best paper award at 2000 IEEE International Conference on Roboticsand Automation (~1,100 submissions)
Sponsored by DARPA (TMR-J.Blitch, MARS-D.Gage, MICA-S.Heise) and NSF (ITR(2), CAREER-E.Glinert, IIS-V.Lumelsky)
Other contributors: Yufeng Liu, Rosemary Emery, Deepayan Charkrabarti, Frank Dellaert, Michael Montemerlo, Reid Simmons, Hugh Durrant-Whyte, Somajyoti Majnuder, Nick Roy, Joelle Pineau, …
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems
3D Mappingwith EM
Real TimeHybrid
ExpectationMaximization
SLAM(Kalman filters)
Motivation
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Museum Tour-Guide Robots
With: Greg Armstrong, Michael Beetz, Maren Benewitz, Wolfram Burgard, Armin Cremers, Frank Dellaert, Dieter Fox, Dirk Haenel, Chuck Rosenberg, Nicholas Roy, Jamie Schulte, Dirk Schulz
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Nursebot Initiative
With: Greg Armstrong, Greg Baltus, Jacqueline Dunbar-Jacob, Jennifer Goetz, Sara Kiesler, Judith Matthews, Colleen McCarthy, Michael Montemerlo, Joelle Pineau, Martha Pollack, Nicholas Roy, Jamie Schulte
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Localization Problem
Estimate robot’s coordinates s=(x,y,) from sensor data• Position tracking (error bounded)• Global localization (unbounded error)• Kidnapping (recovery from failure)
Ingemar Cox (1991): “Using sensory information to locate the robot in its environment is the most fundamental problem to provide a mobile robot with autonomous capabilities.”
see also [Borenstein et al, 96]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping: The Problem
Concurrent Mapping and Localization (CML) Simultaneous Localization and Mapping (SLAM)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping: The Problem
Continuous variables High-dimensional (eg, 1,000,000+ dimensions) Multiple sources of noise Simulation not acceptable
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Milestone Approaches
Mataric 1990
Kuipers et al 1991
Elfes/Moravec 1986
Lu/Milios/Gutmann 1997
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Mapping
Konolige et al, 2001 Teller et al, 2000
Moravec et al, 2000
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Take-Home Message
Mapping is the
holy grail in
mobile robotics.
Every state-of-the-art
mapping algorithm
is probabilistic.
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Robots are Inherently Uncertain
Uncertainty arises from four major factors:– Environment stochastic, unpredictable– Robot stochastic– Sensor limited, noisy– Models inaccurate
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Probabilistic Robotics
)(
)()|()|(
bp
apabpbap
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Probabilistic Robotics
Key idea: Explicit representation of uncertainty (using the calculus of probability theory)
Perception = state estimation Action = utility optimization
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Advantages of Probabilistic Paradigm Can accommodate inaccurate models Can accommodate imperfect sensors Robust in real-world applications Best known approach to many hard robotics
problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Pitfalls
Computationally demanding False assumptions Approximate
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems
3D Mappingwith EM
Real TimeHybrid
ExpectationMaximization
Motivation
SLAM(Kalman filters)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Localization Problem
Estimate robot’s coordinates s=(x,y,) from sensor data• Position tracking (error bounded)• Global localization (unbounded error)• Kidnapping (recovery from failure)
Ingemar Cox (1991): “Using sensory information to locate the robot in its environment is the most fundamental problem to provide a mobile robot with autonomous capabilities.”
see also [Borenstein et al, 96]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
s
p(s)
Probabilistic Localization
[Simmons/Koenig 95][Kaelbling et al 96][Burgard et al 96]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Bayes Filters)|()( 0 ttt dspsb
1011011 ),,|(),,,|()|( tttttttt dsoaspoasspsop
1111 )(),|()|( ttttttt dssbasspsop
),,,,|( 011 ooaosp tttt
),,,|(),,,,|( 011011 ooaspooasop ttttttt Bayes
),,,|()|( 011 ooaspsop ttttt Markov
110111 )|(),|()|( tttttttt dsdspasspsop
[Kalman 60, Rabiner 85]
d = datao = observationa = actiont = times = state
Markov
1021111 ),,|(),|()|( ttttttttt dsoaospasspsop
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Markov Assumption)|(),,,,|( 011 tttttt sopooasop
),|(),,,,|( 110111 ttttttt asspooassp
)|,()|,,()|,,,,( 0101 ttttTtttT soapsoopsoaoop
} used above
Knowledge of current state renders past, future independent:
• “Static World Assumption”• “Independent Noise Assumption”
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Bayes Filters are Familiar to AI!
Kalman filters Hidden Markov Models Dynamic Bayes networks Partially Observable Markov Decision Processes
(POMDPs)
1111 )(),|()|()( tttttttt dssbasspsopsb
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Localization With Bayes Filters1111 )(),|()|()( tttttttt dssbasspsopsb
1111 )|(),,|(),|()|( tttttttt dsmsbmasspmsopmsb
map m
s’a
p(s|a,s’,m)
a
s’
laser data p(o|s,m)p(o|s,m)observation o
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Kalman filter
[Schiele et al. 94], [Weiß et al. 94], [Borenstein 96], [Gutmann et al. 96, 98], [Arras 98]
Piecewise constant(metric, topological)
[Nourbakhsh et al. 95], [Simmons et al. 95], [Kaelbling et al. 96], [Burgard et al. 96], [Konolige et al. 99]
Variable resolution(eg, trees)
[Burgard et al. 98]
Multi-hypothesis
[Weckesser et al. 98], [Jensfelt et al. 99]
What is the Right Representation?
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Idea: Represent Belief Through Samples
• Particle filters[Doucet 98, deFreitas 98]
• Condensation algorithm[Isard/Blake 98]
• Monte Carlo localization[Fox/Dellaert/Burgard/Thrun 99]
1111 )|(),,|(),|()|( tttttttt dsmsbmasspmsopmsb
Monte Carlo Localization (MCL)
MCL: Importance Sampling)(),|()( tttt sbmsopsb
),|( msop tt
tttttt ssbmsaspsb d)(),,|()( 11
MCL: Robot Motion
motion
)|( loP t
MCL: Importance Sampling)(),|()( 1111 tttt sbmsopsb
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
1111 )|(),,|(),|()|( tttttttt dsmsbmasspmsopmsb
Particle Filters
draw s(i)t1 from b(st1)
draw s(i)t from p(st | s(i)
t1,at1,m)
Represents b(st) by set of weighted particles {s(i)t,w(i)
t}
Importance factor for s(i)t:
ondistributi proposal
ondistributitarget )( itw
),|( )( msop itt
)(),,|(
)(),,|(),|()(11
)(1
)(
)(11
)(1
)()(
itt
it
it
itt
it
it
itt
sBelmassp
sBelmasspmsop
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Monte Carlo Localization
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Performance Comparison
Monte Carlo localizationMarkov localization (grids)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Monte Carlo Localization
Approximate Bayes Estimation/Filtering– Full posterior estimation– Converges in O(1/#samples) [Tanner’93]– Robust: multiple hypothesis with degree of belief– Efficient: focuses computation where needed– Any-time: by varying number of samples– Easy to implement
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Pitfall: The World is not Markov!
99.0)(),|()short is (?
ttt
oo
tt dssbdomsopopt [Fox et al 1998]
Distance filters:
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Probabilistic Localization: Lessons Learned
Probabilistic Localization = Bayes filters
Particle filters: Approximate posterior by random samples
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Problem: Concurrent Mapping and Localization
70 m
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Concurrent Mapping and Localization Is a chicken-and-egg problem
– Mapping with known poses is “simple”– Localization with known map is “simple”– But in combination, the problem is hard!
Today’s best solutions are all probabilistic!
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Posterior estimationwith known poses:Occupancy grids
Posterior estimationwith known poses:Occupancy grids
Maximum likelihood:ML*
Maximum likelihood:ML*
Maximum likelihood:EM
Maximum likelihood:EM
Posterior estimation:EKF (SLAM)
Posterior estimation:EKF (SLAM)
Mapping: Outline
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping as Posterior Estimation
1111 )(),|()|()( tttttttt dssbasspsopsb
1111111 ),(),,|,(),|(),( tttttttttttttt dmdsmsbamsmspmsopmsb
1 tt mmAssume static map
1111 ),(),,|(),|(),( tttttttt dsmsbmasspmsopmsb
1111 ),(),|(),|(),( tttttttt dsmsbasspmsopmsb
[Smith, Self, Cheeseman 90, Chatila et al 91, Durrant-Whyte et al 92-00, Leonard et al. 92-00]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Kalman Filters
N-dimensional Gaussian
Can handle hundreds of dimensions
2
2
2
2
2
2
2
1
21
21
21
21
2222221
1111211
,),(
yxlll
yyxyylylyl
xxyxxlxlxl
lylxllllll
lylxllllll
lylxllllll
Nt
N
N
N
NNNNNN
N
N
y
x
l
l
l
msb
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping
By: Louis L. Whitcomb, Johns Hopkins University
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping - Example
“Autonomous Underwater Vehicle Navigation,” John Leonard et al, 1998
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping with SLAMCourtesy of Hugh Durrant-Whyte, Univ of Sydney
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping with Extended Kalman Filters
Courtesy of [Leonard et al 1998]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Key Assumption Inverse sensor model p(st|ot,m) must be Gaussian. Main problem: Data association
Posterior multi-modal
Undistinguishable features
In practice: • Extract small set of highly distinguishable features from sensor data• Discard all other data• If ambiguous, take best guess for landmark identity
Posterior uni-modal
Distinguishable features
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping Algorithms - ComparisonSLAM
(Kalman)
Output Posterior
Convergence Strong
Local minima No
Real time Yes
Odom. Error Unbounded
Sensor Noise Gaussian
# Features 103
Feature uniq Yes
Raw data No
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Posterior estimationwith known poses:Occupancy grids
Posterior estimationwith known poses:Occupancy grids
Maximum likelihood:ML*
Maximum likelihood:ML*
Maximum likelihood:EM
Maximum likelihood:EM
Posterior estimation:EKF (SLAM)
Posterior estimation:EKF (SLAM)
Mapping: Outline
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
dsmszpuzmsp
sdssuspuzmssp
msuzpE
tt
tt
ttt
),|(log),,|(
,),|(log),,|,(
)]|,,([log
11
11111
111
tttttttttttt dsuzsmpsuspsmzpuzsmp ),|,(),|(),|(),|,( 11111111
M-Step: Mapping with known posesE-Step: Localization
[Dempster et al, 77] [Thrun et al, 1998] [Shatkay/Kaelbling 1997]
Mapping with Expectation Maximization
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
map(1)
Uncertainty Models for Motion
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
CMU’s Wean Hall (80 x 25 meters)
15 landmarks 16 landmarks
17 landmarks 27 landmarks
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
EM Mapping, Example (width 45 m)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping Algorithms - ComparisonSLAM
(Kalman)
EM
Output Posterior ML/MAP
Convergence Strong Weak?
Local minima No Yes
Real time Yes No
Odom. Error Unbounded Unbounded
Sensor Noise Gaussian Any
# Features 103
Feature uniq Yes No
Raw data No Yes
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Posterior estimationwith known poses:Occupancy grids
Posterior estimationwith known poses:Occupancy grids
Maximum likelihood:ML*
Maximum likelihood:ML*
Maximum likelihood:EM
Maximum likelihood:EM
Posterior estimation:EKF (SLAM)
Posterior estimation:EKF (SLAM)
Mapping: Outline
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Goal
EM:data association
Not real-time
Kalman filters:real-time
No data association
??
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Real-Time Approximation (ICRA paper)
),|(),|(argmax, 1,
tttsm
tt usspmszpsm
111111111111 ),,|(),|(),|(),,|( ttttttttttttttt dsmuzspusspmszpmuzsp
111111111 ),|,(),|(),|(),|,( tttttttttttt dsuzmspusspmszpuzmsp
Incremental ML
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Incremental ML: Not A Good Idea
path
robot
mismatch
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
ML* Mapping, OnlineIdea: step-wise maximum likelihood
111111 )(),,|(),|()( tttttttttt dssbmasspmsopsb
2. Posterior:
[Gutmann/Konolige 00, Thrun et al. 00]
1111111 ),(),,|,(),|(),( tttttttttttttt dmdsmsbamsmspmsopmsb
),,|,(),|(argmax, 111,
ttttms
tt amsmspmsopms
1. Incremental ML estimate:
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping withPoor Odometry
map andexploration path
raw data
DARPA Urban Robot
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping Without(!) Odometry
mapraw data (no odometry)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Localization in Multi-Robot Mapping
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Mapping
two laser range finders
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Structure Mapping (Real-Time)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Texture Mapping
raw image sequencepanoramic camera
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Texture Mapping
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping (with University of Sydney)
With: Hugh Durrant-Whyte, Somajyoti Majunder, Marc de Battista, Steve Scheding
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping Algorithms - ComparisonSLAM
(Kalman)
EM ML*
Output Posterior ML/MAP ML/MAP
Convergence Strong Weak? No
Local minima No Yes Yes
Real time Yes No Yes
Odom. Error Unbounded Unbounded Unbounded
Sensor Noise Gaussian Any Any
# Features 103
Feature uniq Yes No No
Raw data No Yes Yes
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Posterior estimationwith known poses:Occupancy grids
Posterior estimationwith known poses:Occupancy grids
Maximum likelihood:ML*
Maximum likelihood:ML*
Maximum likelihood:EM
Maximum likelihood:EM
Posterior estimation:EKF (SLAM)
Posterior estimation:EKF (SLAM)
Mapping: Outline
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Occupancy Grids: From scans to maps
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Occupancy Grid Maps
1111 )(),|()|()( tttttttt dssbasspsopsb
Assumptions: poses known, occupancy binary, independenttss 0
[Elfes/Moravec 88]
][1
][11
][1
][][][ )(),|()|()( xyt
xytt
xyt
xyt
xytt
xyt dmmbammpmopmb
)()()|( ][1][][ xyxyt
xy mbmpomp
)()|()( ][][][ xyxyt
xy mbmopmb
][ xytm
][1
][ xyt
xyt mm Assume
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Example
CAD map occupancy grid map
The Tech Museum, San Jose
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping Algorithms - ComparisonSLAM
(Kalman)
EM ML* Occupan. Grids
Output Posterior ML/MAP ML/MAP Posterior
Convergence Strong Weak? No Strong
Local minima No Yes Yes No
Real time Yes No Yes Yes
Odom. Error Unbounded Unbounded Unbounded None
Sensor Noise Gaussian Any Any Any
# Features 103
Feature uniq Yes No No No
Raw data No Yes Yes Yes
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping: Lessons Learned
Concurrent mapping and localization: hard robotics problem
Best known algorithms are probabilistic1. EKF/SLAM: Full posterior estimation, but restrictive
assumptions (data association)
2. EM: Maximum Likelihood, solves data association
3. ML*: less robust but online
4. Occupancy grids: Binary Bayes filter, assumes known poses (= much easier)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Obvious Next Step
EM for object
mapping
EM forconcurrentlocalization
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM(Kalman filters)
ExpectationMaximization
Real TimeHybrid
3D Mappingwith EM
Open Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Take-Home Message
Mapping is the
holy grail in
mobile robotics.
Every state-of-the-art
mapping algorithm
is probabilistic.
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems
2D Indoor mapping and exploration 3D mapping (real-time, multi-robot)
Object mapping (desks, chairs, doors, …)
Outdoors, underwater, planetary Dynamic environments (people, retail stores)
Full posterior with data association (real-time, optimal)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems, con’t
Mapping, localization Control/Planning under uncertainty Integration of symbolic making Human robot interaction
Literature Pointers: “Robotic Mapping” at www.thrun.org “Probabilistic Robotics” AI Magazine 21(4)
Top Related