Method and system for cooperative obstacle avoidance and trajectory planning of unmanned aerial vehicle swarm formation

Abstract

This invention provides a method and system for cooperative obstacle avoidance and trajectory planning in UAV swarm formation, relating to UAV swarm control technology. By introducing three-dimensional affine transformation parameterization of formation motion, the dimensionality and search space complexity of UAV swarm system modeling are reduced, alleviating computational burden. An affine A-RRT planning algorithm is employed to construct path topology in affine space, improving sampling search efficiency and reducing redundant nodes and ineffective expansion. ESDF gradient information is used for path correction and to construct sequential convex regions, simplifying the construction process of safe regions and ensuring sufficient safety margins. By constructing a quadratic programming model integrating multiple performance indicators and solving it online, a balance is achieved between trajectory smoothness, energy efficiency, and swarm formation maintenance performance, meeting the real-time planning requirements for cooperative obstacle avoidance in complex environments.

Description

Technical Field

[0001] This application relates to unmanned aerial vehicle (UAV) swarm control technology, and in particular to a method and system for UAV swarm formation cooperative obstacle avoidance and trajectory planning. Background Technology

[0002] The rapid development of the low-altitude economy has driven the large-scale application of UAV swarms in complex scenarios such as logistics and disaster monitoring. These applications require swarm systems to simultaneously achieve environmental perception, obstacle avoidance, and formation geometry maintenance in a three-dimensional dynamic environment—all three are indispensable. However, existing technologies have significant limitations: traditional formation control methods are mostly based on two-dimensional planar space design, and their affine transformation parameterized models are difficult to directly extend to three-dimensional space, leading to the failure of motion coordination mechanisms in the height dimension and an inability to handle three-dimensional obstacle distribution and multi-layered airspace constraints. Furthermore, when coupling formation configuration constraints with obstacle avoidance conditions in modeling, the system parameter dimension expands dramatically, causing sampling-based path planning algorithms to face search efficiency bottlenecks in high-dimensional affine configuration spaces. Specifically, this manifests as high node redundancy, a large proportion of invalid path segments, and a convergence speed that cannot meet real-time planning requirements. In the trajectory optimization stage, the formation geometry severely compresses the feasible motion domain of individual UAVs, resulting in insufficient safety margins for convex regions in the sequence. This is especially true when waypoints approach obstacles, where the convex region construction process is susceptible to abrupt changes in the Euclidean signed distance field gradient, leading to local oscillations in the quadratic programming solution and significantly increasing computation time. More critically, existing methods fail to establish a synergistic optimization mechanism among trajectory smoothness, energy efficiency, and formation configuration stability. Overemphasis on a single performance indicator often leads to formation deformation or trajectory jitter, compromising the overall collaborative performance of the swarm. These technical deficiencies severely restrict the reliable operation capability of UAV swarms in complex environments such as dense urban airspace.

[0003] To address the aforementioned issues, existing technologies urgently need improvement. Summary of the Invention

[0004] This application provides a method and system for collaborative obstacle avoidance and trajectory planning in UAV swarm formation, which can efficiently realize collaborative obstacle avoidance and trajectory planning of UAV swarm in a three-dimensional dynamic environment, significantly improve path search efficiency, enhance obstacle avoidance safety, and collaboratively optimize trajectory smoothness, energy efficiency and formation maintenance performance, thereby improving the overall collaborative efficiency of the swarm.

[0005] Firstly, the UAV swarm formation cooperative obstacle avoidance and trajectory planning method provided in this application adopts the following technical solution: A method for cooperative obstacle avoidance and trajectory planning in a drone swarm formation includes: Based on the parameterization of UAV swarm formation motion using 3D affine transformation, a model for collaborative planning of UAV swarm trajectory is constructed. Based on the aforementioned trajectory collaborative planning system model, affine A-RRT is adopted. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space; Based on the formation-level geometric path topology, path correction is performed using Euclidean symbolic distance field (ESDF) gradient information, and a convex region of the individual UAV hierarchical sequence is constructed in physical space. Based on the convex region of the sequence and the formation configuration constraints, a quadratic programming model is constructed that integrates trajectory smoothness, energy efficiency and swarm formation maintenance performance, and the optimal flight trajectory for UAV swarm cooperative obstacle avoidance is obtained online.

[0006] Optionally, the step of constructing a UAV swarm trajectory cooperative planning system model based on the parameterized UAV swarm formation motion of three-dimensional affine transformation includes: The constructed parameterized model of drone formation swarm motion is represented as follows: in, Represents the affine configuration vector. Represents the orbit around a fixed coordinate axis Angle of rotation Represents the scaling factor Logarithmic form, Representing along fixed coordinate axes The amount of translation; Considering the configuration space, At this moment The drone is modeled as an equivalent point mass, and its position in physical space is denoted as . Its calculation relationship with the affine configuration vector is as follows: Stacking this mapping relationship at the formation level yields... The overall position vector of a swarm of drones in physical space : in, Represents the Kronecker product. express 3D identity matrix for Let be an all-one vector, given a formation configuration, where For the first The relative position of the UAVs to the midpoint in a pre-defined formation, and the translation vector. ,as well as Construct a path planning model at the formation level: in, For the rotational part under affine parameters, For the scaled portion, For the translation part, These represent the cost weighting coefficients for rotation, scaling, and translation between adjacent path points, respectively. These represent the initial configuration and target configuration of the drone swarm, respectively. in, Represents a collision-free physical space; Construct a trajectory optimization model at the individual level: in, These represent the weighted coefficients for smoothing, energy consumption, and formation maintenance costs, respectively. Let these represent the initial position and the target position of the individual UAV, respectively. Considering that the UAV follows a double-integral dynamics model, Indicates the speed and acceleration of the drone. The control input represents the desired acceleration of the UAV. To ensure the control variable acts within the interval, the state variables are defined at the nodes. Then, the dynamic difference equations are constructed, and to guarantee the continuity and solvability of the trajectory, the optimization model only needs to be defined in... this Over a time interval.

