Precise Localisation with Loop Closures using Graph SLAM
The Graph SLAM (Graph simultaneous localisation and mapping) algorithm plays a key role in our attempts to navigate autonomously in complex environments.
Take an in-depth look at how it works.
Why Graph SLAM is important? How does it roughly work? What are loop closures and why you need them? That and more is answered here with open source code.
Why is graph slam (localisation) so important?
A mobile agent (such as our robot beteigeuze) benefits from a precise localisation when acting autonomously. A good localisation simplifies a lot of things. For example having a map and knowing where you are on it, simplifies the task of navigating from berlin to paris significantly. The robot can plan a route to a target location and follow the route. If required, the robot can update the route and map on the fly.
Graph SLAM (Graph simultaneous localisation and mapping) is a technique to estimate the position of the robot and at the same time create a map of the environment. The key idea is building a graph, which contains all the places the robot has previously visited and connect them. We will go into more detail on that later. For now it is only important to know that this is a clever representation that enables the robot to recognize when it revisits a place (=loop closure) and improve it’s localisation and map. Traditional approaches such as „kalman filter slam“ and „icp slam“ (explained here) perform poor at loop closures.
An image of the rviz visualization for graph slam.
We are proud to announce that our graph slam is open source!
At kamaro enginering, I have implemented a graph slam over the course of the last months. Since there is currently no open source ros node for a graph slam, I decided to release it as open source (LGPL) now. The graph slam can be found on github: https://github.com/penguinmenac3/ros_graph_slam. This gives everyone the possibility to use a graph slam backend for their work without having to implement the tricky bits themself. We use an indev version of gtsam as a solver for our graph to achieve cutting edge performance.
However, since the graph slam data structure is unknown to most navigation solutions there is also a ros node converting the graph slam to a grid map open source. It is available at https://github.com/penguinmenac3/ros_gridmapping.
The ros graph slam node overview for ros users.
How does it work (roughly)?
Graph SLAM (Graph simultaneous localisation and mapping) is a technique to estimate the position of the robot and at the same time create a map of the environment. The key idea is building a graph, which contains all the places the robot has previously visited and connect them.
Dead reckoning
For each time step the robot exactly knows the steering angle and the speed at which it drives. This enables the robot to do dead reckoning. The result is that the robot knows how it moved between any given two time stamps by only looking at the steering angle and the speed. The location and orientation a robot has for a given time stamp is called pose and the movement between two time stamps is called relative pose, because it can be calculated by subtracting the two poses from each other.
Using the poses estimated by dead reckoning the robot builds an initial version of the graph. Each pose is a place and they are connected by the relative poses between two consecutive poses. Only doing this alone would already yield a good result for the localisation. However, it would contain an error subject to the mechanical imperfections (wheelslip, steering play) of the robot and quantification errors.
Improving the results
To eliminate that error, further information is required. In the case of graph slam, they are called constraints, because they bind two places together. There are several possible sources for additional constraints. The most widely spread is probably scan matches. This means, that the laserscans associated with two places are matched against each other and a relative pose is calculated. This relative pose can be added as a constraint between the places just like the dead reckoning. Other sources for constraints would be a compass (we actually use a compass for our robot), gps (which makes sense for autonomous vehicles or robots acting in large outdoor environments) or visual odometry (dead reckoning calculated from a camera feed). Finally, there is also the possibility to improve the estimation with loop closures.
A map created by graph slam without loop closure.
What are loop closures and why you need them?
Loop closures exploit when a robot returns to a place that it has been at previously. Usually, loop closing does a scan matching against a scan it has associated with that place in the past. Drift that has occurred over time can be eliminated, when a place is recognized. Loop closures are simple but can have a huge impact on the performance of localisation and mapping.
Get in contact with us or slam!
If you got interested in slam feel free to contact me (directly or kamaro-engineering) or read my notebooks on the topic.
Localisation Basics (Notebook)
My Localisation Notebooks (github)
Finally some images and videos…
An image of the rviz visualization for graph slam.
An early slam test.
Early experiments with loop closing.
An unoptimized version of the final graph slam.
Keine Kommentare