Real-Time Multi-Robot Mission Planning in Cluttered Environment

Abstract

Addressing a collision-aware multi-robot mission planning problem, which involves task allocation and path-finding, poses a significant difficulty due to the necessity for real-time computational efficiency, scalability, and the ability to manage both static and dynamic obstacles and tasks within a complex environment. This paper introduces a parallel real-time algorithm aimed at overcoming these challenges. The proposed algorithm employs an approximation-based partitioning mechanism to partition the entire unassigned task set into several subsets. This approach decomposes the original problem into a series of single-robot mission planning problems. To validate the effectiveness of the proposed method, both numerical and hardware experiments are conducted, involving dynamic obstacles and tasks. Additionally, comparisons in terms of optimality and scalability against an existing method are provided, showcasing its superior performance across both metrics. Furthermore, a computational burden analysis is conducted to demonstrate the consistency of our method with the observations derived from these comparisons. Finally, the optimality gap between the proposed method and the global optima in small-size problems is demonstrated.

1. Introduction

Autonomous unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) can replace humans for dangerous tasks such as surveillance and search-and-rescue. Recently, some receding-horizon motion planning methods [1,2,3,4] guide an autonomous robot to explore and go to a destination in a complex environment. These methods typically require a planning hierarchy. On top of this hierarchy, a path planner such as [5,6,7,8,9] generates a sequence of sparse way-points based on the perception of the environment. A group of autonomous robots has more capabilities than a single robot in applications such as surveillance, information sensing, navigation, and search-and-rescue. Suppose collision-free, non-conflict sparse paths for the robot team can be generated and adapted at run-time. In that case, the team can explore a complex environment and perform complicated missions efficiently.

In this paper, a task is defined as a location of interest that one robot must visit. Given a set of robots and tasks, a collision-aware multi-robot mission planning (MRMP) problem is defined twofold, i.e., finding optimal and conflict-free task allocations for robots and then generating collision-free paths such that robots can visit these task positions. The former is categorized as a multi-agent task allocation (MATA) problem, and the latter is defined as a multi-agent path-finding (MAPF) problem. The optimal objective of MAPF is typically to minimize the total traveling distance. For a multi-robot system, real-time mission planning in a cluttered environment is necessary when deploying autonomous robots in a complex environment, especially when obstacles and tasks are dynamic. A hardware example and a simulation example of MRMP problems are shown in Figure 1 and Figure 2, respectively. This paper considers MRMP problems defined as ST-SR-TA (Single-Task Robots, Single-Robot Tasks, Time-Extended Assignment) problems [10]. Here, tasks are assumed to be homogeneous and independent of each other, i.e., no temporal logic requirements; robots are assumed to be homogeneous regarding mission functionality. Since this problem is proven to be NP-hard [10], there is a trade-off for MRMP problems between real-time performance and optimality. Furthermore, the scalability of an underlying algorithm, in terms of the number of robots and tasks, is crucial in multi-robot systems.

1.1. Related Work

The literature on MRMP problems can be divided into two categories, i.e., solving MATA and MAPF problems sequentially or in an integrated way.

The methods related to MATA can be mainly categorized as auction-based and searching-based methods. Auction-based approaches are derived from a concept in finance where each agent aims to maximize their reward by giving higher bids. The process must consider maximizing a global reward and include conflict resolution. Ref. [11] utilizes auction-based protocols to bid task assignments. CBBA (Consensus-Based Bundle Algorithm) [12] employs a decentralized consensus procedure for task conflict resolution and then generates task allocation for agents. IACA (Iterated Auction Consensus Algorithm) [13] proposes a similar iterative but resilient auction process and can remove malicious bids during the auction. Ref. [14] proposed an auction-based algorithm to deal with task allocation problems with time window constraints. Ref. [15] utilizes Behavior Tree to coordinate a sequence of actions among agents with the auction process. Ref. [16] produces task sequences with minimum communications by combining the greedy algorithm and the auction process. Although the auction-based approaches are decentralized, the process of auction and conflict resolution can be time-consuming, especially when the problem size is large. In addition, the auction heuristic barely includes environmental information, e.g., the impact of obstacles on the cost/reward. Thus, the auction result is not necessarily optimal when obstacles are present and may even lead to a bad solution.

Search-based methods rely on a fixed structure of information, e.g., the number of assigned tasks for each agent is known and fixed. Ref. [17] proposes a decentralized genetic algorithm (GA) to search a task sequence parallelly. Ref. [18] proposes a graph-based search method to allocate tasks to agents given a finite linear temporal logic objective, where the allocation order is partially known. Ref. [19] builds an Optimized Directed Roadmap Graph (ODRM) by sampling first and then navigates agents on this graph. Although searching for paths on an ODRM is faster than on the most common occupancy grid map, generating and updating such a graph at run-time can be time-consuming in a cluttered and dynamic environment.

Additionally, researchers have explored other methodologies for solving MATA problems in recent years. Ref. [20] investigates a heterogeneous MATA problem, where the objective is to minimize the maximum travel cost for any agents. Ref. [21] proposes a deep reinforcement learning algorithm that utilizes policy gradient updates to determine the optimal allocation schedule for each robot. Ref. [22] approaches MATA as a potential game, using this framework to reach a mutually agreeable task assignment by identifying a Nash equilibrium among the agents. Ref. [23] formulates MATA as a cooperative game, leveraging the Shapley value to compute the average marginal contribution of each robot. By ranking and clustering robots and tasks based on their Shapley value, the initial problem can be partitioned into smaller, more manageable sub-problems.

