a fast conjunctive resampling particle filter for collaborative multi-robot localization
DESCRIPTION
Presentation at AAMAS 2008 - Workshop on Formal Models and Methods for Multi-Robot SystemsTRANSCRIPT
AA
MA
S 2
00
8 –
Est
ori
l -P
ort
ugal
Stefano Panzieri
A Fast Conjunctive Resampling Particle Filter for Collaborative Multi-Robot Localization
Andrea Gasparri, Stefano Panzieri, Federica Pascucci
Dept. Informatica e AutomazioneUniversity “Roma Tre”, Rome, Italy
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 2
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Outline
◊ The mobile robot localization problem◊ The probabilistic framework
◊ Bayesian approach
◊ Particle Filter◊ Formulation ◊ Pros & Cons
◊ The fast Conjunctive Resampling technique◊ Main features
◊ Performance Analysis◊ Simulations
◊ Conclusion and Future Work◊ Simulations and experimental results
◊ A Spatially Structured Genetic Algorithm framework
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 3
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 4
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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.
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 5
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 6
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 7
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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:
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 8
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 9
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
◊ The robot moves according to the unicycle model
◊ Where
◊ We suppose the robot equipped with laser rangefinders, and the environment described by a set M of segments.
◊ The observation model is
Particle Filter for Robot Localization
y
x
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 10
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
x
x
x
x
1z2z
3z
1z2z
3z
Real robot Hypothesis
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 11
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
x
x
AR
BR
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 12
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
A previous approach
◊ 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.
◊ 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!
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 13
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
◊ D. Fox, W. Burgard, H. Kruppa, and S. Thrun
Reconstruct Belief using a density tree
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 14
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
The Fast Conjunctive Resampling Main Features
Conjunction:◊ The conjunction of the best estimates
consists of substituting low weightparticles of one robot with othershaving high weight on remoterobots propagation
Propagation:◊ The propagation of sensory data
consists of an exchange of laserreadings that can be exploited tosolve environmental ambiguities
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 15
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 16
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Propagation of sensory data
1
ARz
x
x
x
x
x
1
BRz
2
BRz
,A BR Rz
2
ARz
3
ARz AR
BR
Integrate observations coming from robot Integrate observations coming from robot RRBB into weight evaluation of particles of into weight evaluation of particles of robot robot RRAA
◊ Using both sensory data only particles fitting well on both locations will survive
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 17
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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.
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 18
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 19
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisFirst Environment
◊ 4 Robots◊ Ambiguous Environment◊ 100 Trials◊ Partial Communication
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 20
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisEstimation Accuracy
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 21
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisSuccessful Trials
# 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
# Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials
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
Autonomous Localization
Collaborative Localization
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 22
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisSecond Environment
◊ 3 Robots◊ Structural
Similarities◊ 100 Trials◊ Partial
Communication
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 24
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisEstimation Accuracy
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 25
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Performance AnalysisSuccessful Trials
# Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials
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
# Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials
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
Autonomous Localization
Collaborative Localization
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 26
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Considerations
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 27
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Future Work
◊ A deeper investigation on the inter-dependence among beliefs when performing conjunction
◊ An implementation of the proposed approach in a real context
AA
MA
S 2
00
8 –
Est
ori
l -P
ort
ugal
Stefano Panzieri
Thanks!
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 29
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
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
◊ It can be seen not only like an improvement of panmictic populations but also a source of new and original dynamics
a regular lattice
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 30
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Small World networks
◊ Watts-Strogatz Algorithm1. Start with a lattice network with
degree k
2. 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
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 31
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Evolving with a Genetic Mating-Rule
1 2
Node 1 Node 2 Action Basic principles
LOW LOW Both Self-Mutate Mutation
HIGH/LOW LOW/HIGH Node 2/1 is replaced with a Mutation of Node 1/2
Elitism & Mutation
HIGH HIGH The lower is replaced with the Cross-over on the two
Elitism & Cross-over
Compute a mean fitness over the net
Then, for each link, compare the two finesses
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 32
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Comparing GA with SSGA in Localization
panmictic GA (n=200)
SSGA (WS, k=3, n=200)
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 33
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Need a circular formation?
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 34
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Multirobot
Roboti
ca A
uto
nom
a &
Fusi
one S
enso
riale
Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 35
AA
MA
S 2
00
8 –
Est
ori
l -
Port
ugal
Thanks again!