Bachelor's Thesis: A Framework for Multi-Robot Task-Motion Planning
A C++ framework that integrates high-level symbolic task planning with low-level, collision-free motion planning to generate robust plans for multiple autonomous robots in a shared environment.
At a Glance
- Situation: Autonomous robots performing complex tasks need to reason about both the sequence of actions (task planning) and the physical feasibility of movement (motion planning). Treating these separately leads to inconsistent or failed plans.
- Task: To design and implement an integrated Task-Motion Planning (TMP) framework in C++ that allows a symbolic task planner to generate plans that are geometrically valid and collision-free for multiple robots.
- Action: I built a system where a PDDL-based task planner calls a custom motion planner via "semantic attachments." The motion planner, implemented from scratch, uses Dijkstra's algorithm to find optimal paths and incorporates a dynamic collision avoidance strategy where robots can lock nodes and perform waits to avoid conflicts.
- Result: The framework successfully generated valid plans in simulations with varying numbers of robots and tasks. The work was formally recognized and presented at the 4th Italian Conference on Robotics and Intelligent Machines (I-RIM 2022).
Technical Deep Dive
System Architecture & Design
The core of the project is the tight integration between a high-level, symbolic task planner and a low-level, geometric motion planner. This was achieved using a concept called semantic attachments.
The task planner operates using the Planning Domain Definition Language (PDDL) to define abstract actions like `goto_region` or `pick_up`. When the planner needs to evaluate the cost or feasibility of a motion-related action (e.g., the cost of moving from region A to B), it doesn't use a fixed number. Instead, by overloading the PDDL `(increase (total-cost) ...)` function, it triggers an external call to my custom C++ motion planner.
The motion planner then computes the optimal, collision-free path, returns its actual cost (e.g., path length) to the task planner, which uses this real-world cost to inform its search for the best overall plan. This ensures that the final task plan is not just symbolically correct, but also geometrically feasible.
Core Implementation Details
The motion planner was implemented from scratch in C++ and consists of two main components:
- Pathfinding: I implemented Dijkstra's algorithm to find the shortest path on a 2D grid representing the environment. The `dijkstraPath` function takes the graph, source, and destination nodes as input and returns a vector of integers representing the nodes in the optimal path.
- Collision Avoidance: This was the most complex part of the implementation. The `checkCollision` function iterates through a robot's proposed path and compares it against the planned paths of all other robots, which are shared in a central map structure. It checks for two types of collisions:
- Vertex Collision: Two robots occupying the same node at the same time step.
- Edge Collision: Two robots crossing each other on the same edge between two time steps.
Challenges & Solutions
A primary challenge was designing a decentralized collision avoidance strategy that was both effective and not computationally prohibitive. A simple "first-come, first-served" approach could easily lead to deadlocks where robots block each other indefinitely.
My solution was a priority-based re-planning and waiting mechanism. When `checkCollision` detects a future conflict, the lower-priority robot's plan is invalidated from the point of collision. Its pathfinding function is then called again with a modified graph where the colliding node is temporarily "locked" (made inaccessible). If a valid alternative path is found, the plan is updated. If no alternative exists, the algorithm implements a wait maneuver: the robot pauses at the node just before the collision, allowing the higher-priority robot to pass before re-evaluating its path. This combination of re-routing and waiting proved effective in resolving conflicts in the tested scenarios.