Since most of the recent literature focuses on the integration of MATA and MAPF problems, this paper omits the literature on MAPF problems. As for the literature on solving MATA and MAPF problems sequentially, it is mainly categorized into auction-based and search-based methods. Based on CBBA, Ref. [24] first generates task sequences without any obstacle information and then utilizes Dijkstra’s algorithm [5] to find collision-free paths given the sequences. Ref. [25] proposes a two-stage GA-based approach where each agent first determines its task sequence using a genetic algorithm and then negotiates with other agents to exchange tasks if that reduces the cost. Then, collision-free paths are generated similarly to the method in [24].

There are also some special cases of MRMP problems that have raised significant interest, such as multi-robot pickup and delivery [26], and vehicle routing problems. Some special specifications are adopted for these problems. For example, the task set for each agent is prescribed; each agent can only be assigned one task; the initial positions for agents are the same; etc. This paper considers a general MRMP problem without these special specifications.

There is also some literature on the integrated MRMP methods. Ref. [27] focuses on simultaneous task allocation and planning for a complex goal that consists of temporal logic sub-tasks. Ref. [27] emphasizes the capability of a heterogeneous robot team to perform a complex goal, whereas the MRMP problem in this paper focuses on homogeneous agents and tasks. Ref. [28], as a fully centralized optimization-based method, first obtains a single tour that connects all the tasks without any obstacle information by solving a traveling salesman problem; then, it uses a heuristic policy to partition the tour to generate a task allocation sequence for each agent; finally, it generates collision-free paths. Although Ref. [28] deals with the same problem as this paper, its computation time is stably around 55 s, with 5–20 agents and 10–50 tasks on a map with random obstacles. Its success rate varies from 0.35 to 1.0, depending on the number of agents.

From the methodology perspective, there are primarily three types of methods for MRMP problems with homogeneous robots/tasks and no temporal logic constraints, i.e., decentralized auction-based, distributed GA-based (genetic algorithm), and centralized optimization-based methods. Decentralized auction-based methods, as mentioned above, suffer from inefficient auction and negotiation processes and a lack of obstacle information during the auction process. Distributed GA-based methods might have good real-time performance for small-size problems but they notably depend on the selection of GA parameters. Also, many methods assume the number of assigned tasks for each robot is known and fixed, whereas this paper does not. As for optimization-based methods, they barely utilize obstacle information in the first place and not in a distributed manner, i.e., directly solving the entire allocation problem.

1.2. Contributions, Organization, and Notations

This paper proposes a real-time parallel multi-robot mission planning algorithm, named RPM, for homogeneous robots and tasks. RPM first utilizes obstacle information as heuristics to approximate the cost of an ordered task allocation and path sequence by a metric from an unordered task set. With this approximation, RPM can partition the entire problem into several parallel sub-problems and distribute them to each robot. Then, each robot finds an optimal task allocation and path sequence for each sub-problem. Due to the approximation and the parallel manner, RPM makes a balance between computational performance and scalability. The main contributions are as follows:

• A parallel real-time algorithm, RPM;
• Capability of handling dynamic obstacles and tasks in a cluttered environment at run-time;
• Good scalability in terms of the number of robots and tasks, and relatively good optimality;
• Computational burden analysis for RPM.

The rest of this paper is organized as follows: Section 2 defines the multi-robot mission planning problem and formulates it as an intractable optimization problem. Section 3 introduces the proposed algorithm, RPM, in three phases. Section 4 shows several experiments with static/dynamic obstacles and tasks, conducts the scalability and optimality comparisons with an existing algorithm, analyzes the computational burden for RPM, and shows the optimality gap among different algorithms in small-size problems. Section 5 concludes the paper and discusses future improvements and challenges.

Notations. For a point $\mathit{p}\in \mathbb{R}$, $\left\{\mathit{p}\right\}\subset \mathbb{R}$ denotes a set containing that point as its only element. Set subtraction is $A\setminus B=\{x\in A\mid x\notin B\}$. $\mathbb{Z}$ denotes the integer set. ${\mathbb{Z}}_{+}$ denotes the positive integer set. The cardinality of a set A is denoted as $\left|A\right|$.

2. Problem Formulation

The configuration space, $X\subseteq {\mathbb{R}}^n$, is the set of all positions reachable by a robot. Denote a robot position set $\mathcal{X}=\{{\mathit{p}}_1,\cdots ,{\mathit{p}}_{n_a}\}$ of $n_a$ robots and ${\mathit{p}}_i\in X$ as the position of robot i. Denote a task positions set $\mathcal{T}=\{{\mathit{t}}_1,\cdots ,{\mathit{t}}_{n_t}\}$ of $n_t$ tasks and ${\mathit{t}}_i\in X$ as the position of task i. Define the robot and tasks index sets $\mathcal{I}\triangleq \{1,\cdots ,n_a\}$ and $\mathcal{J}\triangleq \{1,\cdots ,n_t\}$, respectively. Suppose that a robot completes a task when the distance between two entities is less than a prescribed non-negative constant $ϵ$, i.e., $\left|\right|{\mathit{p}}_i-{\mathit{t}}_j{\left|\right|}_2\le ϵ,ϵ\ge 0$. Denote an obstacle positions set as $\mathcal{O}=\{{\mathit{o}}_1,\cdots ,{\mathit{o}}_{n_o}\}$ of $n_o$ obstacles, where ${\mathit{o}}_i\in X$ is the position of obstacle i. Denote ${\mathcal{P}}_i\triangleq ({\mathit{p}}_i^0,{\mathit{p}}_i^1,\cdots ,{\mathit{p}}_i^{n_{p,i}-1})\subset X$ as an ordered sequence of positions associated with robot i which denotes a path starting from ${\mathit{p}}_i^0$ and ending at ${\mathit{p}}_i^{n_{p,i}-1}$, where $n_{p,i}\triangleq \left|{\mathcal{P}}_i\right|$ denotes the number of positions in ${\mathcal{P}}_i$.

