Sampling-Based Multi-Modal Multi-Robot Multi-Goal Path Planning

In many robotics applications, multiple robots are working in a shared workspace to complete a set of tasks as fast as possible. Such settings can be treated as multi-modal multi-robot multi-goal path planning problems, where each robot has to reach a set of goals. Existing approaches to this type of problem solve this using prioritization or assume synchronous task completion, and are thus neither optimal nor complete. We formalize this problem as a single centralized path planning problem and present planners that are probabilistically complete and asymptotically optimal.

The planners plan in the composite space of all robots and are modifications of standard sampling-based planners with the required changes to work in our multi-modal, multi-robot, multi-goal setting. We validate the planners on a diverse range of problems including scenarios with various robots, planning horizons, and collaborative tasks such as handovers, and compare the planners against a suboptimal prioritized planner.

This benchmark addresses multi-modal, multi-robot, multi-goal path planning, where multiple robots must reach ordered goal sequences while considering interactions like object handovers. Unlike traditional methods, our approach is not restricted to 2D workspaces, supports dynamic environments, and enables heterogeneous robot teams.

The problem is formalized as a single composite-space planning problem, where each robot follows a sequence of goals while avoiding collisions. Some goals require multi-robot coordination, leading to mode transitions where environmental constraints change dynamically.

The goal sequence can be specified as a fully ordered sequence, or more flexibly as a dependency graph between tasks:

The benchmark includes 21 diverse problem instances, ranging from simple validation cases to complex, high-dimensional settings with up to 5 robots and 74 subgoals. Tasks include rearrangement, stacking, mobile assembly, and cooperative manipulation.

Some examples with their corresponding subgoals can be seen below:

Some of the intermediate configurations we show there are in collision since we simply reset the agent without a goal to its home pose (which might be in collision if the robot is now moving an object).

Two baseline planners—an RRT*-based planner and a PRM*-based planner—are adapted to handle mode transitions and dependency constraints. Both are probabilistically complete and asymptotically optimal, with locally informed sampling and partial shortcutting for improved efficiency.

Standard informed sampling techniques struggle in high-dimensional, multi-goal problems due to large search spaces. To improve efficiency, we introduce locally informed sampling, which selects sampling regions based on the structure of the current best path. By sampling between intermediate waypoints rather than globally, this method accelerates convergence and enhances planner performance.

Below, we show plans from multiple environments that were planned with one of the planners that we describe in the paper.