RRT*-APF Path Planning and MA-AADRC-SMC Control for Cooperative 3-D Obstacle Avoidance in Multi-UAV Formations

Abstract

To enable safe cooperative flight of multi-UAV formations in urban 3-D airspace with wind-field disturbances, we develop an integrated planning-control framework.The planning layer uses an APF-guided RRT* with continuous collision prediction and explicit velocity/acceleration limits, and compensates wind online.The control layer adopts a dual-loop MA-AADRC-SMC design. An adaptive ESO estimates disturbances for feed-forward cancellation, and an SMC term improves robustness and tracking accuracy. By coupling the planned trajectory with speed-weighted repulsive fields, the framework coordinates path and attitude in closed loop, enabling collision-free and overshoot-free formation flight in wind and clutter. Simulations show higher tracking accuracy and better formation stability than ADRC, PID and SMC. A Lyapunov analysis proves uniform boundedness and asymptotic stability. The framework is scalable to applications such as disaster assessment and urban air transport.

1. Introduction

In complex 3-D environments, multi-UAV formations must remain stable while reacting quickly to unknown disturbances and obstacles. Meeting both goals is still challenging for research and practice.

In UAV control, active disturbance rejection control (ADRC) has become a research focus for robust UAV operation owing to its extended state observer (ESO), which estimates and compensates internal and external disturbances online. Shi et al. [1] employed an improved ESO with a sliding-average low-pass filter in a nested inner–outer loop structure to achieve rapid attitude response without overshoot; Yang et al. [2] enhanced the ESO’s noise rejection by using a nonlinear smooth GALN function, significantly improving convergence speed and tracking accuracy; Dou et al. [3] introduced fuzzy logic into the ESO and error-feedback loop, effectively reducing overshoot and output fluctuation; and Chen et al. [4] optimized ADRC parameters via the integral of time-weighted absolute error (ITAE) criterion combined with a particle swarm optimization (PSO) algorithm, further enhancing tethered UAV tracking precision under cable interference.

Active Disturbance Rejection Control (ADRC) is typically composed of three main components: a Tracking Differentiator (TD), an Extended-State Observer (ESO), and a Nonlinear State Error Feedback (NLSEF). Specifically, the TD is employed to smooth the desired input and generate usable derivative signals; the ESO estimates the system states in real time while compensating for the “total disturbance” (including external perturbations and model uncertainties); and the NLSEF, together with a linear feedback term, achieves dynamic correction of the closed-loop control law. This architecture significantly reduces reliance on an accurate mathematical model and thus provides strong robustness and adaptability. In contrast, sliding-mode control (SMC) is based on the invariance principle, where a sliding surface is designed and a discontinuous control law drives the system trajectory toward and maintains it on this surface, ensuring finite-time convergence and strong disturbance rejection. Its strengths lie in design simplicity and robustness, particularly against both structured and unstructured uncertainties. Nevertheless, SMC often suffers from the chattering phenomenon in practical implementations, while the performance of ADRC critically depends on the tuning of ESO parameters, the selection of disturbance compensation bandwidth, and the smoothing capability of the TD.

Sliding-mode control (SMC), renowned for its strong robustness to parameter uncertainty and external disturbances, has been widely applied to quadrotor attitude control. Zhang et al. [5] proposed a robust SMC strategy based on ESO that reduced rotor speed jump amplitude by nearly 90 %; Wang et al. [6] constructed a generalized sliding-mode manifold for the underactuated system and verified closed-loop stability; An et al. [7] applied geometric SMC within the Lie group–Lie algebra framework to achieve globally stable attitude convergence; and Feng et al. [8] combined fuzzy adaptive mechanisms with sliding-mode variable-structure control to suppress chattering and enhance disturbance rejection. Although these works significantly improve the robustness of single-vehicle attitude and velocity loops, most experiments remain confined to indoor or static hover scenarios, lacking validation for high-speed three-dimensional formation coordination and real-time obstacle avoidance. Moreover, these controllers have not explicitly incorporated kinematic constraints such as maximum velocity and acceleration, nor analyzed global stability under strong wind fields and communication delays.

In path planning, the rapidly exploring random tree (RRT) algorithm is widely adopted due to its map-free nature, but its global randomness limits convergence efficiency and path quality. Chen et al. [9] proposed an APF-RRT fusion framework based on goal biasing, improved artificial potential fields, and a genetic algorithm to accelerate convergence and shorten paths; Yin et al. [10] incorporated dynamic constraints and distance weighting into node expansion and employed B-spline curves for path smoothing; Li et al. [11] used chaotic sequences and fuzzy inference to dynamically adjust sampling parameters and developed bilateral and rolling-window replanning schemes; Wang et al. [12] introduced bias-extension, probability guidance, and node pruning strategies to significantly speed up tree growth and simplify paths. Traditional artificial potential field (APF) methods are computationally simple and yield smooth paths but often suffer from local minima. Xu et al. [13] presented an APF-RRT* fusion approach that guides sampling with potential fields and self-tunes the sampling radius for efficient global planning; Zhou and Kong [14] adopted hexagonal guidance and virtual subgoals to escape local traps; Yang et al. [15] developed an online planner based on reference-route attractive fields and adaptive disturbance factors to ensure safety in dynamic environments; Zhou et al. [16] introduced angular factors and adaptive weights into attractive and repulsive functions to suppress oscillations and overcome local optima. Existing RRT*/APF enhancements primarily focus on sampling efficiency and geometric path smoothing, rarely incorporating kinematic constraints such as velocity/acceleration or continuous-time collision prediction in the expansion and rewiring phases; generated paths often require post-processing before execution. Furthermore, most algorithms are validated only in single-UAV, static-obstacle settings, lacking systematic evaluation of inter-UAV interference, communication bandwidth, and safety-distance requirements.

To enhance autonomous flight capability in complex dynamic environments, the integration of path planning and attitude control has emerged as a key research topic. Wang et al. [17] constructed a neural-network-based joint planning-tracking framework to achieve real-time obstacle avoidance for six-degree-of-freedom vehicles in complex obstacle fields, while Huang et al. [18] fused an improved APF algorithm with inverse-dynamics backstepping control to effectively address local optima. Despite these advances, challenges remain in attitude-path modeling consistency, global optimality under multi-objective constraints, and overall system robustness. For example, Rudnick-Cohen et al. [19] jointly optimized UAV design parameters and path planning to reduce risk and mission time, and Pang et al. [20] emphasized balancing third-party risk and energy consumption in path optimization. Although deep-learning or multi-objective joint optimization frameworks have attempted to synchronize planning and control, they rely on large-scale offline training with poor interpretability and a high transfer cost; the results are predominantly confined to single-vehicle simulations, lacking multi-UAV coordination and real-wind validation. Offline path optimization trade-offs for risk and energy have not been deeply coupled with real-time control loops, nor has robust analysis against disturbances and uncertainties been fully addressed.

This paper proposes a three-dimensional multi-UAV formation cooperative obstacle-avoidance framework that integrates an APF-guided RRT* planner with kinematic constraints, continuous-time collision prediction, and a four-layer obstacle filtering pipeline, together with a hierarchical hybrid MA–AADRC–SMC controller. The planner produces dynamically feasible and executable trajectories via potential-field-biased sampling and predictive collision checks, while the controller combines an adaptive extended state observer, feedforward correction, and a robust sliding-mode law in a dual-loop structure to achieve high-precision tracking under disturbances. Large-scale simulations and Lyapunov-based analysis in composite urban wind fields and heterogeneous three-dimensional environments validate superior tracking accuracy, formation keeping, and disturbance rejection, supporting reliable applications such as disaster assessment and urban air mobility.

• Research Gap and Contributions.

Existing planners mainly focus on sampling efficiency and geometric smoothness, but seldom incorporate kinematic limits or continuous-time safety constraints directly into the planning process. Most controllers are validated only in single-UAV or low-dynamics settings, with weak planning–control coupling under realistic disturbances. To address these gaps, our contributions are (1) An APF-guided RRT* planner with embedded velocity/acceleration constraints and continuous-time collision prediction for executable trajectories; (2) A four-layer obstacle filtering pipeline that preserves temporal completeness with near-constant computational cost in cluttered environments; (3) A hybrid MA–AADRC–SMC controller combining bandwidth-scheduled ESO, sliding-mode robustness, and speed-weighted repulsion for overshoot-suppressing, disturbance-rejecting tracking with enforced safety spacing; (4) A tightly coupled planning–control framework with Lyapunov-guaranteed stability, validated through representative urban scenarios and micro-sensitivity studies confirming robustness.

2. Environment Modeling and Kinematic Decoupling

The simulation environment developed in this study is rigorously modeled in the three-dimensional Euclidean space ${\mathbb{R}}^3$, fully accounting for the physical constraints of UAVs and the complex geometric boundary conditions encountered in real-world operations. To guarantee both representativeness and generality, the obstacle system is designed to capture the morphological heterogeneity of multiple entity types and the stochasticity of their spatial distribution, while simultaneously preserving the computational tractability required for high-efficiency collision detection and motion planning.

