Extending Conflict-Based Search for Optimal and Efficient Quadrotor Swarm Motion Planning

1. Introduction

Swarm motion planning is a fundamental technology for the autonomous flight of Unmanned Aerial Vehicle(UAV) swarm, aiming to solve a dynamically feasible and collision-free flight trajectory for each one. Based on the organizational structure of the swarm, swarm motion planning generally employs either centralized or decentralized methods. Centralized methods struggle to provide feasible solutions within a limited time due to the curse of dimensionality [1]. In contrast, decentralized methods plan independently, offering fast computation but often lacking optimality, resulting in poor solution quality and potential deadlocks(Especially in symmetrical scenarios) [2,3]. Balancing optimality, high-quality solutions, and computational efficiency remains an unresolved challenge in swarm motion planning.

A simplified problem corresponding to swarm motion planning in discrete time-space is Multi-Agent PathFinding(MAPF). MAPF is an NP-hard graph search problem that seeks to find multiple conflict-free paths [4]. The classical algorithm Conflict-Based Search(CBS) and its variants have made great progress in recent years and have been widely applied in fields such as warehouse management, airport towing, and railway planning [5]. CBS consists of two layers: the low-level algorithm plans a collision-free path for each agent in a decentralized manner, like A* or SIPP(Safe Interval Path Planning). The high-level algorithm centrally detects conflicts among all paths and resolves them by imposing constraints at the low level, iterating until no conflicts are found [6]. However, due to the movement constraints of agents in MAPF and various physical assumptions, such as 4/8-neighbors search space and waiting at the same location, CBS is difficult to apply directly to robots like UAVs that require continuous motion planning.

Inspired by the CBS algorithm for solving MAPF, its decentralized planning and centralized conflict resolution provide a new approach for UAV swarm motion planning. By enhancing the low-level discrete path planning to continuous motion planning that accounts for UAV dynamics, defining new conflict types and implementing conflict detection methods at the high level, the CBS can be extended to swarm motion planning.

Similarly, we integrate State Lattice with CBS to propose an optimal and efficient swarm motion planner suitable for a quadrotor swarm, named SL-CBS. The SL-CBS algorithm is also divided into two layers. The low-level is an improved State Lattice based on motion primitive search. To implement waiting actions, we design an emergency stop motion primitive. Besides, we handle spatio-temporal constraints from high-level conflict resolution. At the high level, we define comprehensive conflict types, particularly motion primitive conflicts. Instead of sampling in the time dimension for conflict detection, we propose a linear complexity conflict detection method based on Sturm’s theory. Additionally, our modified Independent Detection(ID) technology applied to the SL-CBS provides the ability to parallel process conflicts, enhancing planning ability in certain scenarios. Our main contributions are as follows:

(1)

We extend the CBS algorithm to quadrotor swarm motion planning and propose the optimal and efficient SL-CBS planner integrating State Lattice and CBS.

(2)

At the low-level, we improve State Lattice based on motion primitive search that considers a complete dynamic model. We design emergency stop motion primitives and address corresponding spatio-temporal constraints.

(3)

At the high-level, we define motion primitive conflicts and propose a conflict detection method with linear time complexity based on Sturm’s theory.

(4)

We validate the planning capabilities of SL-CBS in classical scenarios such as CrossAndSwap, Cross and Swap.

(5)

The benchmarks are designed, and compared to the latest State-Of-The-Art(SOTA) algorithms, SL-CBS demonstrates higher success rates, reduced computation time, and lower flight costs.

(6)

We analyze the performance boundaries of SL-CBS+ID in large-scale batch testing.

2. Related Work

2.1. MAPF

Consider an undirected graph $G\left(V,E\right)$, where V represents vertices and E represents edges connecting two vertices, assume a set of K agents $\left\{a_1,a_2,\dots ,a_K\right\}$ with starting positions $s=\left(s_1,s_2,\dots ,s_K\right)$ and goal positions $g=\left(g_1,g_2,\dots ,g_K\right)$, where $s_i\in V$ and $g_i\in V$ for $1\le i\le K$. Time is discretized as $t=1,2,\dots$. At each time step, agents can perform one of two actions: move to a neighboring node (4/8-neighbors) or wait at their current position. Each action incurs a unit cost of 1. The objective of MAPF is to determine conflict-free paths for each agent while minimizing the total travel cost. Conflicts typically fall into two categories: vertex conflicts and edge conflicts. A vertex conflict $〈a_i,a_j,v_i,v_j,t〉$ indicates that agents $a_i$ and $a_j$ are in conflict at positions $v_i$ and $v_j$ respectively at time t, typically with $v_i=v_j$. An edge conflict $〈a_i,a_j,v_i,v_{i+1},v_j,v_{j+1},t〉$ occurs when agent $a_i$ moves from $v_i$ to $v_{i+1}$ and agent $a_j$ moves from $v_j$ to $v_{j+1}$ at time t, typically with $v_i=v_{j+1}$ and $v_{i+1}=v_j$.

2.2. CBS and Its Developments

The main methods for solving the MAPF include A*-based methods, reduction-based methods, ICTS and CBS, with detailed reviews in  [7,8]. In recent years, the CBS algorithm has attracted significant interest from researchers and scholars due to its powerful conflict resolution capabilities and large-scale conflict-free paths planning abilities.

