Autonomous Navigation, Mapping, AMCL Localization, and A* Goal Pathing in ROS with a TurtleBot
Updated: Oct 2
By the end of this project, the robot was able to complete all six of the defined objectives: localize the robot in the world, drive around the map while avoiding obstacles, find interesting areas to expand the map, find optimal path to the interesting point, identify when the map is complete, navigate back to a point in the map.
Figure 1: The TurtleBot on the field with AMCL running on a map it generated during the demo
During the demo for the final lab, our turtle bot was able to successfully complete all three phases with minimal collisions. After being placed in an unknown map, the robot was able to use A* with the proper c-space and padding to navigate the frontiers of the unexplored cells. These cells were identified through Gmapping and determined based on our frontiers algorithm. We completed the generating the whole map with one collision during the demo. The robot was able to navigate successfully back to the starting position of the robot without any collisions. The robot was then placed in a random location in the map chosen by the professor and was able localize itself with the map using AMCL. The turtle bot was then given a location through RVIZ to travel to using A*. During the demo, the robot was able to make to it to the goal position within 55 seconds. Figure 1 shows the map generated during the demo and after reaching the final goal.
Figure 2: The ROS Node Plot for the final project
We developed a frontier generation algorithm to tell guide the robot in its exploration of an unknown environment. First, we dilated the walls of the map using a dilation algorithm. Using this, we generated a new map that contained the dilated wall. We defined a frontier node as an open cell neighbored by an unknown cell. From our new map, we first created a list of these frontier nodes. We then grouped neighboring frontier nodes together by determining if they had a neighbor that was also a frontier node. Using this list of frontiers, we filtered out frontiers of less than 3 nodes. This eliminated the small frontiers that were not worth exploring. We then determined the centroid of each frontier, which produces a point to which the robot can drive. We first calculated the average of the x values and the average of the y values to produce the actual centroid of the frontier, but we realized the this would often produce a frontier that was in unknown space. This meant that the robot was unable to generate a path to the frontier. Instead of moving determining the nearest open node to this node, we decided to just take the middle value of our node list for our frontier. To do this, we divided the length of the list of nodes for a frontier by 2 and took the node at that index. This produced a node that was always possible for the robot to path to. Using a priority queue, we ordered the remaining frontiers based on the distance between the centroid and the current position of the robot. If there were two frontiers that were the same distance from the robot, the highest weighted frontier was the larger of the two.
Using the prioritized list of frontiers created by our frontier generation algorithm, the robot explored an unknown map. Since the environment was unknown, the robot would often collide with undetected obstacles as it drove on a planned path. To solve this, we did not tell the robot to complete its path to the highest weighted frontier; instead, we had it complete only a portion of the path, regenerate frontiers based on its now more-complete environment, and path to a new frontier. We only had the robot drive until the first turn in the generated path, or, if that was less than 5 cells, it would complete the first 5 cells worth of movement. This ensured that the robot did not drive too far and collide with obstacles in an incomplete environment. Additionally, we found that our generated frontiers were occasionally in a location that the robot could not reach. If the frontier generator created a frontier that the robot was unable to path to, we had it simply move to the next frontier in the list. The robot determined it was done mapping its environment when there are no remaining reachable frontiers. At this point, the robot navigated back to its starting position.
We used A* as our path planning algorithm for all aspects of this project. We weighted the overall cost from one node to another as the sum of the Manhattan distance travelled so far and the Euclidean distance-to-go. Through experimentation, we found that it was difficult to create a large enough C-space for the robot to avoid obstacles while not blocking potential paths. To fix this problem, we used a smaller C-space but added cost for cells that were adjacent or two cells away from the C-space. In this way, the robot could travel through a narrow C-space if it absolutely had to, but it stayed far away from obstacles otherwise. This addition meant that A* no longer generated the most optimal path, but it generated a path that was better suited to the needs of the actual robot.
In order for the robot to reliably follow a path, it needed a consistent drive method. Since errors in heading can cause rapidly compounding error, we implemented proportional control on our drive function that continually corrected the robot’s heading. Additionally, we changed the frame from which the robot received odometry data. Originally, the robot was using its own odometry to traverse the map which did not correct based on the generated map. We instead used a frame published by GMapping (base footprint) to localize the robot as it drove. This made the odometry more true to the actual map and allowed the robot to drive more accurately.
In conclusion, we implemented SLAM and generated our own frontiers inside of a map using a custom edge detection algorithm. Using this, we were able to map a completely unknown environment in under 3 minutes. After map generation, we were able to successfully travel back to the starting position of phase 1. On top of that, our robot was able to localize when the robot was placed anywhere on the end of the map. During this project, we learned how to integrate SLAM and path planning to command a robot to map an unknown environment.