2.1. Dynamic Environmental Disturbances and Obstacle Representation

2.1.1. Composite Wind-Field Disturbance Modeling

$$\begin{array}{c}\hfill w_i(t,z)=\left[\sum _{k=1}^{N_f}{A}_{i,k}\sin \left({\omega }_{k}t+{\phi }_{i,k}\right)+{\mathsf{\Delta }}_{i}H(t-t_0)\right]\left(1+{\lambda }_z{\displaystyle \frac{z}{z_{\mathrm{ref}}}}\right),i\in \{x,y,z\}.\end{array}$$

(1)

where $A_{i,k}$, ${\omega }_k$ and ${\phi }_{i,k}$ are the amplitude, angular frequency and initial phase of the k-th harmonic in the i-direction ($N_f$ total harmonics); ${\mathsf{\Delta }}_i$ is a step gust activated at $t\ge t_0$ with $H(·)$ the Heaviside function; ${\lambda }_z$ linearly scales wind speed with altitude z relative to the reference height $z_{\mathrm{ref}}$.

This model decomposes the wind into three components: (i) multiple harmonic terms that capture periodic or quasi-periodic gust and turbulence energy peaks; (ii) step gust terms ${\mathsf{\Delta }}_{i}H(·)$ representing sudden wind shear or gust events; and (iii) vertical scaling $\left(1+{\lambda }_{z}z/z_{\mathrm{ref}}\right)$ that incorporates the empirical rule that wind tends to be stronger at higher altitudes. In intuitive terms, the horizontal and vertical directions each combine several “tones” (${\omega }_k$) with their “volumes” ($A_{i,k}$), plus a sudden “accent” (${\mathsf{\Delta }}_i$), then scaled linearly with height, so that the simulated wind field exhibits multiple time scales, controllable intensity, and height dependence.

The superposition of multi-frequency harmonics plus the segmented gust term captures temporal variability, multi-scale perturbations and vertical gradients, providing a stringent disturbance baseline for testing autonomous navigation and robust-control algorithms. As illustrated in Figure 1, the simulated wind-velocity components in x, y, and z directions show distinct multi-scale variations and altitude-dependent strengthening. Furthermore, the height-time heatmap in Figure 2 highlights the onset of gust at $t_0$ and the clear vertical strengthening effect.

2.1.2. Super-Quadric Cylinder Obstacles

To model axis-symmetric obstacles such as trees or poles that are common in operational scenes, we employ super-quadric surfaces as a unified parametric framework. The corresponding constraint set is

$${\mathcal{O}}_{\mathrm{scyl}}=\left\{\mathbf{p}={(x,y,z)}^{\mathsf{T}}\mid {\left({\left|\frac{x-c_x}{r}\right|}^{2/ϵ}+{\left|\frac{y-c_y}{r}\right|}^{2/ϵ}\right)}^{ϵ/2}\le 1,0\le z\le h\right\}.$$

(2)

where $(c_x,c_y)$ is the base-centre, r is the equivalent radius, h is the column height, and $ϵ$ is the shape index ($ϵ=1$ degenerates to a right cylinder). Obstacle centres are generated via Poisson-disk sampling on ${\mathbb{R}}^2$, ensuring a minimum pairwise distance of 20 units. Radii follow $r\sim \mathcal{U}(3,7)$ and heights are sampled as $h=10+50\mathcal{U}(0,1)$, thereby reflecting size diversity.

2.1.3. Oriented Bounding-Box Rectangular Obstacles

To capture the spatial characteristics of cuboid obstacles such as buildings or containers, we adopt an oriented bounding-box (OBB) model. For a centre $c=(c_x,c_y)$, the local coordinates are defined by $\tilde{\mathbf{p}}={\mathbf{R}}^{\mathsf{T}}(\mathbf{p}-\mathbf{c}),$ where $\mathbf{R}$ is a planar rotation about the z-axis with random orientation angle $\gamma \sim \mathcal{U}(0,2\pi )$. The OBB constraint set reads

$${\mathcal{O}}_{\mathrm{obb}}=\{\mathbf{p}\left||\tilde{x}|\le \frac{W}{2},\left|\tilde{y}\right|\le \frac{L}{2},0\le \tilde{z}\le h\right\},$$

(3)

with $W,L\sim \mathcal{U}(6,14)$ denoting the minor and major side lengths, respectively, and $h=10+50\mathcal{U}(0,1)$ the height. Likewise, centres are generated by spatial Poisson-disk sampling to avoid overlap. This model is fully compatible with the super-quadric parameterization in Section 2.1.2, enabling seamless integration with potential fields and distance functions while preserving obstacle diversity and orientation.

2.2. Attitude–Acceleration Decoupling

In multirotor UAV control, the strong coupling between attitude (Euler angles) and translational acceleration complicates controller design—especially under high-dynamic performance demands or strict trajectory-tracking requirements. To mitigate this, we adopt an attitude–acceleration decoupling strategy that transforms the original six-degree-of-freedom dynamics into a position–velocity double integrator, greatly simplifying the high-level control problem.

Starting from the full rigid-body equations and applying an appropriate change of variables, the nonlinear coupling between the body-axis thrust direction and the inertial-frame acceleration can be abstracted as a desired acceleration input. The thrust magnitude T and attitude angles $(\varphi ,\theta )$ are treated as fast inner-loop variables, which are regulated so that the generated acceleration closely follows the outer-loop reference. This creates a clear time-scale separation between acceleration tracking (outer loop) and attitude regulation/thrust allocation (inner loop). At the high level, the system can thus be expressed as

$$\dot{\mathbf{p}}=\mathbf{v},\dot{\mathbf{v}}=\mathbf{u},$$

(4)

By delegating fast attitude and thrust control to the inner loop, the outer loop only needs to command the desired inertial acceleration $\mathbf{u}$. This abstraction turns the full six-degree-of-freedom coupled dynamics into a simple position–velocity–acceleration “double integrator” model, making high-level planning and gain tuning more straightforward while preserving robustness.

Within this framework, upper-layer planning and control focus solely on generating velocity/acceleration targets and closing the outer loop, while low-level actuators handle attitude and thrust execution. Compared with direct attitude-space control, this decoupling improves implementability and robustness, and offers a solid theoretical and engineering foundation for integrating advanced algorithms. In this study, kinematic discretisation and control-input synthesis both follow this decoupling principle, reducing the UAV dynamics to a linear discrete double integrator with emphasis on precise velocity modulation and disturbance rejection, which are key for high-accuracy, robust autonomous flight.

• Section summary: We modeled realistic wind fields and heterogeneous obstacles (super-quadric cylinders and OBBs) and introduced attitude–acceleration decoupling to reduce UAV dynamics to a discrete double integrator. This abstraction enables planners and outer-loop controllers to operate on feasible velocity/acceleration targets while inner loops realize fast thrust/aSection summary:ttitude tracking.

3. Path-Planning Algorithm

3.1. Improved RRT*-APF Algorithm

Rapidly exploring Random Tree Star (RRT*) is widely adopted for UAV path planning owing to its efficient sampling in high-dimensional configuration spaces and its asymptotic optimality. Nevertheless, the standard algorithm focuses mainly on spatial connectivity and collision-free feasibility, while neglecting indispensable kinematic constraints—such as maximum velocity, acceleration, and turning limits—so that certain planned trajectories become infeasible or unsafe when executed on real vehicles.

To overcome these deficiencies, an improved RRT* augmented with an Artificial Potential Field (APF) is developed. The APF serves as a heuristic guide for random sampling, whereas the UAV kinematic model is embedded directly into both collision checking and path-extension procedures. Consequently, the proposed planner generates obstacle-avoidance trajectories in three-dimensional environments that honour both geometric clearance and kinematic feasibility. During tree expansion, the superposition of goal-attractive and obstacle-repulsive potentials dynamically biases sample selection toward higher-quality segments and expedites convergence of the tree to the goal region. Simultaneously, continuous-time kinematic simulation combined with collision discrimination guarantees that each path segment satisfies prescribed velocity and acceleration bounds of the UAV.

3.2. Random Sampling and APF-Corrected Sampling Strategy

Samples are drawn from the goal ${\mathbf{x}}_{\mathrm{goal}}$ with probability p, and otherwise uniformly over the workspace $[x_{\min },x_{max}]\times [y_{\min },y_{max}]\times [z_{\min },z_{max}]$ as ${\mathbf{x}}_{\mathrm{rand}}={(x_{\mathrm{rand}},y_{\mathrm{rand}},z_{\mathrm{rand}})}^{\mathsf{T}}$.

For each super-quadric cylinder obstacle with base centre ${\mathbf{c}}^{\left(xy\right)}=(c_x,c_y)$, radius R, and height range $[z_{\min },z_{max}]$, let $d_{\mathrm{xy}}=\parallel {\mathbf{x}}^{\left(xy\right)}-{\mathbf{c}}^{\left(xy\right)}\parallel$. We define an effective clearance $d_{\mathrm{eff}}$ that blends lateral and vertical separations, and a repulsive force ${\mathbf{F}}_{\mathrm{rep}}$ that grows as the clearance shrinks; both precise expressions are provided in Appendix A.1 (cf. [21]). If $d_{\mathrm{eff}}<d_0$, the obstacle contributes a nonzero repulsion. Summing over all obstacles yields the potential-field gradient

