A Multilayer Decision-Making Method for UAV Formation Cooperative Flight in Complex Urban Environments Authors: Junjie Wang, Dongyu Yan, Yongping Hao, Han Miao Published: 2026 ABSTRACT To address the challenges of dynamic obstacles, limited perception, and multi-UAV coordination constraints in complex urban environments, a hierarchical control framework based on a virtual leader-follower architecture is proposed, covering global planning, local obstacle avoidance, and formation coordination. In the global planning layer, a dynamic adaptive strategy rapidly exploring random tree star (DASRRT*) algorithm is proposed. To address the low sampling efficiency and limited path extension in dense environments that affect traditional RRT*, a hybrid guided sampling strategy, inefficient node optimization strategy, and perception-based adaptive step size strategy are designed. Additionally, a multi-objective cost function is introduced to provide smoother trajectories that better comply with dynamic constraints for trajectory tracking. In the local obstacle-avoidance layer, a distributed controller is constructed based on an improved artificial potential field method, integrating collision avoidance control laws derived from a spring-damper model, dynamic obstacle-avoidance laws that account for obstacle velocities, and formation coordination control laws grounded in consensus theory. In the coordination control layer, a real-time local target selection strategy is established to guide the virtual leader to precisely track the global path, and a dual-mode switching mechanism based on environmental complexity is constructed to dynamically adjust the priority between formation maintenance and autonomous obstacle-avoidance tasks. Comparative experimental results show that the proposed DASRRT* algorithm reduces path planning time by an average of 34.78% and shortens path length by 1.15%. Simulation results for formation flight demonstrate that the proposed hierarchical control framework can adaptively adjust control modes in response to changes in environmental complexity, exhibiting strong adaptability to complex environments and a good ability to generalize to various scenes. 1. Introduction With the rapid development of unmanned aerial vehicle (UAV) technology, collaborative control strategies for UAV clusters have attracted significant attention [1,2]. Multi-UAV systems improve the intelligence and decision-making capability of individual agents through inter-agent coordination, offering advantages such as high flexibility, efficiency, low cost, and strong fault tolerance capability. Consequently, they are widely applied in both civil and military domains, including coordinated strikes [3,4,5], monitoring [6], logistics and transportation [7,8], and disaster relief [9]. To maintain an efficient formation configuration while ensuring safe mission execution, the system must be capable of rapid decision-making and accurate control under uncertain disturbances [10]. Although existing cooperative control theories have achieved substantial progress, traditional planning and control methods may fail when operating environments shift from open airspace to more complex and constrained scenarios. With the rapid development of the low-altitude economy and smart cities, urban environments have become a core scenario for multi-UAV systems to execute missions. In applications such as civil urban air mobility and military low-altitude penetration, the safety and timeliness of UAV formations are crucial. However, urban airspace is characterized by significant unstructured and highly dynamic features. UAVs must maintain a compact formation to ensure cooperative effectiveness while also possessing high agility to avoid complex obstacles. Traditional methods struggle to balance the effectiveness of global path planning with the real-time requirements of local dynamic obstacle avoidance, often resulting in collisions or formation breakup when faced with sudden threats. Therefore, complex urban low-altitude formation tasks are inherently a “planning—control—coordination” coupling problem. The global layer needs to provide safe and feasible reference paths. The local layer must implement real-time obstacle and collision avoidance. The coordination layer must dynamically balance formation maintenance with autonomous obstacle avoidance and quickly restore the formation after obstacle avoidance. Thus, constructing a hierarchical cooperative framework that covers global planning, local obstacle avoidance, and formation coordination across three layers is an effective approach to improving the continuity and stability of formation tasks in complex urban environments. Current multi-UAV path-planning methods mainly include graph-search methods, heuristic algorithms, deep reinforcement learning (DRL), and random sampling approaches [11,12,13,14,15]. Graph-search algorithms, such as A* and Dijkstra, can identify optimal paths on discretized grid maps. Reference [16] combined A* with the dynamic window approach and introduced an adaptive evaluation function to improve path quality; however, such methods incur high computational cost in complex or high-dimensional environments, and the resulting paths often lack smoothness. Heuristic algorithms, including genetic algorithms (GA) and ant colony optimization, address multi-constraint optimization problems by mimicking the behavior of biological populations. Reference [17] proposed a hierarchical framework based on an improved genetic algorithm and used time-dimensional path points for trajectory replanning, effectively resolving temporal conflicts in multi-UAV trajectories; nevertheless, such methods are prone to local optima and often exhibit limited solution stability. In recent years, DRL methods such as Proximal Policy Optimization and Deep Deterministic Policy Gradient (DDPG) have shown significant advantages in highly dynamic and unknown environments. Reference [18] introduces a multi-step smoothness metric as an auxiliary optimization objective, effectively suppressing trajectory oscillations by adaptively adjusting weight coefficients through an alternating hierarchical gradient descent strategy. However, DRL training is extremely time-consuming, and the resulting models lack interpretability. In addition, random sampling algorithms, such as RRT* and PRM, avoid full-map preprocessing by constructing path trees through spatial sampling, making them advantageous in high-dimensional spaces. The exploration-RRT* algorithm proposed in [19], for example, effectively balances information gain and travel cost in unknown environments. Nonetheless, such methods generally suffer from slow convergence and low sampling efficiency in narrow, constrained regions. The need to balance planning efficiency, path smoothness, and trajectory trackability in the constrained spaces of complex urban environments exacerbates the aforementioned shortcomings. Moreover, the global path must be capable of supporting subsequent path tracking and formation coordination control. For cooperative formation flight control of multiple UAVs, efficient control strategies mainly include behavior-based methods, virtual structure methods, leader–follower methods, and artificial potential field (APF) approaches [20,21]. Behavior-based methods, inspired by biological swarm behavior, are highly distributed and well suited to large-scale swarms. Reference [22] simulated the flocking mechanism of starlings and achieved efficient formation control by using visual localization to resolve mutual perception. However, such methods generally lack rigorous mathematical guarantees for system stability and convergence. Virtual structure methods treat the formation as a rigid body and maintain the configuration by tracking virtual nodes. Reference [23] designed a sliding-mode tracker that computes tracking points according to the desired trajectory and terrain feature structure, thereby achieving high-precision formation maintenance. Nevertheless, the rigid constraints of this approach limit system flexibility, making it difficult to handle dynamic obstacle avoidance and formation reconfiguration in complex environments. Leader–follower methods maintain the formation by having followers track the leader’s state. Based on this framework, Reference [24] designed a three-channel robust controller to suppress dynamic uncertainties. Compared with the virtual structure approach, this method is more amenable to obstacle avoidance, but it suffers from the risk of single-point failure of the leader and lacks global coordination because the leader does not account for follower states. In addition, APF-based methods achieve formation control by constructing attractive fields toward the target and repulsive fields from obstacles. Reference [25] proposes an improved APF method that integrates predicted states and input thresholds, enhancing the control safety and real-time performance of large-scale swarm systems under delay constraints. However, in complex unstructured terrains such as three-point collinearity or semi-enclosed environments, APF is highly prone to local minima and may induce system oscillations. For urban constrained spaces, this means that formation control must not only be capable of obstacle avoidance but also require a control strategy that can adapt to different complex environments. To address the inherent limitations of the above classical methods, extensive efforts have recently been devoted to targeted improvements. Although RRT* achieves asymptotic optimality through its rewiring mechanism, it still suffers from slow convergence and low sampling efficiency in narrow, constrained spaces [26,27]. To improve planning efficiency, Reference [28] proposed FRRT*, which integrates a risk network and a feedback module to constrain random-tree growth using situational information. Reference [29] further proposed the goal-biased PF-RRT*, which optimizes parent-node generation in obstacle neighborhoods through a bisection strategy, significantly reducing path cost and improving sampling quality. Traditional APF methods are prone to stagnation due to the balance between attractive and repulsive forces, and they may induce trajectory oscillations in complex terrains [30,31]. To address this issue, Reference [32] combined bionic swarm mechanisms with consensus theory and introduced a rotational potential function, effectively alleviating the local-minimum and oscillation problems in multi-UAV formations. In addition, to overcome the leader-failure problem in the leader–follower architecture, Reference [33] introduced a Virtual Leader combined with switching-topology communication, thereby effectively reducing computational burden. Reference [34] designed a finite-time consensus control law and incorporated an improved APF for dynamic obstacle avoidance, resolving the divergence of tracking errors in complex environments. Overall, existing research has made progress in areas such as planning efficiency, local obstacle avoidance, and consistency coordination. However, in complex urban scenarios, there is a need to further develop a hierarchical cooperative framework that can provide high-quality global reference trajectories while simultaneously ensuring formation maintenance, real-time obstacle avoidance, and formation recovery under local dynamic constraints. Although multilayer decision-making and hierarchical control methods for UAVs have been widely studied, existing approaches mainly focus on one or two aspects, such as global path planning, local obstacle avoidance, formation maintenance, or constraint control. In contrast, this paper focuses on cooperative formation flight tasks in complex dynamic environments, with an emphasis on the collaborative design and improvement of global path planning, formation tracking control, and local obstacle avoidance. The main contributions of this paper are as follows:(1)A dynamic adaptive strategy rapidly exploring random tree star (DASRRT*) algorithm is introduced. Unlike traditional RRT* and its variants, which primarily focus on search feasibility or path cost, DASRRT* combines guided sampling, adaptive step size, suboptimal node optimization, and path optimization strategies to improve search efficiency in complex, narrow environments while generating shorter, smoother global reference paths to enhance path traceability.(2)A distributed control law based on an improved APF was designed. Compared with traditional APF or consensus control methods, this paper introduces a relative velocity damping term for collision avoidance and a target attraction factor and velocity adaptive weight for obstacle avoidance. Furthermore, a distributed formation cooperative control law based on consensus theory is developed to mitigate issues with traditional APF, such as local minima, oscillations, and insufficient response to dynamic obstacles. A stability analysis based on the Lyapunov method is also provided.(3)A cooperative mechanism for formation control in complex environments is established. Unlike hierarchical control methods with fixed weights or a single mode, this paper establishes an environment complexity-based dual-mode switching and dynamic control weight adjustment method. This allows the formation to prioritize consistency in open areas, prioritize collision and obstacle avoidance in narrow or dynamically threatened regions, and quickly recover the desired formation once the risk is mitigated. In addition, a curvature-driven dynamic step size and local target selection strategy are designed, enabling the virtual leader to generate effective local guidance targets in real time along the global path. (1)A dynamic adaptive strategy rapidly exploring random tree star (DASRRT*) algorithm is introduced. Unlike traditional RRT* and its variants, which primarily focus on search feasibility or path cost, DASRRT* combines guided sampling, adaptive step size, suboptimal node optimization, and path optimization strategies to improve search efficiency in complex, narrow environments while generating shorter, smoother global reference paths to enhance path traceability. A dynamic adaptive strategy rapidly exploring random tree star (DASRRT*) algorithm is introduced. Unlike traditional RRT* and its variants, which primarily focus on search feasibility or path cost, DASRRT* combines guided sampling, adaptive step size, suboptimal node optimization, and path optimization strategies to improve search efficiency in complex, narrow environments while generating shorter, smoother global reference paths to enhance path traceability. (2)A distributed control law based on an improved APF was designed. Compared with traditional APF or consensus control methods, this paper introduces a relative velocity damping term for collision avoidance and a target attraction factor and velocity adaptive weight for obstacle avoidance. Furthermore, a distributed formation cooperative control law based on consensus theory is developed to mitigate issues with traditional APF, such as local minima, oscillations, and insufficient response to dynamic obstacles. A stability analysis based on the Lyapunov method is also provided. A distributed control law based on an improved APF was designed. Compared with traditional APF or consensus control methods, this paper introduces a relative velocity damping term for collision avoidance and a target attraction factor and velocity adaptive weight for obstacle avoidance. Furthermore, a distributed formation cooperative control law based on consensus theory is developed to mitigate issues with traditional APF, such as local minima, oscillations, and insufficient response to dynamic obstacles. A stability analysis based on the Lyapunov method is also provided. (3)A cooperative mechanism for formation control in complex environments is established. Unlike hierarchical control methods with fixed weights or a single mode, this paper establishes an environment complexity-based dual-mode switching and dynamic control weight adjustment method. This allows the formation to prioritize consistency in open areas, prioritize collision and obstacle avoidance in narrow or dynamically threatened regions, and quickly recover the desired formation once the risk is mitigated. In addition, a curvature-driven dynamic step size and local target selection strategy are designed, enabling the virtual leader to generate effective local guidance targets in real time along the global path. A cooperative mechanism for formation control in complex environments is established. Unlike hierarchical control methods with fixed weights or a single mode, this paper establishes an environment complexity-based dual-mode switching and dynamic control weight adjustment method. This allows the formation to prioritize consistency in open areas, prioritize collision and obstacle avoidance in narrow or dynamically threatened regions, and quickly recover the desired formation once the risk is mitigated. In addition, a curvature-driven dynamic step size and local target selection strategy are designed, enabling the virtual leader to generate effective local guidance targets in real time along the global path. 2. Theoretical Foundation This section introduces the fundamental concepts of algebraic graph theory, followed by the UAV dynamic model and its associated constraints. The content covered in this section pertains to the fundamental theory of cooperative control in multi-agent systems, with relevant definitions drawn from existing research on multi-agent consensus and graph theory-based control. 2.1. Graph Theory Algebraic graph theory can be used to describe the information interaction among multiple UAVs and is widely applied in modeling UAV formation cooperative control [35,36]. The considered formation system consists of one virtual leader and n followers, where an undirected graph G represents the communication topology among followers, as illustrated in Figure 1. During formation flight, each UAV is assumed to receive the leader’s position and velocity information without delay or packet loss. In addition, followers can access information from neighboring agents only when they are within the prescribed communication range. The graph G=V,E consists of a node set V=1,2,⋯,n and an edge set E=i,ji,j∈V,i≠j. For the i-th follower, the set of its neighboring UAVs is defined as Ni=j∈V,i,j∈E. Let A denote the adjacency matrix of G, where aij represents the weight of the edge i,j. Defining the communication topology as an undirected graph implies that the i-th UAV and the j-th UAV can mutually exchange state information. Specifically, if i,j∈E, then aij=1; otherwise, aij=0. It is further assumed that the graph contains no self-loops, such that aii=0. The degree matrix is given by D=diagd1,d2,⋯,dn, where di=∑j=1,j≠inaij, and the corresponding Laplacian matrix is defined as L=D−A. In the context of this leader–follower multi-UAV system, the leader is designated as UAV 0. In the virtual leader–follower multi-UAV system, the virtual leader UAV is denoted as UAV 0. Let H∈Rn×n represent the connection matrix between the followers and the leader, defined as H=diag(h1,h2,…,hn). If the i-th follower exchanges information with the leader, then hi=1; otherwise, hi=0. 2.2. Models and Constraints 2.2.1. UAV Dynamics Model To account for aerodynamic drag and inertial effects during actual flight, the i-th UAV is modeled as a second-order point mass dynamical system with damping characteristics [37,38]. Let pi(t)=xi,yi,ziT∈R3 and vi(t)=[vix,viy,viz]T∈R3 denote the position and velocity vectors of the UAV at time t, respectively.(1)pi˙t=vitvi˙t=ui˙t−γvi˙t where uit∈R3 represents the control input vector, and γ>0 denotes the air damping coefficient, which reflects the energy dissipation characteristics of the actual flight environment. Given a sampling time step Δt, the state update equations at time t+1 are formulated as follows:(2)vit+1=1−γΔtvit+uitΔtpit+1=pit+vit+1Δt 2.2.2. UAV Constraints In practical applications, the maneuverability of a UAV is restricted by its physical actuators and must adhere to safe flight constraints [39]. This research incorporates the following three categories of constraints:(1)Saturation Constraints (1)Saturation Constraints Saturation Constraints To ensure that control commands remain within the feasible operational range of the actuators, saturation constraints are imposed on flight velocity and control input magnitude. Specifically, the flight speed of a UAV must not exceed the maximum allowable velocity vmax.(3)vit≤vmax, ∀i∈n Furthermore, the control input is bounded by the maximum acceleration umax.(4)uit≤umax, ∀i∈n (2)Obstacle Avoidance Safety Constraints Obstacle Avoidance Safety Constraints The flight environment comprises a set of static obstacles Os and dynamic obstacles Od. Let Ωk denote the geometric region occupied by the k-th obstacle. To guarantee flight safety, the distance between a UAV and any obstacle must be maintained above a safety margin δsafe.(5)mino∈Ωkpit−po≥δsafe, ∀k∈Os∪Od A collision is considered to have occurred whenever the distance between the UAV and an obstacle is less than this safety margin. (3)Inter-agent Collision Avoidance Constraints Inter-agent Collision Avoidance Constraints To prevent internal collisions within the formation, the distance between any two UAVs must consistently remain greater than a minimum safety interval rs.(6)pit−pjt>rs, ∀i,j∈n, i≠j A collision is deemed to have occurred if the distance between two UAVs falls below this safety interval. 3. Methodology This study introduces a novel hierarchical decision and control framework based on the virtual leader–follower strategy, designed to collaboratively resolve the challenges of global path planning, local obstacle avoidance, and formation cooperative control for UAVs within complex dynamic environments. The operational flight scenario in a complex environment is shown in Figure 2. 3.1. Global Path Planning To generate high-quality global reference paths, the DASRRT* algorithm is developed. Building upon the traditional RRT, it integrates four synergistic optimization strategies to improve planning efficiency and path quality in narrow and complex environments. The overall framework of DASRRT* is illustrated in Figure 3. 3.1.1. Hybrid Guided Sampling Strategy To improve the low sampling efficiency of traditional RRT algorithms, a hybrid guided sampling mechanism is introduced. At each iteration, dynamic focused sampling is selected with probability Pf, while global uniform sampling is applied with probability 1−Pf. The dynamic focused strategy guides the sampling point qrand within a concentric spherical region that adapts according to the iteration index k.(7)qrand=dynamic focused sampling, with probability Pfglobal uniform sampling, with probability 1−Pf where qrand denotes the geometrically guided sampling point, and Pf represents the sampling bias weight. (1)Global Uniform Sampling Global Uniform Sampling Uniformly distributed sampling points qrand are generated within the map space as follows:(8)qrand=xrandyrandzrand, xrand∈xmin,xmaxyrand∈ymin,ymaxzrand∈zmin,zmax where xmin,xmax×ymin,ymax×zmin,zmax defines the environmental boundaries. (2)Dynamic Focused Sampling Dynamic Focused Sampling The center of the focused region, qc is adaptively adjusted via a bias weight Pg.(9)qc=qgoal, with probability Pgqstart+qgoal2, with probability 1−Pg where qstart denotes the starting point, qgoal is the target point, and Pg represents the target bias weight. Furthermore, the radius of the focused region, r(k), evolves dynamically as a function of the iteration index k and the environmental scale qgoal−qstart.(10)rk=ρqgoal−qstartikkλ where, ρ is the scale factor, ik∼U(0,1) is a random variable, and λ is the shape parameter controlling the concentration of the sampling distribution. When λ=13, sampling is uniformly distributed within the sphere; for λ>13, sampling points concentrate toward the sphere center. For representative cases λ=13 and λ=1, the distribution of 10,000 samples within a unit sphere is shown in Figure 4. Once the sampling center and radius are determined, sampling points are randomly generated within the spherical domain as follows:(11)qrand=qc+rkvθ, vθ=sinϕcosθ,sinϕsinθ,cosϕT where, θ∼U[0,2π denotes the azimuth angle, and ϕ=arccos(2η−1) denotes the polar angle, where η∼U[0,1]. This formulation ensures uniform sampling within the corresponding spherical region in 3D space. To ensure that the hybrid guided sampling point qrand remains within the environmental boundary B=xmin,xmax×ymin,ymax×zmin,zmax, coordinate-wise boundary constraints are enforced as follows:(12)qrandi=maxminqrandi,Bmaxi,Bmini, i∈x,y,z When λ>13, dynamic focused sampling yields an adaptive distribution that is dense near the center and sparse near the boundary, accelerating exploration of the target region and facilitating rapid discovery of an initial feasible path. 3.1.2. Perception-Based Adaptive Step Size Strategy The limitations of conventional RRT* in narrow passages are shown in Figure 5. Owing to the stochastic nature of sampling and the use of a fixed expansion step size, the effective expansion rate remains low in obstacle-dense regions. When nodes approach obstacles, an excessively large step size often causes newly generated nodes to collide with obstacles, resulting in invalid expansions. In contrast, an overly small step size reduces pathfinding efficiency. To prevent expansion failures caused by excessively large step sizes near obstacles, a dynamic obstacle-aware step size strategy is proposed. The expansion step size δ is adaptively adjusted according to the minimum Euclidean distance dobs from the neighboring node qnear to the closest obstacle.(13)δq=maxδmax1−e−βdobsqnear,δmin where, δmax and δmin denote the upper and lower bounds of the step size, respectively; dobs(qnear) is the distance from the node to the nearest obstacle; and β∈(0,1) is a sensitivity parameter. When dobs is small, indicating proximity to obstacles or narrow passages, δ decreases to improve expansion precision and feasibility. Conversely, in open regions, a larger step size is maintained to accelerate exploration. 3.1.3. Inefficient Node Optimization Strategy To alleviate the rapid growth in computational cost caused by neighborhood search and rewiring during tree expansion, an optimization mechanism for the rewiring process is introduced. After identifying an initial feasible path, each newly generated node qi is evaluated using:(14)Φqi=qi−qstart+qi−qgoal The set satisfying Φ(qi)=cbest forms an ellipsoidal surface with qstart and qgoal as the foci. Nodes within or on this ellipsoid are considered efficient. If Φ(qi)>cbest, any path passing through qi will exceed the current optimal path length, as Φ(qi) provides a lower bound on feasible path length via qi. Therefore, such nodes are discarded to reduce memory usage and computational overhead. Moreover, whenever the optimal path is updated, all existing nodes are re-evaluated. Nodes satisfying Φ(qi)>cbest are labeled as inefficient and excluded from subsequent nearest-neighbor and near-neighbor searches. To further analyze the computational complexity of DASRRT*, the algorithm’s final node count is denoted as Nnode, and the average obstacle detection value is represented as Ccol. The number of nodes in the i-th iteration of the search tree is denoted as Ni, and the number of nodes in the neighboring search area is ki. For the base RRT*, each iteration requires searching in the current node set and nearby nodes, where the number of nodes is related to Ni; similarly, the parent nodes need to choose and re-connect to the nearby nodes to perform collision detection. Therefore, the total computational complexity of the base RRT* is expressed as:(15)TRRT∗=O∑i=1NnodeNi+kiCcol where:(16)∑i=1NnodeNi=1+2+⋯+Nnode=NnodeNnode+12 Under normal conditions, kii) for direct connection, thereby significantly shortening the total path length. The optimized path π is defined as follows:(21)π=⋃i=1Nqi where qi+1=max{j∈(i,N]∣C(qi,qj)=True}. C(qi,qj)=True indicates that the straight-line segment connecting qi and qj is collision-free. The specific steps are as follows: Step 1: Initialization. Set π={q1} and initialize the current index i=1. Step 2: Backward Search. Traverse backward from the end point qN to find the furthest node qj(j>i) that satisfies C(qi,qj)=True. Step 3: Path Update. Append qj to π and update the current index i=j. Step 4: Termination. Repeat Steps 2 and 3 until qj=qN. The aforementioned path pruning operation selects the maximum feasible leap at each step, ensuring that the total path length decreases monotonically and that optimality is effectively improved, as illustrated in Figure 7. Path Smoothing To mitigate the issue of sharp turns within the pruned path, an iterative path smoothing algorithm is proposed. By optimizing path continuity and smoothness, this method effectively enhances the path-tracking performance of the UAV formation. (1)Objective Function Design Objective Function Design Given a set of discrete, collision-free path points after pruning, π={q1,q2,…,qN}, the objective is to solve for a new trajectory that ensures both smoothness and obstacle avoidance. To guarantee trajectory smoothness, the second-order difference of path points—representing discrete acceleration—is minimized. For a sequence of discrete points, this second-order difference is approximated as Δ2qi=qi−1−2qi+qi+1. Accordingly, the objective function of the smoothing term is defined as follows:(22)Jsπ=∑i=1Nqi−1−2qi+qi+12 To prevent over-smoothing that might cause the trajectory to penetrate obstacles, a penalty is imposed on the distance between the optimized point qinew and the original reference point qi. Accordingly, the objective function of the penalty term is defined as follows:(23)Jpπ=∑i=1Nwiqi−qinew2 The final composite objective function to be minimized is formulated as follows:(24)Jπ=λsJsπ+λpJpπ (2)Solution Methodology Solution Methodology To facilitate an efficient solution, the aforementioned summation is reformulated into a matrix representation. Let π=[q1,q2,…,qN]T denote the trajectory matrix to be determined. For the smoothness term, the second-order difference operator can be expressed in the following tridiagonal matrix form:(25)A=1h22−10…0−12−1⋱⋮0−12⋱0⋮⋱⋱⋱−10⋯0−12n×n where h denotes the spatial discretization step size, N denotes the number of path points, and n=N−2 (as the start and end points of the path cannot serve as intermediate nodes for acceleration calculations). Accordingly, the smoothness objective function is expressed as follows:(26)Js=πTATAπ For the distance penalty term, the weight assigned to each path point is represented in the following matrix form:(27)W=w100…00w20⋱⋮00w3⋱0⋮⋱⋱⋱00…00wnn×n where wi denotes the distance deviation weight for the i-th path point. Let the updated path trajectory be denoted by πnew. The objective function for the distance deviation is calculated as follows:(28)Jp=π−πnewTWπ−πnewT To obtain the optimal trajectory, we seek π such that ∇J(π)=0. Expanding the total cost function and differentiating with respect to π yields:(29)∂J∂π=λSATAπ+λPWπ−πnew=0 Rearranging the terms results in the following system of linear equations:(30)λSATA+λPWπ=λPWπnew The final solution for π minimizes the combined sum of the second-order differences and the distance deviations of the path points. (3)Solution Procedure Solution Procedure The iterative procedure for smoothing the initial path is summarized as follows: Step 1: Initialization. Assign initial values to the weights wi for all path points. Step 2: Optimization. Solve the system of linear equations based on the current weight matrix W to obtain the trajectory πnew. Step 3: Collision detection. Traverse each point in πnew to determine if it resides within any obstacle O. Points found to be in collision are added to the set C. Step 4: Weight update. If C is non-empty, increase the distance deviation penalty weights for the colliding nodes and their neighboring points. Step 5: Termination condition. Repeat Steps 2 through 4 until C=∅ or the maximum number of iterations is reached. 3.2. Local Cooperative Control This section develops internal collision avoidance, external obstacle avoidance, and formation cooperative control laws. Under the governance of these control laws, each UAV tracks the virtual leader. The integrated formation flight control architecture is illustrated in Figure 8. 3.2.1. Collision-Avoidance Control Law To prevent collisions caused by close proximity and velocity differences among UAVs during formation flight, a collision-avoidance control law is designed. By introducing a relative-velocity damping term on the basis of the conventional distance-based repulsive force, the proposed method suppresses oscillations while maintaining a safe separation distance, thereby improving stability during dense formation maneuvers. The repulsive potential energy is defined as:(31)Ucpji=12k1cpji−rs2 The repulsive force exerted by the j-th UAV on the i-th UAV is derived as:(32)Fjic=−∇piUc=−dUcdpjidpjidpi=−k1cpji−rs3pjipji Consequently, the total collision-avoidance force Fic acting on the i-th UAV is given by:(33)Fic=∑j=1,j≠iN−k1cpji−rs3pjipji−k2cvji where k1c,k2c denote the repulsive gain coefficients, which regulate the influence of the position error and velocity damping terms, respectively. pji denotes the position difference vector between UAV j and i, and vji denotes their relative velocity difference vector. rs denotes the safety distance of the UAV formation. In the expression, the first term represents the positional repulsion responsible for maintaining inter-UAV distances, while the second term provides velocity damping to effectively suppress relative velocities and minimize oscillations. 3.2.2. Obstacle-Avoidance Control Law To enhance the obstacle-avoidance capability of the formation in complex environments with dynamic obstacles, an obstacle-avoidance control law is designed. Based on the artificial potential field method, the proposed law incorporates a goal-attraction factor and a velocity-adaptive mechanism, effectively alleviating the problems of target inaccessibility, local stagnation, and insufficient responsiveness to dynamic obstacles. The repulsive potential energy exerted by obstacle o on UAV i is defined as:(34)Uio=12k1o1pio−1rs2pign, 0ru where pio denotes the distance from UAV i to obstacle o, ru denotes the detection radius of the UAV, and pig denotes the distance from UAV i to the goal point g. (1)Target Attraction Factor Target Attraction Factor By introducing the term pign, the repulsive force decays concurrently with the attractive force as the UAV approaches the goal (pig→0). Upon reaching the destination, the repulsive force becomes zero, effectively resolving the issues of target unreachability and local stagnation. (2)Velocity-Adaptive Weighting Velocity-Adaptive Weighting (35)k1o=k2o1+k3oe−11+vko where k2o and k3o are weight adjustment parameters. The gain k1o is coupled with the obstacle’s velocity; a higher obstacle velocity vko results in a larger gain k1o, enabling a rapid response to high-speed dynamic obstacles. Based on the modified repulsive potential function, the corresponding obstacle-avoidance control force can be derived as follows:(36)Fiof=−∇piUio=∂Uio∂pio∂pio∂pi+∂Uio∂pig∂pig∂pi To ensure system stability, a velocity damping term is incorporated:(37)Fiov=−k4ovi The total control force for the i-th UAV is then obtained as:(38)Fio=Fiof+Fiov The repulsive force analysis of UAV i under the obstacle-avoidance control law is illustrated in Figure 9. 3.2.3. Formation Cooperative Control Law To ensure stable tracking and formation consistency in complex environments, a distributed cooperative controller based on a virtual leader is designed. By integrating position errors, velocity errors, and communication topology information, the proposed method enhances formation consistency maintenance and formation recovery during obstacle-avoidance maneuvers. The virtual leader is the core of global path tracking, with position p0 and velocity v0. The desired position of UAV i is defined as pid=p0−di, where di=[dix,diy,diz]T denotes the position offset of the i-th UAV relative to the leader. The position and velocity tracking errors of UAV i are defined as:(39)eip=pi−pideiv=vi−vid Based on the distributed architecture and multi-agent consensus theory, the formation control input Fif is designed as:(40)Fif=−k1f∑j=1naijeip−ejp−k2f∑j=1naijeiv−ejv−k1fai0eip−k2fai0eiv where aij is an element of the adjacency matrix, indicating the communication link between UAVs i and j (aij=1 if connected, otherwise 0); ai0 denotes the connection status between UAV i and the virtual leader; k1f>0 is the position error feedback gain, which primarily adjusts the convergence speed of the formation position; k2f>0 is the velocity error feedback gain, which mainly adjusts velocity consistency and damping characteristics, used to suppress oscillations and improve convergence stability. Figure 10 illustrates the evolution of the UAV formation state under the coordinated action of the three control laws. In open areas, the cooperative formation controller ensures stable formation maintenance. When dynamic obstacles appear, the external obstacle-avoidance control law dominates trajectory adjustment to achieve safe avoidance. Upon entering a narrow passage, the internal collision-avoidance control and the cooperative formation control act jointly, driving the formation to transition from its original configuration to a compact shape suitable for passage. After clearing the obstacle region, the formation returns to a stable state. 3.3. System Cooperative Mechanism 3.3.1. Virtual Leader In conventional leader–follower architectures, maneuvering errors or failures of a physical leader propagate to followers, posing a threat to overall formation stability. To mitigate this single-point-of-failure risk, a virtual leader–follower strategy is adopted. The virtual leader generates the formation trajectory in real time based on environmental information and the path produced by the DASRRT* algorithm (Section 3.1), while follower UAVs track its motion via the cooperative formation control law (Section 3.2.3). Driven by multi-objective control inputs, the virtual leader dynamics are formulated as follows:(41)p0˙=v0v0˙=−γ1v0+Fvirtual=−γ1v0+Fatt+Frep+Fform(42)Fatt=kattslocal−p0(43)Fform=kform1N∑i=1Npi−p0 where γ1 denotes the velocity damping factor used to suppress oscillations, Fatt represents the target attraction force, slocal denotes a local target point dynamically selected along the global path in real-time (Section 3.3.2), katt denotes the attractive gain coefficient, Frep denotes the obstacle repulsive force, and Fform denotes the formation center regression force. The dynamic gain coefficient kform is computed as follows:(44)kform=kform0e−γ21p0−pgoal, p0−pgoal>δgoal0, otherwise where kform0 denotes the baseline regression gain, γ2 denotes the attenuation factor, and δgoal denotes the target distance threshold. As the leader approaches the final destination, formation constraints are gradually relaxed to avoid target inaccessibility. The forces acting on the virtual leader are illustrated in Figure 11. 3.3.2. Local Target Selection Strategy To address the limited adaptability of fixed-step local target switching in complex environments, a curvature-driven dynamic step-length strategy is proposed to smooth formation speed, mitigate oscillations caused by a constant step size, and improve overall formation stability. (1)Curvature-driven step-length adjustment Curvature-driven step-length adjustment When the virtual leader tracks the global path, the local target point plocal is adjusted dynamically according to the local path curvature k. An adaptive step length Δs is designed to select plocal in real time.(45)∆s=∆smin+∆smax−∆smin11+eαks−k where Δsmin is the minimum step length, Δsmax is the maximum step length, α is the curvature coefficient, and k is the curvature threshold. k(path) denotes the path curvature, which characterizes the degree of bending; s is the arc length. Larger k(s) indicates sharper turns, while k(s)→0 implies the path approaches a straight line. In high-curvature regions k(s)≥kth, denser sampling is required to improve obstacle-avoidance accuracy and ensure dense waypoints at sharp turns to prevent collisions; in low-curvature regions k(s)η1 or eip>δsafe where η1 is the environmental complexity threshold; eip=pi−(p0+di) is the position error between the actual and desired locations; δsafe is the safety threshold for formation distance. Formation Maintenance Mode (Mode 1) The total control input of each UAV, Fi(t), is a weighted sum of the formation control force, the internal collision-avoidance force, and the external obstacle-avoidance force:(50)Fit=ku1Fift+ku2∑j=1,j≠iNFjict+ku3∑o=1KFioot where Fjic is the collision-avoidance control force between UAV i and its neighbor j; Fioo is the obstacle-avoidance control force on UAV i from obstacle o; Fif is the formation consensus control force. ku1, ku2, and ku3 are weighting coefficients that balance the priorities between formation maintenance and collision/obstacle avoidance. ku1 is the weight of formation control. Its primary role is to enforce formation maintenance; in open environments it strengthens formation consensus to ensure accurate tracking of the desired shape led by the virtual leader. In narrow or obstacle-dense regions, reducing ku1 weakens formation constraints and enhances collision/obstacle avoidance. Therefore, ku1 is designed as an inverse proportional saturation function that adapts to the environmental complexity η:(51)ku1=maxk1max1−ηη1,k1min where k1max and k1min are the maximum and minimum weights of the formation control force, and η1 is the environmental complexity threshold. As η→0, ku1→k1max, so formation control dominates; as η→η1, 1−η/η1→0 and ku1→k1min, thereby reducing the formation control strength. ku2 and ku3 are the weights for collision avoidance and obstacle avoidance, respectively, and are designed with linear and exponential variations:(52)ku2=k201+ηη1(53)ku3=k30η−η1σ where ku2 increases linearly to enhance inter-UAV collision avoidance, and ku3 increases exponentially to strengthen obstacle repulsion. The parameter σ controls the growth rate of the gain. For small σ(e.g., σ=0.3), ku3 rises sharply once η1 is exceeded, which suits highly dynamic obstacle scenarios; for larger σ(e.g., σ=1.0), the gain varies smoothly, which is suitable for environments with uniformly distributed obstacles. Considering variations in environmental complexity across scenarios, the dynamic adjustment of ku1, ku2, and ku3 balances safety and formation efficiency. Formation maintenance is prioritized in open environments, while collision and obstacle avoidance are prioritized in confined environments. The total control force of UAV i is shown in Figure 12. Emergency-Avoidance Mode (Mode 2) Using only the control law of formation maintenance mode may be insufficient in extreme, highly complex environments. Therefore, an emergency-avoidance mode is introduced to strengthen the goal-attractive and obstacle-repulsive forces. The control input is adjusted as:(54)Fit=ku1Fiattt+ku2∑j=1,j≠iNFjict+ku3∑o=1KFioot In this mode, the attractive force Fiatt consists of two components:(55)Fiatt=Fiatt1+Fiatt2=katt1p0−pi+katt2pnearest−pi where Fiatt1 is the attraction toward the virtual leader, driving the UAV to follow the leader’s position p0 and maintain formation coherence; Fiatt2 is the attraction toward the local path point, providing global-path guidance by steering the UAV toward the nearest reachable path point pnearest, thereby enhancing path-tracking robustness. kattr1 and kattr2 are the attraction gains. In high-curvature regions, path points are dense and pnearest updates frequently, so the attraction direction adapts quickly to the path; in low-curvature regions, points are sparse and the attraction direction is more stable, reducing control oscillations. As the core driving force in the emergency-avoidance mode, Fiatt combines attraction to the virtual leader and the local path to ensure formation coherence and real-time avoidance in complex terrain. The proposed dual-mode switching mechanism temporarily switches to the emergency-avoidance mode in particularly complex environments, where Fiatt dominates the cooperative control input to guide the UAVs to rapidly and effectively exit obstacle regions under the combined guidance of the virtual leader and the global path; otherwise, the formation maintenance mode is used. 4. Stability and Convergence Analysis This chapter conducts a rigorous stability analysis of the control law proposed in Section 3. For a differentiable function V defined on Rn, if V(x)≥0 and V(x)=0 if and only if x=0, and if V˙(x)≤0 for all x∈Rn, then as t→∞ the system trajectory x(t) converges to the set S={V˙(x)=0}. If S contains no invariant set other than 0, then the trajectory x(t) is asymptotically stable [40]. 4.1. Stability Analysis of the Collision-Avoidance Control Law The collision-avoidance control law accounts for the relative positions and relative velocities among UAVs, ensuring that no collisions occur between any two UAVs. To rigorously establish the stability of the proposed control law and verify that the safety-distance constraint is always satisfied, we construct the following Lyapunov function:(56)Vct=12∑i=1N∑j=1,j≠iNUcpji+12∑i=1NviTvi This Lyapunov function consists of a potential-energy term and a kinetic-energy term. Taking the time derivative yields(57)Vc˙t=12∑i=1N∑j=1,j≠iNdUcdpjidpjidt+12∑i=1Nvi˙Tvi+viTvi˙=12∑i=1N∑j=1,j≠iN−k1cpji−rs3pjiTpji˙pji+∑i=1NviTvi˙=12∑i=1N∑j=1,j≠iN−k1cpji−rs3pjiTpji˙pji−∑i=1N∑j=1,j≠iNviTk1cpji−rs3pjipji−∑i=1N∑j=1,j≠iNviTk2cvji For the second term on the right-hand side of the last equality in (57), we analyze the subsystem composed of UAV i and UAV j. For a pair i,j without an edge, when i is included in the outer summation and j in the inner summation, the contribution to the subsystem total energy is(58)viT−k1cpji−rs3pjiTpji When j is included in the outer summation and i in the inner summation, the contribution to the subsystem total energy is(59)vjT−k1cpij−rs3pijTpij=vjT−k1cpji−rs3pjiTpji Since the forces acting on the two UAVs have equal magnitude and opposite direction, we have:(60)viT−k1cpji−rs3pjiTpji=−12viT+vjTk1cpji−rs3pjiTpji=−12k1cpji−rs3pjiTpjivji Substituting (60) into (57) yields(61)Vc˙t=−∑i=1N∑j=1,j≠iNviTk2cvji=−∑i,j∈Ek2cviTvj−vi+vjTvi−vj =−∑i,j∈Ek2cvj−viTvj−vi=−∑i,j∈Ek2cvji2≤0 From (61), it follows that V˙c(t)≤0, and from (56), Vc(t)>0. According to LaSalle’s invariance principle, the system asymptotically converges to the largest invariant set satisfying V˙c(t)=0, and Vc(t)≤Vc(0). When ∥pji∥→rs, Vc(t)→∞, which contradicts the boundedness established above. Therefore, if the initial inter-UAV distances exceed the prescribed safety distance, collisions will not occur. 4.2. Stability Analysis of the Obstacle-Avoidance Control Law To demonstrate that the UAV can safely avoid obstacles under the obstacle-avoidance control law and ultimately converge to the target point, the following Lyapunov function is constructed:(62)Vo,it=Uiopi+12viTvi The first term represents the obstacle-repulsive potential energy, while the second term corresponds to the kinetic energy. Taking the time derivative gives(63)V˙o,i(t)=Uiopi+12viTvi=∇piUiovi+viT−∇piUio−k4ovi=−k4ovi2≤0 From (63), the system energy is non-increasing, i.e., V˙o,i(t)≤0, and Vo,i(t)>0. Consequently, the system asymptotically converges to the largest invariant set satisfying V˙o,i(t)=0, with Vo,i(t)≤Vo,i(0). Hence, the potential energy term remains bounded. When the distance between the UAV and the obstacle approaches zero, Vo,i(t)→∞, which contradicts the boundedness established above. It follows that, during formation flight, the obstacle-avoidance control law guarantees that the UAV does not collide with obstacles. 4.3. Stability Analysis of Formation Errors Under the Control Law In actual formation flight, UAVs are not solely influenced by a single type of control force, but instead move under the combined influence of formation coordination control forces, inter-UAV collision-avoidance forces, and external obstacle-avoidance forces. Therefore, this section introduces collision-avoidance and obstacle-avoidance forces as active evasion inputs into the formation error system, and combines them with the formation coordination control law. The boundedness and convergence recovery of the errors under the overall control law are analyzed from the perspective of the overall closed-loop system. According to Equation (50), the total control force on the i-th UAV during formation flight is:(64)Fi=ku1Fif+ku2∑j=1,j≠iNFjic+ku3∑o=1KFioo The avoidance inputs are expressed as:(65)di=ku2∑j=1,j≠iNFjic+ku3∑o=1KFioo Define position error as ep=[(e1p)T,(e2p)T,…,(eNp)T], and velocity error as ev=[(e1v)T,(e2v)T,…,(eNv)T]. The main avoidance input is d=[d1T,d2T,…,dNT]. Combining the concepts in Section 2, the formation error system is written as:(66)ep˙=ev ev˙=−k1fL+H⨂I3eP−k2fL+H⨂I3ev+d where L is the Laplacian matrix of the communication topology, and H represents the connection matrix between the leader and followers. The symbol ⊗ indicates the Kronecker product, and I3 is the 3D identity matrix. When the UAVs are in communication and can efficiently receive the leader’s information, L+H is positive definite, i.e., λmin(L+H)>0. First, analyze the avoidance input when d=0, so the formation error system can be written as:(67)ep˙=evev˙=−k1fL+H⨂I3eP−k2fL+H⨂I3ev For the Lyapunov function corresponding to Equation (67), we construct:(68)Vf=12k1fepTL+H⨂I3ep+12evTev Since L+H is positive definite and k1f>0, we have:(69)Vf>0 From Equation (68), we get:(70)Vf˙=k1fepTL+H⨂I3ep˙+evTev˙=k1fepTL+H⨂I3ev+evT−k1fL+H⨂I3eP−k2fL+H⨂I3ev=−k2fL+H⨂I3ev≤0 As seen from (70), Vf is not an increment. According to LaSalle’s invariance principle, the system’s trajectory will eventually converge to the largest invariant set where Vf=0. When ev=0, we substitute into Equation (67) to get k1f(L+H)⊗I3eP=0. Since k1f>0, and (L+H)⊗I3 is positive definite, it follows that ep=0. Therefore, under the condition of zero avoidance input, the formation control law’s stability can be ensured:(71)limt→∞ ept=0, limt→∞evt=0 Next, we analyze the case with non-zero avoidance input, d≠0. In this case, from Equation (66), the formation error system can be written as:(72)Vf˙=−k2fL+H⨂I3ev+evTd From Equation (64), when active collision-avoidance forces and obstacle avoidance forces are present, the term ev)Td acts as an external input to the error system. Under these conditions, it cannot be deduced that V˙f≤0, nor can it be directly proven that the tracking error strictly asymptotically converges to zero throughout the entire obstacle-avoidance process. In fact, during the obstacle-avoidance phase, the UAVs need to temporarily deviate from the desired formation to satisfy the constraints on inter-UAV safety distance and obstacle safety distance. Therefore, the occurrence of short-term fluctuations in the tracking error is reasonable. To further demonstrate the role of avoidance input in affecting the error dynamics, Equation (66) is rewritten in state-space form as:(73)ξ˙=Afξ+Bfd This leads to:(74)ep˙ev˙=0I3N−k1fL+H⨂I3−k2fL+H⨂I3epev+0I3Nd From the analysis of the error system above, it is known that when L+H is positive definite and k1f>0,k2f>0, the matrix Af is a Hurwitz matrix. Therefore, there exist positive constants c>0 and α>0 such that:(75)eAft≤ce−αt If the avoidance input is bounded, i.e., there exists a positive constant d¯ satisfying:(76)dt≤d¯ Then Equation (73) yields:(77)ξt=eAftξ0+∫0teAft−τBfdτdτ Then we have:(78)ξt≤ce−αtξ0+∫0teAft−τBfdτdτ≤ce−αtξ0+cBfαd¯ From Equation (78), it can be seen that the formation tracking error will not diverge, but remains within a bounded neighborhood related to dˉ. In summary, the overall control law designed exhibits the following stability characteristics in the closed-loop system: in the absence of evasion inputs, the formation error asymptotically converges to zero; when evasion inputs are present, the control force—as an external input—causes the UAVs to temporarily deviate from the desired formation, but the formation error remains bounded; once the UAVs move away from the obstacle, the formation tracking error recovers and asymptotically converges to zero. This conclusion aligns with actual flight behavior: when the formation enters an area with complex obstacles, it cannot fully maintain its original formation and must prioritize obstacle avoidance; the formation tracking error may temporarily increase but remains within a controlled range; after leaving the obstacle area, the formation is able to recover and maintain its original formation. 5. Simulation Experiments The simulations were conducted on a laptop running the Windows 11 operating system (Intel(R) Core(TM) i7-10750H processor). All programs were implemented in MATLAB R2022b. In addition, to further verify the feasibility and closed-loop operational capability of the proposed method under conditions closer to practical system deployment, a Gazebo-based hardware-in-the-loop simulation platform was established, as shown in Figure 13. The platform consists of a host computer, a Pixhawk 2.4.8 flight controller, an RK3588-based onboard computing unit, the QGroundControl (QGC) ground control station, and the Gazebo simulation environment. Specifically, the host computer is responsible for running the Gazebo scenario, ROS/MAVROS communication nodes, and the state-monitoring interface; the RK3588 onboard computing unit is used to deploy the formation control, trajectory tracking, and obstacle-avoidance algorithms; the Pixhawk flight controller handles low-level attitude stabilization and position control; and QGC is used for flight-state monitoring and mission management. Closed-loop interaction of state information, control commands, and simulation data among the modules is achieved through MAVLink and ROS communication, thereby forming an integrated verification chain of “simulation environment–onboard computing–flight control–ground monitoring.” Considering the characteristics of low-altitude combat environments, a three-dimensional integrated simulation scenario containing multiple heterogeneous obstacles was constructed. The scene replicates a typical complex terrain characterized by constrained spatial structures and dynamic threats. The static environment includes U-shaped obstacles, narrow passages, walls with windows, and clusters of tree-like obstacles. In addition, randomly moving cylindrical dynamic obstacles are introduced to emulate unknown threats. 5.1. Simulation Validation of the Path Optimization Algorithm 5.1.1. Comparative Experiments In this section, the proposed DASRRT* algorithm is compared with the RRT* algorithm, the Informed RRT* algorithm [41], the RRT*-APF algorithm [42], the RRT*-PRIME algorithm [43], and the BIT* algorithm. The simulation is conducted in a three-dimensional space of 150 m×60 m×20 m. The leader UAV starts from 2 m,30 m,5 m and the target position is set to 145m,30m,5m. To ensure the fairness and reproducibility of the comparison experiments, the common parameters for the RRT* series algorithms are given as follows: maximum number of iterations Nmax=7000, base extension step size δmax=2 m, neighborhood rewire radius rrewire=10 m, goal arrival threshold ϵgoal=3 m, and obstacle safety margin δsafe=0.8 m. In addition, the specific parameters for each comparison algorithm are listed in Table 1. In Table 1, Pg is the target bias sampling probability; γIRRT∗ is the dynamic neighborhood radius adjustment coefficient for Informed RRT*; kAPF, ru, and αAPF are the APF repulsive gain, obstacle influence radius, and APF sampling point correction step size in RRT-APF*, respectively. w1, w2, and w3 are the weights for distance, direction consistency, and obstacle distribution; Ptar is the heuristic probability threshold; Kcand is the number of candidate directions; and θbias is the angular bias range. w4, w5, and w6 are the weights for path length, obstacle safety, and smoothness; Cth and Eth are the cost growth threshold and emergency adjustment threshold, respectively; γ1, γ2, and κare used for obstacle density estimation. In BIT*, Ns is the maximum number of sampling points, Nb is the batch size for sampling, ηBIT is the connection radius adjustment coefficient, and rmin and rmax are the minimum and maximum connection radii, respectively. In DASRRT*, Pf is the hybrid guided sampling probability, ρ and λ are the scaling factor and shape parameter in dynamic focus sampling, β is the perceptual adaptive step size sensitivity parameter, Δssmooth is the path smoothing sampling interval, and δmin is the adaptive step size lower limit. Regarding the termination conditions, RRT*, Informed RRT*, RRT-APF*, RRT-PRIME*, and DASRRT* all stop after reaching the maximum iteration count of Nmax=7000. For the BIT* algorithm, due to its use of batch sampling and graph search mechanism, which differs from the point-by-point sampling strategy of the RRT* series algorithms, the termination condition is set to a cumulative number of sampling points reaching 7000 to align the total sampling scale with that of the RRT* series algorithms. As shown in Figure 14, although all comparative algorithms are able to find feasible paths in a complex three-dimensional environment, the resulting paths exhibit pronounced piecewise polyline characteristics. Owing to the large turning-angle discontinuities along these paths, UAVs are often required to perform frequent aggressive acceleration, deceleration, and large attitude adjustments during tracking, which not only increases control difficulty but also leads to higher energy consumption. In contrast, the path generated by DASRRT* is smoother. By introducing a path optimization strategy, the proposed algorithm effectively eliminates abrupt turns while maintaining a relatively short path length, making it more suitable as a reference trajectory for UAV formation tracking control. To further reveal the fundamental differences among the algorithms in spatial search behavior, Figure 15 presents the evolution of the expansion trees at 1/3, 2/3, and 3/3 of the total iterations. Although Informed RRT* reduces the search region through elliptical heuristic sampling, its expansion tree still exhibits a relatively high node density. RRT*-APF shows a certain degree of directionality under potential-field guidance, but its overall expansion process remains highly stochastic. Although RRT*-PRIME has an advantage in path length, its expansion tree becomes extremely dense at the final iteration stage, indicating that a substantial amount of computational resources is consumed by repeated exploration in non-optimal regions. In contrast, the expanded trees of BIT* and DASRRT* exhibit a more pronounced goal-oriented nature, with DASRRT* demonstrating superior performance in terms of path length and smoothness. Consequently, DASRRT* not only significantly reduces tree size and computational overhead but also generates smooth, high-quality paths while maintaining high search efficiency. Given the inherent randomness of sampling-based algorithms, results from a single trial are insufficient for an objective assessment of robustness. Therefore, a Monte Carlo simulation was conducted, where each algorithm was independently repeated 50 times under the same environment. For each performance metric, the mean, standard deviation, and 95% confidence interval were calculated, and a t-test was performed between DASRRT* and each comparison algorithm. A difference was considered statistically significant when p<0.05. The statistical results are shown in Table 2 and Table 3. As shown in Table 2 and Table 3, the average path length of DASRRT* is 161.13 m, which is reduced by 3.54%, 2.66%, 1.84%, and 1.36% compared with RRT*, Informed RRT*, RRT*-APF, and BIT*, respectively. The path cost differences between DASRRT* and RRT*, Informed RRT*, and RRT*-APF are statistically significant. While RRT*-PRIME has the shortest average path, its node count and solving time are 5276.53 and 49.83 s, which are significantly higher than those of DASRRT*. DASRRT* has an average node count of 2479.27, which is significantly lower than all comparison algorithms, and the solving time is the shortest, with corresponding p-values all less than 0.05. This indicates that the proposed inefficient node optimization strategy effectively suppresses redundant node growth and improves solving speed. Regarding memory requirements, taking RRT* as an example, both RRT* and DASRRT* need to store node positions, parent node indices, and path costs, so the theoretical memory complexity is O(Nnode). DASRRT* introduces an additional inefficient node marking variable, slightly increasing memory usage per node. However, since the number of nodes is reduced by about 42.27%, the overall memory requirement is reduced. In terms of path smoothness, the average curvature and maximum turning angle of DASRRT* are significantly lower than those of all comparison algorithms, indicating that the proposed path optimization strategy effectively reduces the angularity and sharp turns in the sampled path, making the generated path more suitable as a reference trajectory for subsequent UAV formation tracking and cooperative control. 5.1.2. Ablation Studies To systematically evaluate the individual contributions and synergistic effects of the improved modules in the DASRRT* algorithm, comprehensive ablation experiments were conducted under the same simulation environment. All methods used identical parameter settings and were independently executed 50 times in the same scenario, and the averaged results are computed to mitigate the impact of randomness. For clarity, the following five modules are defined: A: baseline RRT* algorithm; B: hybrid guided sampling strategy; C: perception-based adaptive step size strategy; D: inefficient node optimization strategy; E: path optimization strategy. Table 4 quantitatively compares the results of 12 ablation combinations, where Group 12 (A + B + C + D + E) corresponds to the complete DASRRT* algorithm. Figure 16 provides an intuitive illustration of the comparison results of 12 groups under different indicators. By analyzing the results of the ablation experiment, using Group 1 as the reference, the incremental effects of each individual module are as follows: Module B (Hybrid Guided Sampling): It provides some improvement in path length, but its main contribution lies in guiding the sampling points to focus on effective regions, laying the foundation for multi-module collaboration. Module C (Perception-Adaptive Step Size): It shows good improvement in node count and solving time, indicating that this strategy effectively reduces invalid expansion in complex environments and improves search efficiency. Module D (Inefficient Node Optimization): Significant improvements are seen in node count and solving time, validating the effectiveness of real-time redundant node pruning in suppressing the expansion of the search tree. This significantly reduces computational overhead while maintaining the path quality largely stable. Module E (Path Optimization): It mainly affects the trajectory post-processing phase, with the most notable improvements in the maximum turning angle and average curvature, as well as further improvement in path length. 5.2. Simulation Validation of the Formation Control Algorithm In this section, the proposed control algorithm is validated through simulations. The UAV formation consists of five UAVs, denoted as follower U1, follower U2, follower U3, follower U4, and follower U5. The virtual leader U0 flies along the trajectory planned by DASRRT*. U1 tracks the virtual leader, while the remaining followers track U1 and maintain a V-shaped formation with a rear included angle of 45∘. The formation information is listed in Table 5, the parameters of the dynamic obstacles are given in Table 6. Figure 17a illustrate the flight trajectories of the UAV formation in three-dimensional space. Using the proposed algorithm, the UAVs successfully and safely traversed the complex environment, with a total flight time of 158 s. In the initial stage, the environment is relatively open, allowing the UAV swarm to maintain the V-shaped formation. Upon entering the obstacle region, under the dual-mode switching mechanism, each UAV responds rapidly to environmental changes and independently plans its obstacle-avoidance trajectory. After passing through the obstacle region, the UAVs quickly regain and maintain the desired V-shaped formation and reach the target point. The coordinated effect of the proposed dual-mode switching mechanism and the control law ensures both individual flight safety and formation integrity at the mission level. Figure 17b presents snapshots of key moments during the formation flight on the Gazebo platform. It can be seen that the UAV formation successfully completes takeoff, obstacle traversal, and arrival at the destination, indicating that the proposed method can be reproduced on the Gazebo platform. This further verifies the implementability of the algorithm and its potential for engineering applications. Figure 18, Figure 19 and Figure 20 illustrate the dynamic relationship among the environmental complexity η, mode switching, and the control-weight coefficients during flight. As shown in Figure 19, the formation flight undergoes two distinct transitions between obstacle avoidance and formation recovery. When the formation approaches a narrow passage or a densely cluttered mixed-obstacle region, the local environmental complexity increases and η exceeds the threshold, causing the system to switch from the formation maintenance mode to the Emergency-Avoidance Mode. After passing through the obstacle region, η falls below the threshold, and the system returns to the formation maintenance mode. As further illustrated in Figure 20, the variation in environmental complexity not only triggers mode switching, but also directly affects the allocation of control weights. In the formation maintenance mode, ku1 dominates, while ku2 and ku3 remain relatively small, so the system mainly focuses on maintaining the V-shaped formation. In the Emergency-Avoidance Mode, as η increases, ku1 decreases whereas ku2 and ku3 increase, with ku3 showing a more pronounced response in highly cluttered regions. This indicates that the control focus shifts from formation constraints to obstacle avoidance and collision avoidance. After clearing the obstacle region, the formation reconverges to the desired configuration. The results show that the designed dual-mode switching mechanism and dynamic weight adjustment strategy can autonomously adjust the control law weights based on environmental complexity, achieving an efficient balance between formation maintenance and flight safety. The environmental complexity threshold is set based on multiple simulation experiments and an analysis of formation flight risk changes. To verify whether this switching strategy leads to frequent mode switching and control oscillations, the state changes during the flight were further statistically analyzed. Throughout the entire flight process, the system only experienced 4 mode switches, with an average of one switch every 39.4 s, and no high-frequency oscillation near the threshold was observed. The average formation recovery time was 2.8 s, and the maximum oscillation amplitude was 0.032946 m, indicating that the mode switching process did not cause significant control oscillations or flight instability. These results suggest that the set threshold effectively meets the stable switching requirements in this simulation scenario. In subsequent engineering deployments, the threshold switching logic can be further combined with a hysteresis interval to enhance robustness against sensor noise and transient disturbances. As shown in Figure 21a, within the obstacle region, the minimum inter-UAV distance remains around 1 m, and no inter-agent collisions occur throughout the flight. This verifies the effectiveness of the inter-UAV collision-avoidance control law. Figure 21b shows that when the UAVs enter the densely cluttered area, the minimum distance between the formation and obstacles stays at approximately 2.5 m and never drops to zero at any time. This indicates that the improved obstacle-avoidance control law—by incorporating a distance-adaptive factor and a dynamic velocity weighting—provides sufficient repulsive action to ensure safety while maintaining motion toward the goal. As shown in Figure 22, during the initial stage, the tracking errors of all UAVs rapidly converge to a small range near zero, verifying the fast convergence of the proposed control law. After entering the densely cluttered region, the system switches to the emergency-avoidance mode, and the priority on obstacle avoidance induces fluctuations in the tracking errors. Even during the narrow-passage traversal and dynamic avoidance phases, the mean error remains within a controlled range, while higher tracking accuracy is achieved in open areas. Notably, U1 exhibits a relatively large error around 90 s because it waits at the end of the narrow passage to avoid the first dynamic obstacle. The other UAVs perceive U1’s lag, move toward U1, and enter a waiting state. Once the dynamic obstacle passes, U1 immediately accelerates under the control input to catch up. Owing to the rapid convergence of the control law, the formation quickly returns to the prescribed configuration. The formation waiting stage is shown in Figure 23. As shown in Figure 24a, the flight speed of all UAVs is strictly limited to a maximum of 3.5 m/s throughout the process. From Figure 24b,c, it can be observed that during the formation maintenance phase, the heading changes of all UAVs are generally consistent. However, during the obstacle-avoidance phase, both the heading angle and angular velocity show significant fluctuations, reflecting the UAVs’ rapid attitude adjustments to adapt to local obstacle constraints. The orderly parameter Φ is a core indicator of the degree of consistency in the formation’s speed and direction. As shown in Figure 24d, in the formation maintenance mode, Φ remains close to 1, indicating that the flight directions of all UAVs are highly consistent. In the obstacle-avoidance mode (e.g., around 18 s and 100 s), Φ significantly decreases, indicating that the UAVs have adopted differentiated avoidance actions. After the obstacle avoidance is completed, Φ quickly recovers to near 1, showing that the designed control framework has good cooperative recovery capability. As shown in Figure 25a, the UAV formation becomes stalled while attempting to traverse the obstacle region formed by narrow windows. From Figure 25b, U4 fails to pass through the window in time and consequently departs from the formation. After t≈100 s, all position errors converge to nearly constant values, indicating that the system enters a deadlock state: the resultant force becomes zero, causing the formation to halt. This demonstrates that, without the dual-mode switching mechanism, the cooperative control law alone cannot balance formation maintenance and obstacle avoidance in extremely constrained spaces. To validate the effectiveness of the proposed improved APF in mitigating the issue of unreachable targets caused by local minima in the traditional APF, a standard APF control group was constructed, and a comparative experiment was conducted under the same conditions. This control group removed the dual-mode switching strategy, adaptive weight allocation, and the target distance and velocity coupling terms in the improved potential field, relying solely on the traditional APF repulsive force for external obstacle avoidance and inter-UAV collision avoidance. As shown in Table 7 and Figure 26, although the APF does not lead to any collision, it fails to reach the target point and becomes trapped in the obstacle region, indicating that the traditional APF is prone to falling into local minima in complex obstacle environments. Its minimum obstacle distance is only 0.1187 m, and the average formation error is 6.9879 m, suggesting a low obstacle-avoidance safety margin and difficulty in balancing obstacle avoidance with formation maintenance. In contrast, the improved APF successfully reaches the target point, with the minimum obstacle distance increased to 1.9698 m and the average formation error reduced to 4.1844 m, indicating that the improved APF provides better obstacle-avoidance safety and formation maintenance capability in complex dynamic environments. The maximum oscillation amplitude of the improved APF is higher than that of the traditional APF, mainly because the improved APF successfully enters and traverses the subsequent narrow passage and dynamic-obstacle region, during which local trajectory variations become relatively larger due to obstacle avoidance. By contrast, the traditional APF becomes trapped before entering the subsequent obstacle region; therefore, the oscillation amplitudes of the two methods are not measured at the same motion stage, and a direct comparison is not entirely fair. The maximum oscillation amplitude of the improved APF remains within an acceptable range and does not cause collisions or mission failure, and can therefore be regarded as a necessary maneuvering response during complex obstacle avoidance. Overall, compared with the traditional APF, the improved APF demonstrates superior performance in task completion, local-minimum suppression, obstacle-avoidance safety margin, and formation maintenance capability. To further verify the adaptability of the proposed hierarchical cooperative control framework under environmental disturbances and formation configuration changes, experiments are conducted from three aspects: initial disturbance, dynamic obstacle velocity variation, and formation scale variation. The results are shown in Table 8. Here, the average formation recovery time is defined as the average time it takes for the formation’s mean error to drop within the set threshold and remain there for several sampling steps after the system switches from Mode 2 to Mode 1. In the initial disturbance experiment, Groups B and C achieved average formation errors of 3.5799 m and 3.703 m, respectively, both lower than the 4.15 m in the baseline scenario. The average recovery times were also shortened to 1.6 s and 0.9 s, indicating that the system can quickly absorb the formation deviation caused by the initial disturbance and restore cooperative flight. Although the minimum inter-UAV distances in both groups decreased, no collisions occurred, demonstrating that the controller maintains reliable safety constraints even under initial biases. In the dynamic obstacle speed variation experiment, after adjusting the obstacle speed to 0.7 times and 1.3 times the baseline value in Groups D and E, the system still maintained zero collisions, with average formation errors further decreasing, and recovery times staying within a short range. In conditions with more frequent dynamic interactions, although the number of mode switches increased, the system was still able to quickly recover formation after safely avoiding obstacles. This indicates that the proposed dual-mode cooperative mechanism can adjust the control focus in response to time-varying environments and shows strong adaptability to changes in obstacle motion speed. In the formation scale variation experiment, when the formation size was expanded from 3 UAVs to 7 and 9 UAVs, although the local interaction complexity increased significantly and both the minimum inter-UAV distance and minimum obstacle distance decreased, no collisions occurred in any of the scenarios. The average formation error and recovery time remained within acceptable ranges, demonstrating that the framework retains good task maintenance and cooperative recovery capabilities under scale variation conditions, reflecting a certain level of system scalability. 6. Conclusions This paper addressed the coupling issues among global planning, local obstacle avoidance, and cooperative control for multi-UAV formations in complex urban dynamic environments. A hierarchical cooperative control framework based on a virtual leader–follower architecture was proposed. In the global planning layer, the DASRRT* algorithm was developed to overcome the limitations of traditional algorithms, including slow convergence, low expansion efficiency in narrow passages, and excessive turning angles in dense obstacle environments. The generated high-quality reference trajectories provided effective guidance for subsequent formation flight control. In the local obstacle avoidance layer, a distributed controller based on the improved APF method was designed to coordinate the control conflicts among inter-UAV collision avoidance, external obstacle avoidance, and formation maintenance. This controller improved formation consistency, stable tracking, and reconfiguration capability in complex dynamic environments. In the cooperative control layer, an environmental-complexity-based dual-mode switching mechanism was constructed to adaptively coordinate formation maintenance and emergency obstacle avoidance. Meanwhile, the real-time local target selection strategy enhanced tracking smoothness and robustness under complex path conditions. Simulation results show that DASRRT* achieved a maximum turning angle of only 5.11° and a final tree node count of 2479, representing an approximately 34–53% reduction compared with the other algorithms. These results indicate that the proposed method improved search efficiency and reduced the large attitude adjustments required to track paths with excessive turning angles. In complex urban scenarios with initial disturbances, obstacle speed changes, and formation scale variations, all test cases maintained collision-free operation, and the average formation error remained within 3.33–4.15 m. The average formation recovery time after mode switching was also relatively short. These results demonstrate that the proposed hierarchical cooperative control framework achieved strong robustness, environmental adaptability, and scalability. Future work will focus on the real-time onboard implementation of the proposed framework in complex scenarios. In particular, computational efficiency, communication delay, sensor uncertainty, and hardware resource constraints will be further considered to improve the feasibility of deploying the method in real multi-UAV systems. In addition, learning-based alternatives, such as deep reinforcement learning methods for dynamic obstacle avoidance and adaptive decision-making, will be further investigated and compared with the proposed framework to evaluate their respective advantages in complex and uncertain environments.