Inspired by [12], the collision-aware MATA problem is written as the following integer programming:

$$\begin{array}{}\mathrm{(1a)}& \hfill \underset{\mathit{x},{\mathit{r}}_1,\cdots ,{\mathit{r}}_{n_a}}{\min }\hfill & {\sum }_{i=1}^{n_a}{\sum }_{j=1}^{n_t}{c}_{ij}({\mathit{x}}_i,{\mathit{r}}_i,\mathcal{O})x_{ij}\hfill \mathrm{(1b)}& \mathrm{s}.\mathrm{t}.& {\sum }_{j=1}^{n_t}{x}_{ij}\le n_t,\forall i\in \mathcal{I}\hfill \mathrm{(1c)}& & {\sum }_{i=1}^{n_a}{x}_{ij}=1,\forall j\in \mathcal{J}\hfill \mathrm{(1d)}& & {\sum }_{i=1}^{n_a}{\sum }_{j=1}^{n_t}{x}_{ij}=n_t\hfill \mathrm{(1e)}& & x_{ij}\in \{0,1\},\forall (i,j)\in \mathcal{I}\times \mathcal{J},\hfill \end{array}$$

where $x_{ij}=1$ if task j is assigned to robot i and 0 otherwise; ${\mathit{x}}_i\in {\{0,1\}}^{n_t}$ is the task assignment vector for robot i; $x_{ij}$ is the j-th element of ${\mathit{x}}_i$; $\mathit{x}={\left[\begin{array}{ccc}{\mathit{x}}_1^{\prime }& \cdots & {\mathit{x}}_{n_a}^{\prime }\end{array}\right]}^{\prime }\in {\{0,1\}}^{n_t{n}_a}$. The vector ${\mathit{r}}_i\in {\{\mathcal{J}\cup \{\varnothing \}\}}^{n_a}$ denotes an ordered sequence of tasks, i.e., the task allocation order, for robot i; its k-th element is $j\in \mathcal{J}$ if task j is the k-th task of robot i’s assignment; ${\mathit{r}}_i=\varnothing$ if robot i has no assignment. The collision-aware cost of task j being assigned to robot i followed by an order ${\mathit{r}}_{\mathit{i}}$ is defined by $c_{ij}({\mathit{x}}_i,{\mathit{r}}_i,\mathcal{O})\ge 0$. In the context of mission planning, this cost typically represents traveling distance, fuel consumption, etc. Constraint (1b) indicates that each robot can at most be assigned $n_t$ tasks; (1c) requires that each task must be assigned to only one robot; (1d) enforces that every task must be assigned.

Given a particular assignment $\mathit{x}$, the task allocation order ${\mathit{r}}_i$ for each robot i is not unique. Therefore, an implicit mapping from $\mathit{x}$ to the task allocation order set $\mathcal{R}\triangleq \{{\mathit{r}}_1,\cdots ,{\mathit{r}}_{n_a}\}$ needs to be determined. How to find the mapping from $\mathit{x}$ to $\mathcal{R}$ is also a part of the problem (1).

Given an order set $\mathcal{R}$ and the current positions of robots $\mathcal{X}$, the collision-aware MAPF problem is written as follows:

$$\begin{array}{}\mathrm{(2a)}& \underset{{\mathcal{P}}_1,\cdots ,{\mathcal{P}}_{n_a}}{\min }& {\sum }_{i=1}^{n_a}{\ell }_i\left({\mathcal{P}}_i\right)\hfill \mathrm{(2b)}& \mathrm{s}.\mathrm{t}.& {\mathit{p}}_i^0={\mathit{p}}_i,\forall i\in \mathcal{I}\hfill \mathrm{(2c)}& & \mathcal{R}\mathrm{is}\mathrm{determined}\mathrm{by}\left(1\right)\hfill \mathrm{(2d)}& & {\mathcal{P}}_i\mathrm{satisfies}\mathrm{the}\mathrm{order}{\mathit{r}}_i,\forall i\in \mathcal{I}\hfill \mathrm{(2e)}& & {\mathcal{P}}_i\cap \mathcal{O}=\varnothing ,\forall i\in \mathcal{I},\hfill \end{array}$$

where ${\ell }_i\left({\mathcal{P}}_i\right)={\sum }_{j=0}^{|{\mathcal{P}}_i|-2}\left|\right|{\mathit{p}}_i^{j+1}-{\mathit{p}}_i^j{\left|\right|}_2$ is the traveling distance of path ${\mathcal{P}}_i$. This paper assumes that ${\mathcal{P}}_i\cap \mathcal{O}=\varnothing$ if and only if $\left|\right|{\mathit{p}}_i^j-{\mathit{o}}_k{\left|\right|}_2\ge \delta >0\forall {\mathit{p}}_i^j\in {\mathcal{P}}_i\mathrm{and}\forall {\mathit{o}}_k\in \mathcal{O}$.

Based on (1) and (2), the collision-aware MRMP problem in this paper is formulated as follows:

