I ntroduction to Mobile Robotics SLAM Landm ark-based FastSLAM - - PowerPoint PPT Presentation

▶
i ntroduction to mobile robotics
SMART_READER_LITE
LIVE PREVIEW

I ntroduction to Mobile Robotics SLAM Landm ark-based FastSLAM - - PowerPoint PPT Presentation

May 23, 2023 261 likes •645 views

I ntroduction to Mobile Robotics SLAM Landm ark-based FastSLAM Wolfram Burgard Partial slide courtesy of Mike Montemerlo 1 The SLAM Problem SLAM stands for simultaneous localization and mapping The task of building a map while

slide-1
SLIDE 1

1

SLAM – Landm ark-based FastSLAM

Partial slide courtesy of Mike Montemerlo

Wolfram Burgard

I ntroduction to Mobile Robotics

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

Estim ate:

  • Map of features
  • Path of the robot

The SLAM Problem

A robot moving through an unknown, static environment

slide-4
SLIDE 4

4

Typical m odels are:

  • Feature maps
  • Grid maps (occupancy or reflection

probability maps)

Map Representations

today

slide-5
SLIDE 5

5

W hy 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

7

W hy 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

9

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

Particle Filters

  • 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
  • Resample
  • Typical application scenarios are tracking,

localization, …

10

slide-9
SLIDE 9

Localization vs. SLAM

  • 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!

11

slide-10
SLIDE 10

Dependencies

  • Is there a dependency between certain

dimensions of the state space?

  • If so, can we use the dependency to solve

the problem more efficiently?

12

slide-11
SLIDE 11

Dependencies

  • 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
  • f the sensor is known.

13

slide-12
SLIDE 12

Factored Posterior ( Landm arks)

14

Factorization first introduced by Murphy in 1999

poses map

  • bservations & movements
slide-13
SLIDE 13

Factored Posterior ( Landm arks)

15

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

Rao-Blackw ellization

  • Factorization to exploit dependencies

between variables:

  • If can be computed in closed form,

represent only with samples and compute for every sample

  • It comes from the Rao-Blackwell theorem
slide-15
SLIDE 15

Revisit the Graphical Model

Courtesy: Thrun, Burgard, Fox

slide-16
SLIDE 16

Revisit the Graphical Model

know n

Courtesy: Thrun, Burgard, Fox

slide-17
SLIDE 17

Landm arks are Conditionally I ndependent Given the Poses

Landm ark variables are all disconnected ( i.e. independent) given the robot’s path

slide-18
SLIDE 18

23

Factored Posterior

Robot path posterior (localization problem) Conditionally independent landmark positions

slide-19
SLIDE 19

24

Rao-Blackw ellization for SLAM

  • Given that the second term can be computed

efficiently, particle filtering becomes possible!

slide-20
SLIDE 20

25

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-21
SLIDE 21

26

FastSLAM – Action Update

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

slide-22
SLIDE 22

27

FastSLAM – Sensor Update

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

slide-23
SLIDE 23

28

FastSLAM – Sensor Update

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

slide-24
SLIDE 24

29

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-25
SLIDE 25

30

FastSLAM - Video

slide-26
SLIDE 26

FastSLAM Com plexity – Naive

  • Update robot particles

based on the control

  • Incorporate an observation

into the Kalman filters (given the data association)

  • Resample particle set

N = Number of particles M = Number of map features

O(N) O(N) O(N M)

slide-27
SLIDE 27

A Better Data Structure for FastSLAM

Courtesy: M. Montemerlo

slide-28
SLIDE 28

A Better Data Structure for FastSLAM

slide-29
SLIDE 29

FastSLAM Com plexity

  • Update robot particles

based on the control

  • Incorporate an observation

into the Kalman filters (given the data association)

  • Resample particle set

N = Number of particles M = Number of map features

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

slide-30
SLIDE 30

35

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-31
SLIDE 31

36

Multi-Hypothesis Data Association

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

factored out of data association decisions

slide-32
SLIDE 32

37

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-33
SLIDE 33

38

Results – Victoria Park

  • 4 km traverse
  • < 5 m RMS

position error

  • 100 particles

Dataset courtesy of University of Sydney

Blue = GPS Yellow = FastSLAM

slide-34
SLIDE 34

39

Results – Victoria Park ( Video)

Dataset courtesy of University of Sydney

slide-35
SLIDE 35

40

Results – Data Association

slide-36
SLIDE 36

41

FastSLAM Sum m ary

  • 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)