Unified Robotics IV

Introduction

The goal of this lab assignment was to cumulatively apply and integrate everything that has been learned within the class lectures and applied in previous labs in order to navigate and explore an unknown maze. This involved repackaging and reusing packages from previous labs, integrating existing ROS packages such as Gmapping and AMCL (Adaptive Monte Carlo Localization) to map and localize within the map space, and create new nodes and launch files tailored to the goals of the lab. The objectives of this lab were as follows:

• Successfully localize the robot within the world

• Identify the frontier on an unknown map and find interesting areas to explore and expand

• Maneuver within the map without colliding into obstacles

• Find optimal paths within the map once mapping is complete

• Identify when mapping is complete

• Navigate to points within the complete map.

By following the methodology outlined below, all objectives of the lab were met, culminating in a robot capable of completing mapping via simultaneous localization and mapping (SLAM), and navigating within a known map with unknown position using AMCL.

Methodology

A number of new nodes were developed for the final project. Figure 1 is a visual of how these nodes interact.

Figure 1: Node Map

Phase 1

Phase 1 challenges the turtlebot to map and localize within an unknown terrain using ROS’ Gmapping package. The turtlebot must begin in a specified location, identify frontier based on LIDAR readings, and navigate to pieces of the frontier deemed interesting until the entire map has been generated, while ensuring not to collide with obstacles throughout. In order to complete this, the nodes deemed necessary were: Path_Planner, Dora, and Lab3. The functionality of these nodes will be described below.

Path_Planner

The path_planner node was responsible for several main pieces of functionality, including C-Space determination and padding, frontier determination, and A* path planning.

C-Space Determination and Padding

When a new map from Gmapping was published, as a subscriber to the map topic, path_planner iterates through the map data of the cells to determine which cells have been distinguished as obstacles. This was determined as cells with an obstacle probability of greater than 50. Once these were determined, the node iteratively adds padding to the obstacles such that the robot will be able to navigate around the obstacles without crashing, since the A* algorithm used later treats the robot as a point.

In this case, the best padding determined was a padding of 5 cells with 2cm resolution, which corresponds to just over half the distance of the wheel track of the robot,

Frontier Determination and Blobbing

For the purpose of mapping, the frontier was defined as any cells marked unknown within the map, meaning they had a mapdata value of -1, that were a neighbor of 8 with cells that have been determined to not be obstacles, which was an obstacle probability of less than 50.

After the frontier was determined, it was necessary to determine interesting pieces of frontier to continue to expand and explore. The frontier points often form continuous shapes along the edge of the explored area and moving the robot near one of the frontier points usually updates the entire frontier group. If the robot is commanded to drive to an individual frontier point, the point could be near the edge of the C-space and a risky target to navigate to. Instead of picking individual frontier points for the robot to explore, the frontiers are grouped into blobs and the robot is commanded to explore the point at the centroid of a blob.

The frontiers are grouped into blobs by iterating over all of the frontier points and checking if each point is close enough to an existing blob to be part of it. If the frontier point is not close to an existing blob, it becomes the first point in a new blob. Then all of the existing blobs must be iterated through to see if any have combined into larger blobs. Then the centroid, size, and distance to the robot is calculated for each blob. Grouping the frontier into blobs makes it more likely that the robot will be sent to the middle of the frontier and farther away from the C-space and walls.

A* Cost Calculations and Heuristics

As aforementioned, A* was implemented to path plan through the map given. By definition, A* is a modification of Dijksta’s algorithm that is optimized to favor paths leading closer to a goal, rather than finding paths to all locations. Our implementation follows the one discussed in lectures, which utilizes the given Priority queue and begins by adding the start cell to the priority queue, and while the frontier is not empty, grabs the current frontier and examines the cells within. If the current cell is found to be the goal, the algorithm stops, and if the cell is not the goal, A* computes the cost of all of its neighbors by adding the cost of the current cell to the computed cost of the next cell of the neighbors of the current cell. Then routinely, each neighbor is added to the priority queue with its assigned cost being the sum of the previous cost and the heuristic value.