$$\begin{array}{}\mathrm{(3a)}& \underset{\mathit{x},\mathcal{R},\mathcal{P}}{\min }& {\sum }_{i=1}^{n_a}{\sum }_{j=1}^{n_t}{c}_{ij}({\mathit{x}}_i,{\mathit{r}}_i,\mathcal{O})x_{ij}\hfill \mathrm{(3b)}& \mathrm{s}.\mathrm{t}.& {\sum }_{j=1}^{n_t}{x}_{ij}\le n_t,\forall i\in \mathcal{I}\hfill \mathrm{(3c)}& & {\sum }_{i=1}^{n_a}{x}_{ij}=1,\forall j\in \mathcal{J}\hfill \mathrm{(3d)}& & {\sum }_{i=1}^{n_a}{\sum }_{j=1}^{n_t}{x}_{ij}=n_t\hfill \mathrm{(3e)}& & x_{ij}\in \{0,1\},\forall (i,j)\in \mathcal{I}\times \mathcal{J}\hfill \mathrm{(3f)}& & \mathcal{P}\triangleq \{{\mathcal{P}}_1,\cdots ,{\mathcal{P}}_{n_a}\}\mathrm{is}\mathrm{determined}\mathrm{by}\left(2\right)\hfill \mathrm{(3g)}& & \mathcal{R}\mathrm{is}\mathrm{determined}\mathrm{by}\left(1\right),\hfill \end{array}$$

where ${\sum }_{j=1}^{n_t}{c}_{ij}({\mathit{x}}_i,{\mathit{r}}_i,\mathcal{O})x_{ij}$ evaluates robot i’s collision-aware traveling distance given a particular assignment and allocation order.

Solving the task assignment $\mathit{x}$, the allocation order $\mathcal{R}$, and the collision-free path $\mathcal{P}$ altogether is challenging because $\mathit{x}$, $\mathcal{R}$, and $\mathcal{P}$ are coupled together in (1)–(3). Furthermore, the collision-aware MRMP problem (3) is not even tractable since it is proven to be NP-hard [10]. This paper attempts to obtain a sub-optimal solution to the collision-aware MRMP problem scalably and in real-time, especially when the environment is unconstructed and cluttered, and the obstacles and tasks are potentially dynamic.

3. Algorithm

This paper proposes a real-time parallel algorithm RPM to obtain a sub-optimal solution to (3) in a scalable way. Instead of considering the exact coupled cost $c_{ij}({\mathit{x}}_i,{\mathit{r}}_i,\mathcal{O})$, RPM utilizes task-based heuristics to approximate the cost of an ordered path by an unordered task set. With this approximation, RPM can partition the entire task set into several subsets and assign each task subset to one robot given the unordered heuristics. Then, each robot only needs to solve a sub-problem, i.e., a single-robot mission planning problem. Specifically, RPM consists of three phases: (1) Task Segmentation: partitioning the entire task set into several subsets; (2) Cluster Assignment: assigning each robot a task subset; (3) Single-Robot Mission Planning: finding an optimal task allocation order and collision-free path for each robot.

3.1. Task Segmentation

The entire task set $\mathcal{T}$ is partitioned into $n_a$ clusters $\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_{n_a}\}$, where each cluster possibly includes many tasks. Note that ${\mathcal{T}}_i$ has not been assigned to any robots yet. The tasks within a cluster have a minimal distance to the centroid of this cluster. An iterative k-means clustering algorithm [29] is used here, which minimizes the summation of the within-cluster sum of squares (WCSS), i.e.,

$$\begin{array}{}\mathrm{(4a)}& \underset{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_{n_a}}{\min }& {\sum }_{i=1}^{n_a}{\sum }_{\mathit{t}\in {\mathcal{T}}_i}\left|\right|\mathit{t}-{\mathit{c}}_i{\left|\right|}_2^2\hfill \mathrm{(4b)}& \mathrm{s}.\mathrm{t}.& \mathcal{T}={\cup }_{i=1}^{n_a}{\mathcal{T}}_i\hfill \mathrm{(4c)}& & {\mathcal{T}}_i\cap {\mathcal{T}}_j=\varnothing ,\forall i\ne j\hfill \mathrm{(4d)}& & {\mathit{c}}_i=\left({\sum }_{\mathit{t}\in {\mathcal{T}}_i}\mathit{t}\right)/\left|{\mathcal{T}}_i\right|,\forall i,\hfill \end{array}$$

where ${\mathcal{T}}_i=\left\{{\mathit{t}}_j\right|\forall j\in {\mathcal{I}}_{c,i}\}$ and ${\mathcal{I}}_{c,i}$ is the task index set that is associated with the tasks within cluster ${\mathcal{T}}_i$; ${\mathit{c}}_i\in {\mathbb{R}}^n$ is the centroid of tasks within ${\mathcal{T}}_i$. Denote $\mathcal{C}\triangleq \{{\mathit{c}}_1,\cdots ,{\mathit{c}}_k\}$.

As described in (3), the objective is to minimize the total traveling distance. However, the cost of each robot visiting a known task set is unknown before a task allocation order is determined. Hence, for each task subset ${\mathcal{T}}_i$, an ordered sequence’s length is approximated by an unordered set’s WCSS, i.e., ${\sum }_{\mathit{t}\in {\mathcal{T}}_i}\left|\right|\mathit{t}-{\mathit{c}}_i{\left|\right|}_2^2$, since the tasks within ${\mathcal{T}}_i$ have a smaller WCSS associated with ${\mathit{c}}_i$ than ${\mathit{c}}_j\forall j\ne i$. The task segmentation problem (4) can be solved iteratively and the details are in Algorithm 1. An example is shown in Figure 2.

Algorithm 1: Task Segmentation
	Input: $\mathcal{T}$, $N\in {\mathbb{Z}}_{+}$
1	Initialize $\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_{n_a}\}$, $\{{\mathcal{I}}_{c,1},\cdots ,{\mathcal{I}}_{c,n_a}\}$, $\mathcal{C}$ by k-means++ [30], $iter=0$
2	while $iter<N$ do
3			for task ${\mathit{t}}_i={\mathit{t}}_1\mathrm{to}{\mathit{t}}_{n_t}$ do
4					$idx$ ← the index of $t_i$’s nearest centroid
5					${\mathcal{I}}_{c,idx}.$append(i)
6			for $j=1\mathrm{to}{n}_a$ do
7					${\mathit{c}}_j$ ← mean of all tasks within cluster j
8			$iter\leftarrow iter+1$
9	for $i=1\mathrm{to}{n}_a$ do ${\mathcal{T}}_i\leftarrow \left\{{\mathit{t}}_j\right|\forall j\in {\mathcal{I}}_{c,i}\}$$\phantom{(}$
10	return $\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_{n_a}\}$, $\{{\mathcal{I}}_{c,1},\cdots ,{\mathcal{I}}_{c,n_a}\}$, $\mathcal{C}$