[0007] Optionally, affine A-RRT can be used. The planning algorithm constructs a hierarchical path topology for formation in affine space, including: Build and initialize the forward tree With reverse tree Maximum number of iterations and extended step size ; Random sampling is performed in the affine space, and single-step expansion is performed towards random nodes and the opposite tree direction respectively; If the constraints are met, local path reconstruction and topology optimization are performed by backtracking the parent node; The above process is repeated alternately until the bidirectional tree is successfully connected; The bidirectional paths are traced back and spliced ​​to complete the discretization process, and the physical space path sequence of each UAV is generated based on the affine mapping relationship.

[0008] Optionally, the step of randomly sampling in the affine space and expanding step-by-step towards random nodes and the opposite tree direction includes: Expand the forward tree by uniformly sampling in the affine placement space to obtain random affine placement vectors. In the set of forward nodes Traversal and The first node in the positive direction with the minimum Euclidean distance , corresponding affine configuration vector Calculate the new configuration after expansion: in, Indicates the expansion step size; for the new path segment Perform collision detection. If the new path topology check finds no collisions, construct a new forward node. ,in, Indicates in configuration The physical spatial location vector of the UAV swarm is calculated through affine transformation. The index is time-based; the index corresponding to the new node is... That is, increment the index of the starting node of the plan by one; The parent node of the new node in the node set Index in This represents the cumulative path cost of the new node, calculated based on the cost function in the formation hierarchical path planning model; Expanding the reverse tree, in the set of reverse nodes Traversal and The first node with the smallest Euclidean distance in the reverse direction , corresponding affine configuration vector ; Calculate the new configuration after the expansion: For new path segments Perform collision detection; if no collision is detected, construct a new node in the reverse direction. Its construction method is similar to that of a positive new node. They are basically the same, except that the top right corner subscript is used to distinguish the trees being explored in different directions.

[0009] Optionally, the step of reconstructing local paths and optimizing topology by backtracking parent nodes includes: Backtracking forward to the first node Obtain the second node of the forward starting direction ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update positive new node Parent node index Pointing to the transition node ; Update the forward tree point set Update the forward tree edge set ; Backtracking to the first node Obtain the second node from the reverse starting point ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update the reverse new node Parent node index Pointing to the transition node ; Update the reverse tree point set Update the inverse tree edge set .

[0010] Optionally, the process of backtracking and splicing the bidirectional paths to complete the discretization process, and generating the physical space path sequence for each UAV based on the affine mapping relationship, includes: Backtracking the dual-tree structure yields the forward and reverse discrete path sequence points. ,in ,as well as Represents the number of forward and reverse nodes in the backtracking process; Discretize the connection segment path using the same expansion step size: Among them, the number of discrete points , Indicates the start and end points of the connecting segment. This represents the theoretical minimum number of time steps corresponding to the connect segment: in, This represents the floor operator; The complete affine path sequence is represented as: in, Indicates the reverse path sequence after inversion. One node; Based on the constructed affine mapping relationship, the position sequence of each drone in physical space is calculated. .

[0011] Optionally, based on the ESDF-guided path correction mechanism, a single-level sequence convex region is constructed, including: Based on location sequence For each waypoint Construct a side with a length of The hypercube convex region as a sequence convex region : This condition requires the area Completely in free space In the middle section, considering the possibility that path points are too close to obstacles, making it impossible to construct a sufficiently large convex region, a path projection correction mechanism is designed: in, This indicates the corrected position. Indicates the scaling factor. express The ESDF gradient vector at that location.

[0012] Optionally, a quadratic programming model that comprehensively considers trajectory smoothness, energy efficiency, and cluster formation maintenance includes: For the Defining the drone at a given time State variables ,in Representing time respectively Next The position vector, velocity vector, and acceleration vector of the UAV are calculated, with acceleration used as the control input, considering the time interval. The control input, stacked time-domain state vectors: Considering the backend optimization as a single-unit parallel solution process, to simplify the communication relationship between UAVs, and taking into account the relative reference positions between UAVs. Satisfies affine relation:

[0013] in, , This represents the tracking error; therefore, based on the consistency of the reference trajectory, the formation-keeping term in the optimization objective function is decoupled.

[0014] in, Represents higher-order error terms; ignores The cost of coupled formation maintenance can be approximated as the cost of individual UAV trajectory tracking; further, the optimization objective function based on the trajectory optimization model at the individual level can be developed as follows:

[0015] The gradient value of the constant term is 0, which does not affect the optimal value of the optimization; the constant term is ignored. And the following was compiled:

[0016] The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is the corresponding position Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed:

[0017] The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is based on the corresponding position. Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed:

[0018] Among them, linear equality constraints Linear inequality constraints .

[0019] Optionally, the linear equality constraints and linear inequality constraints include: Based on the double-integral dynamic model, dynamic constraints Represented as:

[0020] Start constraints and end constraints Represented as:

[0021] Obstacle avoidance constraints based on sequential convex regions Represented as:

[0022] Actuator constraints based on velocity and acceleration limitations Represented as:

[0023] Secondly, this application provides a drone swarm formation cooperative obstacle avoidance and trajectory planning system, including: The model building module is used to construct a model of a collaborative planning system for UAV swarm trajectory based on the parameterized UAV swarm formation motion of three-dimensional affine transformation. The path topology construction module is used to construct the path topology based on the trajectory collaborative planning system model, employing affine A-RRT. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space; The region construction module is used to perform path correction based on the formation hierarchical geometric path topology, using Euclidean symbolic distance field (ESDF) gradient information, and to construct a single UAV hierarchical sequence convex region in physical space. The output module is used to construct a quadratic programming model that integrates trajectory smoothness, energy efficiency and cluster formation maintenance performance based on the convex region of the sequence and the formation configuration constraints, and solve it online to obtain the optimal flight trajectory for UAV cluster cooperative obstacle avoidance.