CBS is a two-layer search algorithm. The low-level finds optimal paths for each agent that satisfy spatio-temporal constraints, often using the A* algorithm. The high-level maintains a Constraint Tree $CT$, where each node $N\in CT$ consists of three components: (1) a set of constraints applied to the agents, $N.constraints$; (2) a set of solutions satisfying all constraints, $N.solutions$; (3) the cost of the solutions, $N.cost$. The root node of the $CT$ is constructed from the solutions of single-agent planning without constraints, and the optimal node is selected for expansion based on a greedy search strategy. After selecting a node for expansion, the high-level algorithm checks for conflicts among the agents. If no conflicts exist, the current node is considered as the goal node, and the high-level algorithm terminates, returning the solutions. Otherwise, CBS selects a conflict to resolve, such as $〈a_i,a_j,v_i,v_j,t〉$. Two child nodes are created, inheriting the parent’s constraints and adding constraints $〈a_i,v_i,t〉$ and $〈a_j,v_j,t〉$ to each child, respectively. The constraint $〈a_i,v_i,t〉$ prohibits agent $a_i$ from moving to vertex $v_i$ at time t, and similarly for $〈a_j,v_j,t〉$. After adding constraints, the child nodes are replanned using the low-level algorithm and added to the $CT$.

Subsequent developments in CBS have been significant.  [9] classifies conflicts among agents into cardinal, semi-cardinal, and non-cardinal conflicts, prioritizing the resolution of cardinal conflicts. For semi-cardinal and non-cardinal conflicts, agents with alternative paths of equal cost are rerouted to bypass conflicts [10]. In  [11,12], symmetry-breaking technique and weighted dependency graph heuristic are proposed for 4-connected grid maps, solving paths for hundreds of agents in dense maps.

2.3. Swarm Motion Planning Using CBS

In existing work, the CCBS algorithm proposed in  [13] is an attempt to extend CBS to real-world robots. It defines state conflict types that consider the geometric sizes of robots in the high-level algorithm and computes unsafe time intervals. The low-level search uses the SIPP for continuous-time planning to handle unsafe time interval constraints. Although subsequent works, CCBS+ [14] and CCBS-PGA [15] employ techniques like Prioritizing Conflicts(PC), Disjoint Splitting(DC), High-level Heuristics(HH), and Prioritized Planning(PP) to reduce computing time, they are limited to disc robots and have many physical constraints, such as simple motion models, constant velocity, and ignoring inertia. The CBS-MP proposed in  [16] constructs a roadmap using the PRM sampling algorithm during preprocessing, providing feasible paths for all agents. Although roadmap-based CBS efficiently solves paths, the low-level algorithm does not consider dynamic and kinematic models, making the paths suitable only for discrete motion operators.

The work in  [17] first integrates the CBS with the Hybrid-State A* algorithm considering dynamics, proposing the CL-CBS for Ackermann-steering vehicles. However, the low-level Hybrid-State A* requires vehicles to move at constant speed, accelerate instantly to constant speed, and brake instantly, which is unrealistic. Moreover, the CBS high-level only considers state conflicts between vehicles, ignoring motion primitive conflicts, leading to a lack of safety guarantees. Although subsequent work HG-CBS [18] improves computational efficiency through grouping and improved search strategies, it does not overcome the original issues. The K-CBS proposed in  [19] uses a sampling algorithm considering full dynamics for low-level planning, with high-level algorithms performing state conflict detection after uniform time sampling to determine conflict intervals. Although its multiple motion trajectory results are suitable for real-world swarm motion scenarios, inefficient conflict detection and single-agent planning limit its scalability. In  [20], CB-MPC fully considers the dynamic and kinematic models of nonlinear unicycles, using an MPC optimal controller for CBS’s low-level algorithm. It solves multiple motion trajectories for complex scenarios through iterative continuous planning, but the large computational load and long optimization time of the MPC optimal control problem result in low overall computational efficiency. The latest db-CBS [21] implements low-level planning based on db-A* [22] , which pre-solves two-point boundary value problems to compute motion primitives meeting dynamic requirements, followed by bounded discontinuous A* search for motion trajectories. Outside the two-layer CBS algorithm, db-CBS designs a multi-robot trajectory optimization layer to address bounded discontinuities in low-level planning. db-CBS efficiently solves large-scale swarm motion trajectories but relies on time-consuming offline pre-computation of motion primitives.

The aforementioned works either do not consider the complete dynamics of the robots, lack safety guarantees, or suffer from low computational efficiency and require offline pre-computation. Our goal is to overcome these limitations by proposing an optimal and efficient UAV swarm motion planner, which takes into account the full dynamics, based on the CBS.

The organization of this paper is as follows: Section 3 formally defines UAV swarm motion planning problem. In Section 4, we introduce the SL-CBS+ID algorithm framework and its corresponding components. Section 5 presents the experimental results, including tests on classical scenarios, comparisons with baseline algorithms, and performance boundary analysis. Finally, Section 6 provides a summary of the work and suggests potential future research directions.

3. Problem Description

The UAV swarm consists of K identical quadrotors, operating within a workspace W, where $W\subset R^m$ and $m=2,3$. Given an obstacle space $P^{obs}$, the free space is defined as $P^{free}=W\setminus P^{obs}$.

