Monte Carlo Laser Localization

This is the blog of the P5-RM, where I programmed a Monte Carlo Laser Localization algorithm.

Objetive

The objective of this project is to program a localization algorithm based on the Monte Carlo algorithm. The developed algorithm must estimate the robot's position on the map. To achieve this, the robot is equipped with a laser sensor and has access to the environment map.

Introduction to Monte Carlo Laser Localization

Monte Carlo Laser Localization (MCLL) is a probabilistic algorithm used in robotics for determining a robot's position and orientation within a given environment. By utilizing laser range finders, MCLL samples a set of possible positions (particles) and updates their likelihood based on sensor data. This method allows for accurate and efficient localization even in complex and dynamic environments, making it a popular choice for autonomous navigation systems.

How does it works?

1. A number of particles are randomly generated within the map boundaries and in free cells, that is, cells with a value of 255.
2. The program calculates the robot's relative position using the average x, y, and yaw of all the particles.
3. Using ray tracing and the robot's actual laser, the differences between the real measurement and each of the measurements made by the particles are calculated.
4. Weights inversely proportional to the difference between the simulated and real measurements (less error, more weight) are assigned to each particle nd then normalizing all of these, considering whether they are in a valid position on the map.
5. The program performs resampling of the particles based on their weights, selecting those with higher weights (less error) with greater probability to focus the estimation on the most accurate robot positions.
6. The particles move based on the robot's linear and angular velocity and the time that has passed since the previous iteration.
7. Repeats from step 2

Parameters

After several tests, the parameters I have left in the code are:

# Map Data
MAP_FREE = 255
MAP_OCC = 0
MAP_URL = '/resources/exercises/montecarlo_laser_loc/images/mapgrannyannie.png'

# Raytracing and Laser Data
MAX_LASER_DISTANCE = 10
LASER_NUM_BEAMS = 3
RAYTRACING_SKIP_STEPS = 1

# Noise parameters
PROPAGATION_XY_NOISE_STD = 0.005
PROPAGATION_ANGLE_NOISE_STD = 0.001

# Constant robot velocities
LINEAR_VEL = 0.2
ANGULAR_VEL = 0.2

As for the number of particles used, I recommend using between 150 and 1500 particles. Fewer particles would make it quite inaccurate, and more particles would slow down the process due to the increased number of calculations.

Difficulties

• It failed a lot because at first it considered particles that were inside obstacles and/or outside the map. I fixed this by adding a couple of functions that automatically assign a weight of 0 to these particles.
• Finding the appropriate values is important, as performance and accuracy would decrease or increase significantly depending on the number of particles, beams, etc.
• I think it can still be fine-tuned more, but I'm not sure why it doesn't converge exactly. It might be due to the weight assignment method.

Functionality

50 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/NxI02HzUS5k)

150 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/LUVkUov7j7I)

300 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/K9s6kuokz18)

750 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/tKw9rA3ff_Q)

1500 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/mbrDgXKhjvM)