[0024] In summary, this application effectively reduces the dimensionality and search space complexity of UAV swarm system modeling and alleviates the computational burden by introducing three-dimensional affine transformation parameterized formation motion. Affine A-RRT is employed. The planning algorithm constructs a path topology in affine space, improving sampling search efficiency and reducing redundant nodes and invalid expansions. Utilizing ESDF gradient information for path correction and constructing sequential convex regions simplifies the construction of safe regions and ensures sufficient safety margins. By constructing a quadratic planning model integrating multiple performance indicators and solving it online, a balance is achieved between trajectory smoothness, energy efficiency, and swarm formation maintenance performance. This effectively solves the problems of coupling between formation constraints and obstacle constraints, low trajectory planning efficiency, and poor formation configuration stability, meeting the real-time planning requirements for collaborative obstacle avoidance in complex environments. Attached Figure Description

[0025] Figure 1 This is a flowchart illustrating the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning method of this application; Figure 2 This is an indoor map schematic diagram of the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning method of this application; Figure 3 This is a building map schematic diagram of the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning method of this application; Figure 4 This is a schematic diagram illustrating the planning results of the UAV swarm formation cooperative obstacle avoidance and trajectory planning method proposed in this application. Figure 4 (a) is a schematic diagram of scenario 1, which depicts a building. Figure 4 (b) is a schematic diagram of scenario 2, which depicts a building. Figure 4 (c) is a schematic diagram of scenario 3, which is an indoor scene; Figure 4 (d) is a schematic diagram of scenario 4, which is an indoor scene; Figure 5 This is a structural block diagram of the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning system of this application. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0027] This application provides a method for collaborative obstacle avoidance and trajectory planning in unmanned aerial vehicle (UAV) swarm formations, referring to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning method of this application.

[0028] In this embodiment, the method for collaborative obstacle avoidance and trajectory planning in UAV swarm formation includes the following steps: Step S10: Based on the parameterization of UAV swarm formation motion using three-dimensional affine transformation, construct a UAV swarm trajectory collaborative planning system model.

[0029] Understandably, the following terms are explained: Affine transformation is a spatial transformation that preserves the affine relationships between points, lines, and planes. Its general form can be represented as a combination of linear and translation transformations. In geometric space, affine transformations can perform operations such as translation, rotation, scaling, and shearing, but do not necessarily preserve angles and lengths. Its core feature is that it preserves structural invariants such as collinearity and parallelism. It is often used to map local configurations to a global coordinate system through unified parameters, thereby achieving a unified description and low-dimensional parameterized expression of the overall configuration.

[0030] Fast Exploration Random Tree (RRT) is a path planning algorithm based on random sampling. It efficiently explores feasible paths in high-dimensional or non-convex spaces through incremental expansion of a tree data structure. Its core objective is to quickly find the topology of feasible paths from the starting point to the target under complex constraints (such as obstacles or nonholonomic systems).

[0031] Euclidean Signed Distance Function (ESDF): In discrete or continuous space, this is a scalar field function that assigns a numerical value of the Euclidean distance from each point to the nearest obstacle boundary, and distinguishes whether the point is in free space or inside the obstacle by its sign; where a positive function value indicates that it is in free space, a negative value indicates that it is inside the obstacle, and a zero value corresponds to the obstacle boundary; its core function is to provide continuously differentiable distance information and gradient direction, which is used to construct a smooth obstacle avoidance cost function or constraint condition.

[0032] A sequential convex region refers to a series of interconnected convex feasible sub-regions constructed at each discrete time step or iteration step during the planning or optimization process by locally linearizing or constraining the non-convex feasible space. This region is usually represented in the form of a polyhedron or a quadratic inequality and guarantees the convexity of the optimization problem within the current approximation range. Its core purpose is to transform the originally difficult-to-solve non-convex programming problem into a series of efficiently solvable convex sub-problems, thereby improving the stability of the solution and the computational efficiency.

[0033] Quadratic programming (QP) refers to a convex optimization problem where the objective function is a quadratic function and the constraints are linear equations or inequalities. Its standard expression involves minimizing the quadratic cost of the decision variables while satisfying a given set of linear constraints. When the quadratic term matrix of the objective function is semi-positive definite, the problem is a convex optimization problem and can be solved efficiently using algorithms such as the interior-point method and the active set method. Its core advantage lies in its ability to uniformly characterize problems such as energy minimization, trajectory smoothing, and control input constraints, and it possesses good numerical stability and computational efficiency.

[0034] In practical implementation, it is assumed that the UAV is equivalently modeled as a point mass located at its geometric center in the configuration space (C-space), and the obstacle set is equivalently expanded to represent the geometric size and shape of the UAV; the UAV is considered as a low-altitude aircraft in a bounded three-dimensional workspace. In an environment, the area occupied by a static obstacle is represented as... Based on the assumption, with the maximum radius of the drone The original obstacle is expanded, and the expanded obstacle region is represented as follows: Therefore, the obstacle avoidance problem for drones can be expressed as whether a point mass enters the region. .

[0035] Discretized continuous workspace For resolution Three-dimensional voxel grid: ,in, The spatial coordinates of the grid center Indicates the occupancy state of the grid, i.e., when At that time, the grid is defined as occupied. This represents the ESDF value of the raster. Represents the spatial gradient of the raster, and:

[0036] in, Denotes the Euclidean norm. Indicates the area of ​​expansion barriers The topological boundary; in a geometric sense, This indicates the safety margin between the current position and the obstacle. Pointing away from the boundary of the nearest obstacle The direction of maximum velocity; thus, the obstacle region is defined. and collision-free free zone :

[0037] consider A swarm system of isomorphic unmanned aerial vehicles (UAVs) is defined as follows: The spatial position of an individual UAV is represented as... Continuous-time dynamics is expressed as ,in Indicates control input; the overall spatial location of the cluster system is represented as Define cluster formation configuration ,in Indicates the first The relative positions of the drones with reference to the center of the formation.

[0038] It should be noted that the step of constructing a drone swarm trajectory cooperative planning system model based on the parameterized drone swarm motion of three-dimensional affine transformation includes: considering simplifying the three-dimensional affine transformation to include only rotation, isotropic scaling, and translation, the constructed parameterized model of drone swarm motion is expressed as follows:

[0039] in, Represents the affine configuration vector. Represents the orbit around a fixed coordinate axis Angle of rotation Represents the scaling factor Logarithmic form, Representing along fixed coordinate axes The amount of translation; Considering the configuration space, At this moment The drone is modeled as an equivalent point mass, and its position in physical space is denoted as . Its calculation relationship with the affine configuration vector is as follows:

[0040]

[0041] Stacking this mapping relationship at the formation level yields... The overall position vector of a swarm of drones in physical space :

[0042] in, Represents the Kronecker product. express 3D identity matrix for Let be an all-one vector, given a formation configuration, where For the first The relative position of the UAVs to the midpoint in a pre-defined formation, and the translation vector. ,as well as

[0043] Construct a path planning model at the formation level:

[0044] in, For the rotational part under affine parameters, For the scaled portion, For the translation part, These represent the cost weighting coefficients for rotation, scaling, and translation between adjacent path points, respectively. These represent the initial configuration and target configuration of the drone swarm formation, respectively. In this embodiment, , These represent the initial configuration and target configuration of the drone swarm formation, respectively. This embodiment considers four cases:

[0045] as well as

[0046] in, Represents a collision-free physical space; Construct a trajectory optimization model at the individual level:

[0047] in, These represent the weighted coefficients for smoothing, energy consumption, and formation maintenance costs, respectively. Let these represent the initial position and the target position of the individual UAV, respectively. Considering that the UAV follows a double-integral dynamics model, Indicates the speed and acceleration of the drone. The control input represents the desired acceleration of the UAV. To ensure the control variable acts within the interval, the state variables are defined at the nodes. Then, the dynamic difference equations are constructed, and to guarantee the continuity and solvability of the trajectory, the optimization model only needs to be defined in... this Over a time interval.

[0048] It should be noted that 3D affine transformation provides a unified parameterized expression of formation configuration by constructing an affine configuration space, thus transforming the original... The dimensional programming problem is compressed to 7 dimensions, effectively avoiding the computational complexity brought about by high-dimensional search and significantly reducing the model size and computational burden.

[0049] This embodiment introduces a complete affine transformation parameterization model, which abstracts the complex cooperative motion of UAV swarms into low-dimensional affine configuration vectors, greatly simplifying the description and planning of swarm motion. By explicitly defining the swarm affine configuration vector, the calculation method for the physical position of individual UAVs, and the stacking mapping relationship of the overall physical position of the swarm, this embodiment mathematically establishes a precise correlation between the overall swarm motion and the motion of individual UAVs, ensuring strict preservation of the formation configuration. Furthermore, by constructing a path planning model at the formation level and a trajectory optimization model at the individual level, this embodiment achieves hierarchical planning: first planning the overall motion trend of the swarm in affine space, and then finely optimizing the specific trajectory of each UAV in physical space. This hierarchical cooperative planning mechanism not only improves planning efficiency but also effectively balances trajectory smoothness, energy efficiency, and swarm formation maintenance performance, thereby enabling safe, efficient, and cooperative flight of UAV swarms in complex dynamic environments.

[0050] Step S20: Based on the trajectory cooperative planning system model, use affine A-RRT. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space.

[0051] Affine A-RRT The planning algorithm constructs a hierarchical path topology for formation in affine space, including: Build and initialize the forward tree With reverse tree Maximum number of iterations and extended step size .

[0052] Among them, the node set edge set Define the tree nodes as a 5-element array structure:

[0053] in, Represents the affine configuration vector. Represents a spatial location vector. Indicates a time index. Indicates the parent node index. Represents the cumulative path cost; initializes the double-tree structure and node set. edge set Initial node Target node Define the maximum number of iterations. and extended step size In this embodiment, .

[0054] Random sampling is performed in the affine space, and single-step expansion is performed towards random nodes and the opposite tree direction respectively; If the constraints are met, local path reconstruction and topology optimization are performed by backtracking the parent node; The above process is repeated alternately until the bidirectional tree is successfully connected; The bidirectional paths are traced back and spliced ​​to complete the discretization process, and the physical space path sequence of each UAV is generated based on the affine mapping relationship.

[0055] By introducing a bidirectional search mechanism, this embodiment significantly improves the efficiency of searching for UAV swarm formation paths in complex affine spaces. The alternating expansion of forward and backward trees allows path search to proceed simultaneously from both the starting and target points, accelerating the path discovery process, especially in high-dimensional or obstacle-dense spaces. Furthermore, by backtracking parent nodes for local path reconstruction and topology optimization, this embodiment continuously optimizes the discovered paths, ensuring that the generated paths are not only feasible but also optimized to a certain extent, avoiding the suboptimal path problem that may occur in traditional RRT algorithms. Finally, the optimized affine path sequence is discretized and mapped to physical space, generating smooth and executable flight trajectories for each UAV. This effectively solves the challenge of rapidly and robustly generating high-quality swarm formation paths in complex environments, providing favorable initial conditions for subsequent trajectory optimization.

[0056] In specific implementation, the steps of randomly sampling in the affine space and expanding step-by-step towards random nodes and the opposite tree direction include: Expand the forward tree by uniformly sampling in the affine placement space to obtain random affine placement vectors. In the set of forward nodes Traversal and The first node in the positive direction with the minimum Euclidean distance , corresponding affine configuration vector Calculate the new configuration after expansion:

[0057] in, Indicates the expansion step size; for the new path segment Perform collision detection. If the new path topology check finds no collisions, construct a new forward node. ,in, Indicates in configuration The physical spatial location vector of the UAV swarm is calculated through affine transformation. The index is time-based; the index corresponding to the new node is... That is, increment the index of the starting node of the plan by one; The parent node of the new node in the node set Index in This represents the cumulative path cost of the new node, calculated based on the cost function in the formation hierarchical path planning model; Expanding the reverse tree, in the set of reverse nodes Traversal and The first node with the smallest Euclidean distance in the reverse direction , corresponding affine configuration vector ; Calculate the new configuration after the expansion:

[0058] For new path segments Perform collision detection; if no collision is detected, construct a new node in the reverse direction. Its construction method is similar to that of a positive new node. They are basically the same, except that the top right corner subscript is used to distinguish the trees being explored in different directions.