The state of $UA{V}_i$ is represented as $x_i\left(t\right)={\left[p_i^T\left(t\right),v_i^T\left(t\right),\dots ,p_i^{{\left(q-1\right)}^T}\left(t\right)\right]}^T$, where $x_i\left(t\right)\in \chi \subset R^{mq}$, $1\le i\le K$, and $\chi$ is the state space. The state $x_i\left(t\right)$ includes the UAV’s position and its derivatives up to order $q-1$. The q-th derivative of the position trajectory $p_i\left(t\right)$ is the control input $u_i\left(t\right)$, where $u_i\left(t\right)=p_i^{\left(q\right)}\left(t\right)$, and $u_i\left(t\right)\in U:={[-u_{max},u_{max}]}^m$, with $u_{max}$ being the maximum control input. The UAVs follow the system model:

$$\dot{x}=Ax+Bu$$

(1)

where

$$A={\left[\begin{array}{ccccc}0& I_m& 0& \cdots & 0\\ 0& 0& I_m& \cdots & 0\\ ⋮& \ddots & \ddots & \ddots & ⋮\\ 0& \cdots & \cdots & 0& I_m\\ 0& \cdots & \cdots & 0& 0\end{array}\right]}_{qm\times qm},B={\left[\begin{array}{c}0\\ 0\\ ⋮\\ 0\\ I_m\end{array}\right]}_{qm\times m}$$

UAVs are subject to kinematic constraints such as maximum velocity $v_{max}$ and maximum acceleration $a_{max}$. The safe state space ${\chi }^{free}$ is defined as:

$${\chi }^{free}=P^{free}\times {\left[-v_{max},v_{max}\right]}^m\times {\left[-a_{max},a_{max}\right]}^m\times \dots$$

(2)

Additionally, $UA{V}_i$ is modeled as a rigid body, with $S\left(x_i\right)$ representing the workspace occupied by the $UA{V}_i$ in state $x_i\left(t\right)$. The UAV is geometrically modeled as a circle or sphere with its center at the UAV’s centroid and radius $r_c$.

The UAV swarm motion planning problem is described as follows: Given each initial state $x_{i0}\in {\chi }^{free}$ for $UA{V}_i$, the goal is to reach the target region $X_i^G\subset {\chi }^{free}$. Within the given control input U, design the control input $u_i\left(t\right)$ for each UAV, adhering to the system model (1) and kino-dynamic constraints (2), to obtain the trajectory $T_i\left(t\right)$ such that $T_i\left(t_0\right)=x_{i0}$ and $T_i\left(t_{fi}\right)\in X_i^G$, where $t_0$ is the initial time of the swarm and $t_{fi}$ is the arrival time for $UA{V}_i$. The trajectories $T_1,T_2,\dots ,T_K$ must be conflict-free at any time t, meaning:

$$S\left(T_i\left(t\right)\right)\cap S\left(T_j\left(t\right)\right)=\varnothing ,\forall 1\le i<j\le K$$

(3)

$$S\left(T_i\left(t\right)\right)\cap P^{obs}=\varnothing ,\forall 1\le i\le K$$

(4)

4. Algorithm

The SL-CBS algorithm builds upon the classic CBS framework, defining two conflict types: state conflicts and motion primitive conflicts. The low-level employs a State Lattice algorithm based on motion primitive search to effectively handle spatio-temporal constraints, while the high-level introduces a conflict detection method based on Sturm’s theory and implements conflict resolution. Figure 1 clearly illustrates the planning process of the SL-CBS algorithm. Figure 1a depicts the initial state of a UAV swarm consisting of three UAVs, showcasing a map with obstacles, as well as the initial positions and target regions of the UAV swarm. Figure 1b presents the planning result of the first iteration of SL-CBS, where each UAV independently plans its trajectory without considering constraints. A collision occurs between the flight trajectories of $UA{V}_1$ and $UA{V}_2$. Figure 1c shows the planning result of the second iteration. After detecting the conflict in the higher-level algorithm, SL-CBS applies the corresponding constraints to the relevant UAVs and replans the trajectories. It can be observed that the three flight trajectories are now collision-free. The flight trajectories of $UA{V}_1$ and $UA{V}_3$ remain unchanged from Figure 1b, while $UA{V}_2$’s trajectory is replanned with the necessary constraints applied to avoid the collision detected in Figure 1b.

4.1. Definition of Two Conflict Types

We comprehensively define two conflict types: state conflicts and motion primitive conflicts, addressing the limitation in  [17] which only considers state conflicts. As shown in Figure 2, a state conflict, corresponding to a vertex conflict in the MAPF problem, is denoted as $〈x_i\left(t\right),x_j\left(t\right),t+\tau 〉$, indicating a conflict between the states $x_i(t+\tau )$ of $UA{V}_i$ and $x_j(t+\tau )$ of $UA{V}_j$ occurring at time $t+\tau$. A motion primitive conflict, also akin to an edge conflict in MAPF, is represented as $〈x_i\left(t\right),x_j\left(t\right),{pr}_i\left(t\right),{pr}_j\left(t\right),t,t+\tau 〉$, indicating a conflict between the motion primitive segment $p{r}_i\left(t\right)$ of $UA{V}_i$ and $p{r}_j\left(t\right)$ of $UA{V}_j$ over the interval $(t,t+\tau )$. The exact time and location of the conflict require precise calculation, but it occurs within $(t,t+\tau )$. A detailed description of motion primitive segments $p{r}_i\left(t\right)$ and $p{r}_j\left(t\right)$ is defined in Section 4.2.

