Efficient Approaches to Mapping with Rao-

Blackwellized Particle Filters

Department of Computer ScienceUniversity of Freiburg, Germany

Wolfram Burgard

Special thanks to: Austin Eliazar, Giorgio Grisetti, Dirk Hähnel, Mike Montemerlo, Ronald Parr, Cyrill Stachniss, …

Dimensions of Robot Mapping

[Makarenko et al., 02]

mapping

motion control

localizationSLAM

active localization

exploration

integrated approaches

Types of SLAM-Problems

Grid maps or scans

[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…]

Landmark-based

[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…

Why is SLAM Hard: Ambiguity

Start

End

Same position

[Courtesy of Eliazar & Parr]

Properties of Standard EKF-SLAM

Requires pre-defined landmarks/features

Complexity O(n2) where n is the number of landmarks

Data association problem

How can we solve the SLAM problem for not feature-based representations?

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

Occupancy of individual cells (m[xy]) is independent

Robot positions are known!

yx

xyt

tttt

mBel

zuzumPmBel

,

][

110

)(

),,,|()(

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:

Key Parameters of the Model

z+d1 z+d2

z+d3z

z-d1

Occupancy Value Depending on the Measured Distance

Deviation from the Prior Belief(the sphere of influence of the sensors)

Calculating the Occupancy Probability Based on Single Observations

Incremental Updating of Occupancy Grids (Example)

Resulting Map Obtained with Ultrasound Sensors

Mapping with Raw Odometry

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) Rao-Blackwellized particle filters

(landmarks and grids)

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

Key Problems

How to maintain multiple map and pose hypotheses during mapping?

How to control the robot?

Rao-Blackwellized Mapping

Observation: Given the true trajectory of the robot, all measurements are independent.

Idea: Use a particle filter to represent potential

trajectories of the robot (multiple hypotheses). For each particle we can analytically compute

the map of the environment (mapping with known poses).

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]