3.2. Cluster Assignment

Since the entire task set is partitioned into several subsets, the assignment of each subset needs to be determined, which is formulated as an integer linear programming (5). $y_{ij}=1$ if robot i is assigned with cluster j and 0 otherwise. $w_{ij}\triangleq \left|\right|{\mathit{p}}_i-{\mathit{c}}_j{\left|\right|}_2^2+{\sum }_{\mathit{t}\in {\mathcal{T}}_j}\left|\right|\mathit{t}-{\mathit{c}}_j{\left|\right|}_2^2$ defines the cost of cluster j being assigned to robot i, where the first term evaluates how far robot i is from cluster j and the second term estimates the cost of robot i visiting all the tasks within cluster j.

$$\begin{array}{}\mathrm{(5a)}& \underset{\mathit{y}}{\min }& {\sum }_{i\in \mathcal{I}}{\sum }_{j\in {\mathcal{I}}_{c,j}}{w}_{ij}{y}_{ij}\hfill \mathrm{(5b)}& \mathrm{s}.\mathrm{t}.& {\sum }_{i\in \mathcal{I}}{y}_{ij}=1,\forall j\in {\mathcal{I}}_{c,j}\hfill \mathrm{(5c)}& & {\sum }_{j\in {\mathcal{I}}_{c,j}}{y}_{ij}\le 1,\forall i\in \mathcal{I}\hfill \mathrm{(5d)}& & {\sum }_{i\in \mathcal{I}}{\sum }_{j\in {\mathcal{I}}_{c,j}}{y}_{ij}=n_a\hfill \mathrm{(5e)}& & y_{ij}=\{0,1\},\forall (i,j)\in \mathcal{I}\times {\mathcal{I}}_{c,j}.\hfill \end{array}$$

Constraint (5b) ensures that each cluster must be assigned with one robot; (5c) guarantees that each robot can be at most assigned to one cluster; (5d) enforces no unassigned cluster being left. Constraint (5c) is compatible with a situation where the number of agents is greater than the number of nonempty clusters. This situation can happen at run-time when some tasks are completed. Note that when the number of clusters is not equal to $n_a$, the constraints (5c)–(5d) need to be revised accordingly. The cluster assignment problem (5) can be solved by some constrained integer linear programming solvers such as SCIP [31] and OR-Tools [32]. Denote ${\widehat{\mathcal{T}}}_i$ as the task cluster assigned to robot i. Details about the cluster assignment are shown in Algorithm 2. An example is shown in Figure 2.

Algorithm 2: Cluster Assignment
	Input: $\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_{n_a}\}$, $\mathcal{X}$, $\mathcal{C}$
1	for robot $i=1\mathrm{to}{n}_a$  do
2			for $\mathrm{cluster}j=1\mathrm{to}{n}_a$  do
3					$w_{ij}\leftarrow \left|\right|{\mathit{p}}_i-{\mathit{c}}_j{\left|\right|}_2^2+{\sum }_{\mathit{t}\in {\mathcal{T}}_j}\left|\right|\mathit{t}-{\mathit{c}}_j{\left|\right|}_2^2$
4	${\mathit{y}}^{\ast }\leftarrow$ Solve (5) by a numerical solver
5	$\{{\widehat{\mathcal{T}}}_1,\cdots ,{\widehat{\mathcal{T}}}_{n_a}\}\leftarrow$ parse_result($\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_k\},{\mathit{y}}^{\ast }$)
6	return$\{{\widehat{\mathcal{T}}}_1,\cdots ,{\widehat{\mathcal{T}}}_{n_a}\}$

3.3. Single-Robot Mission Planning

After each robot is assigned a task cluster, the task allocation orders and the collision-free paths need to be determined. This problem can be distributed to $n_a$ robots parallelly and robot i solves its sub-problem by formulating it as a traveling salesperson problem (TSP), where the nodes are the robot itself and its assigned tasks. A path-finding algorithm generates collision-free paths for every pair of nodes and the length of these paths is the traveling cost from one node to another. Lazy Theta* [9] is used here due to fewer line-of-sight checks. This problem can be modeled as an integer linear program (6) with a Miller–Tucker–Zemlin (MTZ) formulation [33],

$$\begin{array}{}\mathrm{(6a)}& \underset{\mathit{z},\mathit{u}}{\min }& {\sum }_{i=1}^{n_m}{\sum }_{j=1}^{n_m}{d}_{ij}{z}_{ij}\hfill \mathrm{(6b)}& \mathrm{s}.\mathrm{t}.& z_{ij}\in \{0,1\},\forall i,j=1,\cdots ,n_m,\hfill \mathrm{(6c)}& & u_i\in \mathbb{Z},\forall i=2,\cdots ,n_m,\hfill \mathrm{(6d)}& & u_i-u_j+n_m{z}_{ij}\le n_m-1,2\le i\ne j\le n_m,\hfill \mathrm{(6e)}& & 1\le u_i\le n_m-1,\hfill \mathrm{(6f)}& & {\sum }_{i=1}^{n_m}{z}_{ij}=1,\forall j=2,\cdots ,n_m,\hfill \mathrm{(6g)}& & {\sum }_{j=1}^{n_m}{z}_{ij}=1,\forall i=1,\cdots ,n_m,\hfill \mathrm{(6h)}& & {\sum }_{i=1}^{n_m}{z}_{i1}=0,\hfill \end{array}$$

