A High-Quality and Efficient Trajectory Replanning Method for Quadrotor Swarms Based on Rolling-Horizon Collision Resolution

Highlights

What are the main findings?

• A swarm trajectory replanning method, termed RHCR-Opt, is proposed by integrating rolling-horizon conflict resolution with MINCO-based trajectory optimization.
• On the proposed benchmark, RHCR-Opt achieves significantly higher computational efficiency than state-of-the-art methods while maintaining high-quality trajectories.

What are the implication of the main findings?

• Among CBS-based swarm trajectory replanning approaches, RHCR-Opt is the first method that improves computational efficiency without sacrificing path or trajectory quality at any stage of the replanning process.
• By introducing a time-windowed swarm trajectory joint optimization framework and a two-stage temporal coordination optimization method, RHCR-Opt can flexibly accommodate diverse spatiotemporal constraints.

Abstract

We propose a high-quality and computationally efficient trajectory replanning method for Unmanned Aerial Vehicle (UAV) swarms, termed RHCR-Opt, which is designed to continuously and efficiently generate multiple collision-free trajectories in dense obstacle environments. RHCR-Opt consists of three layers. The first two layers are the rolling-horizon collision resolution (RHCR) algorithm based on the Conflict-Based Search (CBS), while the third layer focuses on trajectory generation and optimization using Minimum Control (MINCO) trajectories. Within the two-layer RHCR framework, the improved Lifelong Planning A* (LPA*) algorithm incorporating spatiotemporal constraints is proposed and employed as the low-level solver of CBS to satisfy the frequent search requirements for feasible paths under varying spatiotemporal constraints, thereby significantly improving computational efficiency. Furthermore, the rolling-horizon collision resolution concept is adopted in the high-level CBS framework, where only the discovery of collision-free paths within a finite time window is considered. This substantially reduces the computational burden associated with trajectory generation and optimization beyond the time window. At the third layer, a MINCO-based trajectory generation scheme is designed, and a swarm trajectory joint optimization framework with a finite time window is proposed to generate dynamically feasible and collision-free trajectories. In addition, for swarm missions requiring simultaneous arrival, a two-stage temporal coordination optimization method is developed. Extensive simulation experiments demonstrate that, compared with state-of-the-art (SOTA) algorithms on the proposed benchmark, RHCR-Opt achieves significant improvements in both trajectory quality and computational efficiency. In particular, when the swarm size becomes large, the computational efficiency is improved by at least 23.7%.

1. Introduction

In autonomous Unmanned Aerial Vehicle (UAV) swarm navigation, swarm motion planning constitutes a core component of the overall navigation framework, aiming to generate multiple collision-free flight trajectories. However, conventional UAV swarm motion planning is typically formulated as a one-shot task [1]. In practical applications such as pesticide spraying [2], search and rescue operations [3], and surveillance and inspection missions [4], UAV swarms are often required to adapt their plans in real time and navigate toward dynamically changing target regions. Consequently, swarm trajectory replanning is more frequently required in real-world operational scenarios and has attracted increasing attention due to its greater practical significance and research value.

According to the communication architecture among swarm members, swarm motion planning methods can be broadly categorized into centralized and decentralized approaches. Centralized approaches generate trajectories for all UAVs within a joint state space, and their computational efficiency is severely constrained by the swarm size [5,6]. In contrast, decentralized approaches, including Velocity Obstacle (VO)-based methods [7,8,9], Distributed Model Predictive Control (DMPC) [10,11,12], and decentralized trajectory optimization methods [13,14,15,16,17,18], distribute computational and communication burdens through an architecture in which each UAV independently performs perception, estimation, planning, and control. As a result, these approaches exhibit excellent flexibility and robustness. However, due to the absence of centralized coordination among UAVs, decentralized approaches are prone to trajectory quality degradation or planning failure when UAVs become densely clustered [19]. Furthermore, deadlock situations may arise in certain symmetric scenarios [20].

Multi-Agent Path Finding (MAPF) is the discrete counterpart of swarm motion planning [21]. The Conflict-Based Search (CBS) algorithm [22] has demonstrated the capability to solve large-scale conflict-free path planning problems by combining decentralized path planning with centralized conflict resolution, thereby providing an effective solution to the MAPF problem. Inspired by the success of CBS, recent swarm motion planning approaches typically employ a CBS-based framework in which conflict-free optimal paths are generated in the front-end, followed by the generation of conflict-free trajectories through decentralized trajectory optimization in the back-end. Moreover, because collision avoidance constraints are effectively addressed by CBS during path planning, the computational burden of the subsequent trajectory optimization process is substantially reduced. In addition, the centralized conflict resolution mechanism in the front-end effectively prevents deadlock situations [23]. However, in existing CBS-based trajectory optimization methods [24,25,26,27,28], the front-end generally relies on suboptimal variants of CBS. Although these variants improve computational efficiency to some extent, they do so at the expense of the quality of the resulting conflict-free paths. Furthermore, the back-end trajectory optimization process still incurs a considerable computational burden when handling complex spatiotemporal constraints.

In summary, centralized approaches suffer from low computational efficiency and limited scalability with respect to swarm size, although they are capable of generating globally optimal trajectories. In contrast, decentralized approaches offer high computational efficiency but are susceptible to trajectory quality degradation and deadlock situations. By combining decentralized planning with centralized conflict resolution, CBS-based trajectory optimization methods effectively address the long-standing challenge of balancing trajectory quality and computational efficiency encountered by both centralized and decentralized approaches. Furthermore, existing CBS-based trajectory optimization methods either adopt techniques that sacrifice path or trajectory quality to improve computational efficiency during path generation and trajectory optimization, or their trajectory optimization methods remain somewhat deficient in flexibility and efficiency. In contrast, our proposed swarm trajectory replanning method builds upon CBS-based trajectory optimization, adopting techniques that improve computational efficiency without sacrificing path or trajectory quality, and employing a flexible and efficient trajectory optimization method, thereby addressing the difficulty in existing work of balancing trajectory quality and computational efficiency. Here, a high-quality flight trajectory refers to one that is optimal with respect to objectives such as flight time, travel distance, or energy consumption.

In response, we propose a UAV swarm trajectory replanning method based on rolling-horizon conflict resolution that balances high trajectory quality with high computational efficiency, namely RHCR-Opt. RHCR-Opt consists of three layers. The first two layers constitute the rolling-horizon collision resolution (RHCR) algorithm based on CBS, while the third layer is dedicated to trajectory generation and optimization using Minimum Control (MINCO) trajectories. Within the RHCR framework, the incremental search algorithm Lifelong Planning A* (LPA*) [29,30] is adopted as the low-level solver of CBS and further enhanced to accommodate the frequent path-search requirements arising from varying spatiotemporal constraints. Subsequently, a rolling-horizon collision resolution concept [1] is incorporated into the high-level CBS framework, where only conflicts within a finite time window are considered and conflict-free paths are searched within the corresponding time window. Consequently, neither path conflicts beyond the time window nor subsequent trajectory generation and optimization processes need to be considered during swarm motion planning. At the third layer, a MINCO-based trajectory generation scheme is developed, including waypoint selection and local target point selection strategies. Furthermore, a swarm trajectory joint optimization framework with a finite time window is proposed. This framework formulates the problem as an unconstrained optimization task, in which dynamic constraints, obstacle avoidance constraints, and collision avoidance constraints are incorporated as penalty costs and optimized together with smoothness and time costs. In particular, penalty cost formulations specifically designed for finite time windows are also developed. Moreover, simultaneous arrival is a common requirement in swarm missions. To address this requirement, a two-stage temporal coordination optimization method is proposed. In the first stage, a task allocation method based on temporal load balancing is developed. The problem is formulated as a Mixed-Integer Quadratic Programming (MIQP) model with the minimization of arrival-time variance as the optimization objective, and a dedicated solver is designed accordingly. In the second stage, a soft-constrained temporal optimization method is proposed, in which the simultaneous-arrival requirement is formulated as a time cost term and incorporated into the objective function of the swarm trajectory joint optimization framework. To the best of our knowledge, RHCR-Opt is the first swarm trajectory replanning method built upon the CBS paradigm that simultaneously achieves high-quality trajectory and high computational efficiency throughout all stages of the planning framework.

The main contributions of this work are summarized as follows:

(1)

A swarm trajectory replanning method, termed RHCR-Opt, is proposed based on the CBS paradigm to simultaneously achieve high-quality trajectory and high computational efficiency. The method innovatively integrates the CBS-based RHCR algorithm with a MINCO-based trajectory generation and optimization framework.

(2)

An LPA* algorithm incorporating spatiotemporal constraints is developed and employed as the low-level solver of CBS. By leveraging its incremental search capability, the computational efficiency of the CBS low-level search is significantly improved when feasible paths must be repeatedly generated under varying spatiotemporal constraints.

(3)

A rolling-horizon collision resolution concept is incorporated into the high-level CBS framework. The proposed approach focuses exclusively on conflicts and corresponding feasible paths within a rolling time window, thereby eliminating the need to consider path generation and trajectory optimization beyond the time window.

(4)

A MINCO-based trajectory generation scheme and a swarm trajectory joint optimization framework with a finite time window are developed to generate smooth, dynamically feasible, and collision-free trajectories.

(5)

Extensive comparisons with the current state-of-the-art (SOTA) algorithms on the proposed benchmark demonstrate that RHCR-Opt achieves substantial improvements in both trajectory quality and computational efficiency. Furthermore, all source code associated with this work has been publicly released.

2. Related Work

In this section, the technical routes and corresponding algorithms for swarm motion planning and trajectory replanning are presented in detail, and the advantages as well as limitations of existing algorithms are analyzed. Furthermore, since the MAPF problem and its classical algorithm, CBS, constitute the foundation of the technical framework adopted in this work, they are introduced separately.

2.1. Swarm Motion Planning and Trajectory Replanning

According to the computational architecture and inter-UAV communication paradigm, UAV swarm motion planning methods can generally be categorized into centralized and decentralized approaches, both of which have been extensively investigated in the literature. Centralized approaches, such as those presented in References [5,6], formulate swarm planning in continuous space as an MIQP or Sequential Convex Programming (SCP) problem, in which the decision variables corresponding to all parameterized trajectories of the swarm are optimized simultaneously. The complexity of the resulting optimization problem increases exponentially with swarm size, rendering such approaches applicable only to small-scale swarms. Moreover, the substantial computational burden makes them difficult to deploy in trajectory replanning scenarios. To improve the efficiency of centralized planning, the methods proposed in References [31,32] assign priorities to UAVs and require lower-priority UAVs to avoid the flight trajectories of higher-priority UAVs. Although these approaches perform well in relatively simple environments, they are prone to planning failures in congested scenarios. Furthermore, how to assign priorities appropriately without sacrificing trajectory quality remains an open research challenge.

Decentralized approaches are better suited for large-scale UAV swarms because they distribute computational workloads and communication bandwidth requirements among individual UAVs. Moreover, they endow swarm systems with excellent flexibility, including adaptability, scalability, and maintainability, as well as strong robustness in terms of reliability, survivability, and fault tolerance. Reactive controllers based on reciprocal collision avoidance using the VO paradigm [7,8,9] represent a classical class of decentralized planning methods. These methods can rapidly generate collision-free control commands for the next time step. However, because they do not directly generate smooth high-order continuous trajectories, their applicability to mission scenarios with stringent operational constraints is limited. Furthermore, they cannot explicitly optimize mission objectives such as total flight time, travel distance, or energy consumption. DMPC has also been widely applied to UAV swarm motion planning. Nevertheless, its favorable trajectory quality is achieved at the expense of substantial computational effort and strict time synchronization among UAVs [10,11,12].

