Robots Autonomos PDF

See discussions, stats, and author profiles for this publication at: https://www.researchgate.
net/publication/268404735
Robots Autónomos
Article
CITATIONS READS
0 23
1 author:
Miguel Cazorla
University of Alicante
202 PUBLICATIONS 1,080 CITATIONS
SEE PROFILE
Some of the authors of this publication are also working on these related projects:
RoboCup View project
RETOGAR: Retorno al hogar. Sistema de mejora de la autonomía de personas con daño cerebral adquirido y dependientes en su integración en la sociedad View project
All content following this page was uploaded by Miguel Cazorla on 19 January 2015.
The user has requested enhancement of the downloaded file.

Robots Autónomos
FastSlam
Miguel Cazorla
Robot Vision Group

http://www.rvg.ua.es/master/robots
domingo 28 de febrero de 2010

The SLAM Problem
 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 and egg problem:
a map is needed to localize the robot and
a pose estimate is needed to build a map
Depto DCCIA - Robots Autónomos 2

The SLAM Problem
A robot moving though an unknown, static
environment
Given:
 The robot’s controls
 Observations of nearby
features
Estimate:
 Map of features
 Path of the robot

Why is SLAM a hard problem?
SLAM: robot path and map are both unknown!
Robot path error correlates errors in the map




Robot pose
uncertainty
 In the real world, the mapping between observations

and landmarks is unknown
 Picking wrong data associations can have catastrophic
consequences
 Pose error correlates data associations
Data Association Problem
 A data association is an assignment of observations to

landmarks
 In general there are more than
(n observations, m landmarks) possible associations
 Also called “assignment problem”


landmarks


landmarks


landmarks

EKF Slam problem
• When the number of landmarks grows, the covariance
matrix grows: not efficient when working with large
environments
• EKF slam algorithms works well with low number of
landmarks
• FastSlam tries to solve this

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
Resampling
 Typical application scenarios are tracking,
localization, …

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!

Dependencies
 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
 Is there a dependency between the

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.

Factored Posterior (Landmarks)
poses map observations & movements
SLAM posterior
Robot path posterior
landmark positions
Does this help to solve the problem?
Mapping using Landmarks
Landmark 1 l1
observations z1 z3
Robot poses x0 x1 x2 x3 ... xt

controls u0 u1 u1 ut-1
z2 zt
Landmark 2 l2

Mapping using Landmarks
Landmark 1 l1
observations z1 z3
Robot poses x0 x1 x2 x3 ... xt

controls u0 u1 u1 ut-1
z2 zt
Landmark 2 l2
Knowledge of the robot’s true path renders landmark
positions conditionally independent
Factored Posterior

(localization problem) Conditionally
independent
landmark positions

Rao-Blackwellization
 This factorization is also called Rao-

Blackwellization
 Given that the second term can be
computed efficiently, particle filtering
becomes possible!

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
Particle Landmark
#1 x, y, θ Landmark 1Landmark 2…
M
Particle Landmark
#2 x, y, θ Landmark 1Landmark 2…
M
…
Particle Landmark
N x, y, θ Landmark 1Landmark 2… M 15
Depto DCCIA - Robots Autónomos
FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3

FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3

FastSLAM – Sensor Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3

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

Particle #1 Weight = 0.8

FastSLAM Complexity
O(N)
• Update robot particles based Constant time per particle
on control ut-1
• Incorporate observation zt into O(N•log(M))

Kalman filters Log time per particle
• Resample particle set

O(N•log(M))
Log time per particle
N = Number of particles O(N•log(M))

M = Number of map features Log time per particle
 Which observation belongs to which landmark?
 A robust SLAM must consider possible data

associations
 Potential data associations depend also
on the pose of the robot