where $n_m\triangleq \left|{\widehat{\mathcal{T}}}_i\right|+1$ denotes the number of nodes; node 1 always indicates the robot’s current position; $z_{ij}=1$ if the robot goes from node i to node j, $\mathit{z}\in {\{0,1\}}^{n_m^2}$; $\mathit{u}\in {\mathbb{Z}}^{n_m-1}$ is a dummy variable to indicate tour ordering such that $u_i<u_j$ implies node i is visited before node j; $d_{ij}$ is the cost of the robot traveling from node i to node j, which is the length of the underlying collision-free path.

Constraints (6c)–(6e) guarantee only one tour covering all nodes [33]. Constraints (6f)–(6g) ensure that each node is visited from another node and from each node there is a departure to another node. Constraint (6h) indicates that the robot does not go back to its initial position after visiting all the tasks. (6h) can be changed if the robot needs to go back to a base. To ensure that there is no collision between robots, each robot considers the other robots as obstacles. Details are shown in Algorithm 3. An example is shown in Figure 2.

Algorithm 3: Parallel Multi-Robot Mission Planning
	Input: $\{{\widehat{\mathcal{T}}}_1,\cdots ,{\widehat{\mathcal{T}}}_{n_a}\},\mathcal{X},\mathcal{O}$
1	// $n_a$ robots parallelly execute the content in parfor
2	parfor robot $i=1to{n}_a$ do
3			Initialize $P_{lib}$ as empty
4			for start, goal in $({\widehat{\mathcal{T}}}_i\cup \left\{{\mathit{p}}_i\right\})$ do
5					${\mathcal{O}}_{now}\leftarrow \mathcal{O}\cup \mathcal{X}\setminus \left\{{\mathit{p}}_i\right\}$
6					$P_{start,goal}\leftarrow$ path_finding($start,goal,{\mathcal{O}}_{now}$)
7					$P_{lib}.$append($P_{start,goal}$)
8			for node $i=1to1+|{\widehat{\mathcal{T}}}_i|$ do
9					for node $j=1to1+|{\widehat{\mathcal{T}}}_i|$ do
10							$P_{i,j}\leftarrow$ load_path($P_{lib},i,j$)
11							$d_{ij}\leftarrow$ compute_cost($P_{i,j}$)
12			${\mathit{z}}^{\ast },{\mathit{u}}^{\ast }\leftarrow$ solve (6) by a numerical solver
13			${\mathcal{P}}_i,{\mathit{r}}_i\leftarrow$ parse_path($P_{lib},{\mathit{z}}^{\ast },{\mathit{u}}^{\ast }$)
14	return $\{{\mathcal{P}}_1,\cdots ,{\mathcal{P}}_{n_a}\},\{{\mathit{r}}_1,\cdots ,{\mathit{r}}_{n_a}\}$

3.4. RPM at Run-Time

This subsection illustrates how RPM operates at run-time. First, RPM utilizes k-means++ [30] to initialize the cluster centroids. During the mission, the centroids from the previous iteration are the initial centroids for the next iteration. As some tasks are completed, the number of nonempty clusters $n_c$ might be less than $n_a$. If $n_c<n_a$, one needs to remove the empty clusters and revise constraint (5d) as ${\sum }_{i\in \mathcal{I}}{\sum }_{j\in {\mathcal{I}}_{c,j}}{y}_{ij}=n_c$. Note that all the constraints are compatible with the case where $n_c<n_a$. If there exist dynamic obstacles and tasks, RPM updates their information (positions) at each iteration. More details are shown in Algorithm 4.

Algorithm 4: RPM at Run-time
1	Initialize $\mathcal{C}$ by k-means++ [30]
2	while $\mathcal{T}\ne \varnothing$ do
3			$\mathcal{T}\leftarrow$ update task set
4			$\mathcal{X}\leftarrow$ update robot position
5			$\mathcal{O}\leftarrow$ update obstacle
6			$\{{\mathcal{T}}_1,\cdots ,{\mathcal{T}}_k\},\mathcal{C}\leftarrow$ Algorithm 1 with previous $\mathcal{C}$
7			remove empty task cluster
8			$\{{\widehat{\mathcal{T}}}_1,\cdots ,{\widehat{\mathcal{T}}}_{n_a}\}\leftarrow$ Algorithm 2
9			$\{{\mathcal{P}}_1,\cdots ,{\mathcal{P}}_{n_a}\},\{{\mathit{r}}_1,\cdots ,{\mathit{r}}_{n_a}\}\leftarrow$ Algorithm 3
10			robots move one step along $\{{\mathcal{P}}_1,\cdots ,{\mathcal{P}}_{n_a}\}$
11			time moves one step forward
12			${\mathit{t}}_j\leftarrow$ current assigned task of robot $i,\forall i\in \mathcal{I}$
13			delete task ${\mathit{t}}_j$ if $\left|\right|{\mathit{p}}_i-{\mathit{t}}_j{\left|\right|}_2\le ϵ,\forall i\in \mathcal{I}$

4. Comparisons and Experiments

This section presents several experiments with static/dynamic obstacles/tasks and conducts scalability and optimality comparisons between RPM and a decentralized method [24]. From here on, CBBA is interchangeable with the method in [24] because it consists of CBBA and posterior path-finding. In addition, this section analyzes the computational burden for RPM and presents the optimality gap in small-size problems.

