SLAM Landmark-based FastSLAM Wolfram Burgard, Maren Bennewitz, - - PowerPoint PPT Presentation

▶
slam
SMART_READER_LITE
LIVE PREVIEW

SLAM Landmark-based FastSLAM Wolfram Burgard, Maren Bennewitz, - - PowerPoint PPT Presentation

Feb 16, 2023 41 likes •366 views

Introduction to Mobile Robotics SLAM Landmark-based FastSLAM Wolfram Burgard, Maren Bennewitz, Diego Tipaldi, Luciano Spinello Partial slide courtesy of Mike Montemerlo 1 The SLAM Problem SLAM stands for simultaneous localization and

slide-1
SLIDE 1

1

SLAM – Landmark-based FastSLAM Introduction to Mobile Robotics

Partial slide courtesy of Mike Montemerlo

Wolfram Burgard, Maren Bennewitz, Diego Tipaldi, Luciano Spinello

slide-2
SLIDE 2

2

  • SLAM stands for simultaneous localization

and mapping

  • The task of building a map while estimating

the pose of the robot relative to this map

  • Why is SLAM hard?

Chicken-or-egg problem:

  • A map is needed to localize the robot
  • A pose estimate is needed to build a map

The SLAM Problem

slide-3
SLIDE 3

3

Given:

  • The robot’s

controls

  • Observations of

nearby features

Estimate:

  • Map of features
  • Path of the robot

The SLAM Problem

A robot moving though an unknown, static environment

slide-4
SLIDE 4

4

Typical models are:

  • Feature maps
  • Grid maps (occupancy or reflection probability

maps)

Map Representations

today

slide-5
SLIDE 5

5

Why is SLAM a Hard Problem?

SLAM: robot path and map are both unknown! Robot path error correlates errors in the map

slide-6
SLIDE 6

6

Why is SLAM a Hard Problem?

  • In the real world, the mapping between
  • bservations and landmarks is unknown
  • Picking wrong data associations can have

catastrophic consequences

  • Pose error correlates data associations

Robot pose uncertainty

slide-7
SLIDE 7

7

Data Association Problem

  • A data association is an assignment of
  • bservations to landmarks
  • In general there are more than

(n observations, m landmarks) possible associations

  • Also called “assignment problem”
slide-8
SLIDE 8

8

  • Represent belief by random samples
  • Estimation of non-Gaussian, nonlinear

processes

  • Sampling Importance Resampling (SIR) principle
  • Draw the new generation of particles
  • Assign an importance weight to each particle
  • Resampling
  • Typical application scenarios are tracking,

localization, …

Particle Filters

slide-9
SLIDE 9

9

  • A particle filter can be used to solve both problems
  • Localization: state space < x, y, >
  • SLAM: state space < x, y, , map>
  • for landmark maps = < l1, l2, …, lm>
  • for grid maps = < c11, c12, …, c1n, c21, …, cnm>
  • Problem: The number of particles needed to

represent a posterior grows exponentially with the dimension of the state space!

Localization vs. SLAM

slide-10
SLIDE 10

10

  • Is there a dependency between the

dimensions of the state space?

  • If so, can we use the dependency to solve

the problem more efficiently?

Dependencies

slide-11
SLIDE 11

11

  • Is there a dependency between certain

dimensions of the state space?

  • If so, can we use the dependency to solve

the problem more efficiently?

  • In the SLAM context
  • The map depends on the poses of the

robot.

  • We know how to build a map given the

position of the sensor is known.

Dependencies

slide-12
SLIDE 12

12

Factored Posterior (Landmarks)

Factorization first introduced by Murphy in 1999

poses map

  • bservations & movements
slide-13
SLIDE 13

13

Factored Posterior (Landmarks)

SLAM posterior Robot path posterior landmark positions

Factorization first introduced by Murphy in 1999

Does this help to solve the problem? poses map

  • bservations & movements
slide-14
SLIDE 14

14

Mapping using Landmarks

. . . Landmark 1

  • bservations

Robot poses controls x1 x2 xt u1 ut-1 l2 l1 z1 z2 x3 u2 z3 zt Landmark 2 x0 u0

slide-15
SLIDE 15

Bayes Network and D-Separation (See AI or PGM course)

  • and are independent if d-separated by
  • d-separates from if every undirected