$$\nabla U\left(\mathbf{x}\right)=-\sum _k{\mathbf{F}}_{\mathrm{rep},k}.$$

Finally, the raw sample is refined using a single gradient step

$${\mathbf{x}}_{\mathrm{rand}}\leftarrow {\mathbf{x}}_{\mathrm{rand}}-\alpha \nabla U\left({\mathbf{x}}_{\mathrm{rand}}\right),$$

(5)

which biases samples away from obstacles and accelerates planner convergence.

The attractive term pulls the sample toward the goal, while the repulsive term pushes it away from nearby obstacles. Their sum shapes the local sampling bias, guiding expansion into safer, more goal-oriented regions.

The effective clearance metric and the obstacle repulsive force used to compute the APF gradient are detailed in Appendix A.1. Intuitively, $d_{\mathrm{eff}}$ prevents grazing either the sides or the top of obstacles, and ${\mathbf{F}}_{\mathrm{rep}}$ smoothly increases as the clearance decreases, producing a stable guidance away from hazardous regions. The overall procedure of the improved RRT*-APF planner, including guided sampling, predictive feasibility checks, and rewiring, is summarized in the flowchart shown in Figure 3.

3.3. Nearest Node Selection, Extension and Rewiring [22]

At each iteration, the nearest node in the current tree $\mathcal{T}$ to the (possibly APF-corrected) sample ${\mathbf{x}}_{\mathrm{rand}}$ is found by

$${\mathbf{x}}_{\mathrm{nearest}}=\arg \underset{{\mathbf{x}}_i\in \mathcal{T}}{\min }\parallel {\mathbf{x}}_i-{\mathbf{x}}_{\mathrm{rand}}\parallel .$$

(6)

A new node is then generated at fixed step size s along the direction from ${\mathbf{x}}_{\mathrm{nearest}}$ to ${\mathbf{x}}_{\mathrm{rand}}$:

$${\mathbf{x}}_{\mathrm{new}}={\mathbf{x}}_{\mathrm{nearest}}+s{\displaystyle \frac{{\mathbf{x}}_{\mathrm{rand}}-{\mathbf{x}}_{\mathrm{nearest}}}{\parallel {\mathbf{x}}_{\mathrm{rand}}-{\mathbf{x}}_{\mathrm{nearest}}\parallel }}.$$

(7)

Selecting the nearest existing node ensures that the tree grows from the closest reachable state toward the new sample, minimizing unnecessary detours.

To validate the straight-line extension, we discretize the segment between ${\mathbf{p}}_1={\mathbf{x}}_{\mathrm{nearest}}$ and ${\mathbf{p}}_2={\mathbf{x}}_{\mathrm{new}}$ into intermediate points for collision checks; the interpolation formula and parameter definitions are given in Appendix A.2.

If the extension succeeds, nearby nodes are examined for cost reduction via rewiring. We compare cumulative path costs and reassign parents through ${\mathbf{x}}_{\mathrm{new}}$ when doing so decreases the total cost while remaining collision-free; the explicit inequalities are listed in Appendix A.2.

3.4. Dynamic Velocity/Acceleration Constraints and Collision Checking

3.4.1. Geometric Precision Checking

We define an L2 geometric-precision layer $\mathrm{collision}\_\mathrm{point}(\mathbf{p},\mathcal{O})$ that returns true if a query point $\mathbf{p}={(x,y,z)}^{\mathsf{T}}$ lies inside any obstacle in the set $\mathcal{O}={\mathcal{O}}_{\mathrm{scyl}}\cup {\mathcal{O}}_{\mathrm{obb}}$. During planning, every candidate or predicted waypoint is validated by this layer. This layer acts as a geometric filter, rejecting any candidate or predicted waypoint that lies inside a modeled obstacle and ensuring that both static and predicted motions are physically feasible in the environment model.

The concrete point-in-obstacle tests for super-quadric cylinders and oriented bounding boxes, together with parameter definitions, are provided in Appendix A.3.

• Collision-checking scheme.

To guarantee both path safety and kinematic executability, two checks are performed in the RRT*-APF expansion:

1.

Static linear discrete check - Along the straight line between two nodes, sample at equal intervals and apply the L2 geometric test to each point (sampling formula and symbols in Appendix A.4).

2.

Dynamic predictive check - Using the UAV kinematics, propagate the state forward for several look-ahead steps; each predicted waypoint is, again, subjected to the L2 test. The resulting recursion and terminal velocity assignment are summarized in Appendix A.5.

The two-stage check first rules out collisions along a static straight line, then predicts motion under dynamic constraints to catch potential collisions that could occur due to the UAV’s inertia or acceleration limits.

3.4.2. Static Linear-Discrete Check

We partition the candidate straight segment from ${\mathbf{x}}_{\mathrm{nearest}}$ to ${\mathbf{x}}_{\mathrm{new}}$ into a fixed number of equally spaced samples and apply the L2 geometric test to each. The exact interpolation expression, index range, and symbol definitions are listed in Appendix A.4. If any sampled point lies inside ${\mathcal{O}}_{\mathrm{scyl}}\cup {\mathcal{O}}_{\mathrm{obb}}$, the segment is rejected; otherwise, the planner proceeds. By sampling evenly along the candidate segment, this check detects any intersection with obstacles that could be missed by testing only the endpoints.

3.4.3. Dynamic Predictive Collision Checking

Under the UAV’s current velocity and maximum acceleration constraints, the straight-line segment between two nodes is validated by predicting uniformly accelerated motion and verifying each predicted waypoint via the L2 geometric-precision layer. Let ${\mathbf{v}}_{\mathrm{nearest}}$ be the velocity at the nearest node with magnitude $v_0=\parallel {\mathbf{v}}_{\mathrm{nearest}}\parallel$, $a_{max}$ the acceleration limit, $D=\parallel {\mathbf{x}}_{\mathrm{new}}-{\mathbf{x}}_{\mathrm{nearest}}\parallel$ the inter-node distance, and $\mathsf{\Delta }t$ the time step. The minimum reachable time and the number of look-ahead steps are

$$\begin{array}{cc}\hfill T_{\min }& ={\displaystyle \frac{2D}{v_0+\sqrt{v_0^2+2a_{max}D}}},\hfill \\ \hfill N& =⌈\frac{T_{\min }}{\mathsf{\Delta }t}⌉.\hfill \end{array}$$

(8)

These expressions compute the shortest possible traversal time and the required number of prediction steps to ensure dynamic feasibility given current speed and acceleration limits.

We propagate along the segment with a clipped acceleration recursion and assign a terminal velocity for continuity; the full recursions and symbols (including the terminal assignment) are given in Appendix A.5. The recursion simulates the UAV’s motion step by step, clipping acceleration to physical limits so that predicted positions remain both collision-free and kinematically achievable.

3.5. Goal Determination, Path Retracing, and Termination Criteria [22]

When a newly generated node ${\mathbf{x}}_{\mathrm{new}}$ satisfies $\parallel {\mathbf{x}}_{\mathrm{new}}-{\mathbf{x}}_{\mathrm{goal}}\parallel <s$ and the edge ${\mathbf{x}}_{\mathrm{new}}\to {\mathbf{x}}_{\mathrm{goal}}$ is collision-free, the goal node is appended to the search tree:

$$\begin{array}{cc}\hfill \mathrm{Cost}\left({\mathbf{x}}_{\mathrm{goal}}\right)& =\mathrm{Cost}\left({\mathbf{x}}_{\mathrm{new}}\right)+∥{\mathbf{x}}_{\mathrm{goal}}-{\mathbf{x}}_{\mathrm{new}}∥,\hfill \\ \hfill {\mathbf{v}}_{\mathrm{goal}}& ={\displaystyle \frac{{\mathbf{x}}_{\mathrm{goal}}-{\mathbf{x}}_{\mathrm{new}}}{\mathsf{\Delta }t}},\hfill \\ \hfill \mathrm{parent}\left({\mathbf{x}}_{\mathrm{goal}}\right)& ={\mathbf{x}}_{\mathrm{new}}.\hfill \end{array}$$

(9)

Once ${\mathbf{x}}_{\mathrm{goal}}$ has been inserted into the tree $\mathcal{T}$, the optimal path is retraced recursively from the goal back to the root:

$$\begin{array}{cc}\hfill {\mathcal{P}}_0& ={\mathbf{x}}_{\mathrm{goal}},\hfill \\ \hfill {\mathcal{P}}_{k+1}& =\mathrm{parent}\left({\mathcal{P}}_k\right),\hfill \\ \hfill k& =0,1,\dots ,K-1.\hfill \end{array}$$

(10)

until ${\mathcal{P}}_K={\mathbf{x}}_{\mathrm{start}}$. The resulting trajectory, ordered in time, is

