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

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

229

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

329


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,))

429


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

529


Algorithm Taxonomy

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

– Gaussian distributions

• Particle Filters– Discrete space state

– Limited number of states

– Multi-modal distributions

629


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.

SNi

ik

ik wx 1},{

11

SN

i

ikw

S

i

ikk

ikkk xxwZxp

1

)()|(

iw

ix

729


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

829


Particle Filters schema

ResamplingResampling

PredictionPrediction

Weight Weight

ComputatioComputatio

nn

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

929


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

j

ijj

ik

zzw

1

2)(

1029


Clustered Genetic Resampling

The proposed resampling approach introduces two strategies:

•Dynamical clustering•Genetic action

The resampling is triggered by the following threshold:

N

i

ik

effw

N1

2)( )(

1

1129


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

1229


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

1329


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]

1429


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

1529


Asymmetrical environment

1k

Particles

Most likely particle

Real Robot Pose

Laser becon

500SN

x [meters]

y [m

eter

s]

1629


Asymmetrical environment

2k 20k

Real Robot Pose


500SN

x [meters]

y [m

eter

s]

1729


Symmetrical environment1k


Real robot pose

500SN

x [meters]

y [m

eter

s]

1829


Symmetrical environment2k 20k

Real robot pose


500SN

x [meters]

y [m

eter

s]

1929


Symmetrical environment

Posizione del robotPosizione del robot

Posizione del robot

60k


Real robot pose

500SN

x [meters]

y [m

eter

s]

2029


Symmetrical environment500SN 80k

Real robot pose


x [meters]

y [m

eter

s]

2129


Highly symmetrical environment

Most Likely

Particle

Real Robot Pose

500SN 20k

x [meters]

y [m

eter

s]

2229



Real Robot Pose


500SN 40k

x [meters]

y [m

eter

s]

2329



Real robot pose


20k500SN

x [meters]

y [m

eter

s]

2429


Highly symmetrical environment 40k

Real robot pose


500SN

x [meters]

y [m

eter

s]

2529


Simulation ResultsConvergence Velocity

CGR

SR

2629


Simulation ResultsAbsolute Average Error

SR

CGR

2729


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

2829


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

2929


Thank you!

3029


Future Works

• Real robot implementation

• Different clusterization methods

• Different genetic operators

• Dynamic environment localization

• Dynamical size of the population

3129


The genetic engineering miracles!

Thank you for your

attention!

Any questions?

3229


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:0

:1:0ik

ik

ik

iki

k zxq

zxpw

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

),|(

)|()|(

:11

11

kik

ik

ik

ik

ikki

kik zxxq

xxpxzpww

3329


Algorithm

3429


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

)|(

)|(),|(

11

11

ikk

ik

ik

kkkkk

xzpww

xxpzxxq

3529



70k


Real robot pose

500SN

3629


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

3729



2k

Real robot pose


500SN

Date post:	15-Jan-2016
Category:	Documents
Upload:	solomon-roberts
View:	212 times
Download:	0 times

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

Documents