Background
This is part of a series of posts where I document my progress in building an autonomous mobile bot from scratch. The ultimate goal of this project is to build a robot that can accomplish the following tasks:
Autonomously map an indoor space using SLAM and save the generated map
Load the generated map file and localize the robot inside the map
Move the robot around by sending move commands from a GUI that shows the map
There are two major steps in autonomous navigation: path-planning and path-following. The first step, path-planning, involves the robot using a map of the environment to plan an obstacle-free path to the destination. The map can be a 2D or 3D grid that indicates where the obstacles are located.
To generate a map of an unmapped environment, a process called Simultaneous Localization and Mapping (SLAM) is employed. As the name suggests, the purpose of SLAM is to map out an environment while localizing the robot inside of that map. For a simple robot, a 2D map is generated with a 2D LiDAR sensor.
Figure 1: 2D SLAM using a LiDAR sensor.
In order to map out an entire indoor space, however, the robot needs to be manually controlled to visit all unmapped regions as shown in Figure 1. To automate this task, I developed an algorithm to identify these regions and send the robot to said places.
Set Up
To develop and test the algorithm, I needed a scenario where a robot is performing SLAM on an indoor environment. To do so, I used the Gazebo simulator. The simulated robot was a two-wheeled robot with wheel encoders, an IMU, and a LiDAR. These sensors were added using the Gazebo plugins (diff_drive, imu, lidar).
Figure 2: 3D rendering of robot in RViz.
The simulated environment was the turtlebot3_world, which is a default world file that comes with installing ROS2.
Figure 3: Layout of turtlebot3_world in Gazebo simulator.
For SLAM, I used the Slam-Toolbox library and for basic navigation features, such as obstacle avoidance, and path follower, I used the Navigation2 ibrary. The point of this project was not to implement an entire navigation stack from scratch, but to develop and algorithm to automate SLAM. Hence, I made use of these off-the-shelf libraries.
Implementation
The basic working principle of the algorithm is to use these inputs and generate the output
Input:
Map (being generated by SLAM).
robot's pose inside of the map.
Output:
Set waypoint to unmapped regions of the map.
In order to accomplish this task, the following algorithm is used:
Use Breath-FIrst-Search (BFS) to find all cells that are adjacent to at least one cell with unknown occupancy (I will call these cells frontier cells from now on).
Upon encountering a frontier cell, use BFS again to find all connected frontier cells.
Score these "islands" of frontier cells based on their size (number of cells) and distance (distance from robot's current position to the centroid of the island).
Navigate the robot to the centriod of the island of frontier cells.
Figure 4: Autonomous SLAM in action. Color-coded cells are frontier cells. The color gradient goes from green to blue and it indicates the cost of to travel to that frontier island (green is lowest and blue is highest). The robot travels to green frontier islands first.
BFS that searches for a frontier cell:
frontier cell is a cell with at least one neighboring cell whose occupancy is unknown
nhood4 and nhood8 are helped functions that return the 4 or 8 neighboring cells
up, down, left, right for nhood4
up, upper_right, right, bottom_right, bottom, bottom_left, left, upper_left for nhood8
After finding all the frontier islands, the algorithm greedily chooses the frontier island with the least cost. The cost of each frontier island is calculated like so:
Here, min_distance is the distance between the robot's current position and the closest frontier cell in the frontier island and points.size() is the number of frontier cells in the island. The cost is a combination of these two metrics and each metric are multiplied by their weight, which reflects their relative importance in calculating the cost. In plain english, the robot is set to explore frontier islands that are both close to the robot and have a lot of frontier cells.
Each time the SLAM algorithm sends an updated map over a ROS topic, all the frontier islands are found and they sorted according to their cost. The algorithm then picks the frontier island with the lowest cost and traveles towards its centriod. A video of the process is shown in this video:
Comments