$$\mathcal{P}={\left[{\mathbf{x}}_{\mathrm{start}},{\mathcal{P}}_{K-1},\dots ,{\mathcal{P}}_1,{\mathbf{x}}_{\mathrm{goal}}\right]}^{\mathsf{T}}.$$

If the iteration count reaches the maximum $N_{max}$ without generating any node satisfying $\parallel {\mathbf{x}}_{\mathrm{new}}-{\mathbf{x}}_{\mathrm{goal}}\parallel <s$ with a collision-free connection, planning is deemed to have failed and an empty path $\mathcal{P}=⌀$ is returned. This termination criterion, governed by $N_{max}$, balances the global search capability against computational efficiency.

• Section summary: A hierarchical MA-AADRC-SMC scheme is designed: an adaptive ESO estimates disturbances for feedforward rejection; PD gains scale with bandwidth; an SMC term provides robust compensation; and an SDF-based repulsive field supplies precision obstacle avoidance. A speed-weighted blending integrates avoidance with tracking for agile yet smooth responses.

4. Flight Control Algorithm

4.1. Hierarchical Hybrid Control Algorithm Architecture Overview

The hierarchical control framework integrates environment perception, path planning, formation maintenance, hybrid tracking, and emergency recovery into a unified workflow. Each module exchanges information in real time to ensure safe and reliable flight in dynamic 3D urban environments. The overall architecture is summarized in Figure 4.

4.2. Adaptive Design of the Extended-State Observer (ESO)

4.2.1. Problem Statement

Consider a general nth-order, single-input single-output (SISO) nonlinear system with partially unknown dynamics:

$$x^{\left(n\right)}=f\left(x,\dot{x},\dots ,x^{(n-1)},t\right)+bu\left(t\right)+w\left(t\right).$$

(11)

where the measurable output is $x\in \mathbb{R}$, $u\left(t\right)\in \mathbb{R}$ is the control input, $b>0$ is the (known or identified) input gain, and the lumped total uncertainty is $d\left(t\right):=f(·)+w\left(t\right)$.

4.2.2. Extended-State Variables and Observer Structure [23]

We employ an $(n+1)$-dimensional ESO whose state augments the plant with the total disturbance. The full continuous-time realization, matrices, and symbols are provided in Appendix A.6.

4.2.3. Bandwidth-Pole Alignment Design

We place the observer poles by matching a bandwidth-selected characteristic polynomial. The polynomial and nominal gain expressions are summarized in the Appendix A.7. This placement yields equally spaced poles along the negative real axis, with the bandwidth ${\omega }_o$ directly determining the observer’s response speed and noise sensitivity.

4.2.4. Adaptive Bandwidth Mechanism

To accelerate convergence under time-varying disturbances, we schedule the ESO bandwidth with an error-driven law; the online gain update follows automatically from this bandwidth. The scheduling law and the resulting time-varying gains are given in Appendix A.8. When the estimation error grows, the bandwidth temporarily increases to speed up correction; as the error shrinks, the bandwidth decreases to reduce noise amplification.

The complete adaptive active disturbance rejection control (AADRC) scheme for UAV trajectory tracking is illustrated in Figure 5, where the ESO, nonlinear state error feedback (NLSEF), and adaptive law interact to reject disturbances in real time.

4.3. Multi-Agent Cooperative Formation and Adaptive Repulsive Force Scheduling

4.3.1. Cooperative Formation Mechanism

Under the leader–follower framework, let the leader’s trajectory ${\mathbf{r}}_{\mathrm{lead}}\left(t\right)$ be provided by the high-level path planner. The desired trajectory for follower i is defined as

$${\mathbf{r}}_{d,i}\left(t\right)={\mathbf{r}}_{\mathrm{lead}}\left(t\right)+\mathsf{\Delta }{\mathbf{r}}_i,i=2,\dots ,N$$

(12)

where $\mathsf{\Delta }{\mathbf{r}}_i$ is the fixed formation offset. Define the unit vector in the following direction as

$${\widehat{\mathbf{d}}}_i={\displaystyle \frac{{\mathbf{r}}_{d,i}-{\mathbf{x}}_i}{\parallel {\mathbf{r}}_{d,i}-{\mathbf{x}}_i\parallel +\delta }},$$

(13)

with $\delta$ a small positive constant to avoid division by zero. The nominal speed is then

$${\mathbf{v}}_{d,i}=v_{\mathrm{norm}}{\widehat{\mathbf{d}}}_i.$$

(14)

The velocity ${\mathbf{v}}_{d,i}$ captures only the ideal geometric formation requirement. When inter-agent distances approach the safety threshold $d_{\min }$, relying solely on ${\mathbf{v}}_{d,i}$ may lead to formation collapse or collision. To prevent this, a repulsive force–based velocity compensation mechanism is introduced.

First, the local repulsive force at agent i is constructed as

$${\mathbf{F}}_{\mathrm{rep},i}=K_c\sum _{j\ne i}{\displaystyle \frac{{\mathbf{x}}_i-{\mathbf{x}}_j}{\parallel {\mathbf{x}}_i-{\mathbf{x}}_j{\parallel }^2}}\mathsf{\Theta }\left(d_0-\parallel {\mathbf{x}}_i-{\mathbf{x}}_j\parallel \right),$$

(15)

where $\mathsf{\Theta }(·)$ is the Heaviside function, $d_0>d_{\min }$ is the repulsion activation radius, and $K_c$ is updated online via the gradient–projection law in Equation (20).

Next, map ${\mathbf{F}}_{\mathrm{rep},i}$ linearly to a velocity correction:

$$\mathsf{\Delta }{\mathbf{v}}_i=k_{\mathrm{obs},i}\left(\parallel {\mathbf{v}}_i\parallel \right){\mathbf{F}}_{\mathrm{rep},i},$$

(16)

where $k_{\mathrm{obs},i}(·)$ is a speed-weighting function (see Section 4.4.4), ensuring more agile repulsive response at higher speeds. The actual velocity command issued by this layer is therefore

$${\mathbf{v}}_i^{\mathrm{cmd}}={\mathbf{v}}_{d,i}+\mathsf{\Delta }{\mathbf{v}}_i.$$

(17)

When the average distance $d_{ij}^{\mathrm{avg}}<d_{\min }$, Equations (15) and (16), amplified by $K_c$, generate sufficient repulsion to restore safe separation; once distances recover, $\mathsf{\Delta }{\mathbf{v}}_i$ naturally decays, avoiding unnecessary disturbance to the formation.

4.3.2. Gain Projection Law Based on Group Average Inter-Agent Distance

After defining the ideal formation positions ${\mathbf{r}}_{d,i}$ and nominal velocities ${\mathbf{v}}_{d,i}$ via Equations (12)–(14), we next evaluate and dynamically feedback the safety separation between the actual formation and this ideal reference. First, the pairwise inter-agent distances are computed as

$$d_{ij}=\parallel {\mathbf{x}}_i-{\mathbf{x}}_j\parallel ,$$

(18)

and the group’s average inter-agent distance is defined by

$$d_{ij}^{\mathrm{avg}}={\displaystyle \frac{2}{N(N-1)}}\sum _{1\le i<j\le N}{d}_{ij}.$$

(19)

When $d_{ij}^{\mathrm{avg}}$ falls below the preset safety threshold $d_{\min }$, the formation is at risk of “collapse” and the repulsive force must be temporarily amplified; once the distances recover into the safe region, the repulsion should remain moderate. To achieve this while preventing excessive gain growth that could destabilize the formation, we introduce the following gradient-projection update law:

$${\dot{K}}_c=\gamma \sigma \left(d_{\min }-d_{ij}^{\mathrm{avg}}\right),K_c\in [K_{c,\min },K_{c,max}],$$

(20)

where $\gamma >0$ is the learning rate and $\sigma (·)$ is the continuous, piecewise-linear mapping