Rao-Blackwellized Mapping (2)

),|,( 1:0:1:1 ttt uzmxp

),|(),,|( 1:0:1:11:0:1:1 tttttt uzxpuzxmp

Compute a posterior over the map and possible trajectories of the robot :

robot motionmap trajectory

map and trajectory

measurements

A Graphical Model of Rao-Blackwellized Mapping

m

x

z

u

x

z

u

2

2

x

z

u

... t

t

x 1

1

0

10 t-1

FastSLAM

Robot Pose 2 x 2 Kalman Filters

Landmark 1 Landmark 2 Landmark N…x, y,

Landmark 1 Landmark 2 Landmark N…x, y, Particle#1

Landmark 1 Landmark 2 Landmark N…x, y, Particle#2

Landmark 1 Landmark 2 Landmark N…x, y, Particle#3

ParticleM

…

[Begin courtesy of Mike Montemerlo]

FastSLAM – Simulation

Up to 100,000 landmarks

100 particles

103 times fewer parameters than EKF SLAM

Blue line = true robot pathRed line = estimated robot pathBlack dashed line = odometry

Victoria Park Results

4 km traverse 100 particles Uses negative

evidence to remove spurious landmarks

Blue path = odometryRed path = estimated path

[End courtesy of Mike Montemerlo]

Key Questions

Can we apply Rao-Blackwellized particle filters to mapping with large grid-maps?

How can we compactly represent the individual maps carried by the particles?

How can we reduce the number of particles needed?

Tasks to be Solved

Mapping (occupancy grids) Each particle carries its own map m. The history of each particle represents a

potential trajectory of the robot. Localization

Propagate the particles according to the motion model (draw from p(x|u,x’)).

Compute importance weight according to the likelihood of the observation z given the pose x and the map m of the particle.

Computing the Likelihood of a Measurement: Ray Casting

1. Determine the distance to the closest obstacle in the direction of the measurement (ray-casting).

2. Approximate the likelihood p(z | m, x) by the likelihood p(z | d) of z given the “expected measurement d for x.”

[Fox et al., 98]

Mixture Approximation of p(z | d)

[Choset et al., to appear, Thrun et al., to appear]

Computing the Likelihood of a Measurement: Correlation Models

Determine the cell [xy] a beam ends in. Approximate the likelihood p(z | m, x) by

the occupancy probability Bel(m [xy])

contained in m[xy] (correlation model).[Konolige, 99]

Smoothing of Bel(m[xy]) yields a better gradient and improves the robustness.[Thrun, 01] (likelihood fields).

RPBF with Grid Maps

map of particle 1 map of particle 3

map of particle 2

3 particles

Map Maintenance Challenges

High resolution maps are big Typically 100’s or 1000’s of particles are

needed One full map per particle requires

O(|m|·n) work (re-sampling) Gigabytes of memory movement

Anecdotal reports: Tried, but impractical(see later)

Begin courtesy of Eliazar & Parr

DP-SLAM: Distributed Particle Mapping

Exploit sampling/re-sampling steps of PF Common ancestry = Redundant map sections

History representation: Ancestry Tree Leaves correspond to current particles

New map Representation Store multiple maps in a single grid

Ancestry Trees

Ancestry Trees

Ancestry Trees

Ancestry Trees

Ancestry Trees

Ancestry Trees

Ancestry Trees

Ancestors with no children can be removed

Ancestry Trees

Ancestors with only one child can be merged

Ancestry Trees

Ancestry Trees

Maintain a minimal tree (improves complexity) Exactly n leaves Branching factor at least 2 Depth no more than n

Explicitly store the ancestry info Node = Ancestor particle with unique ID Stores parent link, map updates

Map Representation

Map is an occupancy grid Avoid one map per particle

Naïve Map Representation

DP-Mapping

Distribute particles over a single map

Each grid square stores: ID of each ancestry node

that has seen this square Associated observations No redundant data No unnecessary data

Localization

For each laser cast of the current particle Trace laser cast through grid For each grid square return map occupancy

Store observations as balanced trees (keyed on IDs)

Linear storage ancestry Logarithmic access/updates

Complexity

Localization: O(An2) n particles check A grid

squares Worst case cost n to check

occupancy(harder than it sounds)

Map Maintenance: O(Anlogn) Additions, Deletions: O(Anlogn) Ancestry Tree Maintenance : O(Anlogn) Amortized analysis

(see papers by Eliazar&Parr)

A = Area observed

n = Number of particles

|m| = Map size

Complexity Summary

Total Time : O(An2) Compare to O(|m|n) |m| >> An

Linear in observation size Independent of map size

A = Area observed

n = Number of particles

|m| = Map size

DP-SLAM Results

Run at real-time speed on 2.4GHz Pentium 4 at 10cm/s

scale: 3cm

Consistency

Results obtained with DP-SLAM 2.0 (offline)

Eliazar & Parr, 04

Close up

End courtesy of Eliazar & Parr

Observations

DP-SLAM is an efficient and elegant way to store the individual maps assigned to the particles.

Complexity O(An2) where n is the number of particles

How can we reduce the number of particles?

Techniques to Reduce the Number of Particles Needed

Better proposals (put the particles in the right place in the prediction step).

Avoid particle depletion (re-sample only when needed).

Generating better Proposals

Use scan-matching to compute highly accurate odometry measurements from consecutive range scans.

Use the improved odometry in the prediction step to get highly accurate proposal distributions.

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

...

...

Rao-Blackwellized Mapping with Scan-Matching

Map:

Inte

l R

ese

arc

h L

ab

Seatt

le

Loop Closure

RBPF Mapping with Scan-Matching

Map:

Inte

l R

ese

arc

h L

ab

Seatt

le

Loop Closure

Rao-Blackwellized Mapping with Scan-Matching

Map:

Inte

l R

ese

arc

h L

ab

Seatt

le

Comparison to Previous Techniques Standard Rao-Blackwellized mapping

with grid maps (Intel Research Lab data set)

Wean Hall (32m x 10m), noise added to odometry (simulation)

Scan Matching Single map plus posterior about

poses

Comparison to the Original Approach• Same model for observations

• Odometry instead of scan matching results

• Number of particles varying from 500 to 2.000

• Typical result:

Dynamically Adapting the Motion Model The previous approach used a

constant motion model p(x|u, x’).

It needs to be more peaked than the model for raw odometry.

Accordingly, it will fail in situations in which scan matching yields bad results (e.g., in wide open spaces)

Goal: better proposal distribution

The Optimal Proposal Distribution

For lasers is extremely peaked and dominates the product.

[Arulampalam et al., 01]

We can safely approximate by a constant:

Resulting Proposal Distribution

Gaussian approximation:

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

Selective Re-sampling

Re-sampling is dangerous, since important samples might get lost(particle depletion problem)

In case of suboptimal proposal distributions re-sampling is necessary to achieve convergence.

Key question: When should we re-sample?

Number of Effective Particles

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

We only re-sample 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

Example (Intel Lab)

15 particles

four times faster than real-timeP4, 2.8GHz

5cm resolution during scan matching

1cm resolution in final map

Courtesy by Giorgio Grisetti & Cyrill Stachniss

Outdoor Campus Map

30 particles

250x250m2

1.75 km (odometry)

20cm resolution during scan matching

30cm resolution in final map

Courtesy by Giorgio Grisetti & Cyrill Stachniss

30 particles

250x250m2

1.088 miles (odometry)

20cm resolution during scan matching

30cm resolution in final map

The approaches seen so far are purely passive.

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

Exploration

Where to Move Next?

Combining Rao-Blackwellized Mapping with Exploration

mapping

motion control

localizationSLAM

active localization

exploration

Decision-Theoretic Formulation of Exploration

reward (expected information gain)

cost (path length)

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

Trajectory graph stores the path traversed by the robot.

Grid map represents the space covered by the sensors.

Loops correspond to long paths in the trajectory graph and short paths in the geometric map.

Dual Representation for Loop Detection

Application Example

Real Exploration Example

Corridor Exploration

Comparison

Map and pose uncertainty:

Map uncertainty only:

Example: Entropy Evolution

Summary Rao-Blackwellization is well-suited for

maintaining multiple hypotheses during occupancy grid mapping.

Grid-based approaches can be scaled to larger environments by

using appropriate data structures for the maps carried by the individual particles (DPSLAM), by

using improved motion models (better proposals), by

using adaptive re-sampling schemes, and by

actively controlling the actions of the robot.

Potential Projects

Dynamic environments Detection of errors Recovery from errors Three-dimensional maps Objects in maps Adaptive models (motion, sensor, …) …