RPM is written in C++ and compiled as a Python library to be invoked. The integer programs in Algorithms 2 and 3 are solved by OR-Tools [32]. The C++ implementation utilizes multithreading as parallelization, i.e., the parfor in Line 2, Algorithm 3. First, a main thread, i.e., the central robot, runs Algorithms 1 and 2. Then, the results of Algorithm 2 are distributed to multiple robots/threads, where each thread runs Algorithm 3 parallelly for each robot. All the results are obtained by a computer with a 2.8 GHz Intel Core i7-7700HQ CPU and 16 GB memory. This implementation does not require a GPU but one can accelerate it with a GPU if needed.

4.1. Experiments

The test area is 6 m × 5.6 m and the grid map size is $120\times 112$. RPM executes real-time mission planning for two Parrot Mambo quadrotors. In the experiments, each quadrotor follows the discrete paths returned from RPM. Then a low-level trajectory tracking controller (https://github.com/zehuilu/Mambo-Tracking-Interface (accessed on 25 February 2024)) broadcasts the desired control commands given the desired paths to each Mambo individually. Some details are explained in Figure 1. In the case of a dynamic task, a cone moves from one side to another side and RPM updates its planning result accordingly. Footage for these experiments is included in Video S1 of Supplementary Materials.

4.2. Comparison with Increased Number of Robots

Section 4.2 and Section 4.3 show scalability comparisons between RPM and [24]. The grid map is $50\times 50$. Given a particular number of robots $n_a$ and tasks $n_t$, there are 100 different scenarios where the positions of robots and tasks are generated randomly. For each scenario, there are 200 randomly generated obstacles; each method runs 20 times, and the average computation time and total distance are collected.

Although [24] utilizes Dijkstra’s algorithm [5] as the path-finder, this paper replaces Dijkstra’s algorithm with Lazy Theta* [9] as the path-finder of [24] to present a fair comparison, regarding the computation. In other words, this paper eliminates the performance difference between the two path-finders although Lazy Theta* is faster, occupies less memory, and generates shorter paths due to any-angle movement.

In Figure 3a,b, the computation time of [24] is increased exponentially and is up to over 4.5 s when there are 20 robots and 60 tasks, whereas the computation time of RPM is increased linearly, in the order of milliseconds. The fully centralized method [28] has a similar scenario with a 32 × 32 map and random obstacles. According to Figure 3 of [28], it takes about 55 s to generate sequences for 5–20 robots and 10–50 tasks. This paper omits the comparison with [28] because [28] is not a real-time algorithm. The CBBA’s computation time is increased exponentially because all robots need to take auctions iteratively and repeat for every task. The negotiation process for each task is more time-consuming and less efficient when $n_a$ is larger, whereas, for RPM, the increased $n_a$ only raises the burden of Algorithms 1 and 2 slightly. The most computationally heavy part of RPM is finding the collision-free path between every pair of nodes in each sub-problem, i.e., Line 4–Line 7 of Algorithm 3. Since Algorithm 3 is distributed over robots, the increased $n_a$ does not raise the computational load significantly. Section 4.4 analyzes the computational burden of RPM and shows consistency with the comparisons.

As for optimality (total distance), RPM outperforms [24] because RPM utilizes the global information of tasks and robots in Algorithms 1 and 2, while [24] performs an auction for one task at a time. Thus, the fully decentralized auction process does not utilize global information, resulting in less optimality. Moreover, the bid price in [24] is the Euclidean distance between robot and task, and [24] only generates collision-free paths after the task order is determined. In a cluttered environment, the Euclidean distance is not the actual cost. For completeness, Section 4.4 further analyzes and compares the computational burden if [24] utilizes collision-aware cost as the bid price.

On the other hand, as the number of robots increases, the optimality difference between the two methods is roughly the same. This observation is caused by the constant ratio between the numbers of unassigned tasks and robots. When this ratio is kept constant, for CBBA, the amount of negotiation between every two robots for every task is roughly the same as the number of robots increases.

4.3. Comparison with Increased Number of Tasks

In Figure 4a,b, CBBA’s computation time is increased exponentially and is about 0.75 s for 60 tasks and 3 robots, whereas the computation time of RPM is increased almost linearly. The increasing rate of CBBA’s computation time in Figure 4a is much less than Figure 3a because there is less negotiation among robots and thus the auction for each task needs fewer iterations when $n_a$ is smaller.

The increasing rate of RPM’s computation time in Figure 4b is greater than Figure 3b because the linearly increased $n_t$ leads to the computational burden increasing quadratically (see Section 4.4). Nevertheless, the magnitude of computation time is still relatively small because each robot only needs to deal with a task subset due to Algorithm 1. The detailed analysis is shown in Section 4.4. Figure 4c shows that RPM outperforms CBBA regarding optimality. These comparisons show that, by using some global information in a parallel manner, RPM achieves better performance than a decentralized method and a centralized method.

The optimality gap between the two methods in Figure 4c increases as the number of tasks increases since the total traveling distance by CBBA is increased. This is because the ratio between the number of unassigned tasks and robots is increasing, which leads to more negotiation between every two robots for every task.

4.4. Computational Burden Analysis

RPM approximates the traveling cost from one node to another by the length of the underlying collision-free path. An intuitive way to improve the optimality of CBBA is to utilize the lengths of collision-free paths as bid prices. This subsection analyzes the computational burden of RPM and this approach.

To find all possible paths, each robot connects to all the tasks and every two tasks connect. Thus, the total number of paths ${\widehat{N}}_p$ for CBBA is

$${\widehat{N}}_p={ }_{n_t}{P}_2+n_a·n_t=n_t(n_t+n_a-1),$$

(7)

where ${ }_{n_t}{P}_2=\frac{n_t!}{(n_t-2)!}$ is the number of permutations for selecting two elements from a total of $n_t$ elements.

As for RPM, the upper bound ${\overline{N}}_p$ for the number of paths to be found for each robot is $n_t$, i.e.,

$${\overline{N}}_p\triangleq supmax\left(\right|{\widehat{\mathcal{T}}}_1|,\cdots ,|{\widehat{\mathcal{T}}}_{n_a}\left|\right)=n_t.$$

(8)

Denote $\mathrm{ceil}(·):\mathbb{R}\mapsto \mathbb{Z}$ as the ceiling function, and $\mathrm{ceil}\left(x\right)$ as the least integer greater than or equal to x. Since the entire task set is partitioned into $n_a$ subsets and the path-finding for each robot is parallel, the lower bound ${\underline{N}}_p$ is

$${\underline{N}}_p\triangleq infmax\left(\right|{\widehat{\mathcal{T}}}_1|,\cdots ,|{\widehat{\mathcal{T}}}_{n_a}\left|\right)\triangleq n_c=\mathrm{ceil}(n_t/n_a).$$

(9)

Hence, the maximum number of paths $N_p$ for RPM is

$$n_c+{ }_{n_c}{P}_2\le N_p\le n_t+{ }_{n_t}{P}_2\Rightarrow {\left(\frac{n_t}{n_a}\right)}^2⪅N_p\le n_t^2.$$

(10)

Combining with (7) yields

$$1<1+\frac{n_a-1}{n_t}\le \frac{{\widehat{N}}_p}{N_p}⪅(1+\frac{n_a-1}{n_t})n_a.$$

(11)

Since ${(n_t/n_a)}^2⪅N_p\le n_t^2$, when $n_a$ is increased linearly and the ratio of $n_t$ to $n_a$ is a constant $a\triangleq n_t/n_a$, the lower bound of $N_p$ increases linearly as ${\underline{N}}_p=a^2{n}_a$. This conclusion is consistent with Figure 3b. Based on observation of comparisons, the actual computational burden of RPM is skewed towards the lower bound. When $n_t$ is increased linearly and $n_a$ is fixed, ${\underline{N}}_p$ increases quadratically to $n_t$. In addition, the standard deviation of computation time in Figure 4b is increasingly larger than in Figure 3b. This observation appears because the number of assigned tasks for each robot $|{\widehat{\mathcal{T}}}_1|,\cdots ,|{\widehat{\mathcal{T}}}_{n_a}|$ tends to be more diverse as $n_t$ increases and $n_a$ is constant. As for Figure 3b, the task–robot ratio is fixed and thus the deviation remains relatively the same when $n_a$ increases.

As for replacing the bid cost as the length of a collision-free path, the extra computational burden of CBBA is greater than the actual burden of RPM. The difference between the two upper bounds is $n_t(n_a-1)$, which increases linearly as $n_t$ or $n_a$ increases. When the task–robot ratio is fixed and $n_a$ increases, ${\widehat{N}}_p/{\underline{N}}_p$ is still greater than 1 and it increases with a rate of $1/n_a$. The upper bound ${\widehat{N}}_p/{\overline{N}}_p$ increases with a rate of $n_a$. When $n_a\gg n_t$, $n_a·N_p\le {\widehat{N}}_p⪅n_a^2·N_p$. Therefore, the computation burden of CBBA is at least $n_a$ times heavier than RPM and up to $n_a^2$ times heavier. When $n_a\ll n_t$, $N_p\le {\widehat{N}}_p⪅n_a·N_p$. The worst case of RPM is that its computation burden is slightly less than CBBA’s but CBBA’s burden at most is $n_a$ times greater than RPM’s. Thus, task segmentation and parallelizable mission planning benefit run-time computation. Revising the bid prices of CBBA is not computationally efficient and, hence, the scalability is not good.

4.5. Optimality Gap in Small-Size Problems

Section 4.5 shows the optimality gap between RPM and the global optimum. The global optimum is found by exhaustive search and thus the search is only feasible in small-size problems. Figure 5 shows the optimality gap with two cases, 2 robots + 4 tasks and 3 robots + 6 tasks. For each case, there are 20 scenarios with different positions of robots, tasks, and obstacles. It is impossible to search a global optimum exhaustively for problems with a larger size since the MRMP problem is NP-hard. The total number of solutions for $n_a$ robots and $n_t$ tasks is $\frac{(n_t+2n_a-1)!n_t!}{(n_a+n_t)!(n_a-1)!}$. For the case with three robots and six tasks, there are 39,600 possible solutions and it takes about 10 s to find a global optimum. For four robots and eight tasks, there are 18,345,600 solutions and the estimated time to find an optimum is 78 min.

In the case of two robots, the optimality gap between RPM and the optimum is on average 4.3% while CBBA’s cost is on average 36.3% greater than the optimum. As for another case, RPM’s cost is on average 8.3% greater than the optimum whereas CBBA’s cost is 43.1% greater than the optimum. The RPM’s optimality gap increases when the problem size increases since the task segmentation algorithm cannot explore all the permutations of the number of assigned tasks for each robot. Nevertheless, the algorithm makes the MRMP problem tractable and solves it at run-time.

5. Conclusions

The collision-aware multi-robot mission planning problem is NP-hard but requires real-time computational performance in many applications. This paper presents a real-time parallel algorithm RPM, which partitions the entire task set into several subsets such that each robot can determine the task allocation order and collision-free path parallelly. This process reduces the dimension of the original problem and, hence, makes RPM able to run in real time with good scalability. The above results show that, by using global information in a parallel manner, RPM achieves better performance on both computation and optimality.

There are still numerous challenges in real-time multi-agent mission planning, such as enforcing endurance and capacity constraints, handling dynamic obstacles and tasks with intention prediction, introducing the heterogeneity of agents and tasks, etc. Also, designing distributed algorithms with good optimality and computational performance is another interesting direction.