[0059] The above technical solution enables efficient and robust path exploration in affine configuration space. Specifically, the expansion of the forward tree, by exploring randomly sampled points with a finite step size, ensures the breadth of the search and coverage of unknown regions, avoiding getting trapped in local optima. Simultaneously, the expansion of the reverse tree targets newly generated nodes in the forward tree, allowing the bidirectional trees to approach each other, significantly accelerating the path discovery process. Collision detection after each expansion ensures that the generated path segments are collision-free in the affine space, thus guaranteeing the safe flight of UAV swarms in physical space. This bidirectional, finite-step expansion strategy, combined with real-time collision detection, effectively solves the problems of low path search efficiency and poor robustness in complex affine configuration spaces, laying a solid foundation for subsequent path reconstruction and optimization, thereby enabling faster construction of the formation-level geometric path topology.

[0060] It should be noted that the local path reconstruction and topology optimization by backtracking the parent node includes: Backtracking forward to the first node Obtain the second node of the forward starting direction ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update positive new node Parent node index Pointing to the transition node ; Update the forward tree point set Update the forward tree edge set ; Backtracking to the first node Obtain the second node from the reverse starting point ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update the reverse new node Parent node index Pointing to the transition node ; Update the reverse tree point set Update the inverse tree edge set .

[0061] Through the above technical solutions, in the process of collaborative obstacle avoidance and trajectory planning in UAV swarm formation, it is possible to perform affine A-RRT. The algorithm-generated paths undergo refined local optimization. Specifically, potential optimized path segments are identified by backtracking parent nodes, and collision detection and cost evaluation are performed on these segments to ensure the effectiveness and safety of the new path segments. After confirming the optimization potential, transition nodes are inserted at the midpoints of the optimized path topology, and the parent node index of the new nodes is updated, enabling smoother path transitions and avoiding abrupt path changes. Simultaneously, the vertex and edge sets of the forward and reverse trees are updated synchronously, ensuring the integrity of the entire tree structure and the effective integration of optimization results. This local path reconstruction and topology optimization mechanism enables the formation-level geometric path topology constructed by the UAV swarm in affine space to not only achieve connectivity but also achieve better smoothness and lower path costs in local regions. This significantly improves the quality of the UAV swarm's flight trajectory, enabling it to complete cooperative obstacle avoidance and trajectory planning tasks more efficiently and safely in complex environments, thus providing a higher-quality initial path for subsequent trajectory optimization models.

[0062] In the above steps, through affine A-RRT The algorithm constructs a formation-level path topology in affine space and alternately performs expansion and optimization processes until the bidirectional tree is successfully connected. However, after the bidirectional tree is successfully connected, the resulting path is still a discrete and abstract sequence of affine configurations, and the connecting segments may have discontinuities or insufficient density. This makes the path unsuitable for direct guidance of the UAV's actual flight in physical space and difficult to optimize for subsequent trajectories.

[0063] To address this, this embodiment further proposes backtracking and splicing bidirectional paths to complete the discretization process, and generating the physical space path sequence for each UAV based on affine mapping relationships. This process includes: Backtracking the dual-tree structure yields the forward and reverse discrete path sequence points. ,in ,as well as Represents the number of forward and reverse nodes in the backtracking process; Discretize the connection segment path using the same expansion step size:

[0064] Among them, the number of discrete points , Indicates the start and end points of the connecting segment. This represents the theoretical minimum number of time steps corresponding to the connect segment:

[0065] in, This represents the floor operator; The complete affine path sequence is represented as:

[0066] in, Indicates the reverse path sequence after inversion. One node; Based on the constructed affine mapping relationship, the position sequence of each drone in physical space is calculated. .

[0067] By backtracking the bidirectional tree structure, the initial affine path sequence from the starting point to the target point can be accurately extracted, laying the foundation for subsequent fine-tuning. Secondly, discretization of the connecting path segments effectively solves the bidirectional RRT problem. The algorithm addresses the potential for sparse or discontinuous pathpoints at connection points, ensuring the uniformity and smoothness of the entire affine path and avoiding trajectory planning errors or collision risks caused by insufficient pathpoint density. Finally, the complete affine path sequence is transformed into a sequence of specific physical positions for each UAV through affine mapping, allowing the abstract affine space planning results to be directly applied to the physical world. This provides precise and executable input for trajectory optimization and actual flight of individual UAVs, significantly improving the practicality and reliability of UAV swarm collaborative obstacle avoidance and trajectory planning.

[0068] Step S30: Based on the formation hierarchical geometric path topology, the path is corrected using the Euclidean symbolic distance field (ESDF) gradient information, and a convex region of the individual UAV hierarchical sequence is constructed in physical space.

[0069] Based on the ESDF-guided path correction mechanism, a single-level sequence convex region is constructed, including: Based on location sequence For each waypoint Construct a side with a length of The hypercube convex region as a sequence convex region :

[0070] This condition requires the area Completely in free space In the middle section, considering the possibility that path points are too close to obstacles, making it impossible to construct a sufficiently large convex region, a path projection correction mechanism is designed:

[0071] in, This indicates the corrected position. Indicates the scaling factor. express The ESDF gradient vector at that location.

[0072] Through the above technical solution, this embodiment introduces an ESDF-guided path correction mechanism based on the initially planned UAV swarm formation path. This mechanism first constructs an initial hypercube convex region for each UAV's position sequence Σi in physical space. When a waypoint is found When the distance to an obstacle is too close to prevent the construction of a sufficiently large convex region that meets the collision-free requirement, this mechanism can utilize the distance and gradient information from the ESDF to calculate the distance from the current point. Move away from the nearest obstacle and along that direction Projection Correction Corrected position A sufficient safe distance was maintained from the obstacles, thus ensuring the safety of the surrounding area. Constructed sequence convex region Able to be completely in a collision-free physical space This approach provides sufficient margin and effectively addresses the issue of fixed-size convex regions potentially colliding with obstacles in densely populated environments, offering a reliable and ample collision-free constraint space for subsequent quadratic programming models. Consequently, this correction mechanism significantly improves the success rate and safety of collaborative obstacle avoidance trajectory planning for UAV swarms, and lays a solid foundation for optimizing trajectory smoothness, energy efficiency, and swarm formation maintenance performance.

