Bologna 6-8 September Genetic Approach for a Localisation Problem based upon Particle Filters A....

transcript

Bologna6-8

September

Genetic Approach for a Genetic Approach for a

Localisation Problem based Localisation Problem based

upon Particle Filtersupon Particle Filters

A. Gasparri, A. Gasparri, S. Panzieri, F. Pascucci, G. UliviS. Panzieri, F. Pascucci, G. UliviDipartimento Informatica e Automazione

Università degli Studi “Roma Tre”

8th International IFAC Symposium on Robot Control

SYROCO 2006

GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS

Outline

• Robot Localisation

• Bayesian Framework

• Particle filters

• Proposed Algorithm– Weight Computation

– Clustering

– Genetic Resampling

– Examples

• Conclusion

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 autonomyLocalisation = Realise the robot autonomy

Localisation = Find out the pose (x,y,Localisation = Find out the pose (x,y,))

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

Algorithm Taxonomy

• Kalman filters (KF, EKF, UKF) – Continuous space state

– Gaussian distributions

• Particle Filters– Discrete space state

– Limited number of states

– Multi-modal distributions

Particle Filters• The posterior distribution function (p.d.f.) is The posterior distribution function (p.d.f.) is

represented by means of a set Nrepresented by means of a set NSS of weighted of weighted

samples.samples.

wherewhere

• In this way it is possible to approximate the In this way it is possible to approximate the

continous posterior density at a generic k-step as:continous posterior density at a generic k-step as:

• NNSS → ∞: → ∞: The approximation tends to the p.d.f. The approximation tends to the p.d.f.

ik wx 1},{

ikkk xxwZxp

Degeneracy problem

• It is the problem of having most samples It is the problem of having most samples with a negligible weight after few with a negligible weight after few iterations.iterations.

• Possible solutions:Possible solutions:

– Increase the number of particlesIncrease the number of particles

– Performe a resampling stepPerforme a resampling step

Particle Filters schema

ResamplingResampling

PredictionPrediction

Weight Weight

ComputatioComputatio

Each hypothesis evolves Each hypothesis evolves independently according to independently according to system model and inputssystem model and inputs

A weight is computed for each A weight is computed for each

hypothesis according to the robot hypothesis according to the robot

sensor data and the expected onesensor data and the expected one

• Each particle represents a robot pose within the Each particle represents a robot pose within the

environment where the weight defines its likelihoodenvironment where the weight defines its likelihood

Unlikely hypotheses with a Unlikely hypotheses with a negligible weight are cut off and negligible weight are cut off and replaced by ones with a higher replaced by ones with a higher weightweight

Weight Computation

• Let’s call:Let’s call:

- - zziij j the j-th laser beam measure the j-th laser beam measure

related to the i-th particlerelated to the i-th particle

- - zzjj the j-th laser beam measure the j-th laser beam measure

related to the real robotrelated to the real robot• Each weight can be obtained by Each weight can be obtained by

means of the quadratic error:means of the quadratic error:

Each estimated measure is compared with the Each estimated measure is compared with the

relative one coming from the real robotrelative one coming from the real robot

NumSens

Clustered Genetic Resampling

The proposed resampling approach introduces two strategies:

•Dynamical clustering•Genetic action

The resampling is triggered by the following threshold:

2)( )(

Dynamical Clustering• Clusterization is Clusterization is

performed regarding performed regarding

to the spatial to the spatial

coordinates (x,y)coordinates (x,y)

• The euclidean The euclidean

distance is used as distance is used as

similarity metricsimilarity metric

• As a result a limited As a result a limited

number of clusters number of clusters

are obtainedare obtained

Genetic Action

RandomRandom

CrossoverCrossover

Useful to recover the robot location if a kidnap occursUseful to recover the robot location if a kidnap occurs

Creates new particles Creates new particles

combinig parent’s chromosomescombinig parent’s chromosomes

MutationMutation

Selects new particles within Selects new particles within

a specified areaa specified area

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:Parameter Description Value

L Beams Number 16

v Velocity 0.4 [m/s]

n_x,y Model Noise ±10 [cm]

n_ Model Noise ±0.1 [rad]

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

Asymmetrical environment

Particles

Most likely particle

Real Robot Pose

Laser becon

x [meters]

Asymmetrical environment

2k 20k

Real Robot Pose

x [meters]

Symmetrical environment1k

Real robot pose

x [meters]

Symmetrical environment2k 20k

Real robot pose

x [meters]

Symmetrical environment

Posizione del robotPosizione del robot

Posizione del robot

Real robot pose

x [meters]

Symmetrical environment500SN 80k

Real robot pose

x [meters]

Highly symmetrical environment

Most Likely

Particle

Real Robot Pose

500SN 20k

x [meters]

Real Robot Pose

500SN 40k

x [meters]

Real robot pose

20k500SN

x [meters]

Highly symmetrical environment 40k

Real robot pose

x [meters]

Simulation ResultsConvergence Velocity

Simulation ResultsAbsolute Average Error

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

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

Thank you!

Future Works

• Real robot implementation

• Different clusterization methods

• Different genetic operators

• Dynamic environment localization

• Dynamical size of the population

The genetic engineering miracles!

Thank you for your

attention!

Any questions?

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

:1:0ik

),|(),|( :11:11:0 kkkkkk zxxqzxxq

)|()|(

kik zxxq

xxpxzpww

Algorithm

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

)|(),|(

xxpzxxq

Real robot pose

Algorithm Taxonomy

• Kalman filters (KF, EKF, UKF) – Continuous space state

– Gaussian distributions

• Grid Based Filters– Discrete space state

• Particle Filters– Discrete space state

– Multi-modal distributions

Real robot pose

Bologna 6-8 September Genetic Approach for a Localisation Problem based upon Particle Filters A....

Documents