Localization

Particle Filter

The goal of this project is to determine the robot’s pose in the world frame (localization) using a particle filter with information from the robot’s LiDAR sensor and odometry. The particle filter generates and resamples particles within the bounds of the map based on its confidence of where the robot could be. Then, it compares the LiDAR sensor data that is plotted onto each particle and compares it with the real map. Skills used in this project are:

ROS2: subscribes to image topics and publishes array of clusters (custom messages)
Bayesian filters: particle Filter

github: https://github.com/oscarbaox/robot_localization

Brief methodology for this project is shown below. Full writeup is in Github README.

Nodes and Topics Layout

The particle filter has four main steps: initialization, update with odometry, weight assignment, and resampling. Throughout the steps, the particles are normalized to ensure consistency. At the end of a cycle, we will use the particles and their weights to determine the pose estimate of the robot in the world frame.

The two topics subscribed to are /scan and /odom. The scan topic provides the LiDAR data used in the weight assignment step. The odom topic provides information on the robot’s movement which is used in the update with odometry step.

Particle Update

We assigned weight based on the LiDAR readings from the Turtle Bot. For each LiDAR reading passed in, we translate it from the original polar coordinates to Cartesian coordinates, and offset it by each particle’s position and heading. With each particle's weight calculated, we normalize them and ship it off to resampling. More detailed information on particle weight assignment in write up.

Particle Resampling

Initially, we had problems with the filter converging at a local minimum. And since all particles are fairly confident, they slowly collapsed into one point and the filter was stuck. To tackle this, we introduced random particles that are generated using a uniform distribution, meaning that random particles will be generated every cycle with an equal chance at every location in the map. This ensures that, when our filter converges at a local minimum, there will always be a chance we can get out if a randomly generated particle is at a more likely pose. We tested the filter with differing ratios and qualitatively decided that the ratio 10% allows for the best performance, with just enough random particles to escape local minimums consistently without trading off the confidence that we have in our current filter’s most confident pose. We also had a problem with our particles collapsing onto exactly one pose. Our solution to this was to add noise, generating new points so that our filter does not get stuck with only one guess. The reason we chose the normal distribution was because it gave us an algorithmic way to change how much noise we are adding as the particles converge.

Final Result!!!