Among decentralized trajectory optimization methods, the method proposed in Reference [13] projects both dynamic obstacles and the preplanned trajectories of neighboring UAVs onto Spatiotemporal Occupancy Grid Maps (SOGMs), and subsequently generates collision-free trajectories through a Bézier trajectory optimization framework constrained by safe flight corridors. However, the trajectory prediction process and continuous map maintenance introduce a considerable burden on real-time computation. Ego-Swarm [14] is a recently proposed fully decentralized, asynchronous, and online swarm planner. In its front-end, initial trajectories are generated using Ego-Planner [33], which operates without a Euclidean Signed Distance Field (ESDF) map, while in the back-end, B-spline parameterized trajectories are optimized to achieve swarm obstacle avoidance and collision avoidance. However, the inherent properties of B-spline curves make temporal adjustment and optimization difficult. To address this limitation, the subsequent Ego-Swarm-v2 [15] incorporates MINCO trajectories [34] to enable swarm trajectory joint optimization. Owing to their powerful spatiotemporal deformation capability and computational efficiency, MINCO trajectories substantially enhance swarm scalability [16]. Nevertheless, the methods presented in References [14,15,16] tend to suffer from degraded trajectory quality and reduced success rates when UAVs become densely clustered in confined environments. MADER [17] and its communication-delay-aware extension RMADER [18] constitute another class of fully decentralized and asynchronous swarm planners. These methods parameterize trajectories using MINVO curves [35], which possess a minimum-volume property, and achieve collision avoidance in dense dynamic environments through a detect-and-repair mechanism. The methods proposed in References [14,15,16,17,18] feature concise and clear swarm planning architectures, all relying on decentralized swarm trajectory optimization frameworks to handle smoothness costs, time costs, dynamic feasibility constraints, and collision/obstacle avoidance constraints. However, this design also leads to a substantial computational burden associated with trajectory optimization. In addition, due to the lack of explicit coordination mechanisms among swarm members, the aforementioned decentralized approaches are highly susceptible to deadlock situations in symmetric scenarios.

Another line of research in swarm planning augments fully decentralized trajectory optimization frameworks with a centralized coordination mechanism for conflict resolution. MAPF is the discrete counterpart of swarm motion planning. Its classical algorithm, CBS, is an optimal and complete two-level search framework that effectively addresses large-scale conflict-free path planning problems in discrete spatiotemporal environments through the combination of decentralized path planning and centralized conflict resolution. Inspired by CBS, numerous studies [24,25,26,27,28] have extended the CBS paradigm to robotic platforms operating in continuous spaces. PSL [24] combines Priority-Based Search (PBS) with mobile robots moving at constant velocities and subsequently optimizes robot trajectories through Linear Programming (LP). PSB [25] relaxes the constant-velocity assumption adopted in PSL and optimizes dynamically feasible trajectories using Bézier curves. Crazyswarm [26] first employs Enhanced Conflict-Based Search (ECBS) to generate conflict-free paths on grid maps or probabilistic roadmaps and subsequently optimizes flight trajectories using Bézier curves within preconstructed safe flight corridors. RBP [27] extends the Crazyswarm framework by employing relative Bernstein polynomial curves to transform nonconvex collision-avoidance constraints into convex constraints, thereby enabling more efficient trajectory optimization. EDG-TEAM [28] further reduces the search space of ECBS through a tie-breaking strategy and replaces the trajectory optimization module with a MINCO-based framework that is better suited for spatiotemporal optimization. In the aforementioned studies [24,25,26,27,28], the front-end planners, namely PBS and ECBS, are all suboptimal variants of CBS that improve computational efficiency at the cost of a certain degree of path quality degradation. As shown in Table 1, RHCR-Opt follows the same overall technical framework as the existing methods by first generating collision-free discrete paths through front-end discrete path planning and then producing collision-free trajectories via back-end continuous trajectory optimization. Unlike previous works, however, RHCR-Opt employs optimal-search CBS in the front-end and the flexible and efficient MINCO trajectory representation in the back-end, while further enhancing computational efficiency from multiple aspects.

Table 1. Comparison between RHCR-Opt and planners following the same technical framework.

Planner	Front-End Discrete Path Planning	Back-End Continuous Trajectory Optimization
PSL [24]	PBS (Suboptimal)	Linear Programming (Low efficiency)
PSB [25]	PBS (Suboptimal)	Based on Bézier curve (Low efficiency)
Crazyswarm [26]	ECBS (Suboptimal)	Based on Bézier curve (Low efficiency)
RBP [27]	ECBS (Suboptimal)	Based on relative Bernstein polynomial curves (Low efficiency)
EDG-TEAM [28]	ECBS (Suboptimal)	Based on MINCO curves (High efficiency)
RHCR-Opt	RHCR (CBS) (Optimal)	Based on MINCO curves with time window (High efficiency)

UAV swarm trajectory replanning inevitably imposes more stringent requirements on the computational efficiency of swarm motion planning algorithms. Existing swarm replanning strategies can generally be categorized into event-triggered, distance-triggered, and time-triggered approaches [36]. Among these, time-triggered replanning has become the dominant paradigm. For example, DMPC [12] solves the trajectory optimization problem corresponding to an optimal control formulation within a fixed time horizon and predicts all swarm actions to be executed during the subsequent planning horizon. Similarly, decentralized trajectory optimization frameworks such as Ego-Swarm-v2 [15] and EDG-TEAM [28] utilize an initially planned global trajectory to guide local target selection and subsequently perform trajectory replanning within a rolling time horizon. However, due to the stringent computational efficiency requirements of real-time replanning, these methods typically incorporate a variety of techniques that improve computational performance at the expense of trajectory quality.

2.2. MAPF and CBS

2.2.1. MAPF

A MAPF problem involving K agents, denoted as $\left\{a_1,a_2,\dots ,a_K\right\}$, can be represented by the tuple $⟨G,s,g⟩$ [21]. Here, $G(V,E)$ denotes an undirected graph, where $s:[1,2,\dots ,K]\to V$ defines the mapping from agents to their start vertices, and $g:[1,2,\dots ,K]\to V$ defines the mapping from agents to their goal vertices. V denotes the set of vertices, with $v\in V$, and E denotes the set of edges, where $(v,v^{\prime })\in E$ represents an edge connecting vertex v and vertex $v^{\prime }$. Time is discretized into $t=0,1,2,\dots$. At each time step, an agent is allowed to execute only one action, denoted by $\pi :V\to V$, where $\pi \left(v\right)=v^{\prime }$. Two types of actions are permitted: wait and move. A wait action corresponds to the agent remaining at its current vertex for one time step, i.e., $\pi \left(v\right)=v$, whereas a move action corresponds to the agent moving to an adjacent vertex $v^{\prime }$, i.e., $\pi \left(v\right)=v^{\prime }$. Both actions incur a unit cost of 1. After reaching its goal vertex, an agent typically follows one of two behaviors: it either remains at the goal vertex until all agents have reached their respective goals or disappears upon arrival at the goal vertex. The latter convention is commonly adopted to prevent conflicts between the occupied goal vertex and the paths planned for other agents.

A path is defined as a sequence of wait and move actions that enables an agent to travel from its start vertex to its goal vertex, i.e., $P=\left({\pi }_1,{\pi }_2,\dots ,\right)$. The solution to a MAPF problem consists of a set of K paths, denoted by $\left\{P_1,P_2,\dots ,P_K\right\}$, such that all paths are conflict-free. Two common types of conflicts are vertex conflicts and edge conflicts. A vertex conflict, denoted by $⟨a_i,a_j,v,t⟩$, occurs when agents $a_i$ and $a_j$ occupy or attempt to move to the same vertex v at time step t. An edge conflict, denoted by $⟨a_i,a_j,v_i,v_j,t⟩$, occurs when agent $a_i$ traverses edge $(v_i,v_j)$ while agent $a_j$ simultaneously traverses edge $(v_j,v_i)$ at time step t, resulting in a head-on collision.

The MAPF problem seeks to generate K conflict-free paths, $\left\{P_1,P_2,\dots ,P_K\right\}$, by minimizing either the Makespan or the SumOfCosts objective. The Makespan objective represents the total time required for all agents to reach their goal vertices, which is equivalent to the travel time of the agent with the longest path. Formally,

$$\mathrm{Makespan}=\underset{1\le i\le K}{\max }∥P_i∥$$

(1)

where $∥P_i∥$ denotes the length of path $P_i$, i.e., the length of the corresponding action sequence. The SumOfCosts objective represents the total travel time accumulated by all agents before reaching their goal vertices and is defined as:

$$\mathrm{SumOfCosts}={\displaystyle \sum _{i=1}^K}∥P_i∥$$

(2)

2.2.2. CBS

MAPF is a classical NP-hard problem that has attracted extensive research attention due to its significant theoretical and practical value. In recent years, a variety of solvers have been proposed [37,38], including A*-based joint-search methods, reduction-based methods, priority-planning methods, and CBS [22]. Among these approaches, CBS [22] is a state-of-the-art optimal and complete two-level search algorithm that has achieved remarkable success in solving large-scale conflict-free path planning problems.

At the low level, CBS independently computes an optimal path for each agent under a given set of constraints in a decentralized manner, typically using algorithms such as A* or Safe Interval Path Planning (SIPP) [39]. At the high level, CBS maintains a binary constraint tree (CT). Each node $N\in CT$ contains three components: (1) the set of constraints imposed on all agents, denoted by $N.\mathrm{Constraints}$; (2) a feasible solution satisfying all imposed constraints, denoted by $N.\mathrm{Solutions}$; and (3) the cost of the current feasible solution, denoted by $N.\mathrm{Cost}$.

CBS begins by constructing a root node containing an empty constraint set, the corresponding unconstrained solution, and its associated cost. Subsequently, a best node is selected from the constraint tree $CT$ according to a best-first search strategy for expansion. After conflict detection is performed on the selected node, the earliest conflict in time is chosen for resolution. Conflicts are converted into corresponding constraints. For example, a vertex conflict $⟨a_i,a_j,v,t⟩$ can be divided into two vertex constraints, $⟨a_i,v,t⟩$ and $⟨a_j,v,t⟩$. The vertex constraint $⟨a_i,v,t⟩$ specifies that agent $a_i$ is prohibited from occupying vertex v at time step t, and the interpretation of $⟨a_j,v,t⟩$ is analogous. Similarly, an edge conflict $⟨a_i,a_j,v_i,v_j,t⟩$ can be decomposed into two edge constraints, $⟨a_i,v_i,v_j,t⟩$ and $⟨a_j,v_j,v_i,t⟩$. The edge constraint $⟨a_i,v_i,v_j,t⟩$ prohibits agent $a_i$ from traversing the edge from vertex $v_i$ to vertex $v_j$ at time step t, while $⟨a_j,v_j,v_i,t⟩$ is defined analogously. Next, child nodes are generated by inheriting all information from the currently expanded node, including its constraint set and solution. After a newly generated constraint is added, the low-level solver is invoked to compute a feasible solution satisfying the updated constraint set. The resulting child nodes are then inserted into the constraint tree. This process is repeated iteratively until the currently expanded node is free of conflicts, at which point CBS terminates and returns an optimal conflict-free path solution.

3. Problem Description

Consider a homogeneous quadrotor swarm consisting of K UAVs, denoted by $\left\{{\mathrm{UAV}}_1,{\mathrm{UAV}}_2,\dots ,{\mathrm{UAV}}_K\right\}$, operating within a workspace W, where $W\subset {\mathbb{R}}^m$ and $m=2$ or 3. Let the static obstacle space be denoted by $P^{\mathrm{obs}}$, which consists of a set of obstacles $p_{\mathrm{obs}}$, i.e., $P^{\mathrm{obs}}=\left\{p_{\mathrm{obs}}\right\}$. The corresponding obstacle-free space is defined as $P^{\mathrm{free}}=W\setminus P^{\mathrm{obs}}$.