The cost of moving between two nodes was determined based on the Euclidean distance between their respective X and Y value differences. In this case, this meant simply determining the hypotenuse distance between their coordinates. For this particular implementation, Euclidean distance was chosen over Manhattan for the simple reason that we were able to assume the grid to be 8-connected, as that is the way the Turtlebot is capable of traversing maps. Had the Turtlebot only been capable of 90 degree turns, and as a result, only capable of traversing 4-connected grids, then Manhattan may have proved to be more effective.

For determining the heuristic, the Euclidean distance between the cell in question and the goal was used; also implemented via taking the hypotenuse distance between the respective coordinates. In this case, due to how obstacles can only be traversed by maneuvering entirely around them, rather than through, Manhattan distance may have been more effective for this heuristic rather than Euclidean. However we did not find this to hinder the efficiency of our paths enough to justify the added complexity. Additionally, in this heuristic, if a cell was determined to be an obstacle based on the cell data it carried after the C space was padded, the cost was multiplied by a large value to incentivize not travelling into C space unless absolutely necessary and to minimize time spent within the C space. This proved to be an efficient way to navigate around obstacles, rather than removing them from the explorable frontier entirely.

Handling Erroneous Lidar Readings

When testing began with the physical robot, it was found that a problem that required mitigation was handling of erroneous lidar readings, as it was noted that as the lidar operated, it would frequently identify false readings beyond the bounds of the map, which would then subsequently get marked as frontier to explore. We first began by decreasing the range of the LIDAR slightly, which seemed to improve, but not fully correct the problem. Then, instead of attempting to filter the lidar readings to remove the erroneous readings, we allowed them to still occur, but made it such that the algorithm would not allow the robot to pathplan to them. In this way, once the robot no longer had any navigable pieces of frontier, it determined it was complete and able to save the map.

Dora

The Dora node commands the robot to go to locations that are near the frontier and will expand the known map. The basic logic for Dora is: when a new frontier packet comes in, calculate the blobs of the frontier and find the closest navigable blob to the robot. If the point that the robot is currently travelling to is not near a blob or if the robot has been following a path for more than a set amount of time, Dora sends a new goal for the robot to drive to. When the robot gets close to the frontier the map will update and the frontier will move, causing Dora to issue a new waypoint for the robot. The waypoint republishing after a set amount of time makes the Lab3 node recalculate the path the robot is currently following to prevent collisions by looking at new map data.

When there are no more frontier blobs that are reachable and large enough to explore, the Dora node places a waypoint at the initial robot position and waits for the robot to return to the start location. Then Dora runs the map_saver node and closes.

Lab3

The Lab3 node that we used for this project is used for motion control of the robot. Lab3 interprets path commands and directs the drive system using the /cmd_vel topic. Lab3 subscribes to several topics, such as: /odom, /plan_path, and /move_base_simple/goal.

RViz Config

Within the RViz configuration, we utilized the different forms of visualizations such as grid cells, pose arrays, laser scans, paths, and poses. We were able to set different colors and weights to all of the different display types, which allowed us to easily identify the different data that was being observed. When launching RViz, we were able to use a saved config file to automatically load in all of the configuration settings. This streamlined all of our testing.

Phase 2

As phase 1 completed, we saved the generated map for use in phase 2. We saved our current position when we started phase 1, then once phase 1 was completed, we published a nav goal with the pose of the starting location from phase 1. The phase 2 functionality is built into the Dora node.

Phase 3

In order to complete phase three, the robot was required to use AMCL (Adaptive Monte Carlo Localization) in order to localize from a random position on the map generated in Phase 1, and navigate to a 2D nav goal given by the professor.

AMCL Parameter Adjustments

One of the first steps taken to improve the performance of phase 3 was adjusting the parameters for the AMCL node. Reference 1 describes how the optimal operating parameters for a differential drive robot for AMCL were determined, so the results presented in this paper were used as a basis for tuning our parameters. An example of the parameters determined to be optimal is shown in Figure 3.