[0073] Step S40: Based on the convex region of the sequence and the formation configuration constraints, construct a quadratic programming model that integrates trajectory smoothness, energy efficiency and cluster formation maintenance performance, and solve it online to obtain the optimal flight trajectory for UAV cluster cooperative obstacle avoidance.

[0074] A quadratic programming model that comprehensively considers trajectory smoothness, energy efficiency, and cluster formation maintenance includes: For the Defining the drone at a given time State variables ,in Representing time respectively Next The position vector, velocity vector, and acceleration vector of the UAV are calculated, with acceleration used as the control input, considering the time interval. The control input, stacked time-domain state vectors:

[0075] Considering the backend optimization as a single-unit parallel solution process, to simplify the communication relationship between UAVs, and taking into account the relative reference positions between UAVs. Satisfies affine relation:

[0076] in, , This represents the tracking error; therefore, based on the consistency of the reference trajectory, the formation-keeping term in the optimization objective function is decoupled.

[0077] in, Represents higher-order error terms; ignores The cost of maintaining coupled formation can be approximated as the independent trajectory tracking cost of a single UAV; further, the optimization objective function based on the trajectory optimization model at the single-unit level can be developed as follows:

[0078] The gradient value of the constant term is 0, which does not affect the optimal value of the optimization; the constant term is ignored. And the following was compiled:

[0079] The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is the corresponding position Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed:

[0080] The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is based on the corresponding position. Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed:

[0081] Among them, linear equality constraints Linear inequality constraints .

[0082] By integrating the position, velocity, and control inputs of the UAV throughout the entire planning time domain into a unified time-domain state vector, this embodiment transforms the complex trajectory optimization problem into a well-structured and easily tractable standard quadratic programming model. This transformation enables the efficient use of mature quadratic programming solvers to quickly solve online for the optimal cooperative obstacle avoidance flight trajectory of the UAV swarm. Specifically, through the designed quadratic objective function, the smoothness of the trajectory, energy efficiency, and swarm formation maintenance performance can be optimized simultaneously, ensuring that the generated trajectory is not only safe and collision-free but also maintains a stable formation configuration and exhibits good flight quality during flight. Furthermore, by precisely expressing various constraints such as dynamics, initial / termination states, obstacle avoidance, and actuator limitations in linear and equality / inequality forms, the solved trajectory is ensured to be physically feasible and satisfies all operational constraints. This significantly improves the autonomous decision-making ability and flight efficiency of the UAV swarm in complex dynamic environments.

[0083] In specific implementation, the linear equality constraints and linear inequality constraints include: Based on the double-integral dynamic model, dynamic constraints Represented as:

[0084] Start constraints and end constraints Represented as:

[0085] Obstacle avoidance constraints based on sequential convex regions Represented as:

[0086] Actuator constraints based on velocity and acceleration limitations Represented as:

[0087] By explicitly defining the dynamic constraints, start and end constraints, obstacle avoidance constraints, and actuator constraints in the quadratic programming model, this embodiment ensures that the planned optimal flight trajectory for cooperative obstacle avoidance by the UAV swarm is physically feasible, meets mission requirements, and effectively avoids collisions. Specifically, dynamic constraints guarantee that the trajectory conforms to the motion laws of the UAVs; start and end constraints ensure that the start and end points of the mission are satisfied; obstacle avoidance constraints effectively prevent collisions with obstacles by confining the UAVs to a safe sequential convex region; and actuator constraints ensure that the planned velocity and acceleration are within the physical limitations of the UAV hardware. The introduction of these specific constraints enables the quadratic programming model to be solved more accurately and efficiently, thereby improving the reliability, safety, and executability of trajectory planning and avoiding planning failures or unsafe flights caused by unclear constraints.

[0088] In practice, .

[0089] Simulations were performed using MATLAB software, with the built-in function quadprog used as the quadratic programming solver.

[0090] In this embodiment, Figure 2 Indicates adoption Building map, Figure 3 Indicates adoption Indoor map. Under the same target map and planning parameters, RRT will be used. With Bi-RRT Extend to the same front-end and back-end planning framework, as A-RRT The benchmark comparison is performed; the efficiency of robot path planning is compared using three evaluation metrics: average time for front-end planning, average time for back-end optimization, average cost of the final trajectory, and planning success rate. In this embodiment, the cost of the swarm trajectory is quantitatively evaluated: the energy consumption metric is the discrete integral average of the square of the acceleration; the smoothness metric is the discrete integral average of the square of the acceleration difference (Jerk); and finally, the... The trajectory quality is obtained through weighted calculation. Statistical ensemble A-RRT RRT and Bi-RRT The front-end and back-end frameworks underwent 50 independent runs across four planning scenarios, as detailed in Table 1, integrating A-RRT. The proposed planning framework demonstrates superior performance across all evaluation metrics, particularly in front-end planning efficiency. Furthermore, all three comparative methods achieved a 100% planning success rate in the test scenario, validating the stability and reliability of the proposed framework. Please refer to the diagram illustrating the planning results. Figure 4As can be seen, the drone swarm adaptively adjusts its formation scale and attitude while maintaining a consistent overall formation structure. This is particularly evident in building-based scenarios. Figure 4 (a) and Figure 4 In (b), the formation is able to traverse narrow gaps formed by columnar building clusters while maintaining overall structural similarity; in indoor scenes... Figure 4 (c) and Figure 4 In (d), the formation passes through obstacles and corners in a highly compact manner, effectively avoiding local congestion and collisions between individual drones.

[0091] Table 1

[0092] This embodiment effectively reduces the dimensionality and search space complexity of UAV swarm system modeling and alleviates the computational burden by introducing three-dimensional affine transformation parameterized formation motion. Affine A-RRT is employed. The planning algorithm constructs a path topology in affine space, improving sampling search efficiency and reducing redundant nodes and invalid expansions. Utilizing ESDF gradient information for path correction and constructing sequential convex regions simplifies the construction of safe regions and ensures sufficient safety margins. By constructing a quadratic planning model integrating multiple performance indicators and solving it online, a balance is achieved between trajectory smoothness, energy efficiency, and swarm formation maintenance performance. This effectively solves the problems of coupling between formation constraints and obstacle constraints, low trajectory planning efficiency, and poor formation configuration stability, meeting the real-time planning requirements for collaborative obstacle avoidance in complex environments.