After detecting conflicts and selecting one for resolution, the selected conflict is divided into specific constraints for the low-level algorithm to handle. A state conflict, $〈x_i\left(t\right),x_j\left(t\right),t+\tau 〉$, can be divided into constraints $〈UA{V}_i,\overline{x_j\left(t\right)},t+\tau 〉$ and $〈UA{V}_j,\overline{x_i\left(t\right)},t+\tau 〉$, where $〈UA{V}_i,\overline{x_j\left(t\right)},t+\tau 〉$ prohibits $UA{V}_i$ from reaching the vicinity of state $x_j\left(t\right)$ at time $t+\tau$, and similarly for $〈UA{V}_j,\overline{x_i\left(t\right)},t+\tau 〉$. A motion primitive conflict, $〈x_i\left(t\right),x_j\left(t\right),{pr}_i\left(t\right),{pr}_j\left(t\right),t,t+\tau 〉$, can be divided into constraints $〈UA{V}_i,\overline{{pr}_j\left(t\right)},t,t+\tau 〉$ and $〈UA{V}_j,\overline{{pr}_i\left(t\right)},t,t+\tau 〉$, where $〈UA{V}_i,\overline{{pr}_j\left(t\right)},t,t+\tau 〉$ prohibits $UA{V}_i$ from conflicting with motion primitive $p{r}_j\left(t\right)$ during $(t,t+\tau )$, and similarly for $〈UA{V}_j,\overline{{pr}_i\left(t\right)},t,t+\tau 〉$.

4.2. Improved State Lattice

State Lattice is a motion planning method based on motion primitive search, integrating the advantages of considering UAV dynamics and A* search [23]. The State Lattice algorithm first uniformly samples each dimension of the control input U over $[0,u_{max}]$ with a sampling number $\mu$, generating a control input sample set $U_M:=\left\{u_1,u_2,\dots ,u_M\right\}$, where $M={(2\mu +1)}^3$. Given the initial state $x_0:={\left(p_0^T,v_0^T,a_0^T,\dots \right)}^T$, time interval $t>0$, and control input $u_m$, the motion primitive $pr\left(t\right)$ is generated based on the UAV system model (1):

$$pr\left(t\right)=x\left(t\right)=e^{At}{x}_0+\left[{\int }_0^t{e}^{A\left(t-\sigma \right)}Bd\sigma \right]u_m$$

(5)

The state transition equation $x\left(t\right)$ is represented by $pr\left(t\right)$ for clarity. According to  [23], the motion primitive $pr\left(t\right)$ is a polynomial curve of order $2q-1$. Similarly, we design the motion primitive cost J as follows:

$$J=J_s+\rho T=\underset{0}{\overset{T}{\int }}{∥u\left(t\right)∥}^{2}dt+\rho T$$

(6)

It is a linear combination of smoothness cost $J_s$ and time cost T (Note: T here refers to time interval of motion primitive as described in  [23]). Assuming the time interval $t=\tau$, the motion primitive cost is $\left({∥u_m∥}^2+\rho \right)\tau$, where $\rho$ is the time cost weight. After constructing the motion primitive graph, the final trajectory is searched using the A* algorithm.

4.2.1. Emergency Stop Motion Primitive

In MAPF, the wait action is crucial, offering more possibilities in the motion space. When extended to UAV motion planning, an emergency stop motion primitive corresponding to the wait action is needed. Most existing work assumes that robots can stop instantaneously. However, we design an emergency stop motion primitive based on a maximum acceleration braking model, which is implemented through the following three steps: (1) Calculate the maximum braking time $t_{stop}$:

$$t_{stop}={\displaystyle \frac{{∥v∥}_{\infty }}{\lambda a_{max}}}$$

(7)

Here, $\lambda$ is a factor adjusting braking intensity to ensure completion within $\tau$; (2) Calculate braking acceleration:

$$a_{stop}=-{\displaystyle \frac{v}{t_{stop}}}=-{\displaystyle \frac{\lambda v{a}_{max}}{{∥v∥}_{\infty }}}$$

(8)

(3) For each axis, calculate the emergency stop motion primitive $p{r}_{stop}$ based on the braking model $d_i$:

$$d_i={\displaystyle \frac{v_i^2}{2a_{stop,i}}}$$

(9)

where $d_i$ is the displacement vector of braking distance in different dimensions. Taking a 2D workspace as an example, $i\in \left\{x,y\right\}$, as shown in Figure 3. The cost of the emergency stop motion primitive is designed as $J_{stop}=\rho \tau$, assuming no control input effect.

4.2.2. Spatio-Temporal Constraints

The high-level algorithm defines state and motion primitive conflicts, dividing them into specific state and motion primitive constraints. In the improved State Lattice algorithm, these constraints are processed to prevent corresponding conflicts. The legality detection process after motion primitive generation involves two steps: (1) Check if the constraint occurrence time overlaps with the current expanded state/motion primitive time. If not, skip the check; otherwise, proceed to (2); (2) For state constraints, check if the current state conflicts with the constrained state. For motion primitive constraints, check if the current motion primitive conflicts with the constrained motion primitive. The specific detection method is detailed in the conflict detection scheme in Section 4.3.

