Monte Carlo Localization
Project Overview
For this project, we will be learning and implementing Monte Carlo Localization (MCL), a popular and broad mobile robot localization algorithm. Mobile robot localization is one of the most important aspects of any robotics motion planning or SLAM problem.
About Us
Evan Lockwood
Olin College of Engineering Class of 2025
Major: Robotics Engineer, Engineering with Computing
Tyler Ewald
Olin College of Engineering Class of 2025
Major: Mechanical Engineering, Electircal Engineernig
Mobile Robot Localization
The goal of mobile robot localization is to find the pose of a robot in a given environment's map. Localization proves to be one of the fundamental foundation in any robotics system: knowledge of the robot's location within a given environment. The problem of accurately tracking a robot's location is that there is no sensor for directly that can perfectly measure it's pose (location and orientation). Various types of sensors such as cameras, encoders, LiDARs, etc. are needed to determine the robot's location.
Localization has been a problem in robotics since its inception, and while the problem is not new, multiple techniques have been developed throughout the years. The Probabilistic Robotics textbook best describes these techniques, as well as the various types of localization problems. There are several types of localization situations that are dependent on the initial information that is available. They are as follows:
Position tracking- The initial robot pose is known. The surrounding environment is not accounted for.
Global localization- The initial robot pose is unknown. The environment is known.
Kidnapped robot problem- The initial robot pose and environment are unknown.
For this project we will be looking at a global localization problem, where the initial pose of the robot and a map of the environment is given. The robot does not know where it is in this environment. This process will be simplified by the fact that our given environment will be static, without any objects that change position and rotation over time. There are multiple methods to solve a global localization situation, all of which are variations of the Bayes filter. The two most well-known approaches are grid localization and the popular Monte Carlo localization.
Monte Carlo Localization
To tackle global localization, we used Monte Carlo Localization (MCL) via a particle filter. The basic concept of MCL can be described in 4 steps:
Generate Particles
Measure likelihood
Resample
Apply odometry
Repeat steps 2-4
This algorithm can be furthered visualized and explained by a simple 1-dimensional situation. The following figures were derived from Probabilistic Robotics [1].
Before beginning the MCL algorithm, particles have to be randomly generated in the system. Particles are initialized with a uniform distribution, with the robot having an equal likelihood of being at each particle.
After generating the initial set of particles, sensor data is taken from the robot. The resulting data is compared to the estimated sensor data of each particle. From this comparison, the likelihood the robot is at each at particle is calculated. From these calculations, a probability density function is calculated based on the probability of the robot being at each location. The PDF is then normalized to ensure it sums to one. As shown in the figure above, the probability and likelihood the robot is in front of each a door spikes in front of each door, since this is a notable feature that the robot is able to sense. If, for example, the robot was in front of the brick wall, the probabilities and likelihoods would be distributed across the wall and not in front of the doors, since these features share no similarities (for the sake of this example).
A new set of particles are generated using the PDF from the previous step. In Figure (c), this can be seen by the non-uniform distribution of particles on the graph below in the image. In Figure (a), when the particles were initialized, they were uniformly distributed across the environment, but as more information is gained, the distribution begins to lack uniformity. This highlights one of the many benefits of MCL: its use of Bayesian statistics to update probability throughout the duration of the function. This benefit means that as the program continues, the continuous updating of probability will help the new sets of particles further concentrate towards the robot. .
The other factor highlighted here is the use of odometry to in the generation of new particles. As shown above, the particles are slightly offset from the door. This is because sensors have indicated the robot has move forward a set distance, so the particles act accordingly.
As we did before, the likelihood of the robot being at each particle is calculated and used to form a PDF. One thign to note is that the p(z|x) graph looks identifcal to the stage 2 graph above, however the distrubution of particles is no longer uniform. This means that the regeneration of particles will now be more heavily focused on the actual location of the robot.
As mentioned in the previous section, when the particles are regenerated they are heaviest one the location of the robot. The particles will continue to concentrate on the robot with each iteration and begin to give a estimation of the robots location with a great deal of accuracy.
Implementation
Structure
To effectively simulate an environment and robot to implement MCL, we used Python and ROS2 Foxy. To visualize our simulation, RViz2 and Gazebo were used.
ROS2 (Robot Operating System) is a collection of libraries and software used in robotics applications. Foxy is the specific version of ROS2 we will be using. More information can be found here.
The gif to the right shows the mapping of our environment. This is done using simulated LiDAR data from our robot. As the robot moves through the environment, the grid colors change from gray-green to black or gray. Gray-green represents a grid square that has not been classified yet, while gray means unoccupied and black means occupied. The red seen throughout the gif represents laser scan points from the simulated LiDAR.
The Environment
The environment we chose for this implementation was a square room created in Gazebo, pictured to the left.
Gazebo is an open-source software with which you can model a vast range of simulated environments. More information about Gazebo can be found here.
This is a static environment that consists of four walls, 3 differently oriented boxes, and a barrel. It has been dubbed "The Gauntlet" by Olin College's Quantitative Engineering Analysis 2 teaching team, from who we obtained this map from.
Occupancy Grid Mapping
An occupancy grid is a binary grid representation of our given environment. A grid in our map can be represented as either occupied or unoccupied. The purpose of this grid is to create a simplified space for the robot to move through. In the physical world, this is very helpful for translating noisy sensor data into a comprehensive map that algorithm's and the robot can easily understand.
The gif to the right shows the mapping of our environment. This is done using simulated LiDAR data from our robot. As the robot moves through the environment, the grid colors change from gray-green to black or gray. Gray-green represents a grid square that has not been classified yet, while gray means unoccupied and black means occupied. The red seen throughout the gif represents laser scan points from the simulated LiDAR.
Final Live Demo
The gif above is a demonstration of our final implementation of MCL. On the right, you can see the the actual robot's orientation while on the left, the particles and simulated laser scan data can be seen. The particles are represented by red arrows, so visualizing the orientation of the particles is easier. During this demo, the robot was controlled manually via keyboard input. Since the robot is being controlled with teleoperation, we have to establish an initial orientation guess for the starting particles. This is the green arrow that is seen at the beginning of the gif. If a bag file was used instead of live control, an initial guess would not be necessary.
Throughout the course of the gif, you can also see our use of particle decay. As the robot moves through the space, particles are slowly removed from the environment. This technique would be more helpful in a larger and more unique environment. For a map of this size, decay does not appear to be necessary. Also, for this demonstration, the number of particles removed was not limited, as seen by the fact that at the end of the gif, no particles are left in the map.
Reflection and Next Steps
Monte Carlo Location is incredibly useful tool for localization. Although it does have its drawbacks (it often fails to work is a environment with a great deal of symmetry such as a warehouse), its ability to new sets of particles with increasing accuracy makes it a great tool to use in many situations.