In this repository, we designed and implemented path planning and room coverage algorithms along with Simultaneous Localisation and Mapping(SLAM) system which achieved 97% room coverage.
This repository contains the code and documentation for Homework 5 of the Introduction to Robotics course (CS276A). The objective of this assignment was to implement a coverage path planning algorithm and integrate it with a Kalman Filter SLAM system. The coverage path planning algorithm demonstrates good performance on a provided example map. The project addresses challenges such as developing a robust coverage planning algorithm, handling multiple April tags during SLAM, and determining boundary landmarks and obstacles.
The project follows the Sense-Plan-Act architecture to model the solution. The RB5 camera is used for sensing the surroundings and obtaining information about landmark poses relative to the robot. This information is then used for planning the robot's next move. The data is passed through the Kalman Filter, which returns an updated state of the robot and map. The PID controller generates control commands based on the updated state, allowing the robot to move. This sense-plan-act cycle is repeated to achieve the desired path for the robot.
The Sense-Plan-Act architecture was chosen over reactive or hybrid architectures due to the nature of the environment and project requirements. Since the environment is static and real-time performance is not a constraint, the robot can take its time to plan its actions. Additionally, the absence of certain sensors, such as collision detection, makes it challenging to build a reactive architecture. The Sense-Plan-Act architecture is easier to implement and aligns well with the project's time constraints.
The control flow in the system can be described as follows:
-
Move Robot and Perform SLAM: Initially, the robot moves around a square path while performing Kalman Filter-based SLAM. The RB5 vision detects April tags, and their pose information is processed through predict, match, and update steps of the Kalman Filter, resulting in an updated state vector.
-
PID Controller for Robot Movement: After each predict-match-update cycle, the updated state vector is passed to the PID controller, which generates control commands to move the robot.
-
Coverage Planning Algorithm after SLAM: Once SLAM is completed, the final state vector containing information about the entire map is passed to the coverage planning algorithm. This algorithm generates a set of waypoints for achieving good coverage.
-
PID Controller for Robot Movement along Waypoints: The PID controller is used again to move the robot along the waypoints generated by the coverage planning algorithm.
The project involves several modules and algorithms, including:
-
Initial Path Generation for SLAM: To obtain an estimated map using Kalman Filter-based SLAM, the path the robot should take around the world needs to be decided. In this project, a square path is chosen, formed by specific coordinates.
-
Duplicate April Tag Handling in SLAM: To handle multiple April tags with the same IDs, a solution is implemented to distinguish between different tags and decide if a landmark is a new landmark or a previously observed one.
-
Determining the Map Boundary: After performing SLAM, the landmarks that constitute the boundary and the obstacles need to be determined. The concept of convex hulls is used to identify the boundary landmarks.
-
Determining the Obstacles: The landmarks that do not lie on the convex hull are considered obstacles. In this project, each obstacle is assumed to be a 20cm x 20cm square with its center at the corresponding landmark.
-
Coverage Path Planning: Once the map and obstacles are determined, a coverage path planning algorithm is applied to generate a path that covers the desired area while avoiding obstacles. The algorithm uses the concept of visibility graphs and considers obstacle margins to ensure safe navigation.
The project demonstrates satisfactory results in terms of coverage path planning. The actual coverage path closely aligns with the expected coverage path, with minor variations due to surface friction variations and robot imperfections. The coverage percentage achieved on both the generated map from SLAM and the ground truth map is above 97%, indicating a good level of coverage.