The improved State Lattice pseudo-code is shown in Algorithm 1, with major improvements marked with light yellow areas, while the rest remains consistent with the original algorithm. In line 10, the $getNeighbor{s}^{*}$ function includes the design of emergency stop motion primitives, and line 18-*20 adds a new check for potential violations of high-level constraints with $chkCons\left(\right)$.

Algorithm 1 SLPlanner*(Improved SLPlanner)

Input:$x_0$, $X^G$, constraints Output: a safe and feasible trajectory T 1: $\mathrm{root}\leftarrow \left(x_0\right)$; $\mathrm{root}.g\leftarrow 0$; $\mathrm{root}.h\leftarrow h\left(x_0\right)$; 2: $\mathrm{OpenList}.push\left(\mathrm{root}\right)$; 3: while OpenList $is$$not$$Empty$ do 4:    $\mathrm{currNode}\leftarrow \mathrm{OpenList}.top\left(\right)$; 5:    $\mathrm{OpenList}.pop\left(\right)$; 6:    if currNode $is$ $near$$X^G$ then 7:      $T\leftarrow traceTrajectory\left(\mathrm{currNode}\right)$ 8: return true; 9:    end if 10:    $\mathrm{neighbors}\leftarrow getNeighbor{s}^{*}\left(\mathrm{currNode}\right)$; 11:    for neighbor $in$ neighbors do 12:      $\mathrm{nextNode}\leftarrow (\mathrm{neighbor}.\mathrm{state},\mathrm{neighbor}.\mathrm{pr})$; 13:      $\mathrm{nextNode}.g\leftarrow \mathrm{currNode}.g+cost\left(\mathrm{neighbor}\right)$; 14:      $\mathrm{nextNode}.h\leftarrow h\left(\mathrm{neighbor}.\mathrm{state}\right)$; 15:      if $chkDyn\left(\mathrm{nextNode}\right)$$\left|\right|$$chkColl\left(\mathrm{nextNode}\right)$ then 16:         $continue$; 17:      end if 18:      if $chkCons(\mathrm{nextNode},\mathrm{constraints})$ then 19:         $continue$; 20:      end if 21:      if nextNode $is$ $not$ $in$OpenList then 22:         $\mathrm{OpenList}.push\left(\mathrm{nextNode}\right)$; 23:      else 24:         if $\mathrm{nextNode}.g<\mathrm{OpenList}\left[\mathrm{targetIndex}\right].g$ then 25:           $\mathrm{OpenList}.update\left(\mathrm{nextNode}\right)$; 26:         end if 27:      end if 28:    end for 29: end while 30: return false;

4.3. Conflict Detection Method Based on Sturm’s Theory

Detecting state and motion primitive conflicts is a frequent operation that directly impacts the efficiency of swarm motion planning. State conflict detection can be performed directly using the geometric model and pose information of the UAVs. Given that $UA{V}_i$ is in state $x_i\left(t\right)$ with position $p_i\left(t\right)$ at time t, and $UA{V}_j$ is in state $x_j\left(t\right)$ with position $p_j\left(t\right)$ at the same time, a state conflict between $UA{V}_i$ and $UA{V}_j$ at time t is determined by the following condition:

$${\left(p_i\left(t\right)-p_j\left(t\right)\right)}_2<2r_c$$

(10)

Detecting motion primitive conflicts is more complex. The motion primitive of $UA{V}_i$ is $p{r}_i\left(t\right)$, and for $UA{V}_j$, it is $p{r}_j\left(t\right)$, with the time interval for motion primitives being $(t,t+\Delta t)$. Due to the presence of emergency stop motion primitives, the time step $\Delta t$ is less than $\tau$. For simplicity, we normalize the process and check whether the motion primitives $p{r}_i\left(t\right)$ and $p{r}_j\left(t\right)$ conflict over the interval $t\in (0,\Delta t)$ using the following condition:

$${∥{pr}_i\left(t\right)-{pr}_j\left(t\right)∥}_2<2r_c,t\in (0,\Delta t)$$

(11)

[19] indicates that sampling-based detection in the time dimension is inefficient, and its completeness depends on sampling precision. Solving the intersection of two motion primitives analytically from Formula (11) can lead to numerical instability, and finding the roots of high-order polynomials is challenging. Inspired by the method in [24] using Sturm’s theory for polynomial constraint satisfaction problem, which determines the number of roots without calculating their exact values, we apply this approach to conflict detection process because it perfectly suits our needs. Using Sturm’s theory, we detect motion primitive conflicts by first constructing a multivariable polynomial function $G\left(t\right)$:

$$G\left(t\right)={∥p{r}_i\left(t\right)-p{r}_j\left(t\right)∥}^2-4r_c^2,t\in (0,\Delta t)$$

(12)

If $G\left(t\right)$ has roots within the interval $t\in (0,\Delta t)$, a conflict occurs; otherwise, there is no conflict. According to Sturm’s theory, determining the existence of roots involves constructing the Sturm sequence of $G\left(t\right)$ and calculating the difference in the number of sign changes at the endpoints of the interval $(0,\Delta t)$. For detailed information on constructing the Sturm sequence and calculating sign changes at interval endpoints, refer to  [24].

4.4. SL-CBS+ID

