MonteCarlo laser-based robot localization

Goal

The aim of this practical is to develop a localization algorithm based on the particle filter using the robot’s laser.

Montecarlo Laser Loc — MonteCarlo Laser Location.

Note: If you haven’t, take a look at the user guide to understand how the installation is made, how to launch a RADI and how to perform the exercises.

Robot API

import HAL - to import the HAL library class. This class contains the functions that receives information from the webcam.
import GUI - to import the GUI (Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.
HAL.setW() - to set the angular velocity
HAL.setV() - to set the linear velocity
GUI.showParticles(particles) - shows the particles on the map. Accepts a list of particles as an argument. Each particle must be a list with [position_x, position_y, angle_in_radians]. The values must be in gazebo world coordinate system.
GUI.showPosition(pose) - shows the estimated user position in the map view in blue. Accepts a list with [position_x, position_y, angle_in_radians]. The values must be in gazebo world coordinate system. The map view will also show the real position of the robot in red, so you can compare how good your algorithm is.
GUI.getMap('/RoboticsAcademy/exercises/static/exercises/vacuum_cleaner_loc_newmanager/resources/mapgrannyannie.png') - Returns a numpy array with the image data in a 3 dimensional array (R, G, B, A). The image is 1024x1024.
GUI.mapToPose(pose)- converts a map pixel to gazebo world coordinate system position.
GUI.poseToMap(pose)- converts a gazebo world coordinate system position to a map pixel.

Theory

Probabilistic localisation seeks to estimate the position of the robot and the model of the surrounding environment:

A. Probabilistic motion model. Due to various sources of noise (bumps, friction, imperfections of the robot, etc.) it is very difficult to predict the movement of the robot accurately. Therefore, it would be more convenient to describe this movement by means of a probability function, which will move with the robot’s movements [1].
B. Probabilistic model of sensory observation. It is related to the sensor measurements at each instant of time. This model is built by taking observations at known positions in the environment and calculating the probability that the robot is in each of these positions [2].
C. Probability fusion. This consists of accumulating the information obtained at each time instant, something that can be done using Bayes’ theorem. This fusion achieves that in each observation some modes of the probability function go up and others go down, so that as the number of iterations advances, the probability will be concentrated in only one of the modes, which will indicate the position of the robot [3].

The following figure shows an example of probabilistic localisation. In the first phase, the robot does not know its initial state, the initial probability distribution is uniform. In the second phase, the robot is looking at a door, the sensory observation model determines that there are three zones or modes with equal probability of being the zone where the robot is. In the third phase, the robot is moving forward so the probabilistic motion model is applied, the probability distribution should move the same distance that the robot has moved, but as estimating the motion is difficult, what is done is to smooth it. In the last phase, the robot detects another door and this observation is merged with the accumulated information. This causes the probability to concentrate on a single possible area where it can be found, and thus ends the global localisation process.

Montecarlo

Monte Carlo localisation is based on a collection of particles or samples. Particle filters allow the localisation problem to be solved by representing the a posteriori probability function, which estimates the most likely positions of the robot. The a posteriori probability distribution is sampled, where each sample is called a particle [4].
Each particle represents a state (position) at time t and has an associated weight. At each movement of the robot, they perform a correction and decrease the accumulated error. After a number of iterations, the particles are grouped in the zones with the highest probability, until they converge to a single zone, which corresponds to the robot’s position.
When the programme starts, the robot does not know where it is. However, the actual samples are evenly distributed, and the importance weights are all equal. evenly distributed, and the importance weights are all equal. After a long time, the samples near the current the current position are more likely, and those further away are less likely. The basic algorithm is as follows:
1. Initialise the set of samples. Their locations are evenly distributed and have the same weights.
2. Repeat for each sample until: a) Move the robot a fixed distance and read the sensor. b) For each particle, update the location. c) Assign the importance weights of each particle to the probability of that sensor, and read that new location.
3. Create a collection of samples, by sampling with replacement from the current set of samples, based on their importance weights.
4. Let the group become the current round of samples.

Diagram of the particle filter algorithm

The following figure shows an example of the operation of the particulate filter. At the initial instant the particles are uniformly distributed in the environment. As new observations are obtained, the particles accumulate in probability zones until they converge to the probability zone [5].

Evolution of particles — Probabilistic location model

Contributors

Contributors: Jose María Cañas, Vladislav Kravchenko
Maintained by David Valladares

Goal

Robot API

Theory

Montecarlo

Contributors

References