path between and is blocked by

  • A path is blocked by if there is a node W
  • n the graph such that either:
  • W has converging arrows along the path

(→ W ←) and neither W nor its descendants are

  • bserved (in V), or
  • W does not have converging arrows along the

path (→ W → or ← W →) and W is observed (W ).

15

slide-16
SLIDE 16

16

Knowledge of the robot’s true path renders landmark positions conditionally independent

Mapping using Landmarks

. . . Landmark 1

  • bservations

Robot poses controls x1 x2 xt u1 ut-1 l2 l1 z1 z2 x3 u

2

z3 zt Landmark 2 x0 u0

slide-17
SLIDE 17

17

Factored Posterior

Robot path posterior (localization problem) Conditionally independent landmark positions

slide-18
SLIDE 18

18

Rao-Blackwellization

  • This factorization is also called Rao-Blackwellization
  • Given that the second term can be computed

efficiently, particle filtering becomes possible!

slide-19
SLIDE 19

19

FastSLAM

  • Rao-Blackwellized particle filtering based on

landmarks [Montemerlo et al., 2002]

  • Each landmark is represented by a 2x2

Extended Kalman Filter (EKF)

  • Each particle therefore has to maintain M EKFs

Landmark 1 Landmark 2 Landmark M … x, y,  Landmark 1 Landmark 2 Landmark M … x, y, 

Particle #1

Landmark 1 Landmark 2 Landmark M … x, y, 

Particle #2 Particle N

…

slide-20
SLIDE 20

20

FastSLAM – Action Update

Particle #1 Particle #2 Particle #3 Landmark #1 Filter Landmark #2 Filter

slide-21
SLIDE 21

21

FastSLAM – Sensor Update

Particle #1 Particle #2 Particle #3 Landmark #1 Filter Landmark #2 Filter

slide-22
SLIDE 22

22

FastSLAM – Sensor Update

Particle #1 Particle #2 Particle #3 Weight = 0.8 Weight = 0.4 Weight = 0.1

slide-23
SLIDE 23

23

FastSLAM – Sensor Update

Particle #1 Particle #2 Particle #3 Update map

  • f particle #1

Update map

  • f particle #2

Update map

  • f particle #3
slide-24
SLIDE 24

24

FastSLAM - Video

slide-25
SLIDE 25

25

FastSLAM Complexity

  • Update robot particles based on

control ut-1

  • Incorporate observation zt into

Kalman filters

  • Resample particle set

N = Number of particles M = Number of map features

O(N)

Constant time (per particle)

O(N•log(M))

Log time (per particle)

O(N•log(M)) O(N•log(M))

Log time in the number

  • f landmarks, linear in

the number of particles Log time (per particle)

slide-26
SLIDE 26

27

Data Association Problem

  • A robust SLAM solution must consider

possible data associations

  • Potential data associations depend also
  • n the pose of the robot
  • Which observation belongs to which

landmark?

slide-27
SLIDE 27

28

Multi-Hypothesis Data Association

  • Data association is done
  • n a per-particle basis
  • Robot pose error is

factored out of data association decisions

slide-28
SLIDE 28

29

Per-Particle Data Association

Was the observation generated by the red

  • r the brown landmark?

P(observation|red) = 0.3 P(observation|brown) = 0.7

  • Two options for per-particle data association
  • Pick the most probable match
  • Pick a random association weighted by

the observation likelihoods

  • If the probability is too low, generate a new

landmark

slide-29
SLIDE 29

30

Results – Victoria Park

  • 4 km traverse
  • < 5 m RMS

position error

  • 100 particles

Dataset courtesy of University of Sydney

Blue = GPS Yellow = FastSLAM

slide-30
SLIDE 30

31

Results – Victoria Park (Video)

Dataset courtesy of University of Sydney

slide-31
SLIDE 31

32

Results – Data Association

slide-32
SLIDE 32

33

FastSLAM Summary

  • FastSLAM factors the SLAM posterior into

low-dimensional estimation problems

  • Scales to problems with over 1 million features
  • FastSLAM factors robot pose uncertainty
  • ut of the data association problem
  • Robust to significant ambiguity in data

association

  • Allows data association decisions to be delayed

until unambiguous evidence is collected

  • Advantages compared to the classical EKF

approach (especially with non-linearities)

  • Complexity of O(N log M)