After detecting all conflicts within the UAV swarm, a conflict graph is constructed for the UAVs. As shown in Figure 4, we observe that conflicts between $UA{V}_1$, $UA{V}_2$, $UA{V}_3$, and $UA{V}_5$, as well as those between $UA{V}_4$, $UA{V}_6$ and $UA{V}_7$ can be handled in parallel, thus reducing iteration numbers of SL-CBS algorithm. The Independence Detection (ID) technique addresses this issue by determining that if the optimal solutions for each group of agents do not conflict with one another, the groups are considered independent. We integrate the ID concept from  [25] into the SL-CBS algorithm as follows: (1) Construct a conflict graph from the trajectory results of the UAV swarm; (2) Group the UAV swarm using Depth-First Search(DFS); (3) Solve each group in parallel using the SL-CBS algorithm; (4) Check if there are any conflicts in the trajectory results of the entire UAV swarm. If conflicts exist, return to step (1); otherwise, terminate the algorithm. To prevent ID from entering a loop of iterations, we stipulate that if the ID iteration exceeds a specified value, all agents are grouped together. Overall, with ID acceleration, the optimality and completeness of the original SL-CBS algorithm are maintained, while enabling parallel computation to enhance performance.

The complete pseudo-code SL-CBS+ID algorithm for is shown in Algorithm 2. Lines 5–25 outline the SL-CBS algorithm process, while the remaining lines implement the ID. In line 2, a conflict graph is constructed based on the initial solution from line 1. Line 4 involves grouping the UAV swarm based on the conflict graph. Within lines 5–19, line 6 constructs the root node of the $CT$ tree based on the initial states and target regions, where the $CT$ tree is maintained by the priority queue CBSPriorityQueue. In line 9, a greedy search retrieves the current optimal node from the CBSPriorityQueue. Lines 10–14 assess whether the current node has any conflicts; if none are present, the solution is returned. Otherwise, line 15 extracts the earliest occurring conflict for processing. Subsequently, line 16 partitions the relevant constraints for the current conflict. Lines 17–23 handle these constraints, with line 18 constructing new child nodes that inherit the constraints of the parent node. Line 19 adds new constraints, and line 20 resolves the current conflict through replanning based on the improved State Lattice. Finally, in line 22, the newly created child nodes are added to the CBSPriorityQueue.

Algorithm 2 SL-CBS+ID

Input:K, $\left\{x_{i0}\right\}$, $\left\{X_i^G\right\}$ Output: multiple collision-free trajectories $\left\{T_1,T_2,\dots ,T_K\right\}$ 1: $\left\{T_1,T_2,\dots ,T_K\right\}\leftarrow {\mathbf{SLPlanner}}^{*}(x_{i0},X_i^G)$; 2: $\mathrm{conflicts}\leftarrow findAllConflicts\left(\left\{T_1,T_2,\dots ,T_K\right\}\right)$; 3: while$\mathrm{conflicts}.size\left(\right)>0$do 4:    $\mathrm{groups}\leftarrow divideGroups\left(\mathrm{conflicts}\right)$; 5:    for group$in$groups do 6:      $\mathrm{root}\leftarrow generateRootNode(\mathrm{group},\left\{x_{i0}\right\},\left\{X_i^G\right\})$; 7:      $\mathrm{CBSPriorityQueue}.push\left(\mathrm{root}\right)$; 8:      while CBSPriorityQueue$is$ $not$ $Empty$ do 9:         $\mathrm{currNode}\leftarrow \mathrm{CBSPriorityQueue}.top\left(\right)$; 10:         $\mathrm{CBSPriorityQueue}.pop\left(\right)$; 11:         if $validateSolutions\left(\mathrm{currNode}.\mathrm{sols}\right)$ then 12:           $\left\{T_1,T_2,\dots ,T_K\right\}\leftarrow \mathrm{currNode}.\mathrm{sols}$; 13:           $break$; 14:         end if 15:         $\mathrm{conflict}\leftarrow chooseConflict\left(\mathrm{currNode}\right)$; 16:         $\mathrm{constraints}\leftarrow divideConstraints\left(\mathrm{conflict}\right)$; 17:         for constraint $in$ constraints do 18:           $\mathrm{childNode}\leftarrow \mathrm{currNode}$; 19:           $\mathrm{childNode}.addConstraints\left(\mathrm{constraint}\right)$; 20:           $\mathrm{sol}\leftarrow {\mathbf{SLPlanner}}^{*}(x_{i0},X_i^G,\mathrm{childNode}.\mathrm{cons})$; 21:           $\mathrm{childNode}.update\left(\mathrm{sol}\right)$; 22:           $\mathrm{CBSPriorityQueue}.push\left(\mathrm{childNode}\right)$; 23:         end for 24:      end while 25:    end for 26:    $\mathrm{conflicts}\leftarrow findAllConflicts\left(\left\{T_1,T_2,\dots ,T_K\right\}\right)$; 27: end while 28: return$\left\{T_1,T_2,\dots ,T_K\right\}$;

5. Experiment

