Cooperative Online 3D Path Planning for Fixed-Wing UAVs

Highlights

What are the main findings?

• The proposed Cooperative-3D-Quick-Dubins-RRT* algorithm integrates an offline Dubins motion-primitive database specifically tailored for RRT* and four acceleration strategies, enabling real-time kinodynamically feasible planning for fixed-wing UAVs in complex 3D environments.
• Experimental results show an 86.04% reduction in runtime and 22.63% shorter path lengths compared to conventional Dubins-RRT*, while achieving collision-free trajectories in cluttered terrains and no-fly zones. Hardware-in-the-loop simulations further validate the strong engineering applicability of the proposed method under real onboard execution conditions.

What is the implication of the main finding?

• This study demonstrates that offline primitive and online search can be effectively combined to overcome the computational bottleneck in UAV path planning, enabling online replanning in dynamic scenarios.
• The successful deployment in complex 3D environments with restricted zones provides a scalable solution for multi-UAV cooperative missions, paving the way for autonomous swarm navigation in real-world urban or mountainous terrains.

Abstract

Addressing high dynamics, stringent non-holonomic constraints, and limited onboard computation in cooperative online trajectory planning for multiple fixed-wing UAVs in complex 3D obstacle environments, this paper proposes a Cooperative-3D-Quick-Dubins-RRT*. First, an offline motion-primitive database is engineered to align with RRT* mechanics: an unconstrained expansion mode facilitates rapid space exploration, while a constrained rewiring mode ensures kinodynamic continuity. This architecture, synergized with four targeted acceleration strategies (dimensionality reduction, elliptical sampling, tree pruning, and pre-discretized collision checking), significantly accelerates convergence. Second, a Dubins-detour-based time-coordination mechanism is designed to map cooperative timing constraints into controllable path-length adjustments, and the feasible adjustment range is analyzed to ensure realizability. Finally, simulations and hardware-in-the-loop experiments across a variety of representative scenarios are conducted for validation. The results show that, compared with the classical Dubins-RRT*, the proposed method achieves clear advantages in planning time and path length, demonstrating its suitability for online cooperative obstacle-avoidance planning of multiple UAVs.

1. Introduction

With increasing demands for UAVs in complex missions such as infrastructure inspection and precision agriculture, swarm cooperation has become a key trend in UAV system development. Compared to rotary-wing platforms, fixed-wing UAVs offer significant advantages in endurance, speed, and payload capacity, making them particularly well-suited for large-scale, long-range operations. However, a primary challenge in such applications is the development of an efficient online trajectory planning framework capable of supporting cooperative behaviors.

Unlike 2D scenarios, 3D path planning is considered NP-hard due to the additional vertical axis and the exponential expansion of the configuration space [1,2]. This complexity stems from the significantly larger set of candidate points and segments required to ensure safety in 3D compared to 2D environments [3,4]. Consequently, achieving computationally efficient and reliable trajectory generation in 3D spaces remains a fundamental challenge, particularly for fixed-wing UAVs subject to non-holonomic constraints.

In response, researchers have proposed a wide range of path planning algorithms, which can be broadly categorized as follows.

From an optimization perspective, heuristic techniques have been explored, such as using Genetic Algorithms (GAs) for planning on building surfaces [5] or combining GA with Voronoi diagrams [6] to lower computational costs. Some studies even project 3D space onto 2D manifolds to simplify path generation [7]. Some studies have incorporated the concept of receding horizon control or local optimization, updating trajectories only within a finite time window [8,9]. However, these efficiency-oriented strategies often simplify or overlook the inherent non-holonomic constraints of fixed-wing UAVs, potentially leading to dynamically infeasible commands. Other heuristic approaches, including plant growth mechanisms [10] and hydrodynamic-based fluid algorithms [11,12], have also emerged. Although they significantly enhance solution speed, such methods are typically restricted to regular geometric obstacles and struggles with complex, irregular 3D terrains.

Sampling-based approaches, such as RRT [13] and PRM [14], have demonstrated strong performance in high-dimensional environments due to their probabilistic exploration capabilities. Specifically, Yang and Sukkarieh [15] enhanced 3D RRT efficiency by developing a path pruning algorithm to remove redundant waypoints. To further enhance path quality, reference [16] introduces the Rapidly exploring Random Tree Star (RRT*) algorithm. By randomly distributing sample nodes in the configuration space to capture environmental topological features, RRT* represents connectivity through an evolving tree structure. Unlike the basic RRT, RRT* incorporates a parent selection and rewiring mechanism based on a cost function, which iteratively optimizes the connections of intermediate nodes to ensure the asymptotic optimality of the resultant path [17].

However, classical sampling algorithms often neglect non-holonomic constraints, yielding piecewise linear paths with sharp turns that are difficult for fixed-wing UAVs to track. To enhance flyability, researchers have integrated smoothing curves into sampling frameworks. Reference [18] utilizes B-splines within the RRT framework to improve path smoothness. Reference [19] employs Dubins curves to smooth piecewise linear tracks; however, such “plan-then-smooth” strategies inevitably cause the trajectory to deviate from the original collision-free path, introducing new safety risks. Furthermore, References [20,21] utilize polynomial trajectories to connect waypoints via differential flatness theory. Nevertheless, these methods suffer from soaring computational complexity as the scenario scale increases due to frequent nonlinear programming or spline interpolation, making them unsuitable for real-time obstacle avoidance.

To explicitly account for kinodynamic constraints during planning, Dubins-curve-based approaches have been introduced within kinodynamic frameworks. Existing methods either incorporate Dubins paths as heuristics to guide sampling [22,23] or construct precomputed roadmaps [24]. However, these approaches still rely heavily on repeated online analytical solutions and dense collision checking, which results in significant computational overhead, particularly in complex 3D environments.

Crucially, even if efficient and dynamically feasible trajectory planning can be achieved for a single UAV, extending these methods to multi-UAV cooperative scenarios introduces additional layers of complexity. Multi-UAV cooperative trajectory planning requires not only obstacle avoidance but also explicit handling of inter-agent collision avoidance and coordination constraints. Conflict-based search (CBS) addresses spatiotemporal conflicts through decoupled planning and conflict resolution [25], while optimization-based methods formulate multi-objective coordination problems considering path length, time, and threat exposure [26]. Other approaches focus on decentralized collision avoidance and network-based coordination mechanisms to improve real-time responsiveness [27], or integrate task allocation with trajectory planning for mission-level optimization [28].

Despite these advances, two key challenges remain unresolved. First, kinodynamic RRT*-based planners suffer from prohibitive computational latency caused by repeated online analytical solving during tree expansion, making real-time replanning in complex 3D environments difficult to achieve. Second, for fixed-wing UAVs operating at near-constant cruise speeds, temporal synchronization among multiple agents is difficult to achieve through speed control alone, and path-length modulation remains an insufficiently explored mechanism for cooperative time coordination.

To address both challenges, this paper proposes a distributed cooperative trajectory planning framework for fixed-wing UAV swarms. Unlike existing approaches that rely on repeated online solving of kinodynamic models or post-processing-based trajectory smoothing, the proposed framework fundamentally shifts the computational burden from online analytical computation to offline-constructed motion primitives and efficient index-based matching.

To tackle the computational intractability of direct 3D kinodynamic planning, the framework adopts a hierarchical structure that decomposes the problem into a 2D path planning stage followed by a 3D altitude refinement stage. Within the 2D planning stage, a dual-mode motion primitive database, combined with dimensionality-reduced sampling, an ellipsoidal heuristic domain, a pruning mechanism, and localized collision detection, collectively reduces planning latency while ensuring kinodynamic feasibility. In the 3D refinement stage, gradient-descent-based altitude optimization smooths the vertical profile while preserving the coupling between vertical increments and horizontal arc-lengths to maintain flyability. Furthermore, in contrast to conventional time coordination methods that rely on speed adjustment or scheduling strategies, a Dubins-detour-based mechanism is introduced to achieve path-length-driven temporal synchronization among multiple UAVs.

The synergistic design of these components enables simultaneous improvements in planning efficiency, kinodynamic feasibility, and multi-agent temporal coordination in complex cluttered 3D environments.

The main contributions are summarized as follows:

(1)

Modeling: A fixed-wing UAV kinodynamic model and a Digital Elevation Model (DEM) for realistic terrain are established. A constrained optimization model is developed, incorporating non-holonomic constraints, no-fly zones (NFZs), altitude limits, and spatiotemporal coordination requirements.

(2)

Method: A multi-UAV cooperative online trajectory planning architecture is proposed, featuring a dual-mode offline motion-primitive database (inspired by [29]) tailored to the expansion and rewiring processes of RRT*. Through the integration of sampling space dimensionality reduction, elliptical heuristic sampling, tree pruning, and pre-discretized collision checking, the computational load is shifted from intensive online numerical solving to efficient index matching. Additionally, a time-coordination scheme leveraging Dubins detours is presented to ensure synchronization by determining feasible path-length adjustment intervals.

(3)

Validation: A series of comparative experiments are conducted, including multi-UAV cooperative planning simulations, parameter sensitivity analysis, comparisons with conventional Dubins-RRT*, and hardware-in-the-loop (HIL) simulations.

2. Problem Description

2.1. Scenario Description

As illustrated in Figure 1, a typical cooperative mission for UAV swarms unfolds within a highly non-structured and uncertain environment. In the initial phase, a heterogeneous swarm departs from dispersed start points, navigating through intricate 3D corridors to bypass complex terrain elevations and pre-defined no-fly zones (NFZs). During transit, the system must continuously respond to real-time command updates from the ground station—such as target re-assignment—and react to emergent threats like dynamic radar detection zones.

The core challenge of such missions lies in spatiotemporal synchronization: each individual UAV must not only perform autonomous local obstacle avoidance but also adaptively modulate its flight velocity or path length. This ensures the entire swarm can negotiate cluttered spaces and achieve precise simultaneous arrival at the target within a pre-defined time window. Such mission-driven, high-frequency trajectory replanning necessitates an algorithm capable of providing near-instantaneous decision feedback while strictly adhering to non-holonomic constraints and resolving the strong coupling between spatial maneuvers and temporal coordination.

Consequently, the cooperative online trajectory planning problem is defined as searching for collision-free paths within a 3D space cluttered with terrain elevations, static and dynamic obstacles, and no-fly zones. This must simultaneously satisfy multi-fold constraints: safety (e.g., minimum clearance), kinodynamic (e.g., minimum turning radius, maximum climbing angle), and mission requirements (e.g., time coordination, formation keeping). The primary objective is to achieve reliable single-agent planning, consistent multi-agent coordination, and smooth task transitions under these constraints, thereby enhancing the swarm’s adaptability in complex dynamic scenarios.

2.2. Problem Formulation

2.2.1. Fixed-Wing UAV Motion Model

The motion of fixed-wing UAVs in 3D space is highly nonlinear. Since trajectory planning focuses on path generation rather than low-level tracking control, a simplified 3-degree-of-freedom (3-DOF) bank-to-turn (BTT) model is adopted. The UAV dynamics with control inputs of overload and roll angle are formulated as

