Your SlideShare is downloading. ×
A Fast Conjunctive Resampling Particle Filter for Collaborative Multi-Robot Localization
Upcoming SlideShare
Loading in...5
×

Thanks for flagging this SlideShare!

Oops! An error has occurred.

×

Introducing the official SlideShare app

Stunning, full-screen experience for iPhone and Android

Text the download link to your phone

Standard text messaging rates apply

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

826
views

Published on

Presentation at AAMAS 2008 - Workshop on Formal Models and Methods for Multi-Robot Systems

Presentation at AAMAS 2008 - Workshop on Formal Models and Methods for Multi-Robot Systems

Published in: Technology

0 Comments
0 Likes
Statistics
Notes
  • Be the first to comment

  • Be the first to like this

No Downloads
Views
Total Views
826
On Slideshare
0
From Embeds
0
Number of Embeds
0
Actions
Shares
0
Downloads
13
Comments
0
Likes
0
Embeds 0
No embeds

Report content
Flagged as inappropriate Flag as inappropriate
Flag as inappropriate

Select your reason for flagging this presentation as inappropriate.

Cancel
No notes for slide
  • <number><number>
  • Transcript

    • 1. 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
    • 2. 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
    • 3. 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 Robotica Autonoma & Fusione Sensoriale ◊ Gaussian hypothesis: AAMAS 2008 – Estoril - Portugal ◊ 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 3
    • 4. 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: Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal ◊ 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. Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 4
    • 5. Probabilistic Framework ◊ A recursive formulation can be obtained by Applying the Total Probability Theorem, the Bayes’rule and some simplifying (Markov) assumptions Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal ◊ Due to computational difficulties of handling the above integral, approximations are required Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 5
    • 6. 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): Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal ◊ 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 6
    • 7. 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): Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal ◊ 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: Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 7
    • 8. Monte Carlo Integration Methods ◊ Advantages ◊ Ability to represent arbitrary densities ◊ Dealing with non-Gaussian noise Robotica Autonoma & Fusione Sensoriale ◊ Adaptive focusing on probable regions of state-space ◊ Issues AAMAS 2008 – Estoril - Portugal ◊ 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 8
    • 9. Particle Filter for Robot Localization ◊ The robot moves according to the unicycle model φ Robotica Autonoma & Fusione Sensoriale y AAMAS 2008 – Estoril - Portugal ◊ Where x ◊ We suppose the robot equipped with laser rangefinders, and the environment described by a set M of segments. ◊ The observation model is Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 9
    • 10. Perceptual model ◊ Any particle, i.e., a possible robot pose, differs from the real state in terms of the following quadratic distance error: Robotica Autonoma & Fusione Sensoriale ◊ Where is the vector of measured distances ◊ The perceptual model adopted is AAMAS 2008 – Estoril - Portugal x x ˆ z1 ˆ z2 z1 x z2 x ˆ z3 z3 x x Hypothesis Real robot Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 10
    • 11. 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 Robotica Autonoma & Fusione Sensoriale x AAMAS 2008 – Estoril - Portugal x RA x x RB Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 11
    • 12. 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 Robotica Autonoma & Fusione Sensoriale ◊ In this way the Belief update of one robot that takes into AAMAS 2008 – Estoril - Portugal 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. Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 12
    • 13. Reconstruct Belief using a density tree Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal ◊ D. Fox, W. Burgard, H. Kruppa, and S. Thrun Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 13
    • 14. The Fast Conjunctive Resampling Main Features Conjunction: Robotica Autonoma & Fusione Sensoriale ◊ The conjunction of the best estimates consists of substituting low weight particles of one robot with others AAMAS 2008 – Estoril - Portugal 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 14
    • 15. Conjunction ◊ Substitute lo weight particles of one robot with high weight ones projected from other robots ◊ We need a status for the particle: Robotica Autonoma & Fusione Sensoriale good, bad, new ◊ A particle is marked good during AAMAS 2008 – Estoril - Portugal input evolution if the weight of its ancestor is above a threshold ◊ During a resample crated particles are set new Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 15
    • 16. Propagation of sensory data x z1RA RA z2 Integrate observations coming from robot RB x into weight evaluation of particles of robot RA Robotica Autonoma & Fusione Sensoriale x RA RA z3 z1RB x z RA , RB AAMAS 2008 – Estoril - Portugal x z 2RB RB ◊ Using both sensory data only particles fitting well on both locations will survive Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 16
    • 17. Lock mechanism for data exchange ◊ Repeated exchange of information will simply result in over-convergence to a bogus result Robotica Autonoma & Fusione Sensoriale ◊ A simple locking mechanism can be introduced AAMAS 2008 – Estoril - Portugal ◊ 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. Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 17
    • 18. 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 Robotica Autonoma & Fusione Sensoriale sensory data (which already implies the re-computation of particles weights) AAMAS 2008 – Estoril - Portugal ◊ 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 18
    • 19. Performance Analysis First Environment ◊ 4 Robots ◊ Ambiguous Environment ◊ 100 Trials ◊ Partial Communication Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 19
    • 20. Performance Analysis Estimation Accuracy Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 20
    • 21. Performance Analysis Successful Trials Autonomous Localization # Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials Robotica Autonoma & Fusione Sensoriale 100 0.297 0.172 0.232 5 - 20 - 51 AAMAS 2008 – Estoril - Portugal 300 0.302 0.158 0.232 14 – 32- 72 500 0.272 0.167 0.222 17 - 40 - 87 Collaborative Localization # 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 21
    • 22. Performance Analysis Second Environment ◊ 3 Robots ◊ Structural Similarities ◊ 100 Trials Robotica Autonoma & Fusione Sensoriale ◊ Partial Communication AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 22
    • 23. Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 23
    • 24. Performance Analysis Estimation Accuracy Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 24
    • 25. Performance Analysis Successful Trials Autonomous Localization # Particles Max Err[m] Min Err [m] MeanErr[ m] Succ. Trials Robotica Autonoma & Fusione Sensoriale 100 0.145 0.117 0.129 23 - 39 – 59 AAMAS 2008 – Estoril - Portugal 300 0.103 0.079 0.089 57 - 66 – 81 500 0.081 0.063 0.073 67 – 76 - 92 Collaborative Localization # 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 25
    • 26. Considerations Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 26
    • 27. Future Work ◊ A deeper investigation on the inter-dependence among beliefs when performing conjunction Robotica Autonoma & Fusione Sensoriale ◊ An implementation of the proposed approach in a real context AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 27
    • 28. AAMAS 2008 – Estoril -Portugal Stefano Panzieri Thanks!
    • 29. 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 Robotica Autonoma & Fusione Sensoriale ◊ That means: give to the GA a topological structure AAMAS 2008 – Estoril - Portugal ◊ 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 29
    • 30. Small World networks ◊ Watts-Strogatz Algorithm  Start with a lattice network with degree k  Randomically (with probability p) Robotica Autonoma & Fusione Sensoriale a rewiring is made of each link moving the connection from one AAMAS 2008 – Estoril - Portugal node to an other ◊ Low Average Path length ◊ Fast propagation ◊ High Clustering coefficient ◊ Evolutionary niches Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 30
    • 31. Evolving with a Genetic Mating-Rule Compute a mean fitness over the net 2 1 Then, for each link, compare the two finesses Robotica Autonoma & Fusione Sensoriale Node 1 Node 2 Action Basic principles AAMAS 2008 – Estoril - Portugal 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 Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 31
    • 32. Comparing GA with SSGA in Localization panmictic GA (n=200) Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal SSGA (WS, k=3, n=200) Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 32
    • 33. Need a circular formation? Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 33
    • 34. Multirobot Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 34
    • 35. Thanks again! Robotica Autonoma & Fusione Sensoriale AAMAS 2008 – Estoril - Portugal Stefano Panzieri A Fast Conjunctive Resampling for Particle Filters - 35