Implementing SLAM
For the final project of my ai for robotics class I implemented a version of a simultaneous localization and mapping algorithm. SLAM allows you to build a robot that can use its sensors to build a map of its surroundings while it is also simultaneously attempting to perform localization so that it can manage the sensor errors as well as the motion errors as it moves and continues to build the map and discover new spaces.
The basic setup for this project was to build the real-time controllers for the robot. The robot itself has two wheels and moves by rotating and then moving forward. The robot can only move forward and spin. The sensors all model error, with two important rules. The first is that the error is greater the further away the sensor is seeing. The second is that the error is uniform. That is very important because otherwise additional readings would not improve the robot’s model of the world. If the error was not uniform, boxes would always appear to move away from their actual location.
The robot’s world is made up of walls with sensors on them that the robot can see, and crates that the robot has to pick up and move to the ‘drop zone’, where the box would be removed from the warehouse. The crates are small cyan boxes in the visualization below.
Here is a video of the robot performing:
As you can see, it begins with a very narrow view of its world. Only sensors within line of sight show up. Also because it starts with just a single scan of the room, many of the boxes are in incorrect locations. As the robot moves around and has more sense data of its environment it improves the locations of the boxes. You can also see in the visual the location that the robot is sensing the box at and a second box that shows the robot’s model of the world.
Other interesting things that you might notice is that boxes have a confidence level associated with them. Boxes close up, are more likely to be real boxes and valid distances, whereas boxes in the distance that are only seen once have a lower starting confidence. Each time the robot senses the box that validates the location, it’s confidence is increased. Each sense/move loop it lowers the confidence of every box. In this way, invalid boxes are removed from the map over time.
Lastly, you will notice that the robot thinks it perceives a crate that is not actually there. It moves towards that crate and attempts to pick it up, once it lifts it, the ghost image of the crate is removed and then it targets and acquires the actual location of the box just behind that.