association
FastSLAM with known data
association
A particle consists of the estimated position of the robot
•
and K EKF
A particle consists of the estimated position of the robot
and Kestimating
EKF estimatingthe landmarks
the landmarks positions:
positions:
ado Depto. CCIA - Robótica y Visión Depto DCCIA - Robots Autónomos 21 9/31
emember we are in the known data association cas
We haveHow
K landmarks.
the whole thing works:
robot position
We suppose that the initial set of particles can be gen
• Remember we are in the known data association case. We
ated arbitrarily. have K landmarks.
• We suppose that the initial set of particles can be gene-
n each initial particle, the robot position s0 is (0, 0, 0).
rated arbitrarily.
• In each initial particle, the robot position s is (0, 0, 0).
0
he robot
• The moves
robot moves utu and
and wewe use
use the
t themodel
motion motionto model
generate (for each particle):
enerate (for each particle):
st ∼ p(st|ut, st−1) = h(ut, st−1) + δt (

do Depto. CCIA - Robótica y Visión 10

How the whole thing works:
mark positions are represented by the mean µ
landmarks
[m] position
he covariance Σn,t−1.
• One EKF (non-linear) for each landmark.
• Landmark positions are represented by the mean µ
ssumeandthat only one landmark is seen in eac
[m]
n,t-1
the covariance Σ [m]
n,t−1
tion.• At
We time
assume t,thatevery landmark
only one landmark not
is seen in eachseen
ob-
servation. At time t, every landmark not seen is not up-
is no
, so: dated, so:
[m] [m] [m] [m]
�µn,t , Σn,t � = �µn,t−1, Σn,t−1�
o. CCIA - Robótica y Visión

landmarks position
landmarks position
• The
The update is:
update is:
p(Θnt |st, z t, nt) = ηp(zt|st, Θnt , nt)p(Θnt |st−1, z t−1, nt−1)

(9)
where the second term is represented by a
where the second term is represented by a Gaussian (theGaussian
(the previous
previous mean mean and variance)
and variance) and theand
first the
termfirst term is
is linearized
linearized as EKF does.
as EKF does.
Doctorado Depto. CCIA - Robótica y Visión 12/31

weight
How forthing
the whole each particle
works: weight
How the whole for each
thing particle
works: weight
• TheThe
weight indicates
for
weight each
indicates the goodness
particle
the goodness of of this
this particle.
particle. It is It is
necessary due to the new robot position is calculated
The weight
based onlyindicates
on u , the goodness
paying no of this particle.
attention to z t. It is
based only on ut, paying no attention to zt.
t
• This
basedweight,
only on w
u
[m]
This weight, twtpaying
, [m]t : no attention to zt.
[m] n o
This weight,
[m] wt : [m] −1/2 1 [m] T [m]−1
− 2 (zt−ẑt ) Qt
[m]
(zt−ẑt )
wt ≈ η det(2πQ t ) exp
n o
1 [m] T [m]−1 [m]
[m] [m] −1/2 − 2 (zt−ẑt ) Qt (zt−ẑt )
det(2πQ
≈ ηthe
wt with t )
covariance exp
• with the covariance
with the covariance
[m] [m]T [m] [m]
Qt = Gt Σn,t−1Gt + Rt
[m] [m]T [m] [m]
Qt = Gt Σn,t−1Gt + Rt

Multi-Hypothesis Data Association
• Data association is done on

a per-particle basis
• Robot pose error is

factored out of data
association decisions










Per-Particle Data Association
Was the observation

generated by the red
or the blue landmark?
P(observation|red) = 0.3 P(observation|blue) = 0.7
 Two options for per-particle data association

 Pick the most probable match
 Pick an random association weighted by
the observation likelihoods
 If the probability is too low, generate a new landmark
Results – Victoria Park
• 4 km traverse
• < 5 m RMS position
error
• 100 particles
Blue = GPS
Yellow = FastSLAM Depto DCCIA
Dataset courtesy - Robotsof
of University Autónomos
Sydney 28
Results – Data Association

Results – Accuracy

Examples

Examples

Examples

Grid-based SLAM
 Can we solve the SLAM problem if no pre-defined

landmarks are available?
 Can we use the ideas of FastSLAM to build grid
maps?
 As with landmarks, the map depends on the poses
of the robot during data acquisition
 If the poses are known, grid-based mapping is
easy (“mapping with known poses”)

Mapping using Raw Odometry

Mapping with Known Poses
 Mapping with known poses using laser

range data

poses map observations & movements
SLAM posterior
Mapping with known poses

This is localization, use MCL
Use the pose estimate

from the MCL part and apply
mapping with known poses
Rao-Blackwellized Mapping
 Each particle represents a possible trajectory of

the robot
 Each particle
 maintains its own map and
 updates it upon “mapping with known poses”
 Each particle survives with a probability

proportional to the likelihood of the
observations relative to its own map

Particle Filter Example
3 particles
map of particle 1 map of particle 3
map of particle 2 Depto DCCIA - Robots Autónomos 40

Problem
 Each map is quite big in case of grid maps

 Since each particle maintains its own map
 Therefore, one needs to keep the number of
particles small
 Solution:
Compute better proposal distributions!
 Idea:
Improve the pose estimate before applying the
particle filter

Pose Correction Using Scan
Matching
Maximize the likelihood of the i-th pose and map relative to the (i-1)-th
pose and map

Matching
pose and map
current measurement

Matching
pose and map
current measurement
map constructed so far

Matching
pose and map
current measurement robot motion
map constructed so far

Motion Model for Scan Matching
Raw Odometry

Raw Odometry

Raw Odometry

Raw Odometry
Scan Matching

Mapping using Scan Matching

FastSLAM with Improved
Odometry
 Scan-matching provides a locally
consistent pose correction
 Pre-correct short odometry sequences using

scan-matching and use them as input to
FastSLAM
 Fewer particles are needed, since the error

in the input in smaller

FastSLAM with Scan-Matching

Loop Closure

Map: Intel Research Lab Seattle

Comparison to Standard FastSLAM
 Same model for observations

 Odometry instead of scan matching as input
 Number of particles varying from 500 to 2.000
 Typical result:

Further Improvements
• Improved proposals will lead to more accurate maps
• They can be achieved by adapting the proposal
distribution according to the most recent observations
• Flexible re-sampling steps can further improve the
accuracy.

Improved Proposal
• The proposal adapts to the structure of the environment

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
• neff describes “the variance of the particle weights”
• neff is maximal for equal weights. In this case, the

distribution is close to the proposal

Resampling with Neff
• Only re-sample when neff drops below a given threshold (n/2)
• See [Doucet, ’98; Arulampalam, ’01]

Typical Evolution of neff

visiting
new areas

visiting
new areas closing
the first
loop

visiting
new areas closing
the first
loop
visiting
known areas

visiting
new areas closing
the first
loop
visiting
known areas
second loop closure
Depto DCCIA - Robots Autónomos
55
Intel Lab
 15 particles
 four times faster
than real-time
P4, 2.8GHz
 5cm resolution
during scan
matching
 1cm resolution in
final map

Intel Lab
 15 particles
 Compared to
FastSLAM with
Scan-Matching,
the particles are
propagated closer
to the true
distribution

Outdoor Campus Map
 30 particles
 250x250m2
 1.75 km
(odometry)
 20cm resolution
during scan
matching
 30cm resolution in
final map

Outdoor Campus Map
 30 30
particles
particles
 250x250m
250x250m
2 2
 1.0881.75
miles
km
(odometry)
(odometry)
 20cm
20cmresolution
resolution
during
during
scanscan
matching
matching
 30cm
30cm resolution
resolution in
in
final map
final map

MIT Killian Court

MIT Killian Court - Video

Conclusion
• The ideas of FastSLAM can also be applied in the
context of grid maps
• Utilizing accurate sensor observation leads to good
proposals and highly efficient filters
• It is similar to scan-matching on a per-particle base
• The number of necessary particles and
re-sampling steps can seriously be reduced
• Improved versions of grid-based FastSLAM can handle
larger environments than naïve implementations in “real
time” since they need one order of magnitude fewer
samples

More Details on FastSLAM
• M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored
solution to simultaneous localization and mapping, AAAI02
• D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm

for generating maps of large-scale cyclic environments from raw laser range
measurements, IROS03
• M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved

particle filtering algorithm for simultaneous localization and mapping that
provably converges. IJCAI-2003
• G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-

blackwellized particle filters by adaptive proposals and selective resampling,
ICRA05
• A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and

mapping without predetermined landmarks, IJCAI03

View publication stats

Robots Autonomos PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robots Autonomos PDF

Uploaded by

Copyright:

Available Formats

See discussions, stats, and author profiles for this publication at: https://www.researchgate.

RoboCup View project

The user has requested enhancement of the downloaded file.

Robot Vision Group

domingo 28 de febrero de 2010

 SLAM stands for simultaneous localization

 Why is SLAM hard?

Depto DCCIA - Robots Autónomos 2

Depto DCCIA - Robots Autónomos 3

Robot path error correlates errors in the map

Robot path error correlates errors in the map

Robot path error correlates errors in the map

Robot path error correlates errors in the map

 In the real world, the mapping between observations

 A data association is an assignment of observations to

Depto DCCIA - Robots Autónomos 6

 A data association is an assignment of observations to

Depto DCCIA - Robots Autónomos 6

 A data association is an assignment of observations to

Depto DCCIA - Robots Autónomos 6

 A data association is an assignment of observations to

Depto DCCIA - Robots Autónomos 6

Depto DCCIA - Robots Autónomos 7

 Represent belief by random samples

Depto DCCIA - Robots Autónomos 8

 A particle filter can be used to solve both problems

Depto DCCIA - Robots Autónomos 9

 Is there a dependency between the

Depto DCCIA - Robots Autónomos 10

 Is there a dependency between the

 In the SLAM context

Depto DCCIA - Robots Autónomos 10

Robot poses x0 x1 x2 x3 ... xt

Depto DCCIA - Robots Autónomos 12

Robot poses x0 x1 x2 x3 ... xt

Robot path posterior

Depto DCCIA - Robots Autónomos 13

 This factorization is also called Rao-

Depto DCCIA - Robots Autónomos 14

Depto DCCIA - Robots Autónomos 16

Depto DCCIA - Robots Autónomos 16

Depto DCCIA - Robots Autónomos 17

Depto DCCIA - Robots Autónomos 17

Particle #1 Weight = 0.8

Particle #2 Weight = 0.4

Particle #3 Weight = 0.1

Depto DCCIA - Robots Autónomos 18

• Incorporate observation zt into O(N•log(M))

• Resample particle set

N = Number of particles O(N•log(M))

 A robust SLAM must consider possible data

Depto DCCIA - Robots Autónomos 20

st ∼ p(st|ut, st−1) = h(ut, st−1) + δt (

Depto DCCIA - Robots Autónomos 22

o. CCIA - Robótica y Visión

p(Θnt |st, z t, nt) = ηp(zt|st, Θnt , nt)p(Θnt |st−1, z t−1, nt−1)

Doctorado Depto. CCIA - Robótica y Visión 12/31

Depto DCCIA - Robots Autónomos 24

Doctorado Depto. CCIA - Robótica y Visión 15/31

Depto DCCIA - Robots Autónomos 25

• Data association is done on

• Robot pose error is

Depto DCCIA - Robots Autónomos 26