1. Bologna 6-8 September Genetic Approach for a Localisation Problem based upon Particle Filters A. Gasparri, S. Panzieri, F. Pascucci, G. Ulivi Dipartimento Informatica e Automazione Università degli Studi “Roma Tre” 8th International IFAC Symposium on Robot Control SYROCO 2006

2. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 2 29 Outline Robot Localisation Bayesian Framework Particle filters Proposed Algorithm –Weight Computation –Clustering –Genetic Resampling –Examples Conclusion

3. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 3 29 Robot Localisation It is the problem of estimating the robot pose for a robot moving in a known environment relying on data coming from sensors. Localisation problem definition: Localisation problem importance: Localisation = Realise the robot autonomy Localisation = Find out the pose (x,y,  )

4. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 4 29 Bayesian Framework The system is modeled by sthocastic equations The state represents the robot pose A predictor/corrector Bayesian Filter is applied to recursively solve the localisation problem

5. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 5 29 Algorithm Taxonomy Kalman filters (KF, EKF, UKF) –Continuous space state –Gaussian distributions Particle Filters –Discrete space state –Limited number of states –Multi-modal distributions

6. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 6 29 Particle Filters The posterior distribution function (p.d.f.) is represented by means of a set N S of weighted samples.The posterior distribution function (p.d.f.) is represented by means of a set N S of weighted samples. where where In this way it is possible to approximate the continous posterior density at a generic k-step as:In this way it is possible to approximate the continous posterior density at a generic k-step as: N S → ∞: The approximation tends to the p.d.f.N S → ∞: The approximation tends to the p.d.f.

7. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 7 29 Degeneracy problem It is the problem of having most samples with a negligible weight after few iterations.It is the problem of having most samples with a negligible weight after few iterations. Possible solutions:Possible solutions: –Increase the number of particles –Performe a resampling step

8. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 8 29 Particle Filters schema Resampling Prediction Weight Computation Each hypothesis evolves independently according to system model and inputs A weight is computed for each hypothesis according to the robot sensor data and the expected one Each particle represents a robot pose within the environment where the weight defines its likelihood Each particle represents a robot pose within the environment where the weight defines its likelihood Unlikely hypotheses with a negligible weight are cut off and replaced by ones with a higher weight

9. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 9 29 Weight Computation Let’s call: Let’s call: - z i j the j-th laser beam measure related to the i-th particle - z i j the j-th laser beam measure related to the i-th particle - z j the j-th laser beam measure related to the real robot - z j the j-th laser beam measure related to the real robot Each weight can be obtained by means of the quadratic error: Each weight can be obtained by means of the quadratic error: Each estimated measure is compared with the relative one coming from the real robot

10. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 10 29 Clustered Genetic Resampling The proposed resampling approach introduces two strategies: Dynamical clustering Genetic action The resampling is triggered by the following threshold:

11. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 11 29 Dynamical Clustering Clusterization is performed regarding to the spatial coordinates (x,y)Clusterization is performed regarding to the spatial coordinates (x,y) The euclidean distance is used as similarity metricThe euclidean distance is used as similarity metric As a result a limited number of clusters are obtainedAs a result a limited number of clusters are obtained

12. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 12 29 Genetic Action Random Crossover Useful to recover the robot location if a kidnap occurs Creates new particles combinig parent’s chromosomes Mutation Selects new particles within a specified area

13. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 13 29 Simulation Framework (I) The algorithm has been tested using a simulation environment developed on Matlab Simulations have been done according to the following robot configuration: ParameterDescriptionValue LBeams Number16 vVelocity0.4 [m/s] n_x,yModel Noise±10 [cm] n_  Model Noise±0.1 [rad]

14. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 14 29 Simulation Framework (II) Several office-like environments have been considered to better understand the algorithm behaviour A comparison with the classical SR Particle Filter has been performed Two different indexes of quality have been considered: –Number of iterations –Average pose estimation error

15. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 15 29 Asymmetrical environment Particles Most likely particle Real Robot Pose Laser becon x [meters] y [meters]

16. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 16 29 Asymmetrical environment Real Robot Pose Most likely particle x [meters] y [meters]

17. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 17 29 Symmetrical environment Most likely particle Real robot pose x [meters] y [meters]

18. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 18 29 Symmetrical environment Real robot pose Most likely particle x [meters] y [meters]

19. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 19 29 Symmetrical environment Posizione del robot Most likely particle Real robot pose x [meters] y [meters]

20. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 20 29 Symmetrical environment Real robot pose Most likely particle x [meters] y [meters]

21. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 21 29 Highly symmetrical environment Most Likely Particle Real Robot Pose x [meters] y [meters]

22. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 22 29 Highly symmetrical environment Real Robot Pose Most likely particle x [meters] y [meters]

23. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 23 29 Highly symmetrical environment Real robot pose Most likely particle x [meters] y [meters]

24. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 24 29 Highly symmetrical environment Real robot pose Most likely particle x [meters] y [meters]

25. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 25 29 Simulation Results Convergence Velocity CGR SR

26. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 26 29 Simulation Results Absolute Average Error SR CGR

27. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 27 29 Conclusion (I) A preliminary study for an improved resampling approach has been proposed. The approach relies on: –a suitable clustering to partition the particles set –a genetic action to apply within each partition The resulting algorithm is able to solve both the global localisation and the kidnap problem. The resulting algorithm turns out to be robust : –in presence of noise on sensor data –in presence of process noise –in presence of systematic errors

28. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 28 29 Conclusion (II) A.Gasparri, S. Panzieri, F. Pascucci, G. Ulivi, “Monte Carlo Filter in Mobile Robotics Localization: A clustered Evolutionary Point of View”, to appear in the Journal of Intelligent and Robotic Systems –Slight different implementation of genetic operators –Improved clustering algorithm (DBSCAN) –Real robot experiments

29. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 29 Thank you!

30. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 30 29 Future Works Real robot implementation Different clusterization methods Different genetic operators Dynamic environment localization Dynamical size of the population

31. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 31 29 The genetic engineering miracles! Thank you for your attention! Any questions?

32. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 32 29 Sequential Importance Sampling (SIS) Non potendo estrarre i campioni dalla p(.) li otteniamo da una q(.) (funzione di importanza scelta liberamente) L’approssimazione è corretta se scegliamo i pesi tali che Se poi assumiamo Possiamo aggiornare i pesi con la

33. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 33 29 Algorithm

34. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 34 29 Possible solutions Increase the number of particles –Computational overhead Ad-hoc choice of the importance function q(.) –e.g. choose the prior distribution function Resampling –Trying to keep the overhead low

35. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 35 29 Highly symmetrical environment Most likely particle Real robot pose

36. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 36 29 Algorithm Taxonomy Kalman filters (KF, EKF, UKF) –Continuous space state –Gaussian distributions Grid Based Filters –Discrete space state –Limited number of states Particle Filters –Discrete space state –Limited number of states –Multi-modal distributions

37. GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS 37 29 Highly symmetrical environment Real robot pose Most likely particle