A Fast Conjunctive Resampling Particle Filter for Collaborative Multi-Robot Localization

A Fast Conjunctive Resampling
Particle Filter for Collaborative
Multi-Robot Localization
AAMAS 2008 – Estoril -Portugal

Andrea Gasparri, Stefano Panzieri, Federica Pascucci

Dept. Informatica e Automazione
University “Roma Tre”, Rome,
Italy

Stefano Panzieri

Outline

◊ The mobile robot localization problem
◊ The probabilistic framework
◊ Bayesian approach
◊ Particle Filter
Robotica Autonoma & Fusione Sensoriale

◊ Formulation
◊ Pros & Cons
AAMAS 2008 – Estoril - Portugal

◊ The fast Conjunctive Resampling technique
◊ Main features
◊ Performance Analysis
◊ Simulations
◊ Conclusion and Future Work
◊ Simulations and experimental results
◊ A Spatially Structured Genetic Algorithm framework

Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 2

The mobile robot localization problem

◊ No a priori knowledge on robot pose
◊ Sensorial data
◊ Environment shape
◊ Motion capabilities

◊ Most of solutions based on the Probabilistic framework

◊ Gaussian hypothesis:

◊ Kalman Filtering
◊ typically unimodal

◊ Relaxing gaussianity:
◊ Grid based approach
◊ Computational effort
◊ Sequential Montecarlo integration (particles)
◊ High number of particles
◊ Not robust on kidnapping
◊ Degeneracy problem
◊ PF enhanced
◊ More complex resampling steps


The Probabilistic Framework

◊ The probability theory provides a suitable framework for the
localization problem
◊ The robot’s pose can be described by a probability distribution,
named Belief:

◊ Prior and Posterior beliefs can be obtained by splitting perceptual
data Zk in this way:

◊ The prior represents the Belief after integration of only input data
and before it receives last perceptual data zk.

Probabilistic Framework

◊ A recursive formulation can be obtained by Applying the Total
Probability Theorem, the Bayes’rule and some simplifying (Markov)
assumptions

◊ Due to computational difficulties of handling the above integral,
approximations are required


Monte Carlo (naive Particle Filters)

◊ Monte carlo integration methodhs are algorithms for the
approximate evaluation of definite integrals
◊ The Perfect Monte Carlo Sampling draws N independent and
identically distributed random samples according to Bel+(xk):

◊ Where is the delta-Dirac mass located in xk(i)

◊ Due to difficulty of efficient sampling from the posterior distribution
Bel+(xk) at any sample time k a different approach is required


Importance Sampling

◊ The key idea is of drawing samples from a normalized Importance
Sampling distribution which ha a support including
that of the posterior belief Bel+(xk):

◊ Where wk(i) is the importance weight that can be recursively
obtained as:

◊ In mobile robotics, a suitable choice of the importance sampling
distribution is the prior Bel-(xk) distribution. With this choice:


Monte Carlo Integration Methods

◊ Advantages
◊ Ability to represent arbitrary densities
◊ Dealing with non-Gaussian noise

◊ Adaptive focusing on probable regions of state-space
◊ Issues

◊ Degeneracy and loss of diversity,
◊ The choice of the optimal number of samples,
◊ The choice of importance density is crucial.
◊ Sampling Importance Resampling (SIR)
◊ Use prior Belief distribution Bel-(xk)
◊ Sistematic Resampling (SR)
◊ To deal with degeneracy problem


Particle Filter for Robot Localization
◊ The robot moves according to the unicycle model

φ

y

◊ Where
x

◊ We suppose the robot equipped with laser rangefinders,
and the environment described by a set M of segments.
◊ The observation model is


Perceptual model
◊ Any particle, i.e., a possible robot pose, differs from the real
state in terms of the following quadratic distance error:

◊ Where is the vector of measured distances
◊ The perceptual model adopted is

x
x
ˆ
z1
ˆ
z2
z1 x
z2
x

ˆ
z3
z3 x
x
Hypothesis
Real robot

Multi robot approach
◊ Suppose collaboration among robots
◊ We need to exchange belief information
◊ How information should be exchanged?
◊ What should be sent through the communication channel?
x

x

x RA
x

x
RB


A previous approach

◊ Called the Belief related to the set of
robots, we suppose that the probability distribution P can be
decomposed in a product using marginal distributions

◊ In this way the Belief update of one robot that takes into

account the an others Belief can be written

◊ But in a Monte Carlo context this integral cannot be easily done
due to Dirac impulses!

◊ D. Fox, W. Burgard, H. Kruppa, and S. Thrun. A probabilistic approach to collaborative
multi-robot localization. In Special issue of Autonomou Robots on Heterogeneous
Multi-Robot Systems, volume 8(3), 2000.

Reconstruct Belief using a density tree

◊ D. Fox, W. Burgard, H.
Kruppa, and S. Thrun


