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
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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 APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Asymmetrical environment
1k
Particles
Most likely particle
Real Robot Pose
Laser becon
500SN
x [meters]
y [m
eter
s]
1629
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Asymmetrical environment
2k 20k
Real Robot Pose
Most likely particle
500SN
x [meters]
y [m
eter
s]
1729
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Symmetrical environment1k
Most likely particle
Real robot pose
500SN
x [meters]
y [m
eter
s]
1829
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Symmetrical environment2k 20k
Real robot pose
Most likely particle
500SN
x [meters]
y [m
eter
s]
1929
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Symmetrical environment
Posizione del robotPosizione del robot
Posizione del robot
60k
Most likely particle
Real robot pose
500SN
x [meters]
y [m
eter
s]
2029
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Symmetrical environment500SN 80k
Real robot pose
Most likely particle
x [meters]
y [m
eter
s]
2129
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment
Most Likely
Particle
Real Robot Pose
500SN 20k
x [meters]
y [m
eter
s]
2229
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment
Real Robot Pose
Most likely particle
500SN 40k
x [meters]
y [m
eter
s]
2329
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment
Real robot pose
Most likely particle
20k500SN
x [meters]
y [m
eter
s]
2429
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment 40k
Real robot pose
Most likely particle
500SN
x [meters]
y [m
eter
s]
2529
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Simulation ResultsConvergence Velocity
CGR
SR
2629
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Simulation ResultsAbsolute Average Error
SR
CGR
2729
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Thank you!
3029
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Future Works
• Real robot implementation
• Different clusterization methods
• Different genetic operators
• Dynamic environment localization
• Dynamical size of the population
3129
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
The genetic engineering miracles!
Thank you for your
attention!
Any questions?
3229
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Algorithm
3429
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment
70k
Most likely particle
Real robot pose
500SN
3629
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
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
3729
GENETIC APPROACH FOR A LOCALISATION PROBLEM BASED UPON PARTICLE FILTERS
Highly symmetrical environment
2k
Real robot pose
Most likely particle
500SN