The state of ${\mathrm{UAV}}_i$ at time t is represented as $x_i\left(t\right)={\left[p_i^{\mathrm{T}}\left(t\right),v_i^{\mathrm{T}}\left(t\right),\cdots ,{p_i^{(s-1)}}^{\mathrm{T}}\left(t\right)\right]}^{\mathrm{T}}$, which includes the position $p_i\left(t\right)$, velocity $v_i\left(t\right)$, and derivatives up to order $\left(s-1\right)$. The state $x_i\left(t\right)$ satisfies $x_i\left(t\right)\in {\mathbb{R}}^{ms}$, where $1\le i\le K$. Here, s denotes the order of the integrator chain in the UAV dynamic model (3). The control input $u_i\left(t\right)$ is defined as the s-th derivative of the position $p_i\left(t\right)$, namely, $u_i\left(t\right)=p_i^{\left(s\right)}\left(t\right)$. The quadrotor swarm is assumed to be homogeneous, and the dynamics of each UAV are described by the following linear system:

$$\dot{x}=\mathbf{A}x+\mathbf{B}u$$

(3)

For simplicity, the UAV index i and time index t are omitted here. Detailed definitions of the system matrices $\mathbf{A}$ and $\mathbf{B}$ can be found in Reference [30].

In addition, each UAV is subject to kinematic constraints, including a maximum velocity $v_{\max }$ and a maximum acceleration $a_{\max }$. Accordingly, the safe state space of ${\mathrm{UAV}}_i$, denoted by ${\chi }^{\mathrm{free}}$, is defined as:

$${\chi }^{\mathrm{free}}=P^{\mathrm{free}}\times {[-v_{\max },v_{\max }]}^m\times {[-a_{\max },a_{\max }]}^m\times \cdots$$

(4)

The geometric models adopted for obstacle avoidance and inter-UAV collision avoidance are illustrated in Figure 1. For obstacle avoidance, each UAV is modeled as a sphere with geometric radius $r_c$. For collision avoidance among UAVs, the downwash effect is taken into consideration, and each UAV is modeled as an ellipsoid with semi-major axis $r_l$ and semi-minor axis $r_s$, where $r_s=r_c$ and $r_l>r_s$.

Figure 1. Geometric models for obstacle avoidance and inter-UAV collision avoidance.

Accordingly, the swarm trajectory replanning problem for K UAVs can be formulated as follows. Given the initial state $x_{i0}$ of ${\mathrm{UAV}}_i$ and its target region $X_i^G$, where the target region may vary over time and is typically generated according to mission requirements or selected as a local target based on the replanning time $t_r$, the objective is to generate a trajectory ${\mathsf{\Phi }}_i\left(t\right)$ for each UAV while satisfying the dynamic model (3) and the kinematic constraints (4). The planned trajectory ${\mathsf{\Phi }}_i\left(t\right)$ must satisfy the boundary conditions ${\mathsf{\Phi }}_i\left(t_{i0}\right)=x_{i0}$, ${\mathsf{\Phi }}_i\left(t_{if}\right)\in X_i^G$, where $t_{i0}$ and $t_{if}$ denote the initial time and arrival time of ${\mathrm{UAV}}_i$, respectively. Consequently, the set of K conflict-free trajectories obtained through swarm trajectory replanning, $\left\{{\mathsf{\Phi }}_1\left(t\right),{\mathsf{\Phi }}_2\left(t\right),\dots ,{\mathsf{\Phi }}_K\left(t\right)\right\}$, must satisfy the following requirements:

$$S\left({\mathsf{\Phi }}_i\left(t\right)\right)\cap P^{\mathrm{obs}}=⌀,\forall 1\le i\le K$$

(5)

$$S\left({\mathsf{\Phi }}_i\left(t\right)\right)\cap S\left({\mathsf{\Phi }}_j\left(t\right)\right)=⌀,\forall 1\le i<j\le K$$

(6)

where the UAV trajectory ${\mathsf{\Phi }}_i\left(t\right)$ corresponds to its position trajectory $p_i\left(t\right)$, and $S(·)$ denotes the geometric space occupied by the UAV at a given time instant, as defined by the obstacle-avoidance and inter-UAV collision avoidance models illustrated in Figure 1. Equation (5) requires that each UAV remain collision-free with respect to obstacles at all times, whereas Equation (6) enforces collision avoidance between any pair of UAVs throughout the entire flight process.

4. RHCR-Opt Algorithm

The proposed UAV swarm system architecture is illustrated in Figure 2. The entire swarm operates under a decentralized architecture, where information is exchanged through a wireless broadcast network. All UAVs are homogeneous and are equipped with independent perception, localization, planning, and control modules. Each UAV acquires environmental map information and self-localization information through an open-source Visual-Inertial Odometry (VIO) perception module and broadcasts this information to the other UAVs via the wireless communication network. Subsequently, one UAV is designated to perform the swarm trajectory replanning task. In practice, the designated UAV may be selected either in a round-robin manner or randomly. After receiving the mission information of the entire swarm, the designated UAV employs the proposed RHCR-Opt algorithm to compute collision-free trajectories for all UAVs. The resulting trajectories are then transmitted to the rest of the swarm through the wireless broadcast network. Finally, each UAV receives its corresponding trajectory and executes the assigned flight mission through its onboard control module. It should be particularly noted that the information transmission throughout the UAV swarm system is assumed to be reliable and in real time, and that failures of designated UAVs are not considered.

Figure 2. System framework diagram of the UAV swarm.

The proposed RHCR-Opt framework consists of three layers. The first two layers constitute the CBS-based RHCR algorithm, while the third layer is dedicated to MINCO-based trajectory generation and optimization. Specifically, the RHCR algorithm modifies the original CBS framework by replacing the low-level planner with an improved LPA* algorithm and incorporating a rolling-horizon collision resolution concept into the high-level search process. As illustrated in Figure 3, Figure 3a presents a set of discrete collision-free paths generated by RHCR on a grid map. However, these discrete paths cannot be executed directly by UAVs. Figure 3b shows the smooth and continuous flight trajectories generated based on MINCO. Nevertheless, it can be observed that some trajectories pass excessively close to obstacles, while others exhibit unnecessary distortions. To address these issues, a MINCO-based swarm trajectory joint optimization framework is further developed, as shown in Figure 3c. This framework produces more intuitive and well-structured trajectories while explicitly incorporating UAV dynamic models and kinematic constraints, thereby ensuring that the generated trajectories are fully executable by the UAVs.

Figure 3. RHCR-Opt algorithm schematic diagram. The gray irregular regions represent randomly generated static obstacles, the dark green solid circles denote the target regions of the UAV swarm, and the light green polylines correspond to the generated initial paths. The colored curves, such as the blue and purple trajectories, represent the trajectories generated or subsequently optimized using the MINCO framework.

4.1. LPA* Considering Spatiotemporal Constraints

In the classical CBS framework, the low-level planner employs A* or SIPP to independently generate discrete paths for individual agents, while the high-level planner detects conflicts among all paths and resolves them by introducing corresponding spatiotemporal constraints. The constrained paths are then replanned at the low level. This process is repeated iteratively until a conflict-free solution is identified by the high-level search. During this iterative procedure, the low-level planner must repeatedly compute feasible paths under varying sets of spatiotemporal constraints. To this end, the low-level planning problem is formally defined as follows. Given an undirected graph $G(V,E)$, where V denotes the set of vertices with $v\in V$, and E denotes the set of edges such that $(v,v^{\prime })\in E$ indicates that a transition from vertex v to vertex $v^{\prime }$ is permitted. Consider a set of K agents, where agent $a_i$ has a start vertex $s_i$ and a goal vertex $g_i$, with $s_i,g_i\in V$ and $1\le i\le K$. Let ${\mathrm{constraints}}_i$ denote the set of spatiotemporal constraints associated with agent $a_i$. The objective is to compute a feasible path that satisfies ${\mathrm{constraints}}_i$. In general, ${\mathrm{constraints}}_i$ consists of a collection of vertex constraints and edge constraints, and can be represented as ${\mathrm{constraints}}_i=\left\{\dots ,⟨a_i,v,t⟩,⟨a_i,v_i,v_j,t⟩,\dots \right\}$ where $⟨a_i,v,t⟩$ denotes a vertex constraint and $⟨a_i,v_i,v_j,t⟩$ denotes an edge constraint.

In the A* algorithm, constraint satisfaction is typically enforced by performing validity checks on each neighboring node and the corresponding edge connecting the current node to that neighbor during node expansion. In this manner, constraint violations can be detected and excluded from the search process. Indeed, the low-level planner employed in the classical CBS framework [22] adopts precisely this strategy. However, it is observed that the A*-based low-level search repeatedly expands nodes in graph G during successive CBS iterations, resulting in substantial redundant computation and consequently reduced search efficiency. In contrast, Lifelong Planning A* (LPA*) [29,30] is an incremental search algorithm designed for dynamic environments with fixed start and goal nodes. By reusing information obtained from previous search results, LPA* avoids performing a complete replanning process from scratch. Instead, only the costs of the affected nodes and edges in graph G need to be updated when environmental changes occur, thereby enabling efficient replanning through incremental search.

For each search node $n_c$ in graph G, LPA* maintains three cost values. The value $g\left(n_c\right)$ represents the actual path cost from the start node $s_i$ to the current node $n_c$. The value $rhs\left(n_c\right)$ denotes the one-step lookahead cost of reaching the current node $n_c$ from the start node $s_i$ through one of its predecessor nodes $n_c^{\prime }$, and is defined as:

$$rhs\left(n_c\right)=\left\{\begin{array}{cc}0& if{n}_c=s_i\\ {\min }_{n_c^{\prime }\in \mathrm{pred}\left(n_c\right)}\left(g\left(n_c^{\prime }\right)+\cos \mathrm{t}\left(n_c^{\prime },n_c\right)\right)& otherwise\end{array}\right.$$

(7)

where $\cos \mathrm{t}(n_c^{\prime },n_c)$ denotes the actual traversal cost from the predecessor node $n_c^{\prime }$ to the current node $n_c$, and $\mathrm{pred}\left(n_c\right)$ represents the set of all predecessor nodes of $n_c$. The heuristic function $h\left(n_c\right)$ estimates the cost from the current node $n_c$ to the goal node $g_i$. To guarantee the optimality of the search result, $h\left(n_c\right)$ is generally required to be admissible, i.e., it must not overestimate the true cost-to-go. When the environment changes, a node $n_c$ may become either locally underconsistent, i.e., $rhs\left(n_c\right)>g\left(n_c\right)$, or locally interventionist, i.e., $rhs\left(n_c\right)<g\left(n_c\right)$. These two situations correspond to the appearance of an obstacle at the node or the removal of an obstacle previously occupying the node, respectively. After the costs of all locally inconsistent nodes have been updated and local consistency is restored, namely $rhs\left(n_c\right)=g\left(n_c\right)$, the optimal path can be recovered through backtracking. Figure 4 illustrates the search process of the LPA* algorithm. Figure 4a shows the search result obtained during the first iteration, whereas Figure 4b presents the result of the second iteration after the introduction of additional obstacles. It can be clearly observed that, in Figure 4b, only the locally inconsistent nodes (represented by gray solid circles) are expanded and updated. Consequently, the search efficiency is significantly improved compared with that of the first iteration.

Figure 4. Expanded nodes and path result of the LPA* algorithm during path search. The black solid dots represent static obstacles, the gray solid dots denote expanded nodes, and the light blue polylines indicate the backtracked path.

To incorporate LPA* into the low-level search of CBS, each vertex constraint or edge constraint contained in ${\mathrm{constraints}}_i$ is first transformed into a corresponding dynamic vertex obstacle or dynamic edge obstacle. The affected nodes and edges in graph G are then updated according to these dynamic obstacles. Furthermore, the original workflow of LPA* is modified such that the relevant data structures generated during the initial LPA* search are preserved and subsequently restored in later iterations. In addition, the same tie-breaking strategy adopted in Reference [28] is incorporated into the proposed method. A more detailed description is provided below in conjunction with the pseudocode of Algorithm 1. Lines 1–7 initialize the data structures OpenList and CloseList used during the LPA* search process. Specifically, Lines 2–4 perform the initialization for the first iteration under the unconstrained setting, whereas Line 6 restores the data structures OpenList and CloseList for the second and subsequent iterations when constraints are present. Here, LastOpenList and LastCloseList store the corresponding data structures obtained from the initial search. It should be noted that, in the implementation, only the modified portions of OpenList and CloseList are restored, rather than performing a complete deep copy of all contents from LastOpenList and LastCloseList. Lines 8–18 process the spatiotemporal constraints ${\mathrm{constraints}}_i$, where AffectedNode is used to store the affected nodes. Specifically, Line 10 adds the nodes associated with each vertex constraint to the affected node set. Lines 13–14 process each edge constraint by adding the two nodes incident to the constrained edge into the affected node set. Subsequently, Line 17 updates the costs of all affected nodes and invoke the corresponding node update procedure. Lines 19–30 compute the shortest path and are identical to the original LPA* algorithm. During the first iteration, Lines 31–33 back up the contents of OpenList and CloseList, while Line 34 reconstructs and returns the optimal path solution through backtracking. As evident from the above algorithmic procedure, the improved LPA* handles the spatiotemporal constraints imposed by the CBS algorithm solely by updating the cost values of the affected nodes. Consequently, it preserves the completeness and optimality guarantees of the original LPA* [29,30].

4.2. Rolling-Horizon Collision Resolution

The concept of receding-horizon control has been widely adopted in swarm trajectory replanning. Unlike existing approaches that directly compute swarm trajectories within a rolling time window, the proposed method applies the receding-horizon principle to the front-end of the trajectory optimization framework, namely, the generation of multiple conflict-free paths for the MAPF problem. WHCA* [40] searches for feasible paths within a time window of length w using Cooperative A* (CA*) and continuously shifts the planning window forward. Building upon this idea, the proposed method takes a further step by avoiding the direct computation of finite-window paths. Instead, the receding-horizon strategy is integrated with the state-of-the-art CBS framework to resolve conflicts only within a finite time window, resulting in the RHCR approach. Accordingly, the original MAPF problem is decomposed into a sequence of Windowed MAPF problems [1]. A Windowed MAPF problem considers only the generation of conflict-free paths within the first w time steps while disregarding conflicts beyond the time window. Let $t_r$ denote the user-defined replanning time duration; in general, $w\ge t_r$ is required. As illustrated in Figure 5, only the conflict-free paths within the first w time steps are computed. Subsequently, conflict-free trajectories are generated using the MINCO-based framework described in Section 4.3. As a result, the trajectory generation and optimization processes associated with potentially conflicting paths beyond the time window are avoided, thereby reducing the overall computational burden.

Algorithm 1: Improved LPA*

Input: $s_i$, $g_i$, ${\mathrm{constraints}}_i$; Output: a collision-free path that satisfies the ${\mathrm{constraints}}_i$, $P_i$;

Figure 5. Schematic diagram of the RHCR algorithm computation flow.

The CBS-based RHCR algorithm consists of two main stages: (1) updating the swarm’s initial states and target regions, and (2) generating conflict-free paths using a CBS framework that considers only the first w time steps. In addition, two key improvements are introduced. First, the CBS low-level planner is implemented using the improved LPA* algorithm described in Section 4.1. Second, the time-window parameter w is dynamically adjusted during execution. Since the computational complexity of conflict-free path planning varies across replanning cycles, the corresponding computation time may differ significantly. To address this issue, the time-window parameter is adaptively adjusted whenever the planning time exceeds a predefined threshold. Specifically, the time window is updated according to $w\leftarrow {\lambda }_{w}w$ where ${\lambda }_{w}w>t_r$, $t_r$ denotes the replanning time and ${\lambda }_w$ is a scaling factor that typically satisfies ${\lambda }_w<1$. The detailed workflow of RHCR is described in Algorithm 2. Line 1 records the algorithm execution time and updates the initial states and target regions of the swarm. Line 2 constructs the root node, where the low-level planner, denoted by $\mathrm{Improved}\mathrm{LPA}*\left(\right)$, computes an optimal path that satisfies the specified constraints. Line 3 inserts the root node into OpenList, a priority queue used to represent the constraint tree $CT$, and the iterative search process is performed in Lines 4–21. At Line 5, the currently best node is extracted from OpenList, and conflicts within the first w time steps are detected in Line 6. If no conflict is found, the current solution is returned in Line 9. Otherwise, Lines 11–17 identify the earliest conflict in time, generate the corresponding constraints, and construct child nodes. The low-level planner $\mathrm{Improved}\mathrm{LPA}*\left(\right)$ is then invoked to replan paths under the updated constraints, and the resulting child nodes are inserted into OpenList. Finally, Lines 18–20 dynamically adjust the time window w according to the actual computation time.

4.3. MINCO-Based Trajectory Generation Scheme

The set of conflict-free discrete paths $\left\{P_1,P_2,\dots ,P_K\right\}$ generated by the CBS-based RHCR algorithm cannot be executed directly by UAVs. Consequently, an additional trajectory generation and optimization stage is required to produce smooth, dynamically feasible, and collision-free flight trajectories. In trajectory generation and optimization problems, the parameterization of trajectories plays a critical role in determining both the trajectory quality and computational efficiency of the optimization process. Owing to the differential flatness property of quadrotors, motion planning can be performed directly in the flat-output space without considering the full state space, thereby eliminating the differential constraints imposed by the system dynamics [41]. As a result, the position trajectory of a multirotor UAV can be parameterized directly. Unlike conventional polynomial trajectories, Bézier curves, or B-spline curves, the proposed framework adopts the MINCO trajectory representation [34], which features highly sparse parameterization and complete decoupling between temporal and spatial optimization variables. These characteristics make MINCO particularly suitable for efficient trajectory generation and optimization.

As illustrated in Figure 6, let the waypoint vector be denoted by $\mathbf{q}={\left[q_1,q_2,\dots ,q_{M-1}\right]}^{\mathrm{T}}$, where $q_0$ and $q_M$ are fixed start and goal waypoints, respectively. The sequence $\left\{q_0,q_1,\dots ,q_M\right\}$ may be obtained from any discrete path generated by RHCR. Let $\left\{t_0,t_1,\dots ,t_M\right\}$ denote the time steps corresponding to waypoints $\left\{q_0,q_1,\dots ,q_M\right\}$, and let the time duration vector be defined as $\mathbf{T}={\left[T_1,T_2,\dots ,T_M\right]}^{\mathrm{T}}$, where each element represents the duration between two consecutive waypoints. Specifically, $T_i=t_i-t_{i-1}$, $T_i\in {\mathbb{R}}_{>0}$, $1\le i\le M$. A MINCO trajectory is defined such that all M trajectory segments connecting the waypoint sequence $\left\{q_0,q_1,\dots ,q_M\right\}$ are minimum-control trajectories, i.e.,

$${\mathfrak{T}}_{\mathrm{MINCO}}=\left\{p\left(t\right):\left[0,T\right]\mapsto {\mathbb{R}}^m|\mathbf{c}=\mathbf{c}\left(\mathbf{q},\mathbf{T}\right),\forall \mathbf{q}\in {\mathbb{R}}^{m\times (M-1)},\mathbf{T}\in {\mathbb{R}}_{>0}^M\right\}$$

(8)

where $p\left(t\right)$ is an m-dimensional piecewise polynomial curve consisting of M segments, each of degree $(2s-1)$. The i-th polynomial segment is expressed as:

$$p_i\left(t\right)={\mathbf{c}}_i^{\mathrm{T}}\beta \left(t-t_{i-1}\right),t\in \left[t_{i-1},t_i\right],1\le i\le M$$

(9)

where ${\mathbf{c}}_i\in {\mathbb{R}}^{2s\times m}$ denotes the polynomial coefficient matrix, and $\beta \left(t\right)={\left[1,t,t^2,\dots ,t^{\left(2s-1\right)}\right]}^{\mathrm{T}}\in {\mathbb{R}}^{2s}$ is the basis function vector of the polynomial trajectory.   

Algorithm 2: RHCR based on CBS

Input: $\left\{s_i\right\}$, $\left\{g_i\right\}$, K; Output: K collision-free paths $\left\{P_1,P_2,\dots ,P_K\right\}$;

Figure 6. Schematic illustration of MINCO trajectory generation.

The entire MINCO trajectory is parameterized by the waypoint vector $\mathbf{q}$ and the time duration vector $\mathbf{T}$. One of the key properties of MINCO trajectories is that their coefficient matrix $\mathbf{c}=\mathbf{c}(\mathbf{q},\mathbf{T})$ can be obtained by solving the linear system $\mathbf{M}\mathbf{c}=\mathbf{b}$, according to Theorem 2 in Reference [34]. It should be noted that $\mathbf{M}$ and $\mathbf{b}$ are the notations adopted in Reference [34], and both are constructed directly from $\mathbf{q}$ and $\mathbf{T}$. The coefficient matrix is given by $\mathbf{c}={\left({\mathbf{c}}_1^{\mathrm{T}},{\mathbf{c}}_2^{\mathrm{T}},\dots ,{\mathbf{c}}_M^{\mathrm{T}}\right)}^{\mathrm{T}}$, where $\mathbf{c}\in {\mathbb{R}}^{2Ms\times m}$. This coefficient matrix is unique and can be computed with linear complexity through PLU decomposition. Furthermore, since the time duration vector is uniformly parameterized, the computational efficiency of MINCO trajectory generation can be further improved by employing a normalized time representation.

4.3.1. Waypoint Selection

During MINCO trajectory generation, the dimension of the linear system $\mathbf{M}\mathbf{c}=\mathbf{b}$ is directly related to the computational efficiency of the trajectory generation process. The path generated by RHCR is closely associated with the map resolution and typically consists of a long sequence of discrete waypoints. Since the dimension of the coefficient matrix $\mathbf{c}$ is directly proportional to the number of trajectory segments, and consequently to the size of the waypoint vector $\mathbf{q}$, the dimensionality of $\mathbf{q}$ is constrained in the proposed framework. As illustrated in Figure 7, the gray waypoints represent the discarded waypoints, whereas the yellow waypoints denote the retained waypoints. A fixed sampling interval is employed for waypoint selection, thereby reducing the number of waypoints and, consequently, the dimension of the coefficient matrix $\mathbf{c}$, which improves the computational efficiency of MINCO trajectory generation.

Figure 7. Schematic illustration of waypoint selection in MINCO trajectory generation.

4.3.2. Local Target Selection

During MINCO trajectory generation, the selection of the start state and target state is of critical importance. Specifically, the complete state information from the 0th-order to the $(s-1)$th-order derivatives must be specified for both the start and target states. The full state of the start point can be obtained directly from the VIO system. However, the full state information of the target point may be unavailable in swarm trajectory replanning tasks. Consequently, local target points must be selected along the initially planned global trajectory to recover the corresponding target-state information required for trajectory generation. In general, local targets are selected within a predefined local target selection range. Nevertheless, the local target points assigned to different UAVs are unlikely to be mutually collision-free. However, this does not affect the proposed planning framework because the CBS-based RHCR algorithm considers conflicts only within the first w time steps. Therefore, collisions occurring among local target points beyond the time window do not influence the path-planning process. As illustrated in Figure 8, the local targets of two UAVs may overlap and thus result in a collision. Despite this, the generated paths are only required to remain conflict-free within the first w time steps. Moreover, only the trajectory segment corresponding to the replanning time $t_r$ is actually executed by the UAVs. Here, $r_t$ denotes the local target selection range.

Figure 8. Illustration of local target selection in MINCO trajectory generation process.

4.4. Time-Windowed Swarm Trajectory Joint Optimization Framework

After a trajectory has been generated using the MINCO framework, further trajectory optimization is required to satisfy specific mission objectives and operational constraints. Let $J(\mathbf{c},\mathbf{T})$ denote the cost function or constraint penalty function, which is assumed to be twice continuously differentiable. The optimization objective of the MINCO trajectory can then be formulated as:

$$H(\mathbf{q},\mathbf{T})=J\left(\mathbf{c}\right(\mathbf{q},\mathbf{T}),\mathbf{T})$$

(10)

A key property of MINCO trajectories is that the gradients ${\displaystyle \frac{\partial H}{\partial \mathbf{q}}}$ and ${\displaystyle \frac{\partial H}{\partial \mathbf{T}}}$ can be computed with linear complexity from ${\displaystyle \frac{\partial J}{\partial \mathbf{c}}}$ and ${\displaystyle \frac{\partial J}{\partial \mathbf{T}}}$. Consequently, the MINCO-based swarm trajectory joint optimization framework only requires the design of an appropriate cost function or constraint penalty function $J(\mathbf{c},\mathbf{T})$ and the computation of its corresponding gradients. The gradients of the objective function $H(\mathbf{q},\mathbf{T})$ can then be obtained through gradient propagation, enabling efficient gradient-based optimization of the trajectory.

The proposed MINCO-based swarm trajectory joint optimization framework consists of a smoothness cost, a time cost, and penalty costs arising from dynamic feasibility constraints, obstacle avoidance constraints, and inter-UAV collision avoidance constraints. To enable unconstrained optimization, all dynamic feasibility, obstacle avoidance, and collision avoidance constraints are formulated as soft constraints and incorporated into the objective function in the form of penalty terms. Accordingly, the proposed MINCO-based swarm trajectory joint optimization framework is formulated as:

$$\underset{\mathbf{q},\mathbf{T}}{\min }J=\sum _K{w}_s{J}_s+w_t{J}_t+w_p{J}_p+w_{us}{J}_{us}$$

(11)

where $w_s$, $w_t$, $w_p$, and $w_{us}$ denote the weighting coefficients associated with the respective cost terms. Specifically, $J_s$ represents the smoothness cost, $J_t$ represents the time cost, $J_p$ represents the penalty cost associated with continuous-time constraints, such as dynamic feasibility, collision avoidance, and obstacle avoidance, while $J_{us}$ represents the cost for enforcing a uniform distribution of sampled constraint points. The waypoint vector $\mathbf{q}$ and the time duration vector $\mathbf{T}$ are taken as the optimization decision variables. The design of each cost component is described in detail as follows.

(1) Smoothness cost $J_s$: $J_s$ is defined as the integral of the squared control input along the MINCO trajectory, which corresponds to the control effort. Specifically,

$$J_s={\displaystyle \sum _{i=1}^M}{\int }_0^{T_i}{∥u_i\left(t\right)∥}^{2}dt={\displaystyle \sum _{i=1}^M}{\int }_0^{T_i}{∥p_i^{\left(s\right)}\left(t\right)∥}^{2}dt$$

(12)

(2) Time cost $J_t$: $J_t$ is generally defined as the sum of the durations of the M trajectory segments, i.e.,

$$J_t={\displaystyle \sum _{i=1}^M}{T}_i,T_i>0$$

(13)

However, to eliminate the time constraint $T_i>0$, a simple diffeomorphic transformation is adopted, following Reference [34], to convert the constrained variable $T_i$ into the unconstrained variable ${\tau }_i$, i.e.,

$${\tau }_i=\left\{\begin{array}{cc}\sqrt{2T_i-1}-1& T_i>1\\ 1-\sqrt{{\displaystyle \frac{2}{T_i}}-1}& T_i\le 1\end{array}\right.$$

(14)

(3) Penalty cost associated with continuous-time constraints $J_p$: The dynamic feasibility constraint ${\mathbf{G}}_d$, obstacle avoidance constraint ${\mathbf{G}}_o$, and collision avoidance constraint ${\mathbf{G}}_r$ are all continuous-time constraints $\mathbf{G}\left(t\right)$, which can be uniformly expressed as:

$$\mathbf{G}\left(p\left(t\right),v\left(t\right),\dots ,p^{\left(s\right)}\left(t\right)\right)\le 0,\forall t\in [0,T]$$

(15)

These continuous-time inequality constraints $\mathbf{G}\left(t\right)$ are incorporated into the objective function as a penalty cost $J_p$. As illustrated in Figure 9, each MINCO trajectory segment $p_i\left(t\right)$ is uniformly sampled along the time dimension, generating $({\mu }_i+1)$ constraint points, denoted by $\left\{c{p}_0,c{p}_1,\dots ,c{p}_{{\mu }_i}\right\}$. The position of constraint point $c{p}_j$ is given by $p_i\left({\displaystyle \frac{j}{{\mu }_i}}{T}_i\right)$, and the corresponding constraint is evaluated as $\mathbf{G}\left({\mathbf{c}}_i,T_i,{\displaystyle \frac{j}{{\mu }_i}}{T}_i\right)$, where $0\le j\le {\mu }_i$. For the continuous-time constraint $\mathbf{G}\left(t\right)$, the penalty cost defined on the MINCO trajectory is formulated as:

$$J_p={\displaystyle \sum _{i=1}^M}{J}_{pi}\left({\mathbf{c}}_i,T_i,{\mu }_i\right)={\displaystyle \sum _{i=1}^M}{\displaystyle \frac{T_i}{{\mu }_i}}{\displaystyle \sum _{j=0}^{{\mu }_i}}{\overline{\omega }}_j{\chi }^T{\max \left(\mathbf{G}\left({\mathbf{c}}_i,T_i,{\displaystyle \frac{j}{{\mu }_i}}{T}_i\right),0\right)}^3$$

(16)

where $J_{pi}\left({\mathbf{c}}_i,T_i,{\mu }_i\right)$ denotes the penalty cost of the i-th trajectory segment. $\left({\overline{\omega }}_0,{\overline{\omega }}_1,\dots ,{\overline{\omega }}_{{\mu }_i-1},{\overline{\omega }}_{{\mu }_i}\right)=\left({\displaystyle \frac{1}{2}},1,\dots ,1,{\displaystyle \frac{1}{2}}\right)$ are the weighting coefficients derived from the trapezoidal rule, and ${\max \left(\mathbf{G}\left({\mathbf{c}}_i,T_i,{\displaystyle \frac{j}{{\mu }_i}}{T}_i\right),0\right)}^3$ represents the penalty incurred when the constraint at the sampling point $c{p}_j$ is violated. The vector $\chi$ is a weighting vector associated with the degree of constraint violation. Specifically, each element of $\chi$ is set to zero when the corresponding constraint is satisfied and takes a positive value when the constraint is violated, thereby characterizing the severity of the violation.

Figure 9. Illustration of constrained points after sampling the i-th segment of the MINCO trajectory.

In the CBS-based RHCR algorithm, only conflicts occurring within the time window w are considered, while conflicts beyond the time window w are not explicitly addressed. In fact, since the MINCO trajectory segments outside the time window w are not executed, handling their constraint violations is unnecessary. Therefore, within a complete replanning time duration $t_r$, constraint violations occurring beyond the time window w are ignored. Specifically, for $t\in [w,t_r]$, the corresponding elements of the weighting vector $\chi$ are set to zero, thereby eliminating any penalty associated with constraint violations in this interval.

(3-1) Dynamic feasibility constraints ${\mathbf{G}}_d$: Based on the maximum velocity $v_{\max }$ and maximum acceleration $a_{\max }$, the dynamic feasibility constraints can be formulated as:

$${\mathbf{G}}_v=\dot{p}{\left(t\right)}^2-v_{\max }^2=v{\left(t\right)}^2-v_{\max }^2$$

(17)

$${\mathbf{G}}_a=\ddot{p}{\left(t\right)}^2-a_{\max }^2=a{\left(t\right)}^2-a_{\max }^2$$

(18)

(3-2) Obstacle avoidance constraint ${\mathbf{G}}_o$: Let $d\left(p\left(t\right),p_{\mathrm{obs}}\right)$ denote the distance between the UAV and an obstacle located at $p_{\mathrm{obs}}$, and let $D_o$ denote the required obstacle-clearance distance. Following Reference [33], when the distance between the UAV and the obstacle, $d\left(p\left(t\right),p_{\mathrm{obs}}\right)$, is smaller than the safety clearance $D_o$, the obstacle avoidance constraint is defined as

$${\mathbf{G}}_o\left(p\left(t\right)\right)=D_o-d\left(p\left(t\right),p_{\mathrm{obs}}\right)=D_o-\left(p\left(t\right)-p_{\mathrm{obs}}\right)\mathbf{v}$$

(19)

where $\mathbf{v}$ is the unit vector directed from $p\left(t\right)$ toward the tangent plane of the obstacle at $p_{\mathrm{obs}}$.

(3-3) Inter-UAV collision avoidance constraint ${\mathbf{G}}_r$: As illustrated in Figure 10, $p_k\left(t\right)$, $p_{k^{\prime }}\left(t^{\prime }\right)$, and $p_{k^{″}}\left(t^{″}\right)$ denote the k-th, $k^{\prime }$-th, and $k^{″}$-th UAV trajectories, respectively, where $1\le k,k^{\prime },k^{″}\le K$. Assuming that the swarm collision-checking timestamp is $\tau$, the following relationship holds:

$$\tau ={\displaystyle \sum _{n=1}^{i-1}}{T}_{kn}+j{\displaystyle \frac{T_{ki}}{{\mu }_{ki}}}={\displaystyle \sum _{n=1}^{i^{\prime }-1}}{T}_{k^{\prime }n}+t^{\prime }$$

(20)

According to Figure 10, the corresponding times of the trajectory segments associated with trajectories $p_k\left(t\right)$ and $p_{k^{\prime }}\left(t^{\prime }\right)$ at the collision-checking timestamp $\tau$ are $t=j{\displaystyle \frac{T_{ki}}{{\mu }_{ki}}}$ and $t^{\prime }$, respectively.

Figure 10. Schematic illustration of the computation of inter-UAV collision-avoidance constraints for swarm trajectories.

Let $D_r$ denote the minimum safe separation distance between UAVs. The constraint imposed on the constraint point $c{p}_{i,j}$ of trajectory $p_k\left(t\right)$ by other trajectories at the timestamp $\tau$ is defined as:

$${\mathbf{G}}_r^{i,j}\left(p_k\left(t\right),\tau \right)={\left(\dots ,{\mathbf{G}}_{r{k}^{\prime }}^{i,j}\left(p_k\left(t\right),\tau \right),{\mathbf{G}}_{r{k}^{″}}^{i,j}\left(p_k\left(t\right),\tau \right),\dots \right)}^{\mathrm{T}}$$

(21)

Specifically, the constraint imposed by trajectory $p_{k^{\prime }}\left(t^{\prime }\right)$ is given by:

$${\mathbf{G}}_{r{k}^{\prime }}^{i,j}\left(p_k\left(t\right),\tau \right)=\left\{\begin{array}{cc}0& k^{\prime }=k\\ D_r^2-d^2\left(p_k\left(t\right),p_{k^{\prime }}\left(t^{\prime }\right)\right)& k^{\prime }\ne k\end{array}\right.$$

(22)

where

$$d\left(p_k\left(t\right),p_{k^{\prime }}\left(t^{\prime }\right)\right)=∥{\mathbf{E}}^{{\displaystyle \frac{1}{2}}}\left(p_k\left(t\right)-p_{k^{\prime }}\left(t^{\prime }\right)\right)∥$$

(23)

The weighting matrix $\mathbf{E}$ is defined as:

$$\mathbf{E}=\left\{\begin{array}{cc}\mathrm{diag}(1,1)& m=2\\ \mathrm{diag}\left(1,1,1/l_d\right)& m=3\end{array}\right.$$

(24)

where the diagonal matrix $\mathbf{E}$ characterizes the weighted Euclidean distance in different dimensions. Specifically, $l_d=1$ indicates that the UAV is modeled as a sphere, whereas $l_d=r_l/r_s>1$ corresponds to an ellipsoidal UAV model that accounts for the downwash effect.

(4) Uniform distribution cost of sampled constraint points $J_{us}$: In the penalty formulation for continuous-time constraints, a non-uniform sampling distribution may cause certain constraint violations to be missed between adjacent sampling points. As a result, all sampled constraint points may appear to satisfy the constraints, while the underlying continuous trajectory may, in fact, violate them. To mitigate this issue, a uniform distribution of constraint points is encouraged by penalizing the variance of the squared distances between adjacent constraint points. For a MINCO trajectory, the total number of constraint points is $N_{cp}={\sum }_{i=1}^M{\mu }_i$. The vector of squared distances between adjacent constraint points is defined as:

$$\mathbf{D}=\left({∥{cp}_{1,0}-{cp}_{1,1}∥}_2^2,{∥{cp}_{1,1}-{cp}_{1,2}∥}_2^2,\dots ,{∥{cp}_{M,{\mu }_M-1}-{cp}_{M,{\mu }_M}∥}_2^2\right)$$

(25)

Accordingly, the uniform distribution cost of the sampled constraint points is defined as:

$$J_{us}=\mathrm{Var}\left(\mathbf{D}\right)=\mathrm{E}\left({\mathbf{D}}^2\right)-{\mathrm{E}\left(\mathbf{D}\right)}^2={\displaystyle \frac{{∥\mathbf{D}∥}_2^2}{N_{cp}}}-{\displaystyle \frac{{∥\mathbf{D}∥}_1^2}{N_{cp}^2}}$$

(26)

This cost term penalizes variations in the spacing between adjacent constraint points, thereby promoting a more uniform sampling distribution along the trajectory and reducing the likelihood of undetected constraint violations between neighboring samples.

The computation of the gradients of the smoothness cost $J_s$, time cost $J_t$, continuous-time constraint penalty cost $J_p$, and uniform sampling distribution cost $J_{us}$ with respect to ${\mathbf{c}}_i$ and $T_i$, as well as the computation of the gradients of the dynamic feasibility constraint ${\mathbf{G}}_d$, obstacle avoidance constraint ${\mathbf{G}}_o$, and inter-UAV collision avoidance constraint ${\mathbf{G}}_r$ with respect to ${\mathbf{c}}_i$ and $T_i$, can be carried out following the methods presented in References [15,28]. Subsequently, by applying the gradient propagation relationships between $\partial H/\partial \mathbf{q}$, $\partial H/\partial T$ and $\partial J/\partial \mathbf{c}$, $\partial J/\partial T$ provided in Reference [34], the optimization problem associated with the objective function H can be efficiently solved.

4.5. Two-Stage Temporal Coordination Optimization Method

In the proposed MINCO-based swarm trajectory joint optimization framework, a wide range of practical constraints and operational requirements encountered in real-world scenarios have been comprehensively considered. In particular, the optimization methods for obstacle avoidance and inter-UAV collision avoidance in the spatial domain are both highly flexible and computationally efficient. However, time optimization remains a major challenge for swarm motion planning solvers due to its computational complexity. On the one hand, time optimization is highly dependent on the initial values of the time duration vector $\mathbf{T}$. In the proposed framework, this issue is largely alleviated by the front-end planner, which generates a time-uniform waypoint sequence and thereby provides a favorable initialization for $\mathbf{T}$. On the other hand, the objectives of swarm time optimization are generally either to minimize the overall mission duration or to achieve simultaneous arrival times among all UAVs, both of which are frequently adopted in swarm task execution. The objective of minimizing the overall mission duration has already been addressed in Section 4.4. However, simultaneous arrival has not been adequately handled. To address this issue, a two-stage temporal coordination optimization method is proposed. Since all UAVs in a swarm are treated as equivalent agents, the assignment of individual UAVs to different target regions can be adjusted accordingly. Therefore, in the first stage, a task allocation method oriented toward temporal load balancing is developed to minimize the variance of flight times across all UAVs. This stage also provides a favorable initialization for the subsequent time optimization process. In the second stage, a soft-constrained temporal coordination optimization method is proposed to encourage all UAVs to arrive at their respective destinations as simultaneously as possible.

4.5.1. Task Allocation Method for Temporal Load Balancing

In the first stage, the problem is formulated as an MIQP problem. Let the number of UAVs be K, and define the binary decision variable ${\mathbf{x}}_{ij}$ to indicate whether ${\mathrm{UAV}}_i$ is assigned to the j-th target region $X_j^G$, i.e.,

$${\mathbf{x}}_{ij}=\left\{\begin{array}{cc}1& {\mathrm{UAV}}_i\text{’}\mathrm{s}\mathrm{target}\mathrm{region}\mathrm{is}{X}_j^G\\ 0& {\mathrm{UAV}}_i\text{’}\mathrm{s}\mathrm{target}\mathrm{region}\mathrm{is}\mathrm{not}{X}_j^G\end{array}\right.,1\le i,j\le K$$

(27)

Let $cos{t}_{ij}^{*}$ denote the estimated time cost for ${\mathrm{UAV}}_i$ to reach the j-th target region. It is defined as:

$${cost}_{ij}^{*}={\displaystyle \frac{d_{ij}^{*}}{{\lambda }_v{v}_{\max }}}$$

(28)

where $d_{ij}^{*}$ is the distance precomputed using the A* algorithm, ${\lambda }_v$ is a velocity scaling factor, and ${\lambda }_v\in (0,1)$. The expected arrival time $T_e$ is then given by:

$$T_e={\displaystyle \frac{1}{K}}{\displaystyle \sum _{i=1}^K}{\displaystyle \sum _{j=1}^K}{cost}_{ij}^{*}·{\mathbf{x}}_{ij}$$

(29)

Here, $T_e$ represents the average estimated arrival time of all UAVs under a given task assignment and serves as a reference for achieving temporal load balancing across the swarm.

The objective function is designed to minimize the variance of the arrival times. Accordingly, the optimization problem can be formulated as:

$$\underset{\left\{{\mathbf{x}}_{ij}\right\}}{\min }{\displaystyle \frac{1}{K}}{\displaystyle \sum _{i=1}^K}{\left({\displaystyle \sum _{j=1}^K}{cost}_{ij}^{*}·{\mathbf{x}}_{ij}-T_e\right)}^2$$

(30)

subject to:

$${\displaystyle \sum _{i=1}^K}{\mathbf{x}}_{ij}=1,\forall j=1,2,\dots ,K$$

(31)

$${\displaystyle \sum _{j=1}^K}{\mathbf{x}}_{ij}=1,\forall i=1,2,\dots ,K$$

(32)

The optimization variable is the assignment matrix ${\mathbf{x}}_{ij}$, and the objective is to minimize the variance of the estimated arrival times. The constraints require that each UAV be assigned to exactly one target region and that each target region be assigned to exactly one UAV. The resulting MIQP problem is solved using OR-Tools, Google’s open-source optimization suite. This approach provides efficient task-allocation performance and is capable of handling swarm sizes of up to 1000 UAVs within practical computational time limits.

4.5.2. Time Optimization Based on Soft Constraints

Based on the assignment solution obtained in the first stage, the expected arrival time $T_e$ can be recomputed. In the second stage, the variance of the UAV arrival times is further optimized. To this end, the time cost $J_t$ in the MINCO-based joint swarm trajectory optimization framework is redefined as:

$$J_t={\displaystyle \frac{1}{K}}{\left({\displaystyle \sum _{i=1}^M}{T}_i-T_e\right)}^2,T_i>0$$

(33)

With this modification, the gradients of the time cost are recalculated as ${\displaystyle \frac{\partial J_t}{\partial {\mathbf{c}}_i}}=0$ and ${\displaystyle \frac{\partial J_t}{\partial T_i}}={\displaystyle \frac{2}{K}}({\sum }_{i=1}^M{T}_i-T_e)$. By incorporating the expected arrival time $T_e$ as a soft constraint into the objective function, the optimization process encourages the flight durations of individual UAVs to converge toward a common value, thereby improving arrival-time simultaneity across the swarm while preserving the flexibility of the trajectory optimization framework.

5. Experiments

The RHCR-Opt algorithm is executed on a computer equipped with an Intel Core i7-8700 CPU running at 3.2 GHz and 16 GB of memory. The software platform consists of a ROS environment based on the Ubuntu operating system. The complete C++ implementation of the project is open-sourced on GitHub at https://github.com/ucas-zihaowang/RHCR-Opt.git, accessed on 10 August 2026. To evaluate the performance of the proposed RHCR-Opt algorithm, we have conducted five experiments, including: (1) performance improvement of CBS through the improved LPA* algorithm, (2) performance analysis of RHCR-Opt, (3) swarm flight in special scenarios, (4) comparison with baseline algorithms, and (5) analysis of the effectiveness of the two-stage temporal coordination optimization. Through these five simulation experiments, we have comprehensively validated the performance improvements achieved by the various components of RHCR-Opt with respect to their corresponding evaluation metrics, confirming that RHCR-Opt maintains high-quality trajectories while simultaneously achieving high computational efficiency.

5.1. Performance Improvement of CBS by Improved LPA*

Compared with A*, the improved LPA* algorithm is more efficient when repeatedly searching for paths under identical spatiotemporal constraints. We employ A* and LPA* as the low-level planners within CBS, respectively, and analyze the performance of CBS (A*) and CBS (LPA*) when solving the same test instances.

5.1.1. Setting

As shown in Figure 11, a $10\mathrm{m}\times 10\mathrm{m}$ map is generated with 20 randomly distributed obstacles of varying sizes. The map resolution is set to 0.1 m, resulting in a grid map of size $100\times 100$. We design a test instance involving four agents that cross paths while moving toward each other’s initial positions, with each agent having a radius $r_c$ of 0.25 m. The MAPF problem illustrated in Figure 11 is solved using both CBS (A*) and CBS (LPA*). The high-level planner remains identical to that of the classical CBS algorithm, while A* and the improved LPA* are respectively employed as the low-level planners to search for paths under identical spatiotemporal constraints whenever a conflict is resolved.

Figure 11. Path planning results in dense obstacle environments using CBS(LPA*). The gray regions represent static obstacles. The solid dark-blue and dark-green circles denote the start and goal positions of the agents, respectively. The four agents move toward their respective goal locations along intersecting routes. The light-green polyline segments represent the agent paths generated by the CBS (LPA*) algorithm.

5.1.2. Results

As shown in Figure 12, the numbers of expanded nodes and computation times of agent 0 and agent 1 are presented for different low-level planners during the high-level iterations of CBS. From the node expansion results in Figure 12a,c, LPA* expands a relatively large number of nodes during the initial planning stage because it needs to process the global consistency of all nodes in the search graph. Except for this initial planning process, the number of expanded nodes remains at a single-digit level, whereas A* consistently expands nodes on the order of hundreds. Correspondingly, as shown in Figure 12b,d, the computation time of LPA* is longer only during the initial planning stage. For all subsequent replanning processes, its computation time is significantly lower than that of A*. It should be noted that when the MAPF problem is extremely simple and CBS requires only a few iterations to resolve conflicts, the improved LPA* does not outperform A*. However, when the MAPF problem becomes moderately complex or the swarm size increases, the improved LPA* exhibits clear advantages over A*. Table 2 presents the performance evaluation results of CBS (A*) and CBS (LPA*) for the problem shown in Figure 11. Compared with CBS (A*), CBS (LPA*) achieves a 73.3% reduction in the number of iterations and an 82.2% reduction in computation time. Therefore, by dynamically maintaining the search graph and flexibly accommodating spatiotemporal constraints, the improved LPA* substantially improves the computational efficiency of CBS when solving MAPF problems in complex environments.

Figure 12. Node expansions and computational time of CBS under different low-level planners (A* and LPA*).

Table 2. Performance evaluation metrics of the CBS algorithm under different low-level planners.

Planner	High-Level Iteration Number	Computation Time
CBS (A*)	360	550 ms
CBS (LPA*)	96	98 ms

5.2. Performance Analysis of RHCR-Opt

To analyze the contribution of each component of RHCR-Opt to the overall performance improvement, we design four algorithmic configurations for the performance analysis experiments: CBS (A*)-Opt, CBS (LPA*)-Opt, CBS (LPA*)-Opt-R, and RHCR (LPA*)-Opt. CBS (A*)-Opt employs A* as the low-level planner within CBS and integrates the MINCO-based swarm trajectory joint optimization framework. In contrast, CBS (LPA*)-Opt adopts LPA* as the low-level planner. Both CBS (A*)-Opt and CBS (LPA*)-Opt perform a one-shot global planning process and do not possess online replanning capability. CBS (LPA*)-Opt-R is the replanning version of CBS (LPA*)-Opt, in which replanning is performed at fixed time intervals. RHCR (LPA*)-Opt, on the other hand, is a swarm trajectory replanning framework based on the RHCR concept, enabling trajectory replanning through rolling-horizon collision resolution.

5.2.1. Setting

As shown in Figure 13, the map size is $14\mathrm{m}\times 8\mathrm{m}\times 3\mathrm{m}$ with a resolution of 0.1 m, and contains 15 cylindrical obstacles of randomly generated sizes. Four UAVs travel from the initial region to the target region along intersecting routes. Each UAV is modeled as a sphere with a radius $r_c$ of 0.25 m, with a maximum velocity $v_{\max }$ of 1 m/s and a maximum acceleration $a_{\max }$ of 6 m/${\mathrm{s}}^2$, subject to jerk-based control, i.e., $s=3$. In addition, the replanning time $t_r$ is set to 2 s, and the local target selection range $r_t$ is set to 3 m. The algorithm performance is analyzed from two perspectives. The first focuses on the performance of CBS in generating conflict-free paths, including the swarm conflict-free path cost, the number of CBS high-level iterations, and the CBS computation time. The second focuses on the performance of the MINCO-based swarm trajectory joint optimization framework, including the flight time cost of swarm trajectories, the control effort cost (i.e., the smoothness cost), and the computation time required for trajectory optimization.

Figure 13. Flight trajectories of RHCR-Opt under four different algorithm configurations. The gray pillars represent static obstacles. The UAV swarm has successfully flown from the initial swarm region to the target region. The trajectories of individual UAVs are illustrated by curves of different colors.

5.2.2. Results

Figure 13 presents the flight trajectories generated by the four algorithmic configurations for the same test instance, while the detailed performance evaluation metrics are reported in Table 3. As observed from the flight trajectories in Figure 13a,b, the final trajectories generated by CBS (A*)-Opt and CBS (LPA*)-Opt exhibit slight differences, which arise from the different initial paths produced by A* and LPA*, respectively. According to the quantitative results in Table 3, both methods achieve identical path costs. However, CBS (LPA*)-Opt significantly reduces the number of CBS high-level iterations and computation time, with reductions of 67.7% and 90.4%, respectively. In terms of the MINCO-based swarm trajectory joint optimization, CBS (A*)-Opt and CBS (LPA*)-Opt produce nearly identical trajectory time costs and control effort costs, while their trajectory optimization times differ only by a few milliseconds. These results further validate the effectiveness of the improved LPA* in improving the computational efficiency of the overall front-end planning framework, as discussed in Section 5.1. Figure 13c,d illustrate the flight trajectories generated by CBS (LPA*)-Opt-R and RHCR (LPA*)-Opt, both of which perform swarm trajectory replanning at fixed time intervals. The resulting trajectories are largely consistent between the two methods. However, the experimental results in Table 3 indicate that neither CBS (LPA*)-Opt-R nor RHCR (LPA*)-Opt guarantees globally optimal paths or trajectories because of the continuous replanning process. Consequently, both the path cost and trajectory cost are noticeably higher than those of CBS (A*)-Opt and CBS (LPA*)-Opt. Nevertheless, the iterative replanning strategy substantially reduces both the CBS computation time and the trajectory optimization computation time. Figure 14 compares the performance evaluation metrics of CBS (LPA*)-Opt-R and RHCR (LPA*)-Opt throughout the replanning process. Compared with the baseline algorithm CBS (LPA*)-Opt-R, RHCR (LPA*)-Opt achieves significant reductions in the number of CBS high-level iterations, CBS computation time, and MINCO-based swarm trajectory optimization computation time. This improvement is attributed to the fact that RHCR (LPA*)-Opt resolves conflicts only within a finite time window at the CBS high level. Furthermore, Figure 14 more clearly demonstrates that the swarm path cost and swarm trajectory flight time cost remain highly consistent throughout the replanning process, while RHCR (LPA*)-Opt achieves a lower swarm trajectory control effort cost. Therefore, RHCR-Opt substantially improves computational efficiency in both path searching and trajectory optimization while maintaining high trajectory quality. Based on the quantitative results in Table 3, the overall computation time is reduced by approximately 37.5%.

Table 3. Performance evaluation metrics of RHCR-Opt under four different algorithm configurations.

Planner	CBS			MINCO-Based Swarm Trajectory Joint Optimization
	Paths Cost	High-Level Iteration Num	Computation Time	Trajs Cost	Control Effort	Computation Time
CBS (A*)-Opt	640	888	2849.2 ms	55.5 s	93.7	7.5 ms
CBS (LPA*)-Opt	640	287	274.7 ms	56.3 s	86.2	12.1 ms
CBS (LPA*)-Opt-R	804	83	86 ms	64.3 s	89.2	14.5 ms
RHCR (LPA*)-Opt	801	11	57.7 ms	66.6 s	41.4	5.1 ms

Figure 14. Comparison of performance evaluation metrics between RHCR-Opt and baseline algorithms during replanning iterations.

5.3. Swarm Flight in Special Scenarios

Scenarios such as wall-passage traversal and symmetric position exchange are commonly used in swarm flight experiments to evaluate algorithm performance under extreme conditions. We employ the state-of-the-art decentralized planning algorithm Ego-Swarm-v2 [15] as a benchmark and conduct flight tests in both wall-passage traversal and symmetric exchange scenarios under identical algorithmic parameters. As shown in Figure 15, Figure 15a,b present a test scenario in which eight UAVs cross through a wall opening, while Figure 15c,d illustrate a symmetric exchange scenario in which eight UAVs, initially arranged in a circular formation, move toward one another’s initial positions. In the wall-passage traversal scenario shown in Figure 15a,b, Ego-Swarm-v2 [15] does not successfully pass through the wall opening. Instead, as the UAVs approach the opening, the increasing complexity of the swarm trajectories causes several UAVs to choose paths that fly over the wall. This phenomenon reflects a limitation of decentralized planning algorithms such as Ego-Swarm-v2 [15]: when UAVs become densely clustered or operate within narrow spaces, each UAV focuses solely on locally optimal trajectories, which may lead to inferior overall swarm trajectory quality. Consequently, the final flight trajectories generated by RHCR-Opt exhibit higher trajectory quality. Similarly, in the symmetric exchange scenario shown in Figure 15c,d, Ego-Swarm-v2 [15] exhibits frequent hovering behaviors, trajectory distortions, and trajectory backtracking. These phenomena arise from the lack of global coordination during real-time swarm flight. In contrast, RHCR-Opt avoids such issues through its decentralized front-end planning and centralized conflict-resolution mechanism. Therefore, RHCR-Opt generates smoother and more intuitive flight trajectories in challenging scenarios such as wall-passage traversal and symmetric position exchange.

Figure 15. UAV swarm flight trajectories in special scenarios. The gray walls or gray pillars represent static obstacles. The UAV swarm has successfully flown from the initial swarm region to the target region. The trajectories of individual UAVs are depicted by curves of different colors. (a) Ego-Swarm-v2 [15]. (b) RHCR-Opt. (c) Ego-Swarm-v2 [15]. (d) RHCR-Opt.

5.4. Comparison with Baseline Algorithms

We further conduct large-scale performance comparison experiments between RHCR-Opt and baseline algorithms. Two recent state-of-the-art methods that follow a technical framework similar to that of RHCR-Opt, namely RBP [27] and EDG-TEAM [28], are selected as baseline algorithms. Among them, RBP [27] is employed as a global planning baseline for comparison with RHCR-Opt. Its front-end planner consists of ECBS, a bounded-suboptimal variant of CBS, while its back-end adopts a swarm trajectory joint optimization framework based on Bernstein polynomial trajectories. In contrast, EDG-TEAM [28] is employed as a replanning baseline. Similar to RBP [27], its front-end planner also utilizes ECBS; however, its back-end adopts a MINCO-based swarm trajectory joint optimization framework.

5.4.1. Setting

As shown in Figure 16, the map size is $10\mathrm{m}\times 10\mathrm{m}\times 3\mathrm{m}$ with a resolution of 0.1 m, and six cylindrical obstacles are randomly generated. The UAV radius $r_c$ is set to 0.1 m, with the maximum velocity $v_{\max }$ being 1.7 m/s and the maximum acceleration $a_{\max }$ being 6.2 m/${\mathrm{s}}^2$, subject to jerk-based control, i.e., $s=3$. For replanning tasks, the replanning time $t_r$ is set to 2.0 s, and the local target selection range $r_t$ is set to 6 m. To evaluate performance under different swarm scales, we consider $K=4,8,12,16$ UAVs. For each swarm size, 20 test instances with randomly generated start and goal regions are created. If the computation time of a swarm planning algorithm exceeds 5 s, the corresponding instance is regarded as unsolved. In the front-end ECBS planner used by both RBP [27] and EDG-TEAM [28], the bounded-suboptimality factor $\mathrm{ecbs}\_\mathrm{w}$ is set to 1.0 to ensure that the front-end planners of the baseline methods and RHCR-Opt all maintain optimality. To ensure a fair comparison, the batch size parameter $\mathrm{batch}\_\mathrm{size}$ in RBP [27] is set equal to the swarm size, thereby disabling priority-based techniques such as batch processing. Furthermore, when comparing RHCR-Opt with RBP [27], we introduce an additional variant, denoted as RHCR-Opt*. To facilitate a clearer analysis of the performance differences among the various components of the overall framework, RHCR-Opt* replaces the CBS-based RHCR module with an ECBS-based RHCR module while keeping all other modules identical to those of RHCR-Opt.

Figure 16. Flight trajectories planned by RHCR-Opt for a swarm size of $K=16$. The gray pillars represent static obstacles, and curves of different colors represent flight trajectories of UAVs.

5.4.2. Results

In a single global planning task, we conduct large-scale batch experiments on RHCR-Opt, RHCR-Opt*, and RBP [27] under different swarm sizes. The corresponding results are presented in Table 4. As shown in Table 4, for problems with different swarm sizes, RBP [27] and RHCR-Opt achieve nearly identical path costs in the front-end planning stage. However, the front-end computation time of RBP [27] is substantially lower than that of RHCR-Opt. This outcome is expected because, in a single global planning task, the CBS-based RHCR framework employs LPA* as the low-level planner, whose node expansion count is considerably higher than that of A*. This characteristic has already been discussed in Section 5.1. Furthermore, RHCR-Opt* adopts an ECBS-based RHCR framework similar to that of RBP [27], and its front-end computation time is also largely comparable to that of RBP [27]. Therefore, the relatively higher front-end computation time of RHCR-Opt in global planning tasks is consistent with expectations. In the back-end optimization stage, the Bernstein polynomial trajectory framework adopted by RBP [27] is theoretically less capable of spatiotemporal trajectory deformation than the MINCO-based framework employed by RHCR-Opt. Consequently, RBP [27] performs noticeably worse than RHCR-Opt in terms of trajectory length, trajectory flight time, and Makespan. It should be noted that Makespan here refers to the maximum trajectory flight time among all UAVs in the swarm. In particular, regarding trajectory flight time and Makespan, RBP [27] relies on fixed time-step settings and essentially lacks the capability to dynamically adjust time duration. Naturally, the longer trajectory flight times and larger Makespan values also result in lower control effort costs for RBP [27] compared with RHCR-Opt. In terms of computational efficiency, RHCR-Opt significantly outperforms RBP [27]. Across all test instances with different swarm sizes, the trajectory optimization time of RHCR-Opt remains within 4 ms to 34 ms and exhibits only a modest approximately linear increase as the swarm size grows. Compared with RBP [27], the computation time is reduced by at least one to two orders of magnitude. Considering the complete planning framework, including both the front-end and back-end stages, RHCR-Opt achieves improvements in success rate across all swarm sizes. Moreover, as the swarm size increases, the overall computation time exhibits a reduction approaching one order of magnitude relative to the baseline method.

Table 4. Performance evaluation metrics of RHCR-Opt and baseline algorithms in global planning tasks.

		Front-End		Back-End					Total
Planner	Size	Paths Cost	Comp. Time	Trajs Length	Flight Time	Makespan	Control Effort	Comp. Time	Success Rate	Comp. Time
RBP [27]	4	246.06	7.84 ms	21.14 m	339.78 s	84.94 s	0.8 × 10−4	170.93 ms	0.9	215.54 ms
	8	505.17	8.8 ms	45.42 m	735.11 s	91.89 s	1.7 × 10−4	594.51 ms	0.9	673.36 ms
	12	767.67	13.57 ms	71.94 m	1158.67 s	96.56 s	3.5 × 10−4	1818.8 ms	0.9	1939.45 ms
	16	972.5	15.34 ms	90.44 m	1510 s	94.38 s	4.6 × 10−4	3733.09 ms	0.4	3886.44 ms
RHCR-Opt	4	245.4	32.76 ms	19.56 m	17.34 s	5.45 s	100.01	3.89 ms	1	37.8 ms
	8	507.85	65.62 ms	40.56 m	35.84 s	5.87 s	222.18	12.38 ms	1	80.41 ms
	12	737.21	107.08 ms	58.19 m	51.89 s	5.89 s	338.09	24.08 ms	0.95	134.88 ms
	16	984	158.77 ms	78.66 m	71.74 s	6.52 s	450.04	33.36 ms	0.5	196.12 ms
RHCR-Opt*	4	245.4	3.74 ms	20.61 m	17.63 s	5.68 s	116.93	11.03 ms	1	15.2 ms
	8	507.68	7.42 ms	44.08 m	37.96 s	6.29 s	238.32	24.98 ms	0.95	32.91 ms
	12	765	12.21 ms	66.03 m	57.52 s	6.74 s	363.94	39.79 ms	1	52.63 ms
	16	996.17	20.92 ms	87.55 m	77 s	6.68 s	528.17	51.75 ms	0.6	73.51 ms

In replanning tasks, Figure 16 presents the flight trajectories generated by RHCR-Opt for a swarm size of $K=16$. Similarly, we conduct large-scale batch experiments to compare RHCR-Opt with the baseline method EDG-TEAM [28] under different swarm sizes in swarm trajectory replanning scenarios. The corresponding results are summarized in Table 5. As shown in Table 5, regarding front-end computation time, when the swarm size is relatively small, $K=4$ or $K=8$, the replanning task is comparatively simple. Consequently, RHCR-Opt requires slightly more computation time than EDG-TEAM [28] because the initial planning stage of the LPA* low-level planner involves a larger number of node expansions. However, when the swarm size increases to $K=12$ or $K=16$, the benefits of the proposed improvements to LPA* become increasingly evident, enabling RHCR-Opt to achieve significantly lower front-end computation times than EDG-TEAM [28]. In the back-end optimization stage, the time-window-based swarm trajectory joint optimization framework employed by RHCR-Opt processes fewer conflicts and constraints within each replanning iteration. As a result, RHCR-Opt consistently requires less back-end computation time than EDG-TEAM [28] across all swarm sizes, with the computational advantage becoming more pronounced as the swarm size increases. Moreover, RHCR-Opt achieves superior performance in terms of trajectory length, trajectory flight time, and control effort cost. Overall, RHCR-Opt achieves improvements in success rate over EDG-TEAM [28] for swarm sizes of $K=8$, $K=12$, and $K=16$. When the swarm size is relatively small, $K=4$ or $K=8$, the larger number of node expansions during the initial execution of LPA* results in a longer overall computation time for RHCR-Opt. However, as the swarm size increases, $K=12$ or $K=16$, the combined benefits of the front-end and back-end improvements lead to increasingly significant reductions in total computation time, exceeding 23.7% in all cases. For example, when $K=12$, RHCR-Opt reduces the total computation time by 54 ms. When $K=16$, the reduction reaches 97 ms.

Table 5. Performance evaluation metrics of RHCR-Opt and baseline algorithms in replanning tasks.

Planner	Size	Front-End Comp. Time	Back-End Comp. Time	Trajs Length	Flight Time	Control Effort	Success Rate	Total Comp. Time
EDG-TEAM [28]	4	6.03 ms	12.09 ms	17.71 m	16.39 s	56.25	1.00	18.12 ms
	8	30.05 ms	28.18 ms	36.65 m	33.56 s	134.50	0.90	58.23 ms
	12	178.37 ms	50.09 ms	55.85 m	50.92 s	205.07	0.95	228.46 ms
	16	263.84 ms	96.91 ms	77.77 m	72.97 s	351.75	0.40	360.75 ms
RHCR-Opt	4	40.25 ms	8.82 ms	17.47 m	16.07 s	64.50	1.00	49.07 ms
	8	73.20 ms	23.18 ms	36.34 m	32.97 s	155.18	0.95	96.37 ms
	12	129.34 ms	45.02 ms	54.69 m	49.88 s	242.93	0.95	174.36 ms
	16	188.24 ms	75.43 ms	72.62 m	68.19 s	330.01	0.50	263.68 ms

Based on the above results, RHCR-Opt incurs a certain computational overhead in the front-end planning stage when the swarm size is relatively small, regardless of whether the task involves one-shot global planning or online replanning. However, as the swarm size increases, the computational efficiency of the RHCR-Opt front-end improves significantly. Furthermore, owing to the time-window-based swarm trajectory joint optimization framework employed in the back-end, the computational efficiency of the overall planning process becomes substantially higher. At the same time, RHCR-Opt achieves superior performance in terms of trajectory length, trajectory flight time, and control effort cost. Specifically, the improved LPA* primarily reduces the redundant low-level searches in computationally intensive CBS instances, whereas RHCR mainly decreases the number of conflicts to be resolved and the optimization constraints to be handled during each replanning cycle. Subsequently, the MINCO-based swarm trajectory joint optimization transforms the resulting discrete paths into dynamically feasible trajectories without sacrificing trajectory quality. For small and simple instances, the initialization overhead of LPA* may limit its advantages. However, as the swarm size and conflict density increase, the benefits of search reuse and rolling-horizon processing become increasingly significant.

5.5. Effect of Two-Stage Temporal Coordination Optimization

To address swarm missions that require simultaneous arrival, RHCR-Opt incorporates an additional two-stage temporal coordination optimization method. The effectiveness of the proposed method is evaluated using three metrics: the average flight time, the flight-time range, and the variance of flight times.

5.5.1. Setting

In Figure 17, we design a swarm mission in which all UAVs are required to arrive at a designated target region simultaneously from their initial region. Three different schemes are considered. In Scheme 1, neither the temporal load-balancing-based task assignment method nor the soft-constraint-based temporal optimization method is employed. Instead, each UAV is randomly assigned a target location within the target region. Scheme 2 extends Scheme 1 by incorporating the soft-constraint-based temporal optimization method. Scheme 3 further extends Scheme 2 by additionally introducing the temporal load-balancing-based task assignment method. Under these three schemes, the simultaneous arrival swarm mission illustrated in Figure 17 is executed, and the resulting performance is analyzed and compared.

Figure 17. Flight trajectories of RHCR-Opt under the two-stage temporal coordination optimization method. The gray pillars represent static obstacles, and curves of different colors represent flight trajectories of UAVs.

5.5.2. Results

Table 6 summarizes the performance evaluation results under different temporal coordination optimization methods. The results clearly indicate that the proposed two-stage temporal coordination optimization method substantially improves the simultaneous arrival performance of the swarm. Specifically, the flight-time range, defined as the time difference between the earliest-arriving UAV and the latest-arriving UAV, decreases from 0.941 s to 0.386 s. In addition, the variance of flight times decreases from 0.164 to 0.027.

Table 6. Performance evaluation metrics of RHCR-Opt under different temporal coordination schemes.

Scheme	Avg. Flight Time	Flight Time Range	Flight Time Var
Scheme 1:	12.86 s	0.941 s	0.164
Scheme 2:	12.74 s	0.442 s	0.038
Scheme 3:	12.77 s	0.386 s	0.027

6. Conclusions

Balancing trajectory quality and computational efficiency in UAV swarm trajectory replanning represents a fundamental challenge, particularly as swarm size grows and operating environments become more densely cluttered. In this paper, we have proposed RHCR-Opt, a CBS-based swarm trajectory replanning framework that addresses this challenge through rolling-horizon collision resolution. The RHCR-Opt framework consists of three layers. The first two layers comprise the CBS-based RHCR framework, while the third layer performs MINCO-based trajectory generation and optimization. Within the RHCR framework, we propose an LPA*-based low-level planner that explicitly considers spatiotemporal constraints and integrates it into CBS. At the high level of CBS, conflicts are resolved using a rolling-horizon collision-resolution concept. After multiple conflict-free paths are generated by RHCR, a MINCO-based trajectory generation scheme is employed, and a time-window-based swarm trajectory joint optimization framework is developed. Furthermore, to satisfy simultaneous arrival requirements in swarm missions, a two-stage temporal coordination optimization method is proposed. All the proposed contributions have undergone comprehensive experimental validation and quantitative analysis. The simulation results demonstrate that, on the proposed benchmark, RHCR-Opt achieves superior trajectory quality and higher computational efficiency compared with state-of-the-art methods. In particular, when the swarm size becomes large, the computational efficiency improvement exceeds 23.7%.

RHCR-Opt also has several practical limitations. Although trajectory execution is decentralized among UAVs, conflict resolution is performed by a designated planning UAV; thus, the framework relies on reliable and sufficiently low-latency communication as well as the availability of that planner. Its real-time advantage may decrease when swarm size, conflict density, or environmental change rate causes the planning time to approach the replanning interval. Moreover, the present validation assumes a static environment and accurate state information, and therefore does not establish robustness against communication loss, perception error, dynamic obstacles, or model mismatch. These conditions define the current applicability of RHCR-Opt and motivate future work on hybrid decentralized coordination and uncertainty-aware replanning.