Our proposed SL-CBS+ID algorithm runs on a system equipped with an Intel Core i7-8700@3.2GHz CPU and 16GB RAM, utilizing Ubuntu and ROS environments. The complete C++ project code is open-sourced at https://github.com/ucas-zihaowang/SL-CBS.git. The complexity of the environment, along with the initial and target states of the UAV swarm and kino-dynamic models, directly affect the results of swarm motion planning. We strictly establish a unified benchmark and consistent UAV kino-dynamic models to ensure the accuracy and fairness of the experimental results. Ultimately, we conduct three types of experiments: classic scenarios testing, comparison with baseline algorithms, and analysis of the algorithm’s performance boundaries.

5.1. Test on Classical Scenarios

5.1.1. Setup

The Tunnel Configuration and Star Configuration are two classic test scenarios [2]. Figure 5a,b illustrate the Tunnel Configuration, while Figure 5c,d show the Star Configuration, both measuring 10m by 10m. The light green rectangular areas represent static obstacles. We conduct three types of tests in these scenarios: CrossAndSwap, Cross, and Swap. The UAV’s maximum speed $v_{max}$ is 2m/s, with acceleration-based control and a maximum control input $u_{max}$ of 1. The control space sampling number $\mu$ is set to 1, the time interval $\tau$ is 0.5s, and the time weight of the motion primitive cost function $\rho$ is 10. Additionally, the UAV’s geometric model is a circle with a radius $r_c$ of 0.1m.

We select sequential planning and decentralized planning from [2] as baseline algorithms. Sequential planning is a centralized algorithm that plans flight trajectories for each UAV in order of priority, treating the trajectories of previously planned UAVs as dynamic obstacles. In decentralized planning, each UAV plans independently, treating others as dynamic obstacles, and iteratively replanning based on a receding horizon control. Both algorithms maintain consistent UAV models and parameters, using computation time, trajectory flight time, and total trajectory cost as evaluation metrics.

5.1.2. Result

In Figure 5a, two UAVs cross a narrow gap to reach each other’s positions, where only one UAV can pass through at a time. $T_1$ is the flight trajectory of $UA{V}_1$ without using an emergency stop, while ${T_1}^{\prime }$ includes the emergency stop motion primitive, and $T_2$ is the trajectory of $UA{V}_2$. The flight trajectory ${T_1}^{\prime }$ shows a noticeable emergency stop at the red circle, with a significantly reduced detour. According to Table 1, using the emergency stop motion primitive reduces both flight time and total trajectory cost. However, it should be noted that the addition of emergency stop motion primitives increases the search space, leading to longer computation time. Typically, the increase in computation time is not substantial, but our CrossAndSwap-2 test represents an extreme case.

In Figure 5b–d, we conduct tests for Cross-5(5 UAVs crossing a gap), Swap-10(10 UAVs swapping positions), and Swap-12(12 UAVs swapping positions), respectively. According to Table 2, our proposed SL-CBS shows a great reduction in both trajectory flight time and total trajectory cost across all test instances compared to sequential planning and decentralized planning. This is because sequential planning sacrifices trajectory quality to obtain suboptimal solutions, while decentralized planning focuses on local optima for each UAV, thereby losing the global optimal solution. In terms of computation time, sequential planning is highly efficient as it plans the trajectory for each UAV only once, but it is prone to unsolvable situations in complex environments. Decentralized planning, on the other hand, requires iterative computation of each UAV’s flight trajectory from its current state to the target region, resulting in longer computation time. As shown in Table 2, SL-CBS generally takes longer computation time than sequential planning in most test instances but performs better than decentralized planning. Additionally, we observe that in the Swap-12 test, due to the dispersed distribution of conflicts, the use of ID technique greatly reduces computation time by grouping.

5.2. Compared to Baseline Algorithms

5.2.1. Setup

Most benchmarks for MAPF are based on 4/8-connected grid maps, which cannot be directly applied to continuous space motion planning for UAV swarm. To address this, we design a continuous map with dimensions of 5m by 5m, featuring 20 randomly generated circular obstacles with a radius of 0.2m, resulting in an obstacle density of approximately 10%. The initial and target positions of the UAV swarm must satisfy the following requirements: (1) The initial and target positions of the UAVs must not collide with each other or with the obstacle regions; (2) The Euclidean distance between each UAV’s initial and target position is greater than 1/4 of the map width and less than 3/4 of the map width. Here, the maximum speed of the UAVs $v_{max}$ is 0.5 m/s, with acceleration-based control and a maximum control input $u_{max}$ of 2, Additionally, the geometric model of the UAVs is a circle with a radius $r_c$ of 0.15m.

We select the latest algorithms suitable for UAVs, K-CBS [19] and db-CBS [21], which share the same technical approach as SL-CBS, as baseline algorithms. The control interval length is crucial for a fair comparison. K-CBS employs a sampling-based low-level algorithm with control interval lengths varying between $\left[0.1,1\right]$. The db-CBS low-level algorithm is based on db-A*, with pre-computed motion primitives consisting of states and actions at time intervals of 0.1s. Analysis of db-CBS local motion primitives shows a basic time intervals length greater than 0.5s. Therefore, we set the time interval $\tau$ for SL-CBS to 0.5s. Given the high obstacle density and complex environment of our benchmark, the SL-CBS control input sampling number $\mu$ is set to 4.

We conduct batch tests with swarm sizes $K=2,4,6,8,10$, testing each scenario with 10 trials, with a time limit of 300s. Consistent with the db-CBS project [21], we use success rate, average computation time, and average trajectory flight time as evaluation metrics.

5.2.2. Result