The Fast Conjunctive Resampling
Main Features

Conjunction:

◊ The conjunction of the best estimates
consists of substituting low weight
particles of one robot with others

having high weight on remote
robots propagation

Propagation:
◊ The propagation of sensory data
consists of an exchange of laser
readings that can be exploited to
solve environmental ambiguities


Conjunction

◊ Substitute lo weight particles of
one robot with high weight ones
projected from other robots
◊ We need a status for the particle:

good, bad, new
◊ A particle is marked good during

input evolution if the weight of its
ancestor is above a threshold
◊ During a resample crated
particles are set new


Propagation of sensory data
x

z1RA
RA
z2 Integrate observations coming from robot RB
x
into weight evaluation of particles of robot RA

x RA
RA
z3 z1RB x
z RA , RB

x
z 2RB
RB

◊ Using both sensory data only particles fitting well on both
locations will survive


Lock mechanism for data exchange

◊ Repeated exchange of information will simply result in
over-convergence to a bogus result

◊ A simple locking mechanism can be introduced

◊ Two robots are free to exchange data when
◊ A conjunction with other robots happened since their
last meeting
◊ Robots have processed a consistent amount of
observations,
◊ An additional percentage of random resampling is
considered.


Complexity

◊ Note that, each time a conjunction of the best estimates is
performed, the weight of particles must be re-computed.
◊ In particular, this can be done without any additional computational
load simply letting follow the conjunction by the propagation of

sensory data (which already implies the re-computation of particles
weights)

◊ This collaborative approach is very simple, it is easy to implement
and it does not increase the asymptotic complexity of the plain
Particles Filter
◊ In fact, it leads to an additional O(N) term to the computational
complexity of the plain Particle Filters that is O(N) as well


Performance Analysis
First Environment
◊ 4 Robots
◊ Ambiguous Environment
◊ 100 Trials
◊ Partial Communication


Estimation Accuracy


Successful Trials
Autonomous Localization

# Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials

100 0.297 0.172 0.232 5 - 20 - 51

300 0.302 0.158 0.232 14 – 32- 72
500 0.272 0.167 0.222 17 - 40 - 87

Collaborative Localization

100 0.371 0.196 0.245 34 – 50 - 78

300 0.274 0.182 0.216 46 - 67 - 95
500 0.248 0.166 0.211 51 - 73 - 97


Second Environment
◊ 3 Robots
◊ Structural
Similarities
◊ 100 Trials

◊ Partial
Communication


Estimation Accuracy


Successful Trials
Autonomous Localization


100 0.145 0.117 0.129 23 - 39 – 59

300 0.103 0.079 0.089 57 - 66 – 81
500 0.081 0.063 0.073 67 – 76 - 92

Collaborative Localization

100 0.125 0.099 0.112 79 - 85 – 90

300 0.090 0.072 0.078 92 - 94 – 96
500 0.076 0.062 0.069 100–100-100


Considerations


Future Work

◊ A deeper investigation on the inter-dependence
among beliefs when performing conjunction

◊ An implementation of the proposed approach in a
real context


AAMAS 2008 – Estoril -Portugal

Stefano Panzieri
Thanks!

An other promising technique:
structuring a GA over a Network
◊ Lets consider the genetic
population as a Complex
System and take advantage of
the Evolutionary Cellular
Automata theory

◊ That means: give to the GA a
topological structure

◊ The topological structure largely
determines the dynamical
processes that can take place in
complex systems
◊ A spatial structure can be given to
the population to exploit a more
biological-like spreading dynamics
a regular lattice
◊ It can be seen not only like an
improvement of panmictic
populations but also a source of
new and original dynamics

Small World networks
◊ Watts-Strogatz Algorithm
 Start with a lattice network with
degree k
 Randomically (with probability p)

a rewiring is made of each link
moving the connection from one

node to an other

◊ Low Average Path length
◊ Fast propagation
◊ High Clustering coefficient
◊ Evolutionary niches


Evolving with a Genetic Mating-Rule

Compute a mean fitness over
the net
2
1
Then, for each link, compare the
two finesses

Node 1 Node 2 Action Basic principles

LOW LOW Both Self-Mutate Mutation

HIGH/LOW LOW/HIGH Node 2/1 is replaced with Elitism &
a Mutation of Node 1/2 Mutation
HIGH HIGH The lower is replaced Elitism & Cross-
with the Cross-over on over
the two


Comparing GA with SSGA in Localization

panmictic GA
(n=200)

SSGA
(WS, k=3, n=200)


Need a circular formation?


Multirobot


Thanks again!


A Fast Conjunctive Resampling Particle Filter for Collaborative Multi-Robot Localization

Recommended

Recommended

More Related Content

Recently uploaded

Recently uploaded (20)

Featured

Featured (20)

A Fast Conjunctive Resampling Particle Filter for Collaborative Multi-Robot Localization

Editor's Notes