Probabilistic Algorithms for Mobile Robot Mapping

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, …


Open Problems

3D Mappingwith EM

Real TimeHybrid

ExpectationMaximization

SLAM(Kalman filters)

Motivation


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


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


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]


Mapping: The Problem

Concurrent Mapping and Localization (CML) Simultaneous Localization and Mapping (SLAM)


Mapping: The Problem

Continuous variables High-dimensional (eg, 1,000,000+ dimensions) Multiple sources of noise Simulation not acceptable


Milestone Approaches

Mataric 1990

Kuipers et al 1991

Elfes/Moravec 1986

Lu/Milios/Gutmann 1997


3D Mapping

Konolige et al, 2001 Teller et al, 2000

Moravec et al, 2000


Take-Home Message

Mapping is the

holy grail in

mobile robotics.

Every state-of-the-art

mapping algorithm

is probabilistic.


Robots are Inherently Uncertain

Uncertainty arises from four major factors:– Environment stochastic, unpredictable– Robot stochastic– Sensor limited, noisy– Models inaccurate


Probabilistic Robotics

)()()|()|(

bpapabpbap


Probabilistic Robotics

Key idea: Explicit representation of uncertainty (using the calculus of probability theory)

Perception = state estimation Action = utility optimization


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


Pitfalls

Computationally demanding False assumptions Approximate


Open Problems

3D Mappingwith EM

Real TimeHybrid


Motivation



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]


s

p(s)

Probabilistic Localization

[Simmons/Koenig 95][Kaelbling et al 96][Burgard et al 96]


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

Markov1021111 ),,|(),|()|( ttttttttt dsoaospasspsop


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”


Bayes Filters are Familiar to AI!

Kalman filters Hidden Markov Models Dynamic Bayes networks Partially Observable Markov Decision Processes

(POMDPs)

1111 )(),|()|()( tttttttt dssbasspsopsb


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


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?


Idea: Represent Belief Through Samples

• Particle filters[Doucet 98, deFreitas 98]

• Condensation algorithm[Isard/Blake 98]

• Monte Carlo localization[Fox/Dellaert/Burgard/Thrun 99]


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



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 proposalondistributitarget )( i

tw

),|( )( msop itt

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

)(11

)(1

)(

)(11

)(1

)()(

itt

it

it

itt

it

it

itt

sBelmasspsBelmasspmsop


Monte Carlo Localization


Performance Comparison

Monte Carlo localizationMarkov localization (grids)


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


Pitfall: The World is not Markov!

99.0)(),|()short is (?

ttt

oott dssbdomsopop

t [Fox et al 1998]

Distance filters:


Probabilistic Localization: Lessons Learned

Probabilistic Localization = Bayes filters

Particle filters: Approximate posterior by random samples


The Problem: Concurrent Mapping and Localization

70 m


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!


Posterior estimationwith known poses:Occupancy grids

Maximum likelihood:ML*

Maximum likelihood:EM

Posterior estimation:EKF (SLAM)

Mapping: Outline


Mapping as Posterior Estimation


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]


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

yxl

ll

msb


Underwater Mapping

By: Louis L. Whitcomb, Johns Hopkins University


Underwater Mapping - Example

“Autonomous Underwater Vehicle Navigation,” John Leonard et al, 1998


Underwater Mapping with SLAMCourtesy of Hugh Durrant-Whyte, Univ of Sydney


Mapping with Extended Kalman Filters

Courtesy of [Leonard et al 1998]


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


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






Mapping: Outline


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


map(1)

Uncertainty Models for Motion


CMU’s Wean Hall (80 x 25 meters)

15 landmarks 16 landmarks

17 landmarks 27 landmarks


EM Mapping, Example (width 45 m)



(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






Mapping: Outline


The Goal

EM:data association

Not real-time

Kalman filters:real-time

No data association

?


Real-Time Approximation (ICRA paper)

),|(),|(argmax, 1,

tttsm

tt usspmszpsm

111111111111 ),,|(),|(),|(),,|( ttttttttttttttt dsmuzspusspmszpmuzsp

111111111 ),|,(),|(),|(),|,( tttttttttttt dsuzmspusspmszpuzmsp

Incremental ML


Incremental ML: Not A Good Idea

path

robot

mismatch


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 amsmspmsopms1. Incremental ML estimate:


Mapping withPoor Odometry

map andexploration path

raw data

DARPA Urban Robot


Mapping Without(!) Odometry

mapraw data (no odometry)


Localization in Multi-Robot Mapping


3D Mapping

two laser range finders


3D Structure Mapping (Real-Time)


3D Texture Mapping

raw image sequencepanoramic camera


3D Texture Mapping


Underwater Mapping (with University of Sydney)

With: Hugh Durrant-Whyte, Somajyoti Majunder, Marc de Battista, Steve Scheding



(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






Mapping: Outline


Occupancy Grids: From scans to maps


Occupancy Grid Maps


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


Example

CAD map occupancy grid map

The Tech Museum, San Jose



(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


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 association3. ML*: less robust but online4. Occupancy grids: Binary Bayes filter, assumes

known poses (= much easier)


The Obvious Next Step

EM for object

mapping

EM forconcurrentlocalization


Motivation



Real TimeHybrid

3D Mappingwith EM

Open Problems


Take-Home Message

Mapping is the

holy grail in

mobile robotics.

Every state-of-the-art

mapping algorithm

is probabilistic.


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)


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)

Probabilistic Algorithms for Mobile Robot Mapping

Documents

acceptablesebastian

mapping slamsebastian

dirk schulzsebastian

jamie schultesebastian

thruns tutorialsebastian

problemconcurrent mapping

d mappingkonolige et

kuipers et