$$\left\{\begin{array}{l}{\displaystyle \frac{dx}{dt}}=v\cos \theta \cos \psi \\ {\displaystyle \frac{dy}{dt}}=v\cos \theta \sin \psi \\ {\displaystyle \frac{dz}{dt}}=v\sin \theta \\ {\displaystyle \frac{dv}{dt}}=g\left(N_x-\sin \theta \right)\\ {\displaystyle \frac{d\theta }{dt}}={\displaystyle \frac{g}{v}}\left(N_z\cos \phi -\cos \theta \right)\\ {\displaystyle \frac{d\psi }{dt}}={\displaystyle \frac{g{N}_z\sin \phi }{v\cos \theta }}\end{array}\right.$$

(1)

where $\left(x,y,z\right)$ denotes the UAV position; $v$ is the velocity; $\theta$ and $\psi$ represent the flight path angle and heading angle, respectively; $N_x$ and $N_z$ represent the tangential and normal overloads, respectively; and $\phi$ represents the roll angle.

The proposed method adopts a 3-DOF reduced-order UAV model, which simplifies high-order aerodynamic transients while preserving the essential non-holonomic constraints, including minimum turning radius and heading continuity. These constraints are the primary determinants of feasible trajectory generation for fixed-wing UAVs in three-dimensional environments. Such a modeling approach is commonly employed in real-time motion planning, where computational efficiency is critical. Within a hierarchical flight control architecture, the trajectory planning layer focuses on generating kinodynamically feasible reference paths, whereas low-level controllers handle tracking errors and compensate for dynamic disturbances. While higher-fidelity models may improve accuracy under aggressive maneuvers or highly dynamic conditions, they also introduce substantial computational overhead that is incompatible with real-time replanning requirements. Therefore, the adopted model provides an effective balance between computational tractability and motion feasibility for online path planning tasks in complex environments.

2.2.2. 3D Terrain Model

The Digital Elevation Model (DEM), a fundamental dataset in Geographic Information Systems (GISs), is widely used in mapping, terrain analysis, engineering, and military applications. Given the raster format’s efficiency in querying elevations, this paper adopts it to represent 3D terrain, as shown in Figure 2. A raster DEM comprises elevation grids and georeferencing data, the latter containing the geographic extent and spatial resolution. Ground elevation at any point can be obtained via interpolation methods, such as bilinear or nearest-neighbor interpolation.

Since the DEM is defined in the geodetic (LLA) coordinate system, while trajectory planning is computed in the local North-East-Down (NED) frame, frequent coordinate transformations are required for terrain collision detection, increasing computational overhead. In practice, the mission scale of UAVs is much smaller than that of ballistic missiles or long-range rockets. To simplify the transformation, this paper approximates the OXY plane of the NED frame as the tangent plane at the initial planning point a, effectively treating the local ellipsoid as a flat surface and neglecting the curvature effect on horizontal distances, as illustrated in Figure 3.

Let point b be the point whose elevation is to be calculated. The arc length $\stackrel{⏜}{ab}$ is approximated by the length of segment ${ab}^{\prime }$, and $\Delta D$ denotes the horizontal distance error:

$$\Delta D=a{b}^{\prime }-\stackrel{⏜}{ab}=R\left(\tan \beta -\beta \right)$$

(2)

where $R$ denotes the Earth’s radius and $\beta$ denotes the central angle of $\stackrel{⏜}{ab}$. With $R=6371 \mathrm{km}$, when $D=100 \mathrm{km}$, $\Delta D$ is merely 8.2111 m, which is negligible compared to D. Given the coordinates of point b in the NED frame, its geodetic coordinates can be rapidly approximated as follows:

$$\left\{\begin{array}{l}lo{n}_b=lo{n}_a+\left(x_b/R\cos la{t}_a\right)\times 180/\pi \\ la{t}_b=la{t}_a+y_b/R\times 180/\pi \end{array}\right.$$

(3)

where $\left(ln{g}_a,la{t}_a\right)$ denotes the geodetic coordinates of the initial planning point. After obtaining the geodetic coordinates of point b, the local elevation $h_b$ is calculated via bilinear interpolation:

$$\left\{\begin{array}{l}ln{g}_1=⌊\left(ln{g}_b-ln{g}_o\right)/\Delta lng⌋\Delta lng\\ ln{g}_2=⌈\left(ln{g}_b-ln{g}_o\right)/\Delta lng⌉\Delta lng\\ la{t}_1=⌊\left(la{t}_b-la{t}_0\right)/\Delta lat⌋\Delta lat\\ la{t}_2=⌈\left(la{t}_b-la{t}_0\right)/\Delta lat⌉\Delta lat\\ h_1={\displaystyle \frac{ln{g}_2-ln{g}_b}{\Delta lng}}h\left(ln{g}_1,la{t}_1\right)+{\displaystyle \frac{ln{g}_b-ln{g}_1}{\Delta lng}}h\left(ln{g}_2,la{t}_1\right)\\ h_2={\displaystyle \frac{ln{g}_2-ln{g}_b}{\Delta lng}}h\left(ln{g}_1,la{t}_2\right)+{\displaystyle \frac{ln{g}_b-ln{g}_1}{\Delta lng}}h\left(ln{g}_2,la{t}_2\right)\\ h_b={\displaystyle \frac{la{t}_2-la{t}_b}{\Delta lat}}{h}_1+{\displaystyle \frac{la{t}_b-la{t}_1}{\Delta lat}}{h}_2\end{array}\right.$$

(4)

where $\Delta lng$ and $\Delta lat$ are the spatial resolutions of the DEM in the longitude and latitude directions, respectively; $\left(ln{g}_0,la{t}_0\right)$ is the coordinate of the lower-left vertex of the DEM grid; and $h\left(ln{g}_1,la{t}_1\right)$, $h\left(ln{g}_1,la{t}_2\right)$, $h\left(ln{g}_2,la{t}_1\right)$ and $h\left(ln{g}_2,la{t}_2\right)$ are the elevations of the lower-left, upper-left, lower-right, and upper-right neighboring vertices, respectively. $\left(ln{g}_b,la{t}_b\right)$ denotes the geodetic coordinates of the initial planning point.

2.2.3. Constraints

(1)

Non-holonomic Constraint

Non-holonomic constraints restrict an aircraft’s motion in specific directions; for instance, fixed-wing UAVs cannot move vertically or slide sideways. Some studies [9,10] treat the UAV as a point mass capable of moving in any direction, imposing constraints only on the maximum turning and pitch angles. This approach essentially searches for paths in geometric space and then approximates kinodynamic feasibility via posterior angular constraints. The planning result heavily depends on the search step size: a small step size leads to long computation times, making online application difficult, while a large step size fails to satisfy velocity direction and non-holonomic constraints at the start and goal, resulting in poor path smoothness.

This paper explicitly considers non-holonomic constraints by searching in a high-dimensional state space and utilizing Dubins motion primitives to satisfy the kinodynamic differential equations of the fixed-wing UAV. The aircraft’s state is described in the path coordinate system, and every path point must satisfy Equation (1).

Furthermore, to ensure flight feasibility and safety, the fixed-wing UAV’s state must satisfy the following constraints:

$$\left\{\begin{array}{l}{\theta }_{\min }\le {\theta }_k^i\le {\theta }_{\max }\\ \left|{\phi }_k^i\right|\le {\phi }_{\max }\\ R_k^i\ge R_{\min }, R_{\min }=V^2/(g\tan {\phi }_{\max })\end{array}\right.$$

(5)

where ${\theta }_k^i$, ${\phi }_k^i$ and $R_k^i$ denote the flight path angle, roll angle, and turning radius of the i-th waypoint for UAV k, respectively. ${\theta }_{\min }$ and ${\theta }_{\max }$ are the minimum and maximum flight path angles. Under the zero angle-of-attack assumption, the flight path angle constraint is equivalent to the pitch angle constraint, typically ranging from −30° to 30°. ${\phi }_{\max }$ is the maximum allowable roll angle, typically ranging from 30° to 45°. $R_{\min }$ denotes the minimum turning radius. This paper employs Dubins curves satisfying the minimum turning radius constraint as motion primitives, explicitly guaranteeing this constraint during sampling and planning without the need for posterior validation.

(2)

No-Fly Zone Constraints

To ensure flight safety and safeguard designated airspace, UAVs must avoid various no-fly zones (NFZs) during missions. These typically include airport airspace, military restricted areas, densely populated zones, nature reserves, and areas surrounding critical infrastructure. According to regulations such as the Civil Unmanned Aircraft System Air Traffic Management Measures, NFZs are established to prevent potential threats to civil aviation, public safety, and sensitive facilities. Since UAV flight is prohibited throughout the volume of an NFZ, it is modeled as a three-dimensional obstacle extending from the ground to the UAV’s maximum flight altitude.

Based on geometric characteristics, NFZs are classified into cylindrical and polygonal types.

Cylindrical NFZs typically represent circular areas centered on a specific point, such as airport runways, communication towers, or power facilities. Let the center of the cylindrical NFZ be $\left(x_{\mathrm{center}},y_{\mathrm{center}}\right)$ and the protection radius be $r_{nfz}$. The NFZ is defined as

$$NF{Z}_{\mathrm{circle}}=\left\{\left(x,y,z\right)|\sqrt{{\left(x-x_{\mathrm{center}}\right)}^2+{\left(y-y_{\mathrm{center}}\right)}^2}\le r_{nfz},0\le z\le h_{\max }\right\}$$

(6)

where $h_{\max }$ denotes the UAV’s maximum flight altitude.

Polygonal NFZs are suitable for representing irregular areas, such as urban built-up zones, industrial parks, and nature reserves. Let a polygonal NFZ be defined by n vertices $\left\{q_1,q_2,\cdots ,q_n\right\}$ ordered counterclockwise, where the j-th vertex has coordinates $q_j=\left(x_j,y_j\right)$. The NFZ can be represented as

$$NF{Z}_{\mathrm{polygon}}=\left\{\left(x,y,z\right)|\left(x,y\right)\in Q,0\le z\le h_{\max }\right\}$$

(7)

where $Q$ denotes the polygonal region bounded by the vertex sequence $\left\{q_1,q_2,\cdots ,q_n\right\}$.

Considering all NFZs collectively, let $m$ denote the total number of NFZs in the planning space. Any waypoint $\left\{{\mathit{p}}_1,{\mathit{p}}_2,\cdots {\mathit{p}}_N\right\}$ along the UAV trajectory must satisfy

$${\mathit{p}}_i\notin {\displaystyle \underset{k=1}{\overset{m}{\cup }}NF{Z}_k},\forall i\in \left\{1,2,\dots ,N\right\}$$

(8)

This constraint ensures the UAV remains within authorized airspace throughout the flight, guaranteeing both flight safety and regulatory compliance.

(3)

Flight Altitude Constraints

In low-altitude environments, UAVs must maintain a sufficient safety margin above the ground to account for terrain variations, turbulence, and positioning errors. For missions requiring proximity to the surface, such as emergency response or geological surveying, an appropriate Above-Ground-Level (AGL) altitude ensures both safety and sensor effectiveness. Therefore, the height of any waypoint in the trajectory ${\mathit{x}}_k$ must exceed the minimum clearance altitude $h_{rel}$:

$$h_{ki}\ge h_{\mathrm{terrain}}+h_{rel},k=1,2,\dots ,N,i=1,2,\dots ,M$$

(9)

where $h_{ki}$ denotes the altitude of the i-th waypoint for UAV k, and $h_{\mathrm{terrain}}$ represents the terrain elevation at that point.

Conversely, while higher altitudes improve obstacle clearance, fixed-wing UAVs are subject to aerodynamic and propulsion limitations that define a service ceiling. Hence, the altitude of any waypoint must not exceed this maximum operational limit $h_{\max }$:

$$h_{ki}\le h_{\max },k=1,2,\dots ,N,i=1,2,\dots ,M$$

(10)

(4)

Terminal State Constraints

In cooperative missions, UAVs must typically satisfy specific terminal state constraints upon reaching the target area to facilitate precise operations or formation assembly. For UAV k, the terminal position and attitude must satisfy

$$\left\{\begin{array}{l}{\mathit{p}}_k^{ter}=\left(x_k^{\mathrm{goal}},y_k^{\mathrm{goal}},z_k^{\mathrm{goal}}\right)\\ {\theta }_k^{ter}={\theta }_k^{\mathrm{goal}},{\psi }_k^{ter}={\psi }_k^{\mathrm{goal}},{\phi }_k^{ter}={\phi }_k^{\mathrm{goal}}\end{array}\right.$$

(11)

where ${\mathit{p}}_k^{\mathrm{goal}}$ denotes the desired terminal position, and ${\theta }_k^{\mathrm{goal}}$, ${\psi }_k^{\mathrm{goal}}$, and ${\phi }_k^{\mathrm{goal}}$ represent the desired terminal flight path angle, heading angle, and roll angle, respectively.

(5)

Time Coordination Constraints

Multi-UAV cooperative missions often require strict temporal coordination. For scenarios such as simultaneous multi-point rescue, time-window delivery, or synchronized monitoring, UAVs must arrive at the target area at specified times or with specific time intervals. Let $t_{fk}$ denote the actual arrival time of the k-th UAV; the time coordination constraint is formulated as

$${\displaystyle \sum _k^{N-1}{t}_{fk+1}-t_{fk}-t_{expk}=0},k=1,2,\dots ,N$$

(12)

where $t_{expk}$ represents the desired time difference between UAV k and UAV k + 1. For simultaneous arrival missions, $t_{expk}$ is set to 0.

2.2.4. Objective Function

Let $T=\left({\mathit{x}}_k\left(t\right)\right),k=1,2,\cdots ,N$ denote the set of all UAV trajectories, where ${\mathit{x}}_k\left(t\right)$ represents the trajectory of the k-th UAV. Each waypoint consists of position p, heading angle $\psi$, and flight-path angle $\theta$. To minimize trajectory length and maximize smoothness, the cost function is defined as

$$\begin{array}{l}\min J\left(T\right)={\displaystyle \sum _k^{N}L\left({\mathit{x}}_k\right)}\\ L\left({\mathit{x}}_k\right)={\omega }_{1}Cos{t}_{\mathrm{Length}}+{\omega }_{2}Cos{t}_{\mathrm{Smooth}}\\ Cos{t}_{\mathrm{Length}}={\displaystyle \sum _k^N{\displaystyle \sum _{i=1}^{M_k-1}‖{\mathit{p}}_{k,i+1}-{\mathit{p}}_{k,i}‖}}\\ Cos{t}_{\mathrm{Smooth}}={\displaystyle \sum _k^N{\displaystyle \sum _{i=1}^{M_k-1}\left(\left|{\theta }_{k,i+1}-{\theta }_{k,i}\right|+\left|{\psi }_{k,i+1}-{\psi }_{k,i}\right|\right)}}\end{array}$$

(13)

where ${\omega }_1$ and ${\omega }_2$ are weighting coefficients. $Cos{t}_{\mathrm{Length}}$ denotes the trajectory length cost, and $Cos{t}_{\mathrm{Smooth}}$ denotes the trajectory smoothness cost. $M_k$ is the number of waypoints for the k-th UAV, and $‖{\mathit{p}}_{k,i+1}-{\mathit{p}}_{k,i}‖$ represents the Euclidean distance between consecutive waypoints.

3. Methodology

3.1. Overall Framework of Cooperative Trajectory Planning

The problem formulated in Section 2 imposes three competing demands: kinodynamic feasibility, real-time replanning, and multi-agent consistency. These demands collectively motivate the proposed hierarchical RRT*-based framework illustrated in Figure 4. Dubins curves serve as motion primitives to explicitly satisfy Equation (5) throughout the search process. The framework consists of three components: single-UAV trajectory planning, desired flight time consensus, and Dubins detour path adjustment. Time coordination is achieved via an ad hoc data link, and the theoretical convergence guarantee of the consensus process is detailed in Section 3.4.2.

To address the computational intractability inherent in direct 3D planning while preserving solution quality and real-time performance, a hierarchical planning framework is proposed—decomposing the problem into a 2D topological path planning stage followed by a 3D trajectory refinement stage. Notably, in conventional single-UAV trajectory planning, dynamic interactions with neighboring UAVs are often neglected, which can lead to spatial conflicts, especially in dense swarm operations where proximity and maneuver coupling are critical. The proposed hierarchical structure addresses this limitation by separating the planning process into two sequential stages: the 2D path planning phase focuses on obstacle-aware routing and waypoint sequencing in the horizontal plane, while the 3D refinement stage optimizes the altitude profile subject to kinodynamic feasibility constraints, and leverages temporal phasing to resolve potential inter-agent conflicts. Importantly, kinodynamic coupling between horizontal and vertical motion is preserved throughout—the altitude optimization explicitly accounts for the coupling between vertical increments and horizontal arc-lengths to ensure flyability. By exploiting the vertical dimension as an additional degree of freedom for conflict resolution, the framework enhances the robustness of multi-UAV cooperation under dynamic constraints without compromising flight smoothness or mission timeliness.

Within this architecture, each UAV first independently generates a kinodynamically feasible trajectory using the Quick-Dubins-RRT* algorithm. Flight time information is then exchanged via the ad hoc network to achieve consensus on the group’s desired flight time under strongly connected topology. Subsequently, each UAV adjusts its path length based on the consensus time. If environmental disturbances cause time deviations, the consensus process is re-triggered to enable dynamic adaptive coordination. This framework operates without a central node, constituting a distributed cooperative planning structure.

3.2. Quick-Dubins-RRT* Algorithm Based on Offline Dubins Motion Primitives

Within the aforementioned cooperative framework, individual UAVs must rapidly generate feasible trajectories in complex 3D environments that satisfy the non-holonomic kinodynamic constraints of fixed-wing dynamics. To this end, this paper proposes the Quick-Dubins-RRT* algorithm, which leverages a precomputed Dubins motion primitive database. By pre-generating motion primitives that satisfy minimum turning radius and heading continuity constraints, and embedding them into the RRT* expansion process, the algorithm ensures kinodynamic feasibility during tree generation. Furthermore, the state space is modeled to accommodate 3D flight characteristics, achieving a sequentially integrated combination of 2D Dubins curves and longitudinal motion, where the horizontal path is planned first and the vertical profile is subsequently optimized subject to kinodynamic coupling constraints.

3.2.1. Motion-Primitives-RRT*

The Motion-Primitives-RRT* was proposed in [29] to address kinodynamic path planning problems for complex nonlinear systems. Inspired by graph-search methods for state space discretization, it solves Two-Point Boundary Value Problems (TPBVPs) using a precomputed motion primitive database, achieving a favorable balance between real-time computational efficiency and solution quality, along with strong generality. The algorithm follows the classic RRT* framework, with the specific procedure outlined in Algorithm 1:

Algorithm 1. Motion-Primitives-RRT* Algorithm

Input: initial state ${\mathit{x}}_0$, terminal state ${\mathit{x}}_f$, motion primitives database, $ite{r}_{\max }$

Output: trajectory $\mathit{x}\left(t\right)$

$V\leftarrow \left\{{\mathit{x}}_0\right\}$; $E\leftarrow Ø$

$\mathbf{for} i=1 \mathrm{to} ite{r}_{\max } \mathbf{do}$

${\mathit{x}}_{\mathrm{rand}}\leftarrow \mathrm{Sample}\left(\right)$

${\mathit{x}}_{\mathrm{nearest}}\leftarrow \mathrm{Nearest}\left(V,{\mathit{x}}_{\mathrm{rand}}\right)$

${\mathit{x}}_{\mathrm{new}},\mathit{p}\leftarrow \mathrm{Steer}\left({\mathit{x}}_{\mathrm{nearest}},{\mathit{x}}_{\mathrm{rand}},\mathrm{DataBase}\right)$

$\mathbf{if} \mathrm{CollisionFree}\left(\mathit{p}\right) \mathbf{then}$

$N_{\mathrm{near}}\leftarrow \mathrm{Near}\left(V,{\mathit{x}}_{\mathrm{new}},\mathrm{DataBase}\right)$

${\mathit{x}}_{\mathrm{parent}}\leftarrow \mathrm{ChooseBestParent}\left(V,N_{\mathrm{near}},\mathrm{DataBase}\right)$

$V\leftarrow V\cup \left\{{\mathit{x}}_{\mathrm{new}}\right\}$;$E\leftarrow E\cup \left\{\mathit{p}\right\}$

$E\leftarrow \mathrm{Rewire}\left(E,N_{\mathrm{near}},\mathrm{DataBase}\right)$

$\mathbf{endif}$

$\mathbf{endfor}$

$\mathit{x}\left(t\right)=\mathrm{FindWayBack}\left(V,E\right)$

During the RRT* tree expansion, the algorithm first generates random sampling within the state space. Unlike the classic RRT, which samples from a continuous state space, Motion-Primitives-RRT* samples from a discretized state space based on the resolution of the motion primitive database. The algorithm then finds the nearest node ${\mathit{x}}_{\mathrm{nearest}}$ in the tree to the sample ${\mathit{x}}_{\mathrm{rand}}$ and queries the database to compute the optimal path p and the corresponding new node ${\mathit{x}}_{\mathrm{new}}$. If p is collision-free, the algorithm identifies neighboring nodes $N_{\mathrm{near}}$ within the database range, selects the optimal parent from them, and adds the new node ${\mathit{x}}_{\mathrm{new}}$ and path p to the tree. Subsequently, it rewires the neighbors $N_{\mathrm{near}}$: if connecting a neighbor to the new node ${\mathit{x}}_{\mathrm{new}}$ yields a lower cost, the edge set E is updated.

3.2.2. Dubins Motion Primitive Database

The motion primitive database is built upon the 3D Dubins path model to ensure kinodynamic feasibility. To achieve high-throughput online indexing while minimizing onboard storage overhead, a two-stage optimization strategy is implemented.

First, considering that a fixed-wing UAV in 3D space is characterized by five state variables $\left(x,y,z,\theta ,\psi \right)$, a direct offline sampling of this 5D space would trigger the “curse of dimensionality”, leading to prohibitive computational costs and excessive memory requirements. To mitigate this, the longitudinal motion is decoupled from the horizontal components during database construction. The database exclusively stores 2D horizontal Dubins paths, while the longitudinal motion is approximated using a bilinear interpolant model [19,24] during path extraction.

Building upon this dimensionality reduction, the primitive types are specifically pruned. Although conventional Dubins paths consist of six types (RLR, LRL, RSL, LSR, RSR, LSL), long-range fixed-wing missions rarely necessitate the “tight-turn” characteristics of RLR and LRL. By excluding these complex types, the database size is further compressed, and unnecessary branching evaluations during the tree expansion process are eliminated.

The resulting database is constructed offline in MATLAB and stored in a structured binary format. Each entry in the database matrix corresponds to a set of primitives sharing the same terminal grid position, and is represented by a structure containing the initial heading, terminal heading, the discretized 2D Dubins path, and its associated cost.

To demonstrate the time efficiency of the database,

Table 1. Comparison of computational time for different trajectory generation methods.

Calculation Method	Motion Primitive Database	3D Dubins	GPOPSII
Average time (ms)	0.0208	0.29913	3268.431
Minimum time (ms)	0.0120	0.2355	1870.385
Maximum time (ms)	0.02981	16.6679	14,432.608

compares the average computation time for 1000 path queries using three methods: database retrieval, 3D Dubins calculation, and GPOSII. The tests were conducted in Matlab 2022b on an AMD R5 5600U CPU. The results show that database retrieval is an order of magnitude faster than real-time 3D Dubins calculation. In contrast, GPOSII takes over 1 s per calculation and may fail to find a solution, making it unsuitable for online path planning.

3.3. Search Efficiency Enhancement Strategies

To improve online planning efficiency and stability, a set of coordinated acceleration and structural optimization strategies is developed. Rather than being independent heuristic components, these strategies are jointly designed to reformulate the 3D kinodynamic planning process into a more structured and computationally efficient pipeline. Specifically, the sampling space dimensionality reduction (Section 3.3.1) and the ellipsoidal predictive domain (Section 3.3.2) act as complementary spatial filtering mechanisms, effectively constraining the search space and concentrating sampling in high-value regions. This focused exploration facilitates faster convergence toward the goal. In parallel, the dual-mode motion primitive database, cost-constrained pruning (Section 3.3.3), and efficient collision checking (Section 3.3.4) provide computational and structural support for rapid node expansion within the refined space. Through this coordinated design, the proposed strategies form an integrated framework that improves search efficiency while maintaining solution quality.

3.3.1. Sampling Space Dimensionality Reduction

The primary cause of slow convergence in Kinodynamic RRT* is the high dimensionality of the sampling space [30], as the algorithm samples not only positions in Euclidean space but also other state variables such as heading and flight-path angles.

To overcome this, the proposed method leverages the dual-mode primitive framework to implement a multi-layered acceleration strategy. First, during database construction, the longitudinal motion is separated from the horizontal Dubins path and handled independently via a decoupled interpolation model, which significantly reduces the dimensionality of the offline storage space and eases the online sampling burden. Second, by specifically pruning the primitive types (excluding RLR and LRL types unnecessary for long-range missions), the database becomes more compact, enhancing retrieval speed.

To further compress the database size and enhance search efficiency without sacrificing terminal precision, a dual-mode strategy is introduced to regulate the interaction between the RRT* tree and the database: During the expansion phase, the algorithm prioritizes spatial permeability and reachability. By invoking a free-terminal sub-database (Free Mode), the solver ignores specific heading constraints at the sampled nodes. This allows the search tree to permeate through complex environments using the most direct and computationally efficient primitives, significantly accelerating the discovery of an initial feasible solution. Subsequently, during the rewiring phase, the algorithm switches to a fixed-terminal sub-database (Fixed Mode). This ensures that during local topological optimization, newly generated connections strictly satisfy heading continuity and non-holonomic constraints. This dual-mode mechanism ensures that the final trajectory is kinodynamically executable while significantly reducing the dependency on large-scale offline storage, making the online search process both lightweight and agile.

As illustrated in Figure 5, the orientation derived from this optimized primitive aligns more naturally with the local path geometry than a random assignment. This mechanism effectively collapses the active search space from 5D to 3D to bypass the curse of dimensionality, while ensuring that discarded angular constraints are progressively re-satisfied during the Fixed Mode rewiring to maintain global kinodynamic consistency.

3.3.2. Elliptical Prediction Domain

Informed-RRT*, proposed by Jonathan et al. [31], improves upon the traditional RRT* by accelerating convergence through sampling restriction. Instead of sampling the entire space, it limits sampling to a region likely to contain superior paths. After finding the first feasible path, the sampling space is constrained within an ellipse (or ellipsoid in 3D) focused on the start and goal points.

To quantify the sampling efficiency, we define the ellipsoidal heuristic subspace ${\mathcal{X}}_{sub}\subseteq {\mathcal{X}}_{\mathrm{free} }$ as the set of all points satisfying the admissible cost constraint. Its Lebesgue measure, denoted by $\mu \left({\mathcal{X}}_{sub}\right)$, represents the volume of the viable sampling region. In a $d$-dimensional Euclidean space (where $d=3$ in this study), the volume of this ellipsoidal hyper-body is given by

$$\mu \left({\mathcal{X}}_{sub}\right)=v_d\cdot a\cdot b^{d-1}$$

(14)

where $v_d$ is the volume of a unit $d$-ball, while $a$ and $b$ represent the semi-major axis and the identical semi-minor axes, respectively. According to the geometric properties of a prolate hyperspheroid, these axes are determined by the current best path cost $c_{best}$ and the theoretical minimum cost $c_{\min }$ (the Euclidean distance between the start and goal):

$$a={\displaystyle \frac{c_{best}}{2}}, b={\displaystyle \frac{\sqrt{c_{best}^2-c_{\min }^2}}{2}}$$

(15)

By substituting these geometric relations into the volume formula, the functional relationship between the sampling measure and the current solution cost can be derived as

$$\mu \left({\mathcal{X}}_{sub}\right)={\displaystyle \frac{v_d}{2^d}}\cdot c_{best}\cdot {\left(c_{best}^2-c_{\min }^2\right)}^{{\displaystyle \frac{d-1}{2}}}$$

(16)

It is evident from this equation that as the algorithm iterates and $c_{best}$ approaches $c_{\min }$, the term ($c_{best}^2-c_{\min }^2$) tends to zero, leading to a rapid contraction of the sampling domain. This reflects the progressive focusing of the sampling distribution from the global configuration space toward a narrow region surrounding the optimal solution.

In the limit,

$$\underset{c_{best}\to c_{\min }}{\lim }{\displaystyle \frac{\mu \left({\mathcal{X}}_{sub}\right)}{\mu \left({\mathcal{X}}_{free}\right)}}=0$$

(17)

where ${\mathcal{X}}_{free}$ denotes the entire obstacle-free configuration space. This contraction increases the probability of generating samples that can further improve the current solution, thereby empirically accelerating the convergence of the algorithm.

In this work, the semi-axis lengths of the ellipsoid are calculated as follows:

$$\left\{\begin{array}{l}a=0.5l\\ b=\sqrt{a^2-{\left(0.5\left|{\mathit{x}}_s-{\mathit{x}}_g\right|\right)}^2}\\ r=0.5\sqrt{{\left({\displaystyle \sum _{i=1}^{n-1}\left|h_{i+1}-h_i\right|}\right)}^2+{\left({\displaystyle \sum _{i=2}^{n-1}\left|h_{i+1}-2h_i+h_{i-1}\right|}\right)}^2-{\left(h_s-h_g\right)}^2}\end{array}\right.$$

(18)

where l is the path length, h is the height of the i-th waypoint, and $h_s$ and $h_g$ are the heights of the start and goal points, respectively. The sampling procedure incorporating the elliptical prediction domain is outlined in Algorithm 2:

Algorithm 2. Informed-Sample

Input: start point ${\mathit{x}}_s$, end point ${\mathit{x}}_g$, the lengths of the three axes $a$, $b$, $r$

Output: sample point ${\mathit{x}}_{\mathrm{rand}}$

$\mathbf{if} a<\infty \mathbf{then}$

${\mathit{x}}_{\mathrm{ball}}\leftarrow \mathrm{SampleUnitBall}$

$\mathbf{L}\leftarrow \mathrm{diag}\left\{a,b,r\right\}$

$\mathbf{C}\leftarrow \mathrm{RotationMatrix}\left({\mathit{x}}_s,{\mathit{x}}_g\right)$

${\mathit{x}}_{\mathrm{center}}\leftarrow \left({\mathit{x}}_s+{\mathit{x}}_g\right)/2$

${\mathit{x}}_{\mathrm{rand}}\leftarrow \left(\mathbf{CL}{\mathit{x}}_{\mathrm{ball}}+{\mathit{x}}_{\mathrm{center}}\right)\cap \mathrm{MapRange}$

$\mathbf{else}$

${\mathit{x}}_{\mathrm{rand}}\leftarrow \mathrm{Sample}\left(\mathrm{MapRange}\right)$

$\mathbf{endif}$

${\mathit{x}}_{\mathrm{rand}}\leftarrow \mathrm{Rasterize}\left({\mathit{x}}_{\mathrm{rand}}\right)$

When sampling within the elliptical domain, a point ${\mathit{x}}_{\mathrm{ball}}$ is first sampled from a unit hypersphere. A rotation matrix L is then computed based on the direction from the start to the goal. Finally, ${\mathit{x}}_{\mathrm{ball}}$ is linearly transformed to obtain the sample point. Since the state space is discrete, the sampled point is gridded according to the motion primitive database resolution before output.

3.3.3. Pruning Strategy

Pruning the RRT* tree accelerates neighborhood searches and reduces redundant parent reselection and rewiring operations, thus decreasing runtime. During parent reselection and rewiring, nodes often exhibit a “ray-shaped clustering” phenomenon, as shown in Figure 6, where numerous nodes share the same parent and have similar headings, differing only slightly in path length. These ray-like paths, originating from a common parent, exhibit high overlap. Consequently, during rewiring, it is difficult to find superior alternatives, resulting in significant computational waste in the later stages of iteration.

Based on the above analysis, the proposed pruning strategy is defined as follows: After expanding a new node ${\mathit{x}}_{\mathrm{new}}$, a pruning neighborhood is established centered at ${\mathit{x}}_{\mathrm{new}}$, defined by an ellipsoid with principal axes $2{\delta }_s$, ${\delta }_s$, and ${\delta }_s$, where ${\delta }_s$ is the pruning radius coefficient. A larger ${\delta }_s$ removes more nodes but may degrade path quality if set excessively. For each neighboring node ${\mathit{x}}_{\mathrm{near}}\in N_{\mathrm{near}}$ within this pruning region, a pruning test is conducted. Let h denote the heuristic cost to the goal ${\mathit{x}}_g$, approximated by the Euclidean distance. If the cost from the start to ${\mathit{x}}_{\mathrm{new}}$ plus the heuristic cost of ${\mathit{x}}_{\mathrm{new}}$ is less than that of ${\mathit{x}}_{\mathrm{near}}$, ${\mathit{x}}_{\mathrm{new}}$ is considered superior. A flag indicates node status; if set to false, the node is pruned and excluded from subsequent neighborhood computations.

3.3.4. Improved Collision Detection

Collision detection significantly impacts the computational speed of obstacle-avoidance path planning. Conventional methods involve traversing every waypoint and checking all regions, resulting in numerous redundant operations. To reduce the number of checks, this paper proposes a strategy where, upon expanding a new node, the distances from this node to all no-fly and threat zones are calculated. Obstacles within the range of the motion primitive database are marked as potential risks. During collision checks for a path from a node to its successor, only the marked obstacles are verified. Since the number of potential obstacles is typically much smaller than the total in the map, this strategy effectively improves detection efficiency. For distance computation, threat zones are modeled as hemispheres, and polygonal no-fly zones are approximated as cylinders centered at their centroid with a radius equal to the maximum distance from the centroid to any vertex.

3.3.5. Computational Complexity and Efficiency Analysis

To provide a theoretical foundation for the observed efficiency gains, this section analyzes the computational complexity of the proposed Quick-Dubins-RRT* framework from both algorithmic and system perspectives.

The total computational cost per iteration of the proposed Quick-Dubins-RRT* can be decomposed into three main components: nearest-neighbor search, steering (trajectory generation), and collision validation:

$$T_{iter}\approx T_{\mathrm{nearest}}+T_{\mathrm{steer}}+T_{\mathrm{collision}}$$

(19)

where $T_{\mathrm{nearest} }$ represents the nearest-neighbor identification. $T_{\mathrm{steer} }$ represents the trajectory generation. $T_{\mathrm{collision}}$ refers to the obstacle-free validation.

The Steer procedure in conventional kinodynamic RRT* planners is typically the most computationally intensive component, requiring the online solution of a Two-Point Boundary Value Problem (TPBVP). For conventional Dubins-RRT*, the need to evaluate six candidate types and discretize the resulting curves online leads to a computational cost $T_{\mathrm{solve}}$ that scales with the complexity of the vehicle’s differential constraints. In this work, we achieve a fundamental acceleration by shifting this burden to a precomputed framework. To ensure the database is both kinodynamically complete and physically feasible for onboard systems, a two-stage manifold compression and indexing strategy is implemented:

First, a canonical state-space transformation is applied: Given any start pose and sampled goal pose, we first apply a coordinate translation and rotation to normalize the relative transformation into a canonical frame. This reduces the mapping complexity from a prohibitive global state space $O\left(X^2{Y}^2{\Phi }^2\right)$ to a localized relative manifold $O\left(XY\Phi \right)$, effectively collapsing the dimensionality of the required storage.

Furthermore, a Symmetry-based Manifold Compression is implemented. Leveraging the geometric symmetry of fixed-wing UAV flight dynamics, the database only stores motion primitives for the first quadrant of the horizontal plane. Any query in the other three quadrants is mapped to this primary manifold through a sequence of reflection and rotation operators $\mathcal{I}$:

$$\left[x^{\prime },y^{\prime },{\psi }_s^{\prime },{\psi }_g^{\prime }\right]=\mathcal{I}\left(x_{rel},y_{rel},{\psi }_s,{\psi }_g\right)$$

(20)

where $\left[x^{\prime },y^{\prime },{\psi }_s^{\prime },{\psi }_g^{\prime }\right]$ is the transformed query key used to index the database, ensuring that the retrieved path geometry is consistent with the original relative pose after an inverse symmetric transformation. $\left(x_{rel},y_{rel}\right)$ denotes the relative displacement of the goal node with respect to the start node in the horizontal plane. ${\psi }_s$ and ${\psi }_g$ represent the initial heading of the start node and the terminal heading of the goal node, respectively.

This strategy effectively compresses the physical storage requirement to approximately 25% of the original manifold size. Since the mapping $\mathcal{I}$ consists solely of elementary arithmetic and logic operations, it introduces negligible computational overhead, thereby maintaining the $O\left(1\right)$ complexity of the steering process.

The computational complexity of the collision validation procedure, denoted as $T_{\mathrm{collision}}$, is synthesized from two decoupled stochastic and physical processes: spatial search reduction and discretized evaluation. In a standard implementation, every waypoint on a candidate path must be checked against the global obstacle set $O$ with cardinality $M$.

The proposed method employs a spatial decoupling strategy that restricts the search to an active obstacle subset $O_{\mathrm{local}}\subset O$, where $\left|O_{\mathrm{local}}\right|=m\ll M$. This subset is dynamically determined by the bounding box of the current motion primitive. By defining $\sigma$ as the local obstacle density and $V_p$ as the spatial volume swept by the primitive, the number of obstacles $m$ to be considered is

$$m\approx \sigma \cdot V_p$$

(21)

Beyond the reduced search space, the actual computational effort is determined by the point-wise evaluation logic along the trajectory. As the path is discretized into a sequence of waypoints, the number of evaluation points is intrinsically proportional to the path length $L_{\mathrm{path}}$. By introducing the adaptive step parameter $K$, the algorithm performs only $L_{\mathrm{path}}/K$ discrete checks. Consequently, the expected complexity of the validation procedure is formulated as

$$T_{\mathrm{collision}}=O\left(m\cdot {\displaystyle \frac{L_{\mathrm{path} }}{K}}\right)$$

(22)

where $m$ represents the count of obstacles in the localized mission space, significantly lower than the global count $M$ used in conventional Dubins-RRT*.

From a theoretical perspective, the asymptotic computational complexity of RRT* is primarily governed by nearest-neighbor and neighborhood queries, which can be performed in $O\left(\log n\right)$ time using efficient spatial data structures. Therefore, the overall complexity of constructing a tree with n nodes is $O\left(n\log n\right)$, as established in [16]. In the proposed framework, the dominant per-iteration cost is effectively shifted away from steering and collision validation toward nearest-neighbor operations. Specifically, $T_{\mathrm{steer}}$ is reduced to near-constant time via database retrieval, and $T_{\mathrm{collision}}$ is significantly reduced through spatial filtering and adaptive discretization, which are either bounded or efficiently approximated. Consequently, the per-iteration complexity is dominated by $T_{\mathrm{nearest}}$, which remains $O\left(\log n\right)$, and the overall asymptotic complexity of the algorithm preserves the $O\left(n\log n\right)$ structure of RRT*.

Based on the aforementioned theoretical derivations, the comparison of computational characteristics among different RRT*-based planners is summarized in

Table 2. Comparison of computational characteristics among different RRT*-based planners.

Procedure	Standard RRT* [16]	Conventional Dubins-RRT*	Proposed Quick-Dubins-RRT*
Search (nearest-neighbor query)	$O\left(\log n\right)$	$O\left(\log n\right)$	$O\left(\log n\right)$ (dominant cost)
Steer	Simple straight-line interpolation (low cost)	Online TPBVP solving (computationally expensive)	Database retrieval (near-constant time)
Sample	Uniform random sampling	Uniform random sampling	Reduced sampling space (guided + constrained)
Obstacle Free	Global collision checking (high cost)	Discretized Dubins curve checking	Localized subset + adaptive discretization
Storage	Minimal	Minimal	Precomputed motion primitive database (compressed)

.

As shown in Table 2, while all methods share a similar asymptotic complexity dominated by nearest-neighbor search, their practical performance differs significantly due to the computational cost of steering and collision validation. In standard RRT*, the steering operation is computationally inexpensive, but the method does not explicitly account for kinodynamic constraints. In contrast, conventional Dubins-RRT* incorporates non-holonomic constraints at the cost of expensive online TPBVP solving and dense collision checking along curved trajectories. The proposed Quick-Dubins-RRT* addresses these limitations by replacing the online steering process with efficient database retrieval and restricting collision checking to a localized obstacle subset with adaptive discretization. As a result, the dominant computational burden is effectively shifted to nearest-neighbor queries, while the costs associated with trajectory generation and collision validation are significantly reduced. This coordinated design leads to a substantial improvement in practical efficiency, even though the asymptotic complexity remains unchanged.

It is worth noting that the near-constant-time steering complexity discussed above primarily applies to the tree expansion phase, where rapid sampling is critical. In the subsequent altitude optimization stage (Section 3.4.1), limited online motion-primitive reconstruction is performed. However, this process is confined to a sparse set of nodes along the final solution path, and therefore does not affect the overall computational efficiency or asymptotic complexity of the proposed method.

3.4. Altitude Optimization and Temporal Coordination Mechanism

After obtaining a kinodynamically feasible path, a path optimization and time coordination mechanism is designed to enhance path executability and satisfy temporal consistency constraints in multi-fixed-wing UAV cooperative missions. First, gradient-based optimization is employed to smooth the path altitude, improving flight stability. Second, a Dubins-detour-based time adjustment strategy is introduced. By controlling the detour radius and number of loops without compromising kinodynamic feasibility, this strategy enables flexible regulation of flight time.

3.4.1. Altitude Optimization Based on Gradient Descent

Sampled 3D paths may exhibit excessive altitude fluctuations, so gradient descent is employed to optimize the path altitude. The core idea is to adjust the altitude of each waypoint along the gradient direction, defined by the sum of a smoothing gradient $f_c$ and a terrain repulsion gradient $f_r$:

$$\Delta z_i=\alpha \left(f_c+f_r\right)$$

(23)

where $\alpha$ is the learning rate.

The smoothing gradient is defined as

$$f_c=k_c\left({\displaystyle \frac{z_{i-1}-z_i}{d_{i-1}}}+{\displaystyle \frac{z_{i+1}-z_i}{d_i}}\right)$$

(24)

where $z_i$ is the altitude of waypoint i, $d_i$ is the horizontal distance between waypoint i and i + 1, and $k_c$ is the smoothing coefficient.

To ensure the minimum ground clearance, the terrain repulsion gradient is defined as

$$f_r=\left\{\begin{array}{l}{k}_r\left(z_i-h{t}_i-d_{\mathrm{safe}}\right),z_i<h{t}_i+d_{\mathrm{safe}}\\ 0,z_i\ge h{t}_i+d_{\mathrm{safe}}\end{array}\right.$$

(25)

where $h{t}_i$ is the local terrain elevation at waypoint i and $d_{\mathrm{safe}}$ is the safety distance. Multiple iterations effectively reduce unnecessary altitude variations, as shown in Figure 7.

Although the altitude optimization in Equations (23)–(25) is performed on the z-axis, the kinodynamic feasibility regarding the maximum flight path angle ${\theta }_{\max }$ is strictly maintained through a primitive-constrained update mechanism.

During each gradient descent iteration, to ensure a stable and incremental deformation of the trajectory, a conservative gradient step size $\alpha$($\alpha =0.1$) is selected, which prevents the optimization from overshooting the feasible kinodynamic manifold in a single iteration. And the candidate altitude $z_i^{(k+1)}=z_i^{(k)}+\alpha \nabla J$ is not directly committed. Instead, it is passed to the 3D Dubins generator as a boundary condition. As defined in our motion primitive logic, the generator explicitly evaluates the coupling between the vertical increment $\Delta z$ and the horizontal arc-length $L_{2D}$. The update is accepted if and only if

$$|\Delta z|\le L_{2D}\cdot \tan \left({\theta }_{\max }\right)$$

(26)

If the gradient step $\alpha$ attempts to push the waypoint beyond this kinodynamic manifold, the segment is flagged as infeasible and the update is discarded. This ensures that the smoothing process converges to a profile that is inherently flyable for fixed-wing UAVs, effectively preventing excessive climb or dive rates.

3.4.2. Dubins Detour Strategy for Temporal Coordination

Common temporal coordination constraints in UAV swarm missions include simultaneous arrival and sequential arrival. The prerequisite for achieving these constraints is a consistent time reference across the distributed nodes. Fundamentally, this distributed coordination is a consensus-seeking process over a networked multi-agent system.

According to the foundational framework established by Olfati-Saber et al. [32], the estimated time of arrival (ETA) ${\tau }_k$ is expected to converge to a synchronized value if the communication graph $\mathcal{G}$ is connected. The existence of a simple zero eigenvalue of the graph Laplacian $L$ provides a sufficient condition for consensus in continuous-time settings.

To account for practical communication constraints such as discrete-time updates, switching topologies, and intermittent packet loss, we further refer to the results of Ren and Beard [33]. Their analysis indicates that consensus can still be achieved if the union of the interaction graphs over a bounded time interval $[t,t+T]$ contains a spanning tree. Under the adopted broadcast protocol, where each UAV periodically transmits its state at a fixed frequency, the time-accumulated communication graph is expected to satisfy this condition in practice, thereby supporting the convergence of the distributed time coordination process.

Based on this theoretically stable consensus, the specific coordination parameters are calculated as follows:

Let $v_k$ denote the flight speed of UAV k, $le{n}_k$ the length of its initial path, and $\Delta le{n}_k$ the detour length introduced by the Dubins maneuver. For simultaneous arrival, $\Delta le{n}_k$ is calculated as

$$\Delta le{n}_k=t_{\max }{v}_k-le{n}_k,k=1,2,\dots ,N$$

(27)

where $t_{\max }$ is the estimated flight time of the slowest UAV in the group.

For sequential arrival, the arrival time of the first UAV, $t_{\mathrm{first}}$, is determined first. Each UAV then calculates its estimated time based on $t_{\mathrm{first}}$. Assuming the arrival sequence is pre-defined and the time interval for UAV k relative to the first UAV is $t_{dk}$, $t_{\mathrm{first}}$ is calculated as

$$\begin{array}{l}{t}_a=\min \left(t_j+t_{dk}-le{n}_k/v_k\right),k=1,2,\dots ,N\\ t_{first}=\left\{\begin{array}{l}{t}_j,t_{dif\min }\ge 0\\ t_j-t_{dif\min },t_{dif\min }<0\end{array}\right.\end{array}$$

(28)

where $t_j$ denotes the flight time of the UAV designated to arrive first, and $t_{dif\min }$ represents the minimum difference between the expected and estimated arrival times when setting $t_j$ as $t_{\mathrm{first}}$. Once $t_{\mathrm{first}}$ is determined, $\Delta le{n}_k$ is calculated as

$$\Delta le{n}_k=\left(t_{first}+t_{dk}\right)/v_k-le{n}_k$$

(29)

After obtaining the required detour length $\Delta le{n}_k$, the path is adjusted via lateral or holding maneuvers. The adjustment first modifies the straight segments of the 2D Dubins path to tune the range, followed by kinodynamic extension to 3D space. When UAV k performs a detour on the i-th Dubins segment, the adjustment length $\Delta L$ of the straight segment is calculated as

$$\Delta L=\sqrt{{\left(le{n}_i+\Delta le{n}_k\right)}^2-{\left(z_E-z_S\right)}^2}-\sqrt{{le{n}_i}^2-{\left(z_E-z_S\right)}^2}$$

(30)

where $le{n}_i$ is the length of the i-th 3D Dubins path. Based on the value of $\Delta L$, the detour strategy is determined.

This paper employs two strategies for the straight segments: holding detours and lateral maneuvers.

Holding detours are suitable for cases where $\Delta L\ge 2\mathrm{\pi }{r}_{\min }$, with no upper limit on path length adjustment. The number of holding loops and turning radius are calculated as

$$n=⌊\Delta L/\left(2\mathrm{\pi }{r}_{\min }\right)⌋$$

(31)

$$R=\Delta L/\left(2\mathrm{\pi }n\right)$$

(32)

Lateral maneuvers are employed for smaller $\Delta L$. To analyze the adjustable range of path length for lateral maneuvers, a coordinate system is established as shown in Figure 8, where the straight segment of the Dubins path coincides with the OX axis and its midpoint is the origin. Let R be the turning radius during the detour, 2a the length of the straight segment, and $B\left(0,b\right)$ the coordinates of the detour point.

The lateral maneuver path is symmetric about the OY axis; hence, only the right half is analyzed. Let $s_1$ and $s_2$ be the endpoints of the straight segment of the right half. With the construction lines shown in Figure 8, the length $L_1$ of segment $s_1{s}_2$ is

$$L_1=\sqrt{a^2+{\left(b-2R\right)}^2-4R^2}=\sqrt{a^2+b^2-4bR}$$

(33)

The length $L_2$ of the arc $\stackrel{⏜}{B{s}_1}$ is

$$L_2=R\left(\arctan \left({\displaystyle \frac{2R}{L_1}}\right)+\arctan \left({\displaystyle \frac{b-2R}{a}}\right)\right)$$

(34)

Thus, the total detour length is $L_{\mathrm{total}}=2L_1+4L_2$.

To ensure $L_1$ is real, $a^2+b^2-4bR>0$. Let $f\left(b\right)=a^2+b^2-4bR$; $f\left(b\right)$ reaches its minimum $a^2-4R^2$ at $b=2R$. Therefore, when $a>2R$, lateral maneuvers can achieve detours of any length. When $a<2R$, $f\left(b\right)$ has two zeros:

$$b_0=2R\pm \sqrt{4R^2-a^2}$$

(35)

In this case, b is only valid within the interval $\left[0,2R-\sqrt{4R^2-a^2}\right]$. When $b=2R-\sqrt{4R^2-a^2}$, $L_1=0$, and the detour path consists of three circular arcs with a total length of

$$L_{\mathrm{totalmax}}=4R\left({\displaystyle \frac{\pi }{2}}-\arctan \left({\displaystyle \frac{\sqrt{4R^2-a^2}}{a}}\right)\right)$$

(36)

Hence, for $a<2R$, the adjustable path length range is $\left[0,L_{\mathrm{totalmax}}-2a\right]$. Since the detour length depends monotonically on both b and R, fixing one parameter allows us to solve for the other via binary search to satisfy $\Delta L=L_{\mathrm{total}}-2a$. For long detours, the UAV must perform this maneuver over multiple Dubins path segments.

To complement the geometric construction of the detour strategies described above, we further provide a qualitative analysis of the convergence behavior of the temporal coordination process.

Define the path length mismatch for UAV $k$ at iteration $t$ as

$$e_k(t)=\left|L_k\left(t\right)-L_{\mathrm{target}}\right|$$

(37)

where $L_k\left(t\right)$ denotes the cumulative path length after detour adjustments, and $L_{\mathrm{target}}$ is the synchronized reference path length determined by the coordination strategy.

In the proposed adjustment framework, the total required compensation is iteratively distributed across multiple Dubins segments. The detour length increment $\Delta L_k^i$ generated on each Dubins segment (denoted as $\Delta L$ in the previous derivations) is interpreted here as an incremental adjustment in an iterative error reduction process. For each segment, a feasible detour $\Delta L_k^i$ leads to the update:

$$e_k\left(t+1\right)=\left|e_k-\Delta L_k^i\right|$$

(38)

Since the detour length is a monotonic function of the geometric parameters (as established in the preceding derivations), each feasible adjustment contributes to reducing the path length mismatch. Although feasible detours may not exist for every segment, due to environmental constraints, the iterative allocation mechanism ensures that the error is progressively reduced over the trajectory. This provides a practical convergence indication for the temporal coordination process, linking the geometric feasibility of the detour strategies with the overall synchronization objective.

To ensure that the insertion of detour maneuvers does not compromise flight safety, a Safety-Embedded Verification mechanism is integrated into the coordination process.

For each candidate detour generated by adjusting geometric parameters, the algorithm explicitly performs a collision validation before acceptance. Specifically, let ${\mathcal{P}}_{\mathrm{detour} }$ denote the spatial footprint of the candidate maneuver. The maneuver is accepted if and only if it satisfies

$${\mathcal{P}}_{\mathrm{detour} }\cap O_{\mathrm{local}}=Ø$$

(39)

where $O_{\mathrm{local}}$ represents the union of all local environmental constraints, including no-fly zones (NFZs) and terrain obstacles derived from the Digital Elevation Model (DEM).

This verification is executed in real time for every detour candidate, ensuring that any inserted holding or lateral maneuver is explicitly checked against the environment before being incorporated into the trajectory. If a candidate detour is found to be infeasible due to collision risk, it is immediately discarded, and the algorithm attempts to allocate the remaining path length compensation to subsequent segments with more favorable spatial conditions. In cases where no feasible adjustment can be found, the original trajectory is preserved locally. As a result, the proposed strategy guarantees that temporal coordination is achieved without introducing unsafe trajectory segments. Collision avoidance is strictly prioritized over time synchronization, ensuring that the expanded footprint of any detour maneuver does not intersect with prohibited regions.

3.5. Algorithmic Procedure and Computational Complexity Analysis

Integrating the single-agent rapid planning algorithm, efficiency enhancement strategies, and temporal coordination mechanism, the complete execution flow of the Cooperative-Quick-Dubins-RRT* cooperative 3D path planning algorithm is outlined in Algorithm 3. The algorithm sequentially undergoes three phases: online rapid planning, altitude smoothing optimization, and Dubins-detour-based temporal coordination.

In the online rapid planning phase, each UAV independently executes the Quick-Dubins-RRT* iteration enhanced with four acceleration strategies. During sampling, besides the conventional goal-biasing strategy [34], a goal-acceleration strategy is added to Algorithm 2: if the newly generated node ${\mathit{x}}_{\mathrm{new}}$ from the previous iteration is non-empty and satisfies $h\left({\mathit{x}}_{\mathrm{new}}\right)<h\left(\mathrm{parent}\left({\mathit{x}}_{\mathrm{new}}\right)\right)$, the algorithm directly targets the goal with ${\mathit{x}}_{\mathrm{new}}$ as the nearest neighbor, enabling continuous tree extension toward the goal in obstacle-free directions; otherwise, Algorithm 2 is invoked for resampling within the ellipsoidal domain. During tree expansion, instead of directly connecting ${\mathit{x}}_{\mathrm{new}}$ to its nearest neighbor, the optimal parent is selected from the neighbor set $N_{\mathrm{near}}$. A lookup table from DataBaseVF is used if the sample target is not the goal; otherwise, a 3D Dubins path is computed in real-time. Rewiring utilizes table lookup from DataBase to ensure global kinematic continuity. Rewiring and pruning are executed sequentially within the same neighborhood loop. Upon meeting the termination condition, FindWayBack is called to backtrack and generate the node list.

In the altitude smoothing optimization phase, PathHSmooth is invoked with nodelist as input. Gradient descent iterations are applied to the altitude of each waypoint: the smoothing gradient $f_c$ equalizes climb rates across adjacent segments, while the terrain repulsion gradient $f_r$ ensures the minimum ground clearance $d_{\mathrm{safe}}$. Each candidate altitude update is validated against the kinodynamic feasibility constraint $|\Delta z|\le L_{2D}\cdot \tan \left({\theta }_{\max }\right)$ before being committed, ensuring the smoothed profile remains flyable for fixed-wing UAVs. The output is the smoothed coordinate matrix pathXYH.

In the temporal coordination phase, UAVs broadcast their estimated flight times via an ad hoc data link. After reaching consensus on the expected arrival time, each UAV calculates its required path extension, allocated proportionally based on the length of each Dubins straight segment. If the required extension per segment satisfies $\Delta L\ge 2\pi r_{\min }$, a holding detour is applied with no upper limit on adjustment range; otherwise, a lateral maneuver is employed for smaller adjustments. AdjUAVpathLength is then called to insert the detour segments, outputting the final path PathLLH.

Algorithm 3. Cooperative-3D-Quick-Dubins-RRT*

Input: Initial state ${\mathit{x}}_0$, terminal state ${\mathit{x}}_f$, maximum iterations $ite{r}_{\max }$, extension step size step, pruning neighborhood parameter ${\delta }_s$, motion primitive database without terminal constraints DataBaseVF, motion primitive database with terminal constraints DataBase

Output: 3D path $\mathit{x}\left(·\right)$

$V\leftarrow \left\{{\mathit{x}}_0\right\}$; $E\leftarrow Ø$; $a\leftarrow \infty$

$\mathbf{for} i=1 \mathrm{to} ite{r}_{\max } \mathbf{do}$

$\mathbf{if} \mathrm{isEmpty}\left({\mathit{x}}_{\mathrm{new}}\right) \mathbf{then}$

${\mathit{x}}_{\mathrm{rand}}\leftarrow {\mathit{x}}_f$

${\mathit{x}}_{\mathrm{nearest}}\leftarrow {\mathit{x}}_{\mathrm{new}}$

$else$

${\mathit{x}}_{\mathrm{rand}}\leftarrow \mathrm{Sample}({\mathit{x}}_s,{\mathit{x}}_g,a,b,r)$

${\mathit{x}}_{\mathrm{nearest}}\leftarrow \mathrm{Nearest}\left(V,{\mathit{x}}_{\mathrm{rand}}\right)$

$endif$

$if \left|{\mathit{x}}_{\mathrm{rand}}-{\mathit{x}}_{\mathrm{nearest}}\right|>\mathrm{range}\left(\mathrm{DataBaseVF}\right) then$

${\mathit{x}}_{\mathrm{new}}\leftarrow {\mathit{x}}_{\mathrm{rand}}+step\times \overrightarrow{{\mathit{x}}_{\mathrm{nearest}}{\mathit{x}}_{\mathrm{rand}}}/\left|\overrightarrow{{\mathit{x}}_{\mathrm{nearest}}{\mathit{x}}_{\mathrm{rand}}}\right|$

$else$

${\mathit{x}}_{\mathrm{new}}\leftarrow {\mathit{x}}_{\mathrm{rand}}$

$endif$

$if \mathrm{isFeasible}\left({\mathit{x}}_{\mathrm{new}}\right) then$

${\mathit{x}}_{\mathrm{parent}},\mathit{p}\leftarrow \mathrm{ChooseBestParent}\left(V,N_{\mathrm{near}},\mathrm{DataBaseVF}\right)$

$\mathbf{if}\mathrm{CollisionFree}\left(\mathit{p}\right) \mathbf{then}$

$V\leftarrow V\cup \left\{{\mathit{x}}_{\mathrm{new}}\right\}$; $E\leftarrow E\cup \left\{\mathit{p}\right\}$

$E\leftarrow \mathrm{Rewire}\left(E,N_{\mathrm{near}},\mathrm{DataBase}\right)$

$E\leftarrow \mathrm{Prune}\left({\mathit{x}}_{\mathrm{new}},N_{\mathrm{near}},{\delta }_s\right)$

$\mathbf{if}\mathrm{isEqual}\left({\mathit{x}}_{\mathrm{new}},{\mathit{x}}_f\right)\left|\right| \mathrm{FindBetterPath} \mathbf{then}$

$a,b,r\leftarrow \mathrm{Getabc}\left(\mathit{x}\left(\mathbf{·}\right)\right)$

$endif$

$endif$

$endif$

$endfor$

$\mathrm{nodelist}=\mathrm{FindWayNode}\left(V,E\right)$

$\mathrm{PathXYH}\leftarrow \mathrm{PathHSmooth}\left(\mathrm{nodelist}\right)$

$\mathrm{PathLLH}\leftarrow \mathrm{AdjUAVpathLength}\left(\mathrm{PathXYH}\right)$

$\mathit{x}(\mathbf{·})=\mathrm{PathLLH}$

4. Simulation and Experimental Results

4.1. Multi-UAV Cooperative Path Planning Simulation

4.1.1. Scenarios and Parameter Settings

The proposed path planning algorithm is simulated based on a cooperative delivery mission. Multiple UAVs depart from their respective distribution stations, traverse a complex terrain area, and converge at a designated assembly point. The flight environment includes mountainous terrain, polygonal no-fly zones (e.g., nature reserves, airport clear zones), and threat zones (e.g., electromagnetic interference areas near communication base stations). The delivery assembly point is fixed, and docking positions are preplanned.

The constant-speed assumption is adopted throughout the simulations to reflect the typical cruise phase of long-range fixed-wing UAV operations, where maintaining an optimal airspeed is essential for endurance efficiency. It is worth noting that the proposed framework can be extended to variable-speed scenarios by augmenting the motion primitive database with additional velocity layers, which will be investigated in future work. Across the four experimental subsections, different speed values are selected to match the specific requirements of each evaluation context: the main cooperative planning simulation in Section 4.1 adopts 140 m/s to reflect the realistic flight envelope of long-range fixed-wing UAVs; the parameter sensitivity analysis in Section 4.2 uses 15 m/s to provide finer spatial resolution in the small-scale 2D test environment; and both the comparative experiment in Section 4.3 and the HIL validation in Section 4.4 adopt 30 m/s to match the flight envelope of the onboard flight controller used in the hardware platform. Since the core mechanisms of the proposed algorithm—database indexing, dimensionality-reduced sampling, ellipsoidal heuristic domain, pruning, and localized collision detection—operate on the kinematic state space defined by heading angles and turning radius constraints rather than on absolute speed values, the observed performance improvements are expected to generalize across different speed settings.

UAV performance parameters are listed in

Table 3. Performance parameters of the fixed-wing UAVs.

Parameters of UAV	Numeric Value
Maximum flight path angle	30°
Maximum roll angle	30°
Maximum fly height	7000 m
Cruising speed	140 m/s
Minimum height above ground	500 m

. Initial states for path planning are given in

Table 4. Initial states of the UAVs.

Number of UAV	Position	Heading Angle	Flight Path Angle
1	(100.452348°, 27.954586°, 4500.0 m)	0°	0°
2	(100.352348°, 27.914586°, 4300.0 m)	0°	0°
3	(100.272580°, 27.836248°, 5000.0 m)	0°	0°
4	(100.178415°, 27.740273°, 4000.0 m)	0°	0°
5	(100.102122°, 27.666560°, 5000.0 m)	0°	0°
6	(100.022122°, 27.596560°, 4500.0 m)	0°	0°

, desired terminal states in

Table 5. Terminal states of the UAVs.

Number of UAV	Position	Heading Angle	Flight Path Angle
1	(100.938226°, 27.145075°, 3000)	0°	0°
2	(100.934318°, 27.135888°, 3200)	0°	0°
3	(100.931072°, 27.126711°, 3150)	0°	0°
4	(100.927853°, 27.118753°, 3400)	0°	0°
5	(100.925587°, 27.100253°, 3500)	0°	0°
6	(100.922434°, 27.093504°, 3550)	0°	0°

, obstacle parameters in

Table 6. Parameters of obstacles in the mission environment.

Number of Obstacles	Position	Type of Obstacles
1	Vertex (100.4486°, 27.6211°), (100.4382°, 27.5478°), (100.3477°, 27.5460°), (100.2613°, 27.6175°), (100.3265°, 27.7379°)	no-fly zone
2	Vertex (100.5982°, 27.2263°), (100.5675°, 27.1858°), (100.4996°, 27.2040°), (100.5433°, 27.2408°) Vertex	Threat zone
3	Center position (100.6332°, 27.5967°), range 4500 m	no-fly zone
4	Center position (100.6860°, 27.3718°), range 6000 m	no-fly zone
5	Center position (100.7896°, 27.7116°), range 9000 m	no-fly zone
6	Center position (100.4559°, 27.3847°), range 8600 m	Threat zone
7	Center position (100.1983°, 27.2877°), range 10,000 m	Threat zone
8	Center position (100.7742°, 27.1877°), range 2500 m	Threat zone

, and Cooperative-Quick-Dubins-RRT* algorithm parameters in

Table 7. Parameter settings of the Quick-Dubins-RRT algorithm.

Parameters	Numeric Value
Database sample range	20,000 m
Course angle resolution	15°
Grid resolution	1000 m
Prune range ${\delta }_s$	1500 m
Maximum expend step	10,000 m

.

4.1.2. Results and Analysis

Figure 9 presents the 3D spatial trajectories of the six UAVs in the cooperative path planning scenario, while Figure 10 illustrates the corresponding flight altitude profiles. As shown in Figure 9, all six UAVs successfully generate feasible paths that avoid no-fly and threat zones. The overall trajectories remain smooth after adjustment via Dubins detours. Figure 10 demonstrates that the altitude profiles adapt smoothly to the terrain variations while strictly maintaining the minimum ground clearance, with no abrupt height changes. This validates the effectiveness of the gradient-descent-based altitude optimization.

Figure 11 presents the 2D path planning results before and after detour adjustment for the six UAVs. Figure 11a shows the original paths generated by individual planning. All paths successfully avoid no-fly and threat zones while satisfying kinodynamic constraints. However, due to dispersed starting positions and varying terrain, the path lengths exhibit significant differences. Figure 11b illustrates the results after Dubins detour adjustment. By inserting holding or lateral maneuver segments into the Dubins straight-line paths, each UAV compensates its range. Consequently, the path length error among UAVs is reduced to within 0.05 km (as shown in

Table 8. UAV trajectory lengths before and after time coordination adjustment.

Number of UAV	Path Length Before Adjustment	Path Length After Adjustment
1	103.2012 km	108.63 km
2	105.8713 km	108.65 km
3	103.1137 km	108.63 km
4	108.6725 km	108.67 km
5	103.2006 km	108.64 km
6	105.4413 km	108.65 km

), which is within the acceptable range. The adjusted paths maintain excellent overall smoothness, validating the effectiveness of the proposed temporal coordination mechanism.

4.1.3. Simulation and Analysis in Dynamic Scenarios

To evaluate the algorithm’s adaptability to dynamic environments, four dynamic simulation scenarios are designed based on the configuration in Section 4.1.1. In each scenario, UAVs have completed initial cooperative planning and flown for a period before dynamic events are triggered. Upon triggering, UAVs restart local planning and cooperative adjustment from neighboring nodes near their current positions.

(1)

Scenario 1 (Delayed Coordination): After independently completing shortest-path planning, all six UAVs trigger the coordination mechanism simultaneously at 100 s to adjust the remaining segments via Dubins detours.

(2)

Scenario 2 (Team Scale Reconfiguration): At 200 s, UAV 1 exits the formation (e.g., due to signal loss), while UAV 7 and UAV 8 join from new start points, triggering a team-scale expansion.

(3)

Scenario 3 (Target Reallocation): At 200 s, UAV 1’s delivery target is reassigned to a new location, and UAV 2 and UAV 6 swap targets. The three affected UAVs trigger local replanning, while the others maintain their original segments. The fleet then performs coordinated detours.

(4)

Scenario 4 (Global Map Switching): At 200 s, the layout of no-fly and threat zones undergoes a global update. The algorithm automatically detects collisions between remaining paths and the new map. UAVs with conflicts trigger online replanning, while conflict-free UAVs maintain their paths. The fleet then performs unified coordination.

Figure 12 presents the simulation results for the four dynamic scenarios. The dashed lines indicate historical paths before the dynamic events, while the solid lines represent the final trajectories after replanning and coordination. The white circles mark the neighboring nodes, which serve as the new planning start points.

For Scenario 1, as shown in Figure 12a, after delaying coordination initiation by 100 s, all UAVs successfully converge their arrival times through Dubins detours. This demonstrates the flexibility of the coordination mechanism, which is insensitive to initiation timing.

For Scenario 2, in Figure 12b, after UAV 7 and UAV 8 join the formation, the remaining five UAVs reconfigure their paths from their respective neighboring nodes. The updated formation of seven UAVs successfully completes coordinated detours under a new time baseline, generating kinodynamically feasible and collision-free trajectories. This validates the framework’s adaptability to dynamic team expansion.

For Scenario 3, illustrated in Figure 12c, after UAVs 1, 2, and 6 complete local replanning from their neighboring nodes, they merge back into the unified coordination process. The new trajectories exhibit smooth transitions and remain collision-free, verifying the compatibility between local replanning and global coordination under target reallocation.

For Scenario 4, as depicted in Figure 12d, upon global obstacle switching, the algorithm automatically identifies affected UAVs and triggers online replanning. The replanned paths successfully avoid the new no-fly and threat zones, while unaffected UAVs maintain their original segments to participate directly in coordination. This validates the algorithm’s real-time responsiveness to sudden environmental changes.

4.2. Parameter Sensitivity Analysis

The discretization resolution, sampling range, and neighborhood pruning range of the motion primitive database significantly impact the performance of the Quick-Dubins-RRT* algorithm. This section investigates the effects of these parameters on solution quality and computational efficiency.

The 2D test scenario is illustrated in Figure 13a. The UAV speed is set to 15 m/s with a maximum bank angle of 30°. The start point is (50, 70) with an initial heading of 0°, and the goal point is (480, 260) with a goal heading of −90°. All results are averaged over 100 runs.

4.2.1. Effect of Motion Primitive Resolution

With a fixed sampling range of 100 m and a pruning range of 0 m, three databases with different resolutions are tested, featuring grid resolutions of 5 m, 10 m, and 20 m, and heading angle discretizations of 15°, 22.5°, and 45°, respectively. The path length and runtime versus iteration curves are shown in Figure 14.

It can be observed that lower database resolution leads to a faster increase in runtime as iterations grow. This is because lower grid resolution results in a smaller discrete state space, causing extensive repeated sampling and sampling failures after a certain number of iterations, thereby increasing the sampling overhead. The average path lengths for different resolutions are similar at the same iteration count; however, higher-resolution databases yield slightly better paths as iterations progress. Therefore, increasing the discretization resolution of the motion primitive database is consistently beneficial for enhancing both solution quality and computational efficiency.

4.2.2. Effect of Motion Primitive Sampling Range

The sampling range of the database determines the maximum extension step and node neighborhood size. With a fixed heading discretization of 15°, a grid resolution of 5 m, and zero pruning range, four databases with sampling ranges of 50 m, 75 m, 100 m, and 150 m are tested. Figure 15 plots the path length and runtime against iterations. Larger sampling ranges result in longer runtimes for the same number of iterations. However, significant time differences only emerge at high iteration counts or with excessively large ranges. Path length is significantly affected: larger ranges enable faster convergence to the optimal path. This is because larger extension steps accelerate tree growth toward the goal, while larger neighborhoods allow more rewiring opportunities, improving path quality. Although larger ranges slightly increase computation time, they greatly enhance path quality.

Note that the sampling range should not be increased blindly. Excessively large ranges drastically increase rewiring time and database storage requirements. Therefore, the sampling range must be selected by comprehensively considering runtime, convergence speed, and memory usage, choosing an appropriate value that keeps runtime acceptable.

4.2.3. Effect of Pruning Neighborhood Size

Although pruning the RRT accelerates computation, excessive pruning degrades solution quality. With a fixed sampling range of 100 m, heading discretization of 15°, and grid resolution of 5 m, tests are conducted with pruning parameters ${\delta }_s$ of 0 m, 10 m, 20 m, and 30 m. Figure 16 shows the path length and runtime versus iterations. Larger pruning ranges result in shorter runtimes and slower runtime growth as iterations increase. However, the final converged path length gradually increases with the pruning range. Small pruning ranges can accelerate computation without significantly affecting path quality, whereas large ranges noticeably degrade it. Setting the pruning range to 1–2 times the grid resolution achieves a good balance between computational speed and path quality.

4.3. Comparison with Conventional Dubins-RRT*

The proposed Quick-Dubins-RRT* is a systematic enhancement of the conventional Dubins-RRT* at the search strategy level, as both share the same kinodynamic modeling framework (Dubins curves) and tree expansion mechanism. This choice of baseline directly isolates and quantifies the marginal contribution of the proposed acceleration strategies under strictly controlled variables. Therefore, this section compares the Quick-Dubins-RRT* algorithm with the traditional Dubins-RRT* in a dense 2D obstacle environment (Figure 13). The UAV speed is set to 30 m/s with a maximum bank angle of 30°. The start point is (1000, 400) with an initial heading of 90°, and the goal point is (4760, 4200) with a goal heading of 0°. All results are averaged over 100 runs.

Figure 17 compares the path length and runtime versus iterations for both algorithms. Quick-Dubins-RRT* demonstrates significant improvements in both solution quality and computational efficiency. After 1000 iterations, the average runtime is reduced by 86.04%, and the path length is shortened by 8.928%, indicating that Quick-Dubins-RRT* is highly suitable for online path planning. Furthermore, as shown in Figure 18, the average time to find the first feasible path is reduced by 97.34%, with a path length reduction of 22.63%. Quick-Dubins-RRT* also exhibits a much lower variance in computation time, demonstrating superior solution stability.

4.4. Hardware-in-the-Loop Simulation Validation

Due to the inherent constraints of real-world flight testing for fixed-wing UAVs, such as limited testing conditions, high operational costs, and safety considerations, it is challenging to directly conduct large-scale outdoor experiments for swarm trajectory planning validation. To address this issue, a hardware-in-the-loop (HIL) simulation framework is adopted to provide a controllable yet realistic evaluation environment. By integrating an onboard flight controller with a high-fidelity flight dynamics simulation module, the system is capable of reproducing key characteristics of real-world UAV flight, including motion dynamics, control responses, and environmental interactions. This enables the experimental setup to approximate real flight conditions as closely as possible within a laboratory environment.

To further verify the engineering feasibility and real-time execution capabilities of the proposed path planning algorithm, we have established a hardware-in-the-loop (HIL) simulation system that integrates virtual and real components. This integrated simulation framework comprises several key modules: swarm mission planning software, a hardware-in-the-loop system, a virtual simulation system, a hybrid real-virtual mobile ad hoc network (MANET) communication system, and a dynamic scenario visualization system. These modules perform real-time, online data exchange to collectively constitute the integrated hardware-in-the-loop simulation platform.

The hardware-in-the-loop simulation procedure is illustrated in Figure 19.

The functional modules of the proposed system are described as follows:

(1)

The swarm mission planning software provides functionalities including UAV scenario configuration, human–machine interaction for command generation, construction of swarm-level command sets, loading of preplanned waypoints, and visualization of two-dimensional operational trajectories of the UAV swarm.

(2)

The hardware-in-the-loop (HIL) simulation system is constructed based on key components such as an intelligent mission planner, a flight control module, an ad hoc communication datalink, and flight dynamics simulation software. This subsystem enables real-time hardware-based simulation of UAV mission planning and autonomous decision-making processes, serving as a core architecture for the integrated virtual–physical simulation framework.

(3)

The virtual simulation system is developed based on digital simulation techniques, incorporating a fixed-wing UAV six-degree-of-freedom (6-DOF) model as well as trajectory tracking and control models. It enables online validation of UAV behaviors in a purely simulated environment and operates in conjunction with the HIL subsystem to support real-time hybrid simulation of swarm operations.

(4)

The hybrid ad hoc communication simulation system is established using both physical ad hoc datalink hardware and a corresponding simulated communication module. By designing appropriate communication strategies, this system forms a unified communication network for the hybrid simulation framework, ensuring reliable data exchange between the virtual and HIL subsystems during cooperative UAV simulations.

(5)

The flight dynamics simulation software provides the flight control module with simulated environmental inputs, including atmospheric conditions and sensor disturbances. This enables realistic emulation of UAV flight processes within a laboratory environment, effectively approximating real-world operating conditions.

(6)

The ground control station primarily features swarm visualization for trajectory display, a graphical interface for flight attitude, and the reception of cluster simulation data. It enables 2D visualized simulation monitoring and telemetry of UAV trajectories and attitude data.

(7)

The dynamic visualization and simulation system supports performance evaluation of swarm mission execution, synchronization of simulation scenarios, and three-dimensional visualization of UAV trajectories. It facilitates comprehensive assessment of swarm operational effectiveness and enables intuitive visualization of coordinated UAV behaviors within the hybrid simulation environment.

The HIL test scenario is configured in accordance with the settings described in Section 4.1.1. During the simulation, the mission platform first transmits no-fly zone and threat zone information to the trajectory planner, followed by task assignment for formation assembly at designated locations. The resulting UAV trajectories, as shown in Figure 20, demonstrate a line-abreast formation executing the cooperative delivery mission.

To evaluate the trajectory tracking performance of the proposed planning framework under realistic onboard flight control constraints, HIL validation experiments are conducted.

A total of six UAV nodes are deployed in the hybrid simulation environment, including three physical nodes and three virtual nodes. The onboard flight controller of the physical UAVs operates within a velocity range of [25, 40] m/s, with a position error not exceeding 5 m, a heading error within 1.5°, and an attitude error within 0.5°. The control system provides a data update frequency of no less than 200 Hz.

In terms of communication, the system adopts a broadcast mechanism based on the User Datagram Protocol (UDP), integrated with serial interfaces for multi-source data interaction. This design supports periodic state broadcasting and synchronous updates among multiple UAV nodes, meeting the requirements of low-latency communication and distributed coordination. The maximum inter-UAV communication range is approximately 10 km.

To ensure compatibility with onboard flight constraints, the experimental scenario is configured according to the flight envelope of the UAV. Specifically, the cruising speed is set to 30 m/s, and the maximum bank angle is limited to 30°. In addition, the mission area is scaled to approximately one-third of the original scenario while preserving the obstacle topology and constraint structure, thereby maintaining both hardware feasibility and task representativeness.

Under this setting, the flight controller receives the planned waypoints and executes trajectory tracking in a closed loop. Figure 21 presents the time histories of the UAV state variables output by the onboard controller, including longitude, latitude, altitude, airspeed, roll angle, pitch angle, and yaw angle.

As shown in Figure 21, the onboard flight controller responds to the planned waypoint sequence throughout the mission, confirming that the trajectories generated by the proposed Quick-Dubins-RRT* algorithm are executable by a real flight control system. It should be noted that, prior to uploading to the flight controller, the planned path undergoes a Douglas–Peucker (DP)-based re-discretization process to ensure stable tracking performance. This intermediate step introduces two observable effects: first, the altitude profile exhibits a stepwise variation pattern, as each discretized waypoint transition triggers a new altitude command rather than a continuously smooth climb or descent; second, the realized arrival times of the six UAVs (1405.71 s, 1411.32 s, 1413.36 s, 1403.73 s, 1406.67 s, and 1407.96 s, respectively) deviate slightly from the pure algorithmic simulation results, since the fidelity of the DP discretization directly affects the conformity of the actual flight trajectory to the originally planned path. Nevertheless, all six UAVs complete the mission within a narrow time window with a maximum inter-arrival deviation of approximately 9.63 s, demonstrating that the proposed cooperative planning framework retains satisfactory temporal coordination capability under real onboard execution conditions, and demonstrating its strong engineering applicability.

However, it should be noted that the HIL framework cannot fully capture all uncertainties present in real-world flight environments, such as complex atmospheric disturbances, unmodeled aerodynamic effects, and sensor noise variability.

Nevertheless, as an intermediate validation stage between pure simulation and real-world deployment, the HIL system provides a reliable and controllable platform for evaluating trajectory tracking performance and multi-UAV coordination under realistic system constraints. Therefore, the experimental results can be considered strong evidence of the practical feasibility of the proposed method.

5. Conclusions

In this paper, a cooperative online 3D path planning framework, centered on the Cooperative-3D-Quick-Dubins-RRT* algorithm, is proposed to address the real-time kinodynamic requirements of fixed-wing UAV swarms in complex environments. By integrating an offline dual-mode motion primitive database with four acceleration strategies—sampling space dimensionality reduction, elliptical prediction domain, pruning, and improved collision detection—the algorithm achieves a significant leap in computational efficiency.

The effectiveness and online planning capability of the proposed algorithm are validated in a baseline scenario involving six UAVs for cooperative delivery, as well as in four dynamic scenarios: delayed coordination, mission scale reconstruction, target reallocation, and global map switching. Parameter sensitivity analysis demonstrates that appropriate increases in discretization accuracy and sampling range, combined with a pruning range set to 1–2 times the grid resolution, yield optimal algorithmic performance. Comparative experiments against the conventional Dubins-RRT* algorithm show that the proposed method reduces average runtime by 86.04% and shortens the first feasible path length by 22.63%, confirming its suitability for real-time online trajectory planning. Hardware-in-the-loop simulations further validate the engineering applicability of the proposed method: all six UAVs successfully completed the cooperative mission within a maximum inter-arrival deviation of 9.63 s under real onboard flight control constraints, demonstrating reliable temporal coordination capability in realistic execution conditions.

Although the proposed framework assumes a constant-speed cruise flight model, future work will investigate trajectory planning for variable-speed flight by incorporating tangential overload constraints and applying differential flatness theory to optimize the UAV velocity profile, thereby further enhancing the algorithm’s applicability to a broader range of operational scenarios.