After a period of research and accumulation, I was finally able to reproduce a complete set of two-dimensional laser SLAM algorithms. In the process of reproduction, I did not refer to an open source framework (such as hector, Cartographer, etc.) for writing, but After understanding the principle, it is implemented according to the principle. Compared with writing it according to open source, this process encounters more bugs, but it also gains more in the process of fixing bugs. Write an article to briefly record this process.
First of all, as usual, first put the renderings:
It can be seen that the map building effect is still very good. In gazebo, the robot is already moving at a relatively fast speed, but the map building speed can fully keep up, and the accuracy is relatively high. There is no overlap or drift. Condition.
Let's look at the terminal output:
The overall SLAM process can be completed in less than 0.03 seconds. The general radar scanning frequency is around 10HZ, which is converted to 0.1 seconds. Therefore, our SLAM algorithm can complete 3 operations in one scanning cycle of the radar. , The real-time performance is absolutely full. I compared the same environment and ran hector and gmapping as a comparison. The overall running time of hector is about 0.2 seconds, while gmapping is 0.4 seconds. In terms of the speed of light, our algorithm is have an absolute advantage.
The following briefly explains the implementation process of the entire project.
At present, the mainstream framework of laser SLAM is the front-end, back-end, and loopback. The front-end scans and matches, the backend optimizes, and the loopback eliminates the cumulative error. We will introduce our algorithm implementation process from these three perspectives. .
front end
The front end of this project directly applies the radar odometer implemented in the previous blog, and uses the PL-ICP algorithm to perform inter-frame matching to obtain the relative pose transformation. This part can refer to the previous article, and there is no more description.
rear end
In laser SLAM, the back-end is mainly for optimization and map building. The mainstream SLAM framework will allocate a separate thread to execute the back-end, because both map building and optimization require considerable computing resources. In order to ensure real-time performance, adopt Multithreading is a good solution. At the same time, in order to intentionally limit the scale of back-end optimization and map building, there are also many small tricks, such as establishing keyframes, using sliding windows, discarding observations and only optimizing pose graphs of poses, etc.
In the mapping part , this paper adopts the most classic occupancy grid map, the essence of which is to discretize the continuous map into grids one by one. If it is hit by the laser, set that grid as occupied, otherwise set It is defined as idle, and the state of the grid that has not been scanned by the laser is unknown. If it is represented by three values, occupied is 100, idle is 0, and unknown is -1. Then we maintain an array, put the state of each grid into it, and expand this one-dimensional array two-dimensionally when building a map, and then distinguish it with color according to the state of each grid (this part of the system will automatically complete for us. ), the simplest occupancy grid map is completed.
When creating a map, there are a few points that need special attention
The first is the coordinate transformation. This step can be understood through TF. TF is the transformation relationship between various coordinate systems in ROS. The relationship between several coordinate systems in ROS can be quickly learned and understood through the following article, and there is not much to do here repeat.
We know that the MAP coordinate system represents the world coordinates in ROS, and the radar is used as a sensor, and its data is in the laser coordinate system. If we directly use the radar data to update the map, then observe in the MAP coordinate system, the establishment position of the map The location and update location are both wrong. The most direct manifestation is that the map cannot be updated following the movement of the robot, but has its own ideas, and it can be updated wherever you want. In order to prevent this from happening, we need to do a good job of laser -base_link-odom-map coordinate transformation, transform the laser data to odom, and then use it to update the map, so as to keep the coordinate system consistent with MAP (because map-odom is generally a static transformation)
The second is the various parameters of the map and the establishment position. Since we discretely processed the continuous map when building the map, we must introduce a discrete coefficient. For example, if we discretize 1m into 10 grids, then the discrete coefficient is 0.1, when setting map parameters, such as the length and width of the map, and the coordinates of the origin of the map, you must remember to multiply this coefficient, otherwise many problems will be caused due to different scales. The second is the establishment position of the map. We need to keep the robot at the center of the map, so the establishment position of the map should be based on the position of the robot, so that it is convenient to view when visualizing in RVIZ.
Finally, there are map update tricks. Generally, there are two ways to update the map. The first is to set the initial value for each grid. If it is occupied, then increase the initial value. If it is idle, reduce the initial value. Set a threshold when publishing a map. If it exceeds a certain threshold, it is considered occupied, and if it is less than it, it is considered idle. The advantage of this method is that it can eliminate accidental errors and introduce dynamic capabilities. It is believed that there is an obstacle in the free position in front, or someone passing by in front, resulting in an obstacle in that frame of data, then use this method, after several scans, to continuously assign the free value to the space that was misjudged, then This occasional error, or dynamic barrier, becomes free again after a few scans. The second method is one-size-fits-all. If a grid is occupied at a certain moment, it will be scored 100 points, and 0 points will be scored if it is idle, and no update will be performed. The disadvantage of this method is that if there are accidental errors or dynamic obstacles, Then the resulting grid will always remain on the map in an occupied state. The advantage is that since the map does not need to be updated repeatedly, the map is built very quickly, and the map will be faster than the first method without errors. Established · more "steady" (that is, it will not change)
optimization
To be added
loop back
To be added