a fast conjunctive resampling particle filter for collaborative multi-robot localization

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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


AA

MA

S 2

00

8 –

Est

ori

l -

Port

ugal

Performance AnalysisSuccessful 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


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


AA

MA

S 2

00

8 –

Est

ori

l -

Port

ugal

Considerations

Roboti

ca A

uto

nom

a &

Fusi

one S

enso

riale


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


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


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


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


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


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


AA

MA

S 2

00

8 –

Est

ori

l -

Port

ugal

Multirobot

Roboti

ca A

uto

nom

a &

Fusi

one S

enso

riale


AA

MA

S 2

00

8 –

Est

ori

l -

Port

ugal

Thanks again!

a fast conjunctive resampling particle filter for collaborative multi-robot localization

Technology

estoril portugal prior

estoril portugal degeneracy

estoril portugal whereis

italy stefano panzieri

possible robot

multi robot approach

space issues aamas

posterior belief bel