Figure 3: AMCL Parameters from Reference 1

While it was not found to be advantageous to apply all of these parameters, increasing the minimum particles parameter to 2000 greatly improved performance and speed of localization. Additionally, decreasing the minimum distance and turning amounts that result in a new filter estimate also greatly improved the amount of time it takes to complete the localization with relative certainty.

Swiper_no_Swiping Node

Swiper was responsible for interpreting the poses with covariance being published from AMCL, and publishing them to the topic that lab3 had remapped from odom, so that paths and command velocities could be given with respect to the AMCL poses, rather than the odom poses.

Launch File Adjustments

The launch file used for phase 2 was responsible for launching Lab3, Swiper_no_Swiping, AMCL, and RVIZ. Using the tags for remapping within the launch file, we were able to remap move_base_simple_goal, which is the topic that the 2D nav goal gets published to, to a new topic named goal. This was done so that the Lab_3 code could now only request a path to the move goal once Swiper_no_Swiping had processed the goal and completed its sweep to localize, such that A* would not start running until it was convinced with relative certainty that the robot knew where it was. Additionally, odom was remapped to the positions being published by AMCL, so that Lab3 could perform A* with the positions as localization was occurring.

RViz Config

The RViz config for this phase loads in the map that was generated in phase 1, along with displaying the pose array being published

Results

Overall, the code structure and functionality of the robot was able to meet all of the requirements of the lab. The robot was able to complete phase 1 and phase 2 with no collisions in a relatively short amount of time. Phase three was also successful, but experienced a single collision and completed localization and navigation to the goal in roughly a minute and thirty seconds.

It is clear the methodology described above was robust with regards to solving network issues and being able to handle accidental navigation into the cspace. Since new paths are frequently being explored and compared through the duration of the robot’s navigation, if the robot begins to enter the C space or gets stuck, its localization is good enough to calculate a new path and maneuver out of C-space. However, if the connection to the network is poor enough to result in significant packet loss (as it was the majority of the time), the functionality of the robot declines noticeably. While some of these issues could be mitigated by publishing messages multiple times, inconsistent LIDAR data cannot be compensated for. In this case, we were able to successfully tether the robot over ethernet so that communications were no longer an issue, although the tether did create extra noise within the lidar data.

Discussion

In this lab we needed to autonomously map, traverse, and localize on a small field. These goals were to be completed using a Turtlebot.

Issues:

Unfortunately, there were several issues when it came to the lab. The biggest issue was the network communication. Since the Turtlebot has very little computation power, nearly all of the computation must be completed off of the robot. This means that the operation of the robot relies on having a stable connection between the Roscore and itself. The network within the robotics lab could not handle the traffic of multiple robots at once, this would cause packet loss and latency. This is a big issue for our robots because they completely rely on LIDAR data. When running the robot on the network while there were other teams testing, the roscore would be subscribing to LIDAR data at about 3 Hz, when the robot is publishing at about 25 Hz.

To mitigate this issue, we decided to try to use an ethernet tether to send and receive data. This process was tricky to figure out at first, but we needed to assign a static IP to the robot with respect to the laptop running the roscore. The downside to this was that the robot needed to be tethered and we would have to make sure that the ethernet cable was not near the robot, or else the LIDAR would sense it and it was just thick enough so the robot would pad it with cspace and ruin our map. The advantage to using the ethernet tether is that we got all of our data without latency and packet loss. The roscore-when tethered-got LIDAR data at the full 25 Hz.

Being able to use ethernet for the communication was crucial for testing when the lab was busy. When it came to our final demo, we performed it on the wifi, as there were not as many groups in the lab as anticipated.

References

1. Reis, W. P. N. D., Silva, G. J. D., Junior, O. M., & Vivaldini, K. C. T. (2021). An extended analysis on tuning the parameters of Adaptive Monte Carlo Localization ROS package in an automated guided vehicle. The International Journal of Advanced Manufacturing Technology, 117(5), 1975-1995.