$$\sigma \left(x\right)=\left\{\begin{array}{cc}x,\hfill & x>0,\hfill \\ 0,\hfill & x\le 0.\hfill \end{array}\right.$$

Thus, when the average inter-agent distance is below $d_{\min }$, ${\dot{K}}_c>0$ increases the repulsion strength; once the distance returns to the safe zone, ${\dot{K}}_c=0$ ensures stable gains. Applying a projection operator constrains $K_c$ strictly within $[K_{c,\min },K_{c,max}]$, preventing over-amplification.

4.4. Main Control Law and Sliding-Mode Compensation

4.4.1. Adaptive Active Disturbance Rejection Control (AADRC) Law Design [23,24]

• Tracking Differentiator (TD)

The high-level path planner’s output ${\mathbf{r}}_{d,i}\left(t\right)$ may contain step or high-frequency components; to avoid noise amplification by direct differentiation, we employ a second-order TD to generate smooth references. The TD equations and symbol definitions are provided in Appendix A.9. This preprocessing smooths the raw reference signal, producing differentiable position and velocity commands suitable for feedback control.

Consider a multi-agent system of N UAVs with the ESO estimate for the ith agent

$${\mathbf{z}}_i={\left[z_{1,i},z_{2,i},z_{3,i}\right]}^{\top }\approx {\left[x_i,{\dot{x}}_i,d_i\right]}^{\top },$$

where $d_i\left(t\right)$ denotes the total disturbance. Given the raw reference ${\mathbf{r}}_{d,i}\left(t\right)$ and its TD outputs $(r_{1,i},r_{2,i})$, define the tracking errors

$$e_i=z_{1,i}-r_{1,i},{\dot{e}}_i=z_{2,i}-r_{2,i}.$$

The tracking errors represent the deviation in position and velocity between the UAV’s estimated state and the smoothed reference.

To balance rapid response with robustness, we introduce an adaptive control bandwidth ${\omega }_{c,i}\left(t\right)$ and update the PD gains online as

$$K_{p,i}\left(t\right)={\kappa }_p{\omega }_{c,i}^2,K_{d,i}\left(t\right)=2{\kappa }_d{\omega }_{c,i},$$

where ${\kappa }_p,{\kappa }_d>0$ are tuning coefficients. Adjusting the gains through the control bandwidth allows the controller to speed up or slow down its response in real time.

The resulting MA-AADRC control law is

$$u_{\mathrm{AADRC},i}=-K_{p,i}{e}_i-K_{d,i}{\dot{e}}_i-z_{3,i}.$$

The PD terms drive the tracking error to zero, while the disturbance estimate $z_{3,i}$ cancels the effect of unknown dynamics.

4.4.2. Sliding-Mode Robust Compensator (SMC)

To further suppress intense disturbances and residual model uncertainties, the sliding surface is designed as

$$s_i={\dot{e}}_i+\lambda e_i,\lambda >0.$$

(21)

A saturation-based strategy is employed to mitigate chattering, yielding the sliding-mode compensation term

$$\begin{array}{cc}\hfill u_{\mathrm{SMC},i}& =-K_s\mathrm{sat}\left({\displaystyle \frac{s_i}{\epsilon }}\right),\hfill \\ \hfill \mathrm{sat}\left(x\right)& =\left\{\begin{array}{cc}x,\hfill & \left|x\right|<1,\hfill \\ \mathrm{sgn}\left(x\right),\hfill & \left|x\right|\ge 1.\hfill \end{array}\right.\hfill \end{array}$$

(22)

where $K_s>0$ is the robust gain and $\epsilon >0$ is the boundary-layer width. The condition $K_s>{\parallel \mathsf{\Delta }{u}_i\parallel }_{\infty }$ ensures finite-time convergence of the sliding variable to the sliding surface [25]. In practice, $K_s$ may be chosen proportionally to the UAV’s flight speed to reduce energy consumption.

4.4.3. Gradient Potential Field-Based High-Precision Obstacle Avoidance Criterion

In the flight control loop, to achieve high-precision avoidance of arbitrarily shaped obstacles, we introduce a differentiable potential field based on the signed distance function (SDF) and apply piecewise smoothing. Let the signed distance to the ith obstacle be

$${\mathrm{SDF}}_i\left(\mathbf{p}\right)=\underset{\mathbf{y}\in {\mathcal{O}}_i}{\min }\parallel \mathbf{p}-\mathbf{y}\parallel (\ge 0\mathrm{outside}{\mathcal{O}}_i),$$

(23)

with the corresponding closest point ${\mathbf{p}}_i^{*}$ and $d_i={\mathrm{SDF}}_i\left(\mathbf{p}\right)$. Define the potential energy function

$$U\left(\mathbf{p}\right)=\sum _{i=1}^{N_{\mathcal{O}}}{\displaystyle \frac{k_{\mathrm{rep}}}{2}}{\left[max\left(0,\frac{1}{d_i}-\frac{1}{d_0}\right)\right]}^2,$$

(24)

where $k_{\mathrm{rep}}>0$ is the repulsion scale factor and $d_0$ is the activation radius (the potential is nonzero only if $d_i<d_0$). The resulting repulsive force field is

$$\begin{array}{cc}\hfill {\mathbf{F}}_{\mathrm{rep}}\left(\mathbf{p}\right)& =-\nabla U\left(\mathbf{p}\right)\hfill \\ \hfill & =\sum _{i=1}^{N_{\mathcal{O}}}{k}_{\mathrm{rep}}\left(\frac{1}{d_i}-\frac{1}{d_0}\right)\frac{1}{d_i^2}\left({\displaystyle \frac{\mathbf{p}-{\mathbf{p}}_i^{*}}{d_i}}\right)\mathsf{\Theta }(d_0-d_i).\hfill \end{array}$$

(25)

4.4.4. Integrated Control Law

After computing the ADRC control input $u_{\mathrm{ADRC},i}$ and the SMC compensation $u_{\mathrm{SMC},i}$, the final control input is obtained by velocity-weighting the obstacle repulsive force according to each UAV’s airspeed:

$$u_i=u_{\mathrm{ADRC},i}+u_{\mathrm{SMC},i}+k_{\mathrm{obs},i}\left(\parallel {\mathbf{v}}_i\parallel \right){\mathbf{F}}_{\mathrm{rep},i},$$

(26)

where ${\mathbf{F}}_{\mathrm{rep},i}=-\nabla U\left({\mathbf{p}}_i\right)$ is the composite gradient-based repulsive force from Equation (18). The speed-adaptive weighting function

$$k_{\mathrm{obs},i}\left(\parallel {\mathbf{v}}_i\parallel \right)=k_0\left(1+{\displaystyle \frac{\parallel {\mathbf{v}}_i\parallel }{v_{\mathrm{ref}}}}\right)$$

(27)

increases linearly with the UAV’s current airspeed $\parallel {\mathbf{v}}_i\parallel$, ensuring more agile obstacle-avoidance response at higher speeds.

4.5. Real-Time Multi-Level Collision Detection, Evasive Maneuvers, and Velocity Compensation

4.5.1. Multi-Level Obstacle Collision Detection

To satisfy both real-time performance and safety in high-density obstacle environments, we adopt a four-layer progressive collision detection pipeline:

• Spatial hashing (coarse filter) [26]: place obstacle centroids into a 3D hash grid and only keep the cell of the UAV and its 26 neighbors. Formulas and symbols in Appendix A.10.1. This coarse filter quickly reduces the candidate obstacle set from N to a small constant, ensuring constant-time queries.
• AABB tree culling [27]: traverse an AABB hierarchy and prune nodes by a conservative bounding-sphere test. Inequality and symbols in Appendix A.10.2. This step prunes large portions of space by discarding nodes in such a way that they are too far from the UAV for a collision to occur, leaving only spatially relevant obstacles.
• GJK distance check [28]: estimate the minimum distance between the UAV bounding sphere and each convex obstacle that survived culling. Definition and symbols in Appendix A.10.3. This algorithm precisely computes the closest approach between convex shapes, triggering fine-scale checks when the distance is below a safety margin.
• Continuous-time SAT [29]: within a control period, combine the predicted trajectory with an obstacle SDF to solve the first contact time; flag a potential collision if contact occurs before the period ends. Equations and symbols in Appendix A.10.4. This final layer ensures temporal completeness, detecting fast-moving collisions that discrete sampling could miss.

This hierarchical design progressively narrows the candidate set, keeping per-cycle complexity near constant while maintaining strict collision safety.

4.5.2. Collision Avoidance Maneuver

For the nearest obstacle ${\mathcal{O}}^{★}$, compute the gradient of the signed distance function at the first contact point:

$${\mathbf{n}}_{★}=\nabla {\mathrm{SDF}}_{{\mathcal{O}}^{★}}\left(\mathbf{p}\left(t_{\mathrm{hit}}\right)\right),{\parallel {\mathbf{n}}_{★}\parallel }_2=1,$$

(28)

and construct the equivalent repulsive force:

$${\mathbf{F}}_{\mathrm{rep}}=-{\kappa }_{\mathrm{e}}{\displaystyle \frac{r_{\mathrm{UAV}}+d_{\mathrm{safe}}-d_{\min }}{d_{\min }^2}}{\mathbf{n}}_{★},$$

(29)

where ${\kappa }_{\mathrm{e}}>0$ is the energy-type potential gain, $r_{\mathrm{UAV}}$ the UAV bounding-sphere radius, $d_{\mathrm{safe}}$ the safety distance, and $d_{\min }$ the minimum distance. This repulsive force is injected as an additional input into the MA-AADRC+SMC framework, achieving a soft-constraint avoidance.

To ensure dynamic continuity and rapid exit from the danger zone, the velocity is real-time scaled and biased:

$$\begin{array}{cc}\hfill {\mathbf{v}}_{k+1}& ={\beta }_s{\mathbf{v}}_k+{\alpha }_s{\displaystyle \frac{{\mathbf{F}}_{\mathrm{rep}}}{\parallel {\mathbf{F}}_{\mathrm{rep}}{\parallel }_2}}{v}_{max},\hfill \\ \hfill {\beta }_s& \in (0,1),{\alpha }_s=1-{\beta }_s.\hfill \end{array}$$

(30)

When $d_{\min }\ge d_{\mathrm{safe}}+{\delta }_{\mathrm{hyst}},{\parallel {\mathbf{F}}_{\mathrm{rep}}\parallel }_2<{\epsilon }_{\mathrm{rep}},$ with hysteresis threshold ${\delta }_{\mathrm{hyst}}>0$ and repulsive threshold ${\epsilon }_{\mathrm{rep}}>0$ both satisfied, the system exits emergency mode and starts PID velocity tracking to avoid frequent switching. This maneuver integrates seamlessly with the multi-level detection framework, ensuring UAV flight safety and control convergence in dense obstacle environments.

4.5.3. Speed Compensation

Let $\mathbf{v}\in {\mathbb{R}}^3$ be the UAV’s current linear velocity, and define the unit direction toward the desired position as

$$\widehat{\mathbf{d}}={\displaystyle \frac{{\mathbf{r}}_d-\mathbf{p}}{\parallel {\mathbf{r}}_d-\mathbf{p}\parallel }},$$

(31)

where $v_{\mathrm{ref}}$ is the nominal cruise speed. The desired velocity vector is

$${\mathbf{v}}^{*}=v_{\mathrm{ref}}\widehat{\mathbf{d}}.$$

(32)

Define the velocity error and its derivative by

$${\mathbf{e}}_v={\mathbf{v}}^{*}-\mathbf{v},{\dot{\mathbf{e}}}_v={\displaystyle \frac{\mathrm{d}}{\mathrm{d}t}}{\mathbf{e}}_v.$$

(33)

The three-dimensional PID acceleration compensation law is

$${\mathbf{a}}_{\mathrm{PID}}=K_p{\mathbf{e}}_v+K_i{\int }_0^t{\mathbf{e}}_v\left(\tau \right)\mathrm{d}\tau +K_d{\dot{\mathbf{e}}}_v,$$

(34)

with gains tuned in cascade as $K_p=2\zeta {\omega }_v$, $K_d={\omega }_v$, $K_i=\frac{1}{2}{\omega }_v{K}_p$, where ${\omega }_v$ is the velocity-loop bandwidth and $\zeta$ the damping ratio. Classical PID controllers have been widely used in quadrotor research due to their simplicity and effectiveness [30]. To prevent integral windup, a conditional integration strategy is employed:

$$\begin{array}{cc}\hfill \dot{\mathbf{I}}\left(t\right)& =\left\{\begin{array}{cc}{\mathbf{e}}_v,\hfill & \parallel {\mathbf{a}}_{\mathrm{PID}}\parallel <a_{max},\hfill \\ \mathbf{0},\hfill & \mathrm{otherwise},\hfill \end{array}\right.\hfill \\ \hfill {\mathbf{a}}_{\mathrm{PID}}& =K_p{\mathbf{e}}_v+K_i\mathbf{I}+K_d{\dot{\mathbf{e}}}_v.\hfill \end{array}$$

(35)

Finally, ${\mathbf{a}}_{\mathrm{PID}}$ is added to the control law (46) and the result is norm-saturated $\parallel \mathbf{a}\parallel \le a_{max}$ to respect thrust limits. This vector PID scheme ensures exponential convergence to the desired cruise speed after obstacle avoidance or gust disturbances and, through anti-saturation logic, prevents integral windup, guaranteeing smooth closed-loop transitions.

• Section summary: Under identical initial conditions and gust disturbances, the proposed method achieves faster convergence, lower steady-state tracking error, tighter formation keeping, and smaller spacing overshoot than ADRC and PID. The results confirm robustness and real-time feasibility in cluttered 3-D environments.

5. Simulation Experiments

5.1. Experimental Environment and Parameter Settings

All simulations were conducted in MATLAB R2023b on an Intel i5-9300H CPU. Five UAVs performed trajectory tracking and obstacle avoidance inside a cubic workspace of ${[0,100]}^3{\mathrm{m}}^3$ that contained 17 randomly placed obstacles satisfying a minimum inter-obstacle spacing of 5 m. Every controller was evaluated under identical initial states and an equivalent step-gust wind disturbance.

5.2. Rationale and Micro-Sensitivity

Rationale. The simulation and controller parameters were determined through a hierarchical procedure combining UAV kinematic constraints, control-theoretic considerations, and lightweight empirical tuning. This ensures that each parameter has a clear physical or theoretical basis, and the final set is both feasible and representative of urban UAV operations. Specifically:

(i) Workspace size (${[0,100]}^3$ m³), maximum speed ($v_{max}=12$ m/s), and acceleration limit ($a_{max}=6$ m/s²) were chosen to yield $\mathcal{O}\left(10\right)$-s missions with multiple obstacle interactions, while remaining within realistic thrust and maneuverability bounds.

(ii) Obstacles combine super-quadric cylinders (poles/trees) and OBBs (containers/low facades), with dimensions drawn from ranges (

Table A2. Obstacle geometry and sampling parameters.

Parameter	Value
Super-quadric shape index, $ϵ$	1 (cylinder)
Cylinder radius, r	$\mathcal{U}(3,7)$ m
Cylinder height, $h_{\mathrm{cyl}}$	$10+50\mathcal{U}(0,1)$ m
OBB minor/major sides, $(W,L)$	$\mathcal{U}(6,14)$ m
OBB height, $h_{\mathrm{obb}}$	$10+50\mathcal{U}(0,1)$ m
OBB yaw angle, $\gamma$	$\mathcal{U}(0,2\pi )$ rad
Poisson-disk spacing, $d_{\min }^{\mathrm{obs}}$	20 m

) and Poisson-disk spacing to avoid overlaps. This results in a medium clutter density (17 obstacles) that is neither trivial nor infeasible.

(iii) The composite wind field (multi-harmonic + step gust + altitude scaling; Section 2.1.1) captures steady peaks, sudden gusts, and vertical gradients, reflecting the turbulence patterns encountered in low-altitude urban UAV envelopes.

(iv) Planner hyperparameters (s, goal-bias p, and static check samples $n_{\mathrm{checks}}$) were tuned to balance path optimality and runtime efficiency. Continuous-time feasibility checks further enforce kinematic validity (Section 3.4).

(v) Controller and observer parameters (

Table A6. Observers and baseline controller parameters.

Parameter	Value
ESO gains (Hybrid), $({\beta }_1,{\beta }_2,{\beta }_3)$	(50, 250, 1000)
PD gains (Hybrid), $(K_p,K_d)$	(300, 50)
Sliding-mode gains, $(K_s,\lambda )$	(150, 35)
ESO gains (ADRC), $({\beta }_1,{\beta }_2,{\beta }_3)$	(50, 200, 800)
PD gains (ADRC), $(K_{p,\mathrm{adrc}},K_{d,\mathrm{adrc}})$	(500, 350)
PID gains $(K_p,K_i,K_d)$	(500, 50, 350)
PID anti-windup limit	100

and

Table A7. Adaptive and robust module parameters.

Parameter	Value
Baseline ESO bandwidth, ${\omega }_{o0}$	30 rad/s
Bandwidth scheduling gain, $\kappa$	2
Adaptive PD coefficients, $({\kappa }_p,{\kappa }_d)$	(1, 1)
TD bandwidth / damping, $({\omega }_t,{\zeta }_t)$	(20 rad/s, 0.7)
Velocity-loop bandwidth / damping, $({\omega }_v,\zeta )$	(10 rad/s, 0.75)
Sliding boundary-layer width, $\epsilon$	0.02
Obstacle weighting base, $k_0$	1.2

) were selected so that the closed-loop response time is shorter than the trajectory-curvature time scale, thus limiting overshoot while preserving disturbance rejection. Fine-tuning of gains (e.g., $K_p$, ${\beta }_i$) was performed through lightweight simulation sweeps with three design criteria: (a) stability margin not violated, (b) steady-state error minimized, and (c) robustness maintained under varying wind and obstacle conditions.

Micro-sensitivity (design & criteria). To ensure conclusions do not hinge on a single random draw with minimal compute, we repeat simulations with three independent seeds $\{11,29,47\}$ (affecting obstacle placement/sizes and wind phases) and perform two lightweight sweeps: (i) RRT* step size $s\in \{1,3\}$ (baseline is $s = 2$); (ii) Obstacle count $N_{\mathrm{obs}}\in \{10,25\}$ (baseline is 17). For each setting and controller (Hybrid/ADRC/PID/SMC), we report: (1) The success rate (goal reached within 15 s; same criterion as the main study), (2) The steady-state tracking error averaged over $t\in [6,15]$ s (averaged across UAVs), (3) The minimum inter-UAV separation over the whole horizon (safety threshold 5 m). The summary appears in

Table 1. Micro-sensitivity results across random seeds and settings. Metrics are reported as mean ± std.

Setting	Controller	Success (%)	Steady-State Error (m)	Min Separation (m)
Baseline ($s = 2$, $N_{\mathrm{obs}} = 17$)	Hybrid	$99.85\pm 0.12$	$2.63\pm 0.41$	$5.32\pm 0.22$
	SMC	$99.72\pm 0.18$	$3.28\pm 0.47$	$5.27\pm 0.25$
	PID	$98.62\pm 0.74$	$4.59\pm 0.66$	$5.19\pm 0.31$
	ADRC	$96.24\pm 1.12$	$7.98\pm 0.85$	$5.14\pm 0.38$
Step size $s = 1$	Hybrid	$99.91\pm 0.09$	$2.45\pm 0.39$	$5.36\pm 0.19$
	SMC	$99.78\pm 0.14$	$3.01\pm 0.52$	$5.33\pm 0.21$
	PID	$99.02\pm 0.36$	$4.32\pm 0.59$	$5.18\pm 0.27$
	ADRC	$97.15\pm 0.88$	$7.84\pm 0.74$	$5.22\pm 0.29$
Step size $s = 3$	Hybrid	$99.67\pm 0.21$	$2.81\pm 0.48$	$5.24\pm 0.28$
	SMC	$98.92\pm 0.56$	$3.65\pm 0.63$	$5.18\pm 0.31$
	PID	$97.41\pm 0.92$	$4.77\pm 0.71$	$5.09\pm 0.34$
	ADRC	$95.68\pm 1.43$	$8.45\pm 0.92$	$5.05\pm 0.37$
$N_{\mathrm{obs}} = 10$	Hybrid	$99.94\pm 0.07$	$2.36\pm 0.36$	$5.42\pm 0.23$
	SMC	$99.84\pm 0.11$	$2.97\pm 0.45$	$5.35\pm 0.20$
	PID	$99.11\pm 0.28$	$4.25\pm 0.58$	$5.15\pm 0.26$
	ADRC	$97.86\pm 0.62$	$7.72\pm 0.67$	$5.25\pm 0.24$
$N_{\mathrm{obs}} = 25$	Hybrid	$99.61\pm 0.25$	$2.98\pm 0.53$	$5.18\pm 0.30$
	SMC	$98.34\pm 0.72$	$3.79\pm 0.61$	$5.13\pm 0.32$
	PID	$96.83\pm 1.04$	$4.88\pm 0.76$	$5.06\pm 0.36$
	ADRC	$95.42\pm 1.68$	$8.61\pm 0.98$	$5.02\pm 0.41$

.

5.3. Evolution Trend Analysis of Path-Tracking Error

As shown in Figure 6, the average path tracking errors vary noticeably among different control algorithms. The hybrid control algorithm maintains relatively small errors, mostly within 2–3 m, with a transient peak of about 6 m around 4.5 s, followed by rapid stabilization. The sliding-mode control (SMC) initially rises to approximately 9 m, but decreases sharply after 2.5 s and remains below 3 m in the subsequent stage. By contrast, the PID controller reaches nearly 12 m between 5 and 7 s, and although the error reduces afterwards, a residual of 2.5–3 m persists at 15 s, indicating slower convergence. The ADRC, however, shows larger oscillations ranging between 7 and 12 m for most of the time, with only a limited reduction at the end. Overall, the hybrid approach and SMC exhibit relatively better steady-state accuracy, while PID and ADRC demonstrate a less favorable dynamic performance.

As shown in Figure 7, four error metrics of different control algorithms in path tracking are compared: average error, standard deviation of error, maximum error, and stable average error. Among all methods, the hybrid control algorithm achieves the smallest errors, with an average error of approximately 3 m, a stable average error around 2.5 m, and low variability, indicating both good convergence and stability. The SMC shows comparable steady-state performance, with slightly higher average errors (3.5 m) and a stable error near 2.6 m, though its maximum error reaches about 11.5 m. In contrast, PID and ADRC exhibit substantially larger errors, with average errors of 7.8 m and 8.3 m, and stable errors around 5.5 m and 8.1 m, respectively, while their maximum errors approach or exceed 15 m, reflecting larger fluctuations. Overall, the hybrid algorithm demonstrates the most favorable balance between steady-state accuracy and error stability, outperforming the other three methods, while SMC shows secondary advantages, and PID and ADRC are relatively less effective.

As shown in Figure 8, the tracking errors of five UAVs under different control algorithms are compared. It is evident that the hybrid control algorithm consistently delivers the best stability and accuracy across all UAVs, with errors mostly confined to 2–4 m and a rapid convergence after initial disturbances. Although the sliding-mode control (SMC) also achieves relatively fast convergence, its initial peaks typically range from 8 to 12 m, and its steady-state errors are slightly higher than those of the hybrid method. The PID controller exhibits larger transient errors on some UAVs (e.g., peaks exceeding 15 m for UAV2 and UAV3) and converges more slowly. ADRC shows persistent oscillations in most cases, with errors generally fluctuating between 7–12 m, indicating limited convergence. Overall, the hybrid approach maintains the lowest steady-state errors and stronger robustness across multiple platforms, demonstrating superior performance compared with the other three methods.

5.4. Average Formation Spacing Comparison

In UAV formation control tasks, maintaining a stable formation structure is critical for group cooperative flight, obstacle avoidance capability, and error recovery performance. Different control strategies exhibit significant differences in formation stability, dynamic obstacle avoidance, and error convergence. Therefore, it is necessary to conduct systematic experimental analyses to evaluate the applicability and performance advantages of each control method.

As shown in Figure 9, the evolution of average UAV spacing under different control algorithms is compared with the minimum desired spacing. The hybrid control algorithm consistently maintains the desired spacing, with a peak of about 9.5 m, followed by rapid convergence after 5 s and stable tracking near the desired value (5 m). The SMC shows a similar trend, with an initial peak around 11 m, but it quickly converges and stabilizes near 5 m. In contrast, the PID controller produces much larger fluctuations, with peaks exceeding 18 m, and remains above the desired spacing even after 10 s, indicating slower convergence. The ADRC also exhibits large initial oscillations (peak above 14 m), though it gradually converges to the desired spacing around 6 s. Overall, the hybrid algorithm and SMC achieve better performance in maintaining desired spacing and stability, with the hybrid approach exhibiting the smallest overshoot and the most consistent convergence.

In Figure 10, a statistical comparison of UAV spacing under different control algorithms is presented, including the mean value, minimum value, maximum value, and standard deviation, with the minimum desired spacing indicated as a reference. Among all methods, the hybrid control algorithm achieves the most favorable balance, with a mean spacing of about 6.7 m, a minimum spacing close to the desired threshold (5 m), and the smallest variability, as indicated by its low standard deviation. The SMC shows similar performance with a mean spacing of approximately 7.5 m, also maintaining the minimum spacing requirement, though its maximum spacing (11.5 m) is slightly larger. In contrast, PID and ADRC produce significantly larger deviations: PID yields the largest mean spacing (14.5 m) and maximum spacing (19 m), while ADRC results in a mean spacing of 8.8 m and a maximum of 15 m, both indicating larger fluctuations. Overall, the hybrid algorithm demonstrates the best ability to maintain safe and stable inter-UAV spacing, followed by SMC, whereas PID and ADRC are less effective.

5.5. Formation Maintenance Analysis

In multi-UAV cooperative flight missions, formation stability is a key metric for assessing formation-control performance. Different control strategies vary in their ability to match UAV speeds, regulate formation error, and recover after obstacle avoidance, directly impacting the overall stability and execution quality of the formation. In this section, we analyze the formation-keeping error and its temporal evolution under three control methods—Hybrid Control, ADRC, and PID—to elucidate the performance characteristics and applicability of each strategy.

As shown in Figure 11, the formation-keeping errors under different control algorithms are compared over time. The hybrid control algorithm maintains relatively small errors throughout the process, with an initial fluctuation peaking at about 8.5 m, but rapidly decreasing after 5 s and stabilizing within the range of 2–3 m, demonstrating good convergence and stability. The SMC exhibits a similar trend, with an initial peak of approximately 11 m, but it also decreases quickly after 5 s and remains at a low error level. In contrast, the PID controller shows larger errors between 3 and 7 s, with a peak close to 16 m, and converges more slowly, with errors still around 5 m after 10 s. The ADRC produces even larger oscillations, reaching a maximum error of nearly 21 m, and maintains relatively high errors of 6–8 m beyond 10 s. Overall, the hybrid algorithm achieves the best formation-keeping accuracy and convergence performance, followed by SMC, while PID and ADRC are less effective.

As shown in Figure 12, a statistical comparison of formation-keeping errors under different control algorithms is presented, including average error, maximum error, and standard deviation. The hybrid control algorithm achieves the best overall performance, with the smallest average error (3 m), a low maximum error (9 m), and the lowest variability, indicating stable and accurate formation-keeping. The SMC also performs relatively well, with an average error of about 3.5 m and limited variability, though its maximum error reaches approximately 12 m. In contrast, PID and ADRC show significantly larger errors: both methods yield average errors close to 10 m, while their maximum errors reach 16 m and 21 m, respectively, accompanied by higher standard deviations. Overall, the hybrid approach demonstrates the most favorable formation-keeping accuracy and robustness, followed by SMC, while PID and ADRC are comparatively less effective.

As summarized in

Table 2. Quantitative performance comparison of different controllers. Metrics are reported in meters (m).

Metric	Hybrid	SMC	PID	ADRC
Path-Tracking Error
Average Error (m)	3.0	3.5	7.8	8.3
Stable Error (m)	2.5	2.6	5.5	8.1
Max Error (m)	6.0	11.5	15.0	15.0
Std. Dev. (m)	0.8	1.5	3.0	3.5
Inter-UAV Spacing
Mean Spacing (m)	6.7	7.5	14.5	8.8
Min Spacing (m)	5.0	5.0	9.0	6.0
Max Spacing (m)	9.5	11.5	19.0	15.0
Std. Dev. (m)	1.0	1.3	2.8	2.2
Formation-Keeping Error
Average Error (m)	3.0	3.5	10.0	10.0
Max Error (m)	9.0	12.0	16.0	21.0
Std. Dev. (m)	1.2	1.6	3.5	4.0

, the proposed hybrid control consistently achieves the smallest average and stable errors, the lowest maximum deviations, and the minimum variability across all scenarios. These metrics jointly confirm that the hybrid controller not only ensures high tracking accuracy but also maintains strong robustness against disturbances and fluctuations. In contrast, SMC shows secondary advantages with competitive steady-state performance but larger peak errors, while PID and ADRC exhibit substantially higher errors and variability, reflecting weaker dynamic adaptability. This quantitative evidence further validates the superior effectiveness and stability of the proposed approach compared with conventional methods.

5.6. Comparison of Multiple Path-Planning Schemes

Figure 13 illustrates the planned trajectories for a UAV flying from the start point $(0,0,30)$ to the goal $(100,100,30)$. The layout comprises four views: the top left is the $X-Y$ plane (plan view), the top right is a three-dimensional perspective, the bottom-left is the $X-Z$ plane (side view), and the bottom right is the $Y-Z$ plane (front view). The UAV successfully avoids multiple obstacles in a complex environment, following a smooth path that preserves formation. In the plan view, different paths exhibit varying avoidance strategies, with some showing superior smoothness and clearance. The side and front views confirm consistent altitude control, indicating that the planner maintains the UAVs within an appropriate flight-height band. The 3D perspective further demonstrates how obstacle placement shapes the trajectory, validating the algorithm’s effectiveness in three-dimensional spaces. In Figure 13, Figure 14 and Figure 15, colored solid lines denote the trajectories of five UAVs (UAV1-UAV5), while the dashed line indicates the reference path generated by the path-planning algorithm.

Additionally, two other cases are presented. Figure 14 shows a descent path from $(0,0,100)$ to $(100,100,0)$, where the UAV descends smoothly while avoiding obstacles. Figure 15 depicts a climb from ground level $(0,0,0)$ to $(100,100,100)$, showing gradual, stable ascent with effective obstacle avoidance. Together, these multi-view results confirm the planner’s adaptability and reliability for UAV path control in complex scenarios.

5.7. Lyapunov Convergence Proof and Detailed Analysis of Additional Experimental Results

This section provides a rigorous Lyapunov-based convergence proof for the Hybrid controller under wind-disturbance conditions and offers an in-depth analysis of the remaining simulation outcomes. To illustrate the control performance intuitively, we present the following plots: (i) position-tracking trajectories, (ii) position error and sliding-surface convergence and (iii) the Lyapunov function and its time derivative.

As shown in Figure 16, the time histories of the actual outputs of UAV1-UAV5 (solid lines) and the leader reference (dashed line) are compared along the (a) X, (b) Y, and (c) Z axes. In both horizontal axes, all agents closely track the reference with negligible steady-state error (below 1 m). Along the vertical axis, a transient overshoot of approximately 10–15 m occurs between 2 s and 5 s, after which the tracking errors converge to within $\pm 2$ m. These quantitative results confirm that the proposed hybrid controller achieves precise and robust 3D trajectory tracking in a multi-agent formation.

5.7.1. Error and Sliding-Surface Convergence

As shown in Figure 17, at $t\approx 1$ s a deliberate external disturbance was applied, causing the position error norm $\parallel e\parallel$ to spike to approximately $5.0$ m. The error then decays rapidly to below $0.3$ m within the next second and remains bounded between $0.2$ and $0.5$ m for the remainder of the simulation. Similarly, the sliding surface norm $\parallel s\parallel$ peaks at about 16 at the moment of disturbance and converges to under $1.0$, maintaining values below $0.5$ thereafter. These results confirm the finite-time convergence and robust sliding-mode regulation of the proposed hybrid controller in the presence of abrupt disturbances.

5.7.2. Lyapunov Function and Its Time Derivative

As shown in Figure 18, to assess the energy-dissipation capability of the proposed Hybrid Algorithms under extreme initial conditions, a step disturbance was injected at $t=1\mathrm{s}$. This excitation caused the quadratic Lyapunov function

$$V\left(t\right)=\frac{1}{2}\left({\parallel e\parallel }^2+{\parallel \dot{e}\parallel }^2\right)$$

to increase abruptly, reaching a peak of ${10}^1$ at $t=1.02\mathrm{s}$. Thereafter, $V\left(t\right)$ decays quasi-exponentially, converging to the ${10}^{-2}$ level within $5\mathrm{s}$. In addition, a minor secondary fluctuation around $t\approx 9\mathrm{s}$ reflects transient energy buffering at a trajectory corner. Correspondingly, $\dot{V}\left(t\right)$ remains nonpositive throughout, with a sharp negative spike at the moment of excitation, indicating rapid dissipation of the injected energy. These results confirm that the proposed Hybrid Algorithms maintain closed-loop asymptotic stability and high robustness in the face of sudden large disturbances.

These findings validate that the combined energy analysis based on the tracking error e and its derivative $\dot{e}$ is effectively captured by the Lyapunov function, providing a solid theoretical foundation for controller parameter tuning and guaranteeing reliable convergence of the multi-UAV formation under wind disturbances. In summary, the comprehensive evaluation of position-tracking performance, sliding-surface convergence, and Lyapunov-function evolution confirms that the proposed Hybrid Control Algorithms achieve rapid transient convergence, minimal steady-state error, and strong robustness against external disturbances.

• Section summary: Under identical initial conditions and gust disturbances, the proposed method achieves faster convergence, lower steady-state tracking error, tighter formation keeping, and smaller spacing overshoot than ADRC, SMC, and PID. The results confirm robustness and real-time feasibility in cluttered 3-D environments.

6. Results and Outlook

We address cooperative multi-UAV flight in windy urban 3-D spaces with dense obstacles and present an end-to-end framework from global planning to high-precision formation control. On the planning side, an artificial potential field-guided variant of RRT* embeds velocity and acceleration constraints, together with continuous collision prediction, directly into the sampling, extension, and rewiring stages; a four-layer collision-detection pipeline further reduces the worst-case per-cycle complexity to the expected order of $\mathcal{O}\left(1\right)$. On the control side, a two-loop MA-AADRC–SMC architecture with an error-driven-bandwidth ESO is devised: the outer, slower loop produces nominal commands, while the inner, faster loop allocates attitude and thrust in real time; a sliding-mode compensator, combined with a speed-weighted vector PID term, endows the system with strong robustness in obstacle avoidance and formation keeping. The two layers are deeply coupled via continuously feasible trajectories and real-time replanning, yielding a closed-loop optimisation scheme. Numerical experiments demonstrate marked improvements in path-tracking accuracy and formation stability under composite wind disturbances and random obstacle layouts, while the Lyapunov function remains monotonically decreasing, corroborating asymptotic closed-loop stability. Compared with classical ADRC and PID baselines, the proposed framework achieves rapid convergence, low steady-state error and safe obstacle avoidance without additional tuning, indicating high potential for engineering deployment.

Beyond simulation, the disturbance, obstacle, and kinematic models we adopt are parameterized to match typical civil multirotors (e.g., DJI Matrice-class) operating in urban airspace. The sensing and control assumptions—GNSS/RTK or visual-inertial odometry for localization, 3D LiDAR or depth cameras for mapping, and PX4/ROS2 middleware for control and communication—are standard in commercial UAV platforms. The proposed architecture therefore aligns with real use cases such as urban infrastructure inspection, post-disaster cooperative mapping, and multi-UAV logistics, suggesting that only minimal adaptation is required for deployment in practice.

It should be noted that the current validation relies entirely on large-scale simulations, which, while comprehensive, cannot fully capture real-world hardware nonlinearities, sensor noise, or communication delays. This limitation underscores the necessity of experimental verification as a crucial next step. In future research, we plan to further investigate the scalability of the proposed framework in large-scale UAV swarms, including scenarios with hundreds of agents, limited communication bandwidth, and GPS-denied environments. In addition, decentralized control strategies and energy-efficient optimization will be considered to enhance robustness and practicality. Hardware-in-the-loop experiments will also be conducted to validate the feasibility of the proposed methods under real-world conditions.