It is strongly recommended to use Internet
Explorer and Windows Media Player (version 6.4 or above)
Robot
Localization
A parametric localization consists on identify the pose (x,y position and
orientation) of the vehicle (robot) into a map. This localization has to
be improved using the information acquired by sensors (as a range-finder sensor).
The localization problem solved with a probabilistic approach is robust in
face of sensor limitations, sensor noise and environment dynamics as referred in
[7], where the author identifies the global localization problem as particularly
challenging. Using a probabilistic approach, the localization turns into an
estimation problem with the evaluation and propagation of a probability density
function, see [6]. Within the context of mobile robot localization, this type of
approaches are often referred to as Markov Localization (ML) or Hidden Markov
Models (HMM). See [8].
The next figure illustrates the main goal of localization.
Particle Filter
There
are several approaches to the localization problem as the Monte-Carlo simulation
method, well known as Particle Filter. The main objective of particle
filtering is to track a variable of interest, typically with non-Gaussian
and potentially multi-modal probability distribution function pdf.
Multiple copies (particles) of the variable of interest are used, each one
associated with a weight that signifies the quality of that specific particle. A
more detailed description of the Monte-Carlo simulation method (particle
filtering) used in order to implement Bayesian framework localization is
described in [1].
Simulation
using MATLAB
I developed a small program to simulate particle filter to estimate the
trajectory of a mobile robot using a laser range-sensor. The observation are
also simulated, however the program is prepared to work with real data acquired
by a real range-sensor. I assumed that the map is known and modeled as a set of
straight-lines, defined each one by two points. The direction and the number of
straight-lines is not important. The program shows the real path (defined by a
set of via points), the particles evolution, the respective weights and the
observations. The interface is not optimized, but it is sufficient to illustrate
as the particle filter works! For more information, send me an e-mail.
Path
Planning
The navigation is explained as follows. The robot has to follow some via
points (respecting the input order), without restriction to the direction.
No obstacle avoidance was implemented. It is necessary to know the pose of the
vehicle to control the Translation and Rotation velocities. However in some
examples it is assumed that the real pose is known, just to illustrate other
details.
(If your browser does not support embed Windows Media Player, click here)
With
NO OBSERVATIONS
With no observations, the vehicle's pose is estimated by
dead-reckoning. The weights are not updated and, consequently, the particles are
equally important. The uncertainty is increasing, as depicted in the following
pictures. The path is planned without pose estimation, just to illustrate
the uncertainty evolution.
The simulation were made with 200 particles, where the initial
pose was known. The red path is the real trajectory while the black
is the estimated one. The blue cloud corresponds to the set of particles, which
is becoming sparse along time.
(If your browser does not support embed Windows Media Player, click here)
Range
Finder Simulation
So, it is necessary a sensor. In this case it was
simulated a Laser range finder. Real measurements can be observed in the
page of the RESCUE project.
The observation is a vector, where the values are the distances to an
obstacle (hall, object, etc) according to a set of directions. It
is also simulated the maximum range and the noise in the measurements. The
video gives an idea of the observations.
(If your browser does not support embed Windows Media Player, click here)
Particle
Filter Results
These are the localization results, using 200 particles, where the initial
pose is known. All the particles were initialized with the initial pose of the
vehicle and uniform weights.
The navigation was evaluated using the estimated
pose of the vehicle, which corresponds to the best particle (or the particle
with highest weight). The red and black paths are the real
and the estimated trajectories, respectively.
(If your browser does not support embed Windows Media Player, click here)
The next picture illustrates the particles evolution. The
uncertainty shrinks periodically, as a result of the re-sampling step. The
best particles (with large weights) provides good estimation according to the
acquired measurements. Sometimes small set of particles diverge, since some different places provide similar observations.
When the initial pose is not known, it is necessary to modify the
initialization criteria. For instance, uniform along the map. This requires more
and more particles! Otherwise, the particles (could) converge to unexpected places
(local minimums), as illustrated in the "Drawbacks".
The next 2 videos illustrate the particle filter algorithm, where the vehicle
starts from different places and the initial pose is not known. The estimated
path is not shown, to simplify the visualization.
(If your browser does not support embed Windows Media Player, click here)
(If your browser does not support embed Windows Media Player, click here)
As
described previously, it is necessary to resample the particles, according to
their weights. It addresses two points: the resampling method and the
appropriate time to do that. The author of [1] suggests the Effective Sample
Size (ESSt), using the number of particles (M) and the coefficient of
variation cvt2. When the ESSt is greater then a
threshold value, the particles are resampled.
The
evaluation of the weights in two consecutive iterations, where the second is the
resampling result.
Drawbacks
In this example, the number of particles was too small yielding the pose
estimation to a wrong place. Since the navigation is dependent to estimation, the vehicle can not follow the via points. The vehicle is
lost! How to solve this rough situation? Increasing the number of particles is
the obvious solution, but it becomes a computational burden!
(If your browser does not support embed Windows Media Player, click here)
Usually there are different places in the map, where the robot acquire
similar observations. In these situations, when the initial pose is not known,
the estimation pose converge to the right trajectory...
(If your browser does not support embed Windows Media Player, click here)
(If your browser does not support embed Windows Media Player, click here)
(If your browser does not support embed Windows Media Player, click here)
... or simply lost! The map is so symmetric that the information acquired by
the sensor is never enough to aim a correct estimation!
(If your browser does not support embed Windows Media Player, click here)
Challenges
Choose the appropriate number of particles and an initialization
procedure
How to identify that the estimation path is wrong? (for
instance: a bad initialization or a few number of particles)
The number of particle could be dynamic through time
How to estimate the path? Using the best particle (with the
highest weight)?
Simultaneous Localization And Mapping (SLAM) - not
assuming the map is known; there are several papers about this topic
Matching between observations and the map: how
dependent is the Particle Filter to the matching?
Particle Filtering for cooperative localization
Experimental results with real laser measurements
(maybe soon with an ATRV-Jr!)