Figure 6 illustrates the flight trajectories of SL-CBS and its baseline algorithms for a swarm size of $K=8$. The trajectories planned by K-CBS are relatively rugged and contain numerous bends, primarily because its low-level search relies on a sampling algorithm and lacks back-end optimization. Although the low-level of db-CBS uses db-A* to search discontinuous trajectories, these become smooth and continuous after trajectory optimization. Our SL-CBS employs an improved State Lattice at the low level, which considers complete dynamics, resulting in smooth and continuous trajectories.

Table 3 presents the batch test results for SL-CBS and baseline algorithms across different swarm sizes. Due to the inefficient search of the sampling algorithm and the ineffective conflict interval detection method, K-CBS can only solve most instances for swarm size of 2-6, consistent with the experimental results in [19]. The db-CBS low-level, utilizing pre-computed motion primitives for search, can solve most instances for swarm size of 2-10. However, because it includes an additional layer of trajectory optimization and its conflict detection method in the high level is less effective than that of SL-CBS, its computation time is generally longer than SL-CBS. Only for a swarm size of 10 does SL-CBS take longer time to compute than db-CBS, as our method for calculating average computation time includes all successfully solved instances. SL-CBS, with its optimal two-layer search, substantially outperforms K-CBS and db-CBS in terms of trajectory flight time. Therefore, based on the test results and the above analysis, SL-CBS demonstrates clear improvements over K-CBS and db-CBS in success rate, computation time, and flight time.

5.3. Performance Boundaries

5.3.1. Setup

Similar to CL-CBS [17] used in the car-like robots, we conduct batch testing on SL-CBS to analyze its performance boundaries across different swarm sizes. Here, the map size is 50m by 50m, with 25 randomly generated circular obstacles, each with a radius of 0.5m. The initial and target positions of the swarm are randomly generated, consistent with the requirements in Section 5.2. The maximum speed of the UAVs is 2m/s, controlled by acceleration, with a maximum control input $u_{max}$ of 1. Additionally, we introduce yaw control to achieve full state-space planning for the UAVs, with a maximum yaw of 0.27 radian and a maximum yaw control input of 0.5. We also set the control space sampling number $\mu$ to 1 and the time interval $\tau$ to 1. The geometric model of the UAV is a circle with a radius $r_c$ of 1m.

For each randomly generated scenario, we test it with 30 trials, with the computation time limited to 90s. We conduct batch tests using four algorithm configurations: SL-CBS(Without yaw), SL-CBS+ID(Without yaw), SL-CBS(With yaw), and SL-CBS+ID(With yaw), across swarm size of 5, 10, 15, and 20. The evaluation metrics are success rate, average computation time, average total trajectory flight time, and average makespan, where makespan is defined as the maximum flight time within the UAV swarm.

5.3.2. Result

Figure 7 shows the planned trajectories for SL-CBS with and without yaw for a swarm size of 20. It demonstrates the algorithm’s planning capability to solve large-scale, collision-free trajectories in dense environments. Figure 8 illustrates the relationship and trends between various evaluation metrics and swarm size. According to Figure 8a, when the swarm size is 10 or less, SL-CBS+ID can solve all instances, while SL-CBS solves over 90% of them. As the swarm size exceeds 10, the performance of SL-CBS declines, which aligns with the trend in Figure 8b where computation time increases linearly with swarm size. When the swarm size is 20, SL-CBS+ID(Without yaw) still maintains a success rate of over 50%. From Figure 8b, SL-CBS can solve problems within 5s for swarm sizes of 10 or less, and requires 10 seconds for a swarm size of 15. It is noteworthy that for SL-CBS+ID(With yaw), the computation time decreases at a swarm size of 20 due to its lower success rate, which includes only a limited sample of successful instances.

Figure 8c,d reveal that the total trajectory flight time and makespan cost curves for SL-CBS and SL-CBS+ID nearly overlap, further indicating that our ID technique does not affect actual trajectory results. Combined with Figure 8a,b, SL-CBS+ID consistently outperforms SL-CBS. Particularly for larger swarm size, when the swarm size is 20, the success rate improves by approximately 13.4%, and computation time is reduced by about 8.3s.

6. Conclusions

We extend Conflict-Based Search from the MAPF domain to UAV swarm motion planning, formally defining the swarm motion planning problem and proposing the optimal and efficient SL-CBS+ID algorithm.

SL-CBS+ID eliminates the need for motion constraints and physical assumptions, making it fully suitable for autonomous UAV swarm flight. It features a two-layer search algorithm: the low-level uses an improved State Lattice algorithm tailored for UAVs, incorporating emergency stop primitives and effectively handling spatio-temporal constraints. The high level considers complete state and motion primitive conflicts, introducing a linear time complexity motion conflict detection method based on Sturm’s theory. Additionally, our modified ID is applied to enable parallel conflict processing.

We conduct tests on classic scenarios like swap and cross to validate SL-CBS+ID’s planning capabilities. Compared to SOTA algorithms, our SL-CBS demonstrates great advantages in success rate, computation time, and trajectory flight time. Finally, we perform batch testing of SL-CBS+ID to analyze its performance boundaries.

In the future, we will explore the application of CBS enhancement techniques, such as conflict classification and high-level heuristics, to SL-CBS, aiming to improve its solving capabilities without compromising its optimality and completeness.