[0093] Reference Figure 5 , Figure 5 This is a structural block diagram of the first embodiment of the UAV swarm formation cooperative obstacle avoidance and trajectory planning system of this application.

[0094] like Figure 5 As shown in the embodiments of this application, the UAV swarm formation cooperative obstacle avoidance and trajectory planning system includes: Model building module 10 is used to construct a model of a UAV swarm trajectory collaborative planning system based on the parameterized UAV swarm formation motion of three-dimensional affine transformation. Path topology construction module 20 is used to construct the path topology based on the trajectory collaborative planning system model, using affine A-RRT. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space; The region construction module 30 is used to perform path correction based on the formation hierarchical geometric path topology, using Euclidean symbolic distance field (ESDF) gradient information, and to construct a single UAV hierarchical sequence convex region in physical space. The output module 40 is used to construct a quadratic programming model that integrates trajectory smoothness, energy efficiency and cluster formation maintenance performance based on the convex region of the sequence and the formation configuration constraints, and solve it online to obtain the optimal flight trajectory for UAV cluster cooperative obstacle avoidance.

[0095] It should be understood that the above are merely illustrative examples and do not constitute any limitation on the technical solution of this application. In specific applications, those skilled in the art can make settings as needed, and this application does not impose any restrictions on this.

[0096] This embodiment effectively reduces the dimensionality and search space complexity of UAV swarm system modeling and alleviates the computational burden by introducing three-dimensional affine transformation parameterized formation motion. Affine A-RRT is employed. The planning algorithm constructs a path topology in affine space, improving sampling search efficiency and reducing redundant nodes and invalid expansions. Utilizing ESDF gradient information for path correction and constructing sequential convex regions simplifies the construction of safe regions and ensures sufficient safety margins. By constructing a quadratic planning model integrating multiple performance indicators and solving it online, a balance is achieved between trajectory smoothness, energy efficiency, and swarm formation maintenance performance. This effectively solves the problems of coupling between formation constraints and obstacle constraints, low trajectory planning efficiency, and poor formation configuration stability, meeting the real-time planning requirements for collaborative obstacle avoidance in complex environments.

[0097] It should be noted that the workflow described above is merely illustrative and does not limit the scope of protection of this application. In practical applications, those skilled in the art can select some or all of it to achieve the purpose of this embodiment according to actual needs, and no restrictions are imposed here.

[0098] In addition, for technical details not described in detail in this embodiment, please refer to the method of UAV swarm formation cooperative obstacle avoidance and trajectory planning provided in any embodiment of this application, which will not be repeated here.

[0099] Furthermore, it should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0100] The sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0101] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as read-only memory (ROM) / RAM, magnetic disk, optical disk), and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods of the various embodiments of this application. The above are only preferred embodiments of this application and do not limit the patent scope of this application. All equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for cooperative obstacle avoidance and trajectory planning in unmanned aerial vehicle (UAV) swarm formation, characterized in that, include: Based on the parameterization of UAV swarm formation motion using 3D affine transformation, a model for collaborative planning of UAV swarm trajectory is constructed. Based on the aforementioned trajectory collaborative planning system model, affine A-RRT is adopted. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space; Based on the formation-level geometric path topology, path correction is performed using Euclidean symbolic distance field (ESDF) gradient information, and a convex region of the individual UAV hierarchical sequence is constructed in physical space. Based on the convex region of the sequence and the formation configuration constraints, a quadratic programming model is constructed that integrates trajectory smoothness, energy efficiency and swarm formation maintenance performance, and the optimal flight trajectory for UAV swarm cooperative obstacle avoidance is obtained online.

2. The method according to claim 1, characterized in that, The steps for constructing a collaborative planning system model for UAV swarm trajectory based on the parameterized UAV swarm motion of three-dimensional affine transformation include: The constructed parameterized model of drone formation swarm motion is represented as follows: in, Represents the affine configuration vector. Represents the orbit around a fixed coordinate axis Angle of rotation Represents the scaling factor Logarithmic form, Representing along fixed coordinate axes The amount of translation; Considering the configuration space, At this moment The drone is modeled as an equivalent point mass, and its position in physical space is denoted as . Its relationship with the affine configuration vector is as follows: Stacking this mapping relationship at the formation level yields... The overall position vector of a swarm of drones in physical space : in, Represents the Kronecker product. express 3D identity matrix for A dimensional vector of all 1s For a given formation configuration, where For the first The relative position of the UAVs to the midpoint in a pre-defined formation, and the translation vector. ,as well as Construct a path planning model at the formation level: in, For the rotational part under affine parameters, For the scaled portion, For the translation part, These represent the cost weighting coefficients for rotation, scaling, and translation between adjacent path points, respectively. These represent the initial configuration and target configuration of the drone swarm, respectively. in, Represents a collision-free physical space; Construct a trajectory optimization model at the individual level: in, These represent the weighted coefficients for smoothing, energy consumption, and formation maintenance costs, respectively. Let these represent the initial position and the target position of the individual UAV, respectively. Considering that the UAV follows a double-integral dynamics model, Indicates the speed and acceleration of the drone. The control input represents the desired acceleration of the UAV. To ensure the control variable acts within the interval, the state variables are defined at the nodes. Then, the dynamic difference equations are constructed, and to guarantee the continuity and solvability of the trajectory, the optimization model only needs to be defined in... this Over a time interval.

3. The method according to claim 1, characterized in that, Affine A-RRT The planning algorithm constructs a hierarchical path topology for formation in affine space, including: Build and initialize the forward tree With reverse tree Maximum number of iterations and extended step size ; Random sampling is performed in the affine space, and single-step expansion is performed towards random nodes and the opposite tree direction respectively; If the constraints are met, local path reconstruction and topology optimization are performed by backtracking the parent node; The above process is repeated alternately until the bidirectional tree is successfully connected; The bidirectional paths are traced back and spliced ​​to complete the discretization process, and the physical space path sequence of each UAV is generated based on the affine mapping relationship.

4. The method according to claim 3, characterized in that, The steps of randomly sampling in the affine space and expanding step-by-step towards random nodes and the opposite tree direction include: Expand the forward tree by uniformly sampling in the affine placement space to obtain random affine placement vectors. In the set of forward nodes Traversal and The first node in the positive direction with the minimum Euclidean distance , corresponding affine configuration vector Calculate the new configuration after expansion: in, Indicates the expansion step size; for the new path segment Perform collision detection. If the new path topology check finds no collisions, construct a new forward node. ,in, Indicates in configuration The physical spatial location vector of the UAV swarm is calculated through affine transformation. The index is time-based; the index corresponding to the new node is... That is, increment the index of the starting node of the plan by one; The parent node of the new node in the node set Index in This represents the cumulative path cost of the new node, calculated based on the cost function in the formation hierarchical path planning model; Expanding the reverse tree, in the set of reverse nodes Traversal and The first node with the smallest Euclidean distance in the reverse direction , corresponding affine configuration vector ; Calculate the new configuration after the expansion: For new path segments Perform collision detection; if no collision is detected, construct a new node in the reverse direction. Its construction method is similar to that of a positive new node. They are basically the same, except that the top right corner subscript is used to distinguish the trees being explored in different directions.

5. The method according to claim 3, characterized in that, The method of reconstructing local paths and optimizing topology by backtracking parent nodes includes: Backtracking forward to the first node Obtain the second node of the forward starting direction ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update positive new node Parent node index Pointing to the transition node ; Update the forward tree point set Update the forward tree edge set ; Backtracking to the first node Obtain the second node starting in reverse. ,Right now ; For path segments Perform collision detection. If no collision is found in the path topology detection, calculate the local path cost of the new path topology. ; Insert a transition node at the midpoint of the optimized path topology. ; Update the reverse new node Parent node index Pointing to the transition node ; Update the reverse tree point set Update the inverse tree edge set .

6. The method according to claim 3, characterized in that, The backtracking and splicing of bidirectional paths, the discretization process, and the generation of physical space path sequences for each UAV based on affine mapping relationships include: Backtracking the dual-tree structure yields the forward and reverse discrete path sequence points. ,in ,as well as Represents the number of forward and reverse nodes in the backtracking process; Discretize the connection segment path using the same expansion step size: Among them, the number of discrete points , Indicates the start and end points of the connecting segment. This represents the theoretical minimum number of time steps corresponding to the connect segment: in, This represents the floor operator; The complete affine path sequence is represented as: in, Indicates the reverse path sequence after inversion. One node; Based on the constructed affine mapping relationship, the position sequence of each drone in physical space is calculated. .

7. The method according to claim 1, characterized in that, Based on the ESDF-guided path correction mechanism, a single-level sequence convex region is constructed, including: Based on location sequence For each waypoint Construct a side with a length of The hypercube convex region as a sequence convex region : This condition requires the area Completely in free space In the middle section, considering the possibility that path points are too close to obstacles, making it impossible to construct a sufficiently large convex region, a path projection correction mechanism is designed: in, This indicates the corrected position. Indicates the scaling factor. express The ESDF gradient vector at that location.

8. The method according to claim 1, characterized in that, A quadratic programming model that comprehensively considers trajectory smoothness, energy efficiency, and cluster formation maintenance includes: For the first Defining the time of a drone state variables ,in Representing time respectively Next The position vector, velocity vector, and acceleration vector of the UAV are calculated, with acceleration used as the control input, considering the time interval. The control input, stacked time-domain state vectors: Considering the backend optimization as a single-unit parallel solution process, to simplify the communication relationship between UAVs, and taking into account the relative reference positions between UAVs. Satisfies affine relation: in, , This represents the tracking error; therefore, based on the consistency of the reference trajectory, the formation-keeping term in the optimization objective function is decoupled. in, Represents higher-order error terms; ignores The cost of maintaining coupled formation can be approximated as the independent trajectory tracking cost of a single UAV; further, the optimization objective function based on the trajectory optimization model at the single-unit level is developed as follows: The gradient value of the constant term is 0, which does not affect the optimal value of the optimization; the constant term is ignored. And the following was compiled: The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is corresponding position Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed: The first three summation terms are all optimization variables. The quadratic form can be represented as ,in Its construction method is to place it in the corresponding position. Add to block At the corresponding speed Add to block Corresponding input Add to block All other positions are 0, and This represents a 3x3 identity matrix; the last summation term represents a linear term, which can be expressed as: ,in Its construction method is based on the corresponding position. Add - to block All other elements are 0; Therefore, the standard quadratic form of trajectory optimization is constructed: Among them, linear equality constraints Linear inequality constraints .

9. The method according to claim 8, characterized in that, The linear equality constraints and linear inequality constraints include: Based on the double-integral dynamic model, dynamic constraints Represented as: Start constraints and end constraints Represented as: Obstacle avoidance constraints based on sequential convex regions Represented as: Actuator constraints based on velocity and acceleration limitations Represented as: 。 10. A drone swarm formation cooperative obstacle avoidance and trajectory planning system, characterized in that, include: The model building module is used to construct a model of a collaborative planning system for UAV swarm trajectory based on the parameterized UAV swarm formation motion of three-dimensional affine transformation. The path topology construction module is used to construct the path topology based on the trajectory collaborative planning system model, employing affine A-RRT. The planning algorithm constructs a hierarchical geometric path topology for formation in affine space; The region construction module is used to perform path correction based on the formation hierarchical geometric path topology, using Euclidean symbolic distance field (ESDF) gradient information, and to construct a single UAV hierarchical sequence convex region in physical space. The output module is used to construct a quadratic programming model that integrates trajectory smoothness, energy efficiency and cluster formation maintenance performance based on the convex region of the sequence and the formation configuration constraints, and solve it online to obtain the optimal flight trajectory for UAV cluster cooperative obstacle avoidance.

Images