Genetic Approach for Robust Robot Localization with Particle Filters

1. 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 ControlSYROCO 2006

2. Outline • Robot Localisation • Bayesian Framework • Particle filters • Proposed Algorithm • Weight Computation • Clustering • Genetic Resampling • Examples • Conclusion

3. 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 = Find out the pose (x,y,) Localisation = Realise the robot autonomy

4. 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. 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. Particle Filters • The posterior distribution function (p.d.f.) is represented by means of a set NS of weighted samples. where • In this way it is possible to approximate the continous posterior density at a generic k-step as: • NS → ∞: The approximation tends to the p.d.f.

7. Degeneracy problem • It is the problem of having most samples with a negligible weight after few iterations. • Possible solutions: • Increase the number of particles • Performe a resampling step

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

9. Weight Computation Each estimated measure is compared with the relative one coming from the real robot • Let’s call: - zij the j-th laser beam measure related to the i-th particle - zjthe j-th laser beam measure related to the real robot • Each weight can be obtained by means of the quadratic error:

10. Clustered Genetic Resampling The proposed resampling approach introduces two strategies: • Dynamical clustering • Genetic action The resampling is triggered by the following threshold:

11. Dynamical Clustering • Clusterization is performed regarding to the spatial coordinates (x,y) • The euclidean distance is used as similarity metric • As a result a limited number of clusters are obtained

12. Genetic Action Mutation Crossover Selects new particles within a specified area Creates new particles combinig parent’s chromosomes Random Useful to recover the robot location if a kidnap occurs

13. 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:

14. 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. Asymmetrical environment Particles Most likely particle y [meters] Real Robot Pose Laser becon x [meters]

16. Asymmetrical environment Real Robot Pose y [meters] Most likely particle x [meters]

17. Symmetrical environment Most likely particle y [meters] Real robot pose x [meters]

18. Symmetrical environment Most likely particle y [meters] Real robot pose x [meters]

19. Symmetrical environment Real robot pose y [meters] Most likely particle Posizione del robot Posizione del robot Posizione del robot x [meters]

20. Symmetrical environment Real robot pose Most likely particle y [meters] x [meters]

21. Highly symmetrical environment Real Robot Pose Most Likely Particle y [meters] x [meters]

22. Highly symmetrical environment Real Robot Pose y [meters] Most likely particle x [meters]

23. Highly symmetrical environment Real robot pose Most likely particle y [meters] x [meters]

24. Highly symmetrical environment Most likely particle y [meters] Real robot pose x [meters]

25. Simulation Results Convergence Velocity SR CGR

26. Simulation Results Absolute Average Error SR CGR

27. 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. 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. Thank you!

30. Future Works • Real robot implementation • Different clusterization methods • Different genetic operators • Dynamic environment localization • Dynamical size of the population

31. The genetic engineering miracles! Thank you for your attention! Any questions?

32. Sequential Importance Sampling (SIS) • Non potendo estrarre i campioni dalla p(.) li otteniamo da una q(.) (funzione di importanzascelta liberamente) • L’approssimazione è corretta se scegliamo i pesi tali che • Se poi assumiamo • Possiamo aggiornare i pesi con la

33. Algorithm

34. 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. Highly symmetrical environment Real robot pose Most likely particle

36. 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. Highly symmetrical environment Real robot pose Most likely particle