A Method and System for Cooperative 3D Path Dynamic Planning of Unmanned Aerial Vehicle Swarms

Abstract

This invention discloses a method and system for collaborative 3D path dynamic planning for UAV swarms, relating to the field of swarm path planning technology. The method includes mapping UAV status, target waypoints, and communication adjacency relationships to a 3D swarm configuration graph, extracting edge lengths, node order, and height layer relationships to generate a configuration constraint planning set; dividing the 3D traversable connected domain based on the configuration constraint planning set, and converting target waypoints that disrupt configuration relationships into temporary navigation points to generate a configuration rearrangement navigation set; constructing cubic spline candidate trajectory segments using the configuration rearrangement navigation set, and correcting intermediate control points that do not meet turning radius and vertical variation constraints to generate a candidate trajectory deviation set; converting the candidate trajectory deviation set into a time-spread graph, and using maximum flow minimum cut to extract the minimum associated subgraph corresponding to conflicting edges. This invention avoids irrelevant UAVs repeatedly participating in negotiations by extracting the minimum associated subgraph through the time-spread graph and maximum flow minimum cut.

Description

Technical Field

[0001] This invention relates to the field of cluster path planning technology, and in particular to a method and system for collaborative three-dimensional path dynamic planning of unmanned aerial vehicle (UAV) clusters. Background Technology

[0002] With the development of UAV flight control, airborne communication, 3D positioning, path planning, and swarm collaborative control technologies, UAV swarms are gradually evolving from single-UAV trajectory planning to multi-UAV collaborative 3D path dynamic planning. In existing technologies, swarm UAVs typically plan their paths based on data such as 3D occupancy grid maps, mission waypoints, flight speed, communication adjacency relationships, and safety intervals. Flight trajectories are generated through centralized scheduling, distributed negotiation, potential field obstacle avoidance, spline curve smoothing, graph search, or heuristic optimization. For multi-UAV formation flight, area patrol, collaborative search, and continuous coverage missions, path planning not only needs to meet the turning radius, flight altitude, obstacle distance, and speed direction requirements of individual UAVs, but also needs to consider the relative configuration relationships between UAVs, communication reachability, and mission waypoint advancement order.

[0003] Existing UAV swarm path planning methods typically rely on fixed formation constraints, global path recalculation, or local avoidance mechanisms between conflicting parties when handling large-scale dynamic swarm collaboration. When the number of UAVs increases, the three-dimensional passage space becomes limited, or the mission waypoint distribution becomes complex, fixed formation constraints can easily lead to local passage failures, global recalculation increases communication load and computational latency, and avoiding conflicting parties only may disrupt the configuration relationships between adjacent UAVs, triggering new path intersections or altitude-level conflicts. Furthermore, existing solutions for handling the takeover of failed UAVs that have not completed their target waypoints often remain at the nearest proximity or mission replacement level, lacking a replanning mechanism that integrates trajectory locking, configuration constraints, and time-slice conflict detection. This means that the takeover path may affect the already stable collaborative trajectory. Summary of the Invention

[0004] In view of the aforementioned existing problems, the present invention is proposed.

[0005] Therefore, this invention provides a collaborative three-dimensional path dynamic planning method for UAV swarms to solve the problem of difficulty in coordinating and unifying configuration maintenance and local conflict replanning in UAV swarms within a limited three-dimensional airspace.

[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution:

[0007] In a first aspect, the present invention provides a method for dynamic planning of three-dimensional paths for UAV swarm collaboration, which includes mapping UAV status, target waypoints and communication adjacency relationships into a swarm three-dimensional configuration graph, and extracting graph edge lengths, node order and height layer relationships to generate a configuration constraint planning set;

[0008] The three-dimensional traversable connected domain is divided based on the configuration constraint programming set, and the target waypoints that violate the configuration relationship are converted into temporary navigation points to generate the configuration rearrangement navigation set;

[0009] The configuration rearrangement navigation set is used to construct cubic spline candidate trajectory segments, and intermediate control points that do not meet the turning radius and vertical variation constraints are corrected to generate a candidate trajectory deviation set;

[0010] The candidate trajectory deviation set is converted into a time-spreading graph. The minimum correlation subgraph corresponding to the conflict edge is extracted using the maximum flow minimum cut method. The adjustable local navigation points are rearranged to generate a locked trajectory subgraph set.

[0011] By locking the trajectory sub-map, the takeover drone that failed to complete the target waypoint is identified. The takeover trajectory segment is then recalculated locally to generate collaborative 3D path planning results.

[0012] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps for generating the configuration constraint planning set are as follows:

[0013] The current position, target waypoint, speed direction, remaining battery power, minimum turning radius, and safety interval of the drone are labeled as node attributes to generate a drone node attribute set.

[0014] Connect the drone nodes in the attribute set that can exchange trajectory information according to the communication adjacency relationship, and generate a three-dimensional configuration diagram of the cluster.

[0015] Extract the edge lengths between adjacent nodes, the node order in the direction of the target waypoint advance, and the vertical height layer relationships from the cluster's 3D configuration graph to generate a configuration relationship feature set;

[0016] The configuration relationship feature set is added to the UAV node attribute set according to the correspondence between adjacent UAV nodes to generate a configuration constraint planning set.

[0017] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps of dividing the three-dimensional connectivity domain are as follows:

[0018] Based on the safety intervals in the configuration constraint planning set, spatial expansion is performed on the obstacle occupancy cells of the 3D occupied grid map to generate a safe envelope grid set.

[0019] Remove the spatial cells occupied by the safety envelope grid set and retain the spatial cells that meet the flight altitude range of the UAV to generate a passable grid set;

[0020] Connect consecutive grids in the traversable grid set according to spatial adjacency to generate candidate connected regions. Then, select regions from the candidate connected regions that can cover the current position of the UAV, the target waypoint, and the node sequence relationship to generate a three-dimensional traversable connected region.

[0021] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps of converting the target waypoint that disrupts the configuration relationship into a temporary navigation point are as follows:

[0022] Based on the configuration constraint planning set, detect graph edge length out-of-bounds, node sequence intersection, and height layer relationship reversal corresponding to the target waypoint, and generate a configuration-breaking waypoint set;

[0023] Project the target waypoints in the configuration-breaking waypoint set to reachable locations within the three-dimensional traversable connected domain to generate a candidate temporary point set;

[0024] The candidate temporary point set is corrected according to the graph edge length relationship, node order relationship and height layer relationship in the cluster 3D configuration diagram to generate temporary navigation points;

[0025] The temporary navigation points, target waypoints that do not disrupt configuration relationships, and configuration adjustment relationships are arranged in navigation order to generate a configuration rearrangement navigation set.

[0026] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps for constructing the cubic spline candidate trajectory segment are as follows:

[0027] Arrange the temporary navigation points and target waypoints in the configuration rearrangement navigation set according to the node order to generate a navigation point sequence;

[0028] The current position of the UAV, the sequence of navigation points and the safety interval are converted into cubic spline control points, and three-dimensional candidate trajectory segments are generated by fitting cubic spline curves.

[0029] The curve sampling points of the three-dimensional candidate trajectory segments are arranged according to the speed direction of the UAV, and the UAV node attributes are written into the three-dimensional candidate trajectory segments to generate a candidate trajectory segment set.

[0030] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps for generating the candidate trajectory deviation set are as follows:

[0031] Based on the candidate trajectory segment set, calculate the turning radius of adjacent curve segments, the height difference between adjacent sampling points, the vertical change rate of continuous trajectory segments, the obstacle envelope distance, and the deviation of adjacent unmanned vehicle configuration, and generate a trajectory constraint test set;

[0032] Curve segments that do not meet the requirements for turning radius, height difference between adjacent sampling points, and vertical change rate of continuous trajectory segments in the trajectory constraint test set are marked as trajectory segments to be corrected;

[0033] Keeping the start and end points of the trajectory segment to be corrected unchanged, and moving the corresponding intermediate control points, a candidate trajectory segment for correction is generated. The change in control points and the change in configuration deviation between the candidate trajectory segment for correction and the 3D candidate trajectory segment are compared to generate a candidate trajectory deviation set.

[0034] As a preferred embodiment of the UAV swarm collaborative three-dimensional path dynamic planning method of the present invention, the specific steps of converting the candidate trajectory deviation set into a time-spread graph are as follows:

[0035] The candidate trajectory segments corresponding to the candidate trajectory deviation set are divided according to the prediction time window, the time slice trajectory sequence is generated, and the trajectory occupancy unit is determined according to the grid position traversed by the time slice trajectory sequence in the three-dimensional connected domain.

[0036] The drone number, time slice, and trajectory occupancy unit are combined into a time node to generate a time node set. The time nodes corresponding to adjacent time slices of the same drone are connected to generate a continuous time edge of the trajectory. The time nodes of different drones entering adjacent safety envelopes in the same time slice are connected to generate a conflict edge.

[0037] The time extension graph is composed of a set of time nodes, continuous time edges of the trajectory, and conflict edges.

[0038] As a preferred embodiment of the UAV swarm collaborative 3D path dynamic planning method of the present invention, the specific steps for generating the locked trajectory sub-map set are as follows:

[0039] In the time extension graph, the time nodes at both ends of the conflict edge are selected as the source points of the maximum flow minimum cut, and the end time node before the conflicting UAV reaches the next temporary navigation point is selected as the sink point of the maximum flow minimum cut to generate the conflict cut graph.

[0040] Mark the locked trajectory segments formed in the previous planning cycle, the drone trajectory segments with remaining power below the return power requirement, and the trajectory segments that have entered the neighborhood of the target waypoint as non-cut edges, and generate a set of non-cut edges;

[0041] Mark the adjustable trajectory segments, the unlocked local navigation points, and the adjacent trajectory segments that are not marked as non-cut edges as cut edges, and generate a cut edge set;

[0042] The edge capacity is determined based on the trajectory modification amount, configuration connection relationship and communication connection relationship corresponding to the cut edge, and a capacityd conflict cut graph is generated.

[0043] The saturation boundary is obtained by calculating the flow distribution from the source to the sink in the capacity-scaled conflict cut graph using maximum flow calculation.

[0044] Select cutable edges from the saturated boundary that sever the connection between the source and sink points, generate the minimum cut edge set, and backtrack along the minimum cut edge set to the UAV node, conflict edge node, and adjacent node that maintains the configuration connection to generate the minimum associated subgraph.

[0045] Keep the trajectory corresponding to the non-cut edges in the minimum associated subgraph unchanged, and rearrange the local navigation points corresponding to the cut edges along the three-dimensional traversable connected region to generate a rearranged navigation point set;

[0046] The corresponding local navigation points in the configuration rearrangement navigation set are replaced by the rearranged navigation point set, and the trajectory segments that have not entered the minimum associated subgraph are retained to generate the locked trajectory subgraph set.

[0047] As a preferred embodiment of the UAV swarm collaborative 3D path dynamic planning method of the present invention, the specific steps for generating the collaborative 3D path planning result are as follows:

[0048] Based on the locked trajectory sub-map, identify the faulty UAVs that have exited the collaborative planning and the uncompleted target waypoints, and generate a task compensation waypoint set;

[0049] Within the range of nodes adjacent to the locked trajectory sub-map and the cluster 3D configuration map, select UAVs that still maintain communication connection and have sufficient remaining power to reach the target waypoint, and generate a candidate takeover UAV set.

[0050] The takeover drone is determined based on the trajectory modification, communication adjacency, and remaining battery status of the candidate takeover drone set, and the takeover waypoint allocation relationship is generated.

[0051] The unfinished target waypoints in the takeover waypoint allocation relationship are placed into the replannable segment after the locked trajectory segment of the takeover UAV, thus generating the takeover trajectory segment;

[0052] The candidate trajectory segments of the takeover trajectory segment are recalculated three times and the time expansion graph conflict is verified to generate collaborative three-dimensional path planning results.

[0053] Secondly, the present invention provides a UAV swarm collaborative three-dimensional path dynamic planning system, including a configuration mapping module, which maps the UAV status, target waypoints and communication adjacency relationships into a swarm three-dimensional configuration graph, and extracts the graph edge length, node order and height layer relationship to generate a configuration constraint planning set;

[0054] The domain rearrangement module divides the three-dimensional traversable connected domains based on the configuration constraint planning set, and converts target waypoints that violate configuration relationships into temporary navigation points to generate configuration rearrangement navigation sets;

[0055] The trajectory correction module constructs cubic spline candidate trajectory segments using configuration rearrangement navigation sets, and corrects intermediate control points that do not meet the constraints of turning radius and vertical variation, generating a candidate trajectory deviation set.

[0056] The subgraph locking module converts the candidate trajectory deviation set into a time-spreading graph, extracts the minimum associated subgraph corresponding to conflict edges using maximum flow and minimum cut, rearranges the adjustable local navigation points, and generates a locked trajectory subgraph set.

[0057] The fault takeover module identifies the takeover drone that failed to complete the target waypoint by locking the trajectory sub-map set, performs local trajectory recalculation on the takeover trajectory segment, and generates collaborative 3D path planning results.

[0058] The beneficial effects of this invention are as follows: It maps UAV status, target waypoints, and communication adjacency relationships into a 3D cluster configuration graph. By forming a configuration constraint planning set through graph edge lengths, node order, and altitude layer relationships, the UAV cluster no longer relies solely on fixed formation coordinates but instead uses the structural relationships between adjacent nodes to constrain subsequent path planning, thus improving the cluster's configuration adaptability in complex 3D airspace. Through a 3D connectivity component and temporary navigation point conversion mechanism, this invention can perform local navigation rearrangement when target waypoints disrupt configuration relationships, avoiding direct modification of the entire UAV path and reducing the computational burden caused by expanding the planning range. Through cubic spline candidate trajectory segments and intermediate control point correction, this invention enables paths to simultaneously meet turning radius and vertical change requirements, improving trajectory executability. By extracting the minimum associated subgraph using a time-spread graph and maximum flow minimum cut, this invention can limit multi-UAV conflicts to the necessary local scope of replanning, avoiding repeated participation of unrelated UAVs in negotiations. By locking the trajectory sub-map set to determine the takeover drone for the faulty drone's target waypoint and performing local trajectory recalculation on the takeover trajectory segment, the mission continuity and cluster path stability can be maintained when some drones withdraw from collaboration, thereby improving the real-time performance, security, and mission completion reliability of the drone cluster collaborative 3D path dynamic planning. Attached Figure Description

[0059] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0060] Figure 1 A flowchart for a collaborative 3D path dynamic planning method for unmanned aerial vehicle (UAV) swarms.

[0061] Figure 2 This is a schematic diagram of configuration constraints and rearrangement of three-dimensional traversable connected domains.

[0062] Figure 3 This is a schematic diagram illustrating the conflict resolution between cubic spline trajectory correction and time spread plot.

[0063] Figure 4 This is a schematic diagram of mission takeover and local replanning for a malfunctioning drone. Detailed Implementation

[0064] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0065] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0066] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0067] Reference Figures 1-4 This is one embodiment of the present invention, which provides a method for dynamic planning of three-dimensional paths for collaborative unmanned aerial vehicle (UAV) swarms, including the following steps:

[0068] S1. Map the UAV status, target waypoints, and communication adjacency relationships into a cluster 3D configuration graph, and extract the graph edge lengths, node order, and height layer relationships to generate a configuration constraint planning set.

[0069] S1.1. The airborne positioning unit provides the current position of the UAV, the mission waypoint table determines the target waypoint, the flight controller calculates the speed direction based on continuous positioning points and attitude heading, the battery management unit determines the remaining battery power, the aircraft parameter table determines the minimum turning radius and safety interval, the communication adjacency relationship is determined based on the link handshake result of the airborne communication unit, and the above data are marked accordingly according to the same timestamp. The current position of the UAV, target waypoint, speed direction, remaining battery power, minimum turning radius and safety interval are calibrated to the corresponding UAV number, and a UAV node attribute set is generated.

[0070] The current position of the UAV is represented by three-dimensional coordinates, the target waypoint is used to represent the mission space position that the corresponding UAV needs to reach, the velocity direction is represented by a three-dimensional unit vector, the remaining power is used to represent the available energy state of the UAV to continue to execute path planning, the minimum turning radius is used to limit the curvature of the UAV trajectory, the safety interval is used to limit the minimum allowable distance between UAVs and between UAVs and obstacles, and the communication adjacency relationship is used to represent the connection relationship between UAVs that can directly exchange trajectory information.

[0071] The drone IDs from the drone node attribute set are used as drone nodes, and drone nodes that can exchange trajectory information are connected according to the communication adjacency relationship to generate a cluster 3D configuration diagram.

[0072] In this context, graph edges are established when there is a two-way trajectory information exchange relationship between communication adjacencies, and no graph edges are established when there is no two-way trajectory information exchange relationship between communication adjacencies. Graph edges are used to represent the cooperative constraint relationship between adjacent UAV nodes in the subsequent configuration maintenance, temporary navigation point conversion and local trajectory recalculation process.

[0073] S1.2. Calculate the edge lengths of the graph based on the current positions of adjacent UAV nodes in the cluster's 3D configuration graph, generating the edge length relationships. The edge lengths are derived from the 3D Euclidean distance formula in analytical geometry, derived from the coordinate differences of adjacent UAV nodes in the 3D coordinate system. The expression is:

[0074] ;

[0075] in, Indicates the first The drone node and the first The length of the graph edges between each drone node. Indicates the first The three-dimensional coordinates of the current position of each drone node. Indicates the first The three-dimensional coordinates of the current position of each drone node.

[0076] The UAV nodes in the cluster's 3D configuration diagram are arranged sequentially according to the target waypoint advancement direction to generate node order relationships. The target waypoint advancement direction is from the current position of the UAV to the target waypoint. The node order relationship is used to indicate the relative sequential position of adjacent UAV nodes in the target waypoint advancement direction. The node order relationship is used to avoid path crossing between adjacent UAV nodes during the subsequent configuration rearrangement navigation set generation process.

[0077] S1.3. Determine the height layer relationship based on the vertical coordinate difference between adjacent UAV nodes in the cluster's 3D configuration diagram. The height layer relationship is obtained from the height difference between adjacent UAV nodes.

[0078] The graph edge length relationship, node order relationship, and altitude layer relationship are arranged according to the graph edge correspondence relationship in the cluster 3D configuration graph to generate a configuration relationship feature set. Each item in the configuration relationship feature set corresponds to a graph edge. The configuration relationship feature set records the spatial distance, advance order, and vertical level of adjacent UAV nodes. The configuration relationship feature set is used to detect graph edge length exceeding the limit, node order intersection, and altitude layer relationship reversal corresponding to the target waypoint.

[0079] For each graph edge in the configuration relationship feature set, the adjacent UAV numbers, edge length relationships, node order relationships, and altitude layer relationships corresponding to each graph edge are added to the configuration constraint record items in the UAV node attribute set according to the UAV numbers at both ends of the graph edge, forming an adjacency constraint list corresponding to each UAV node. Among them, the configuration constraint record items include adjacent UAV numbers, graph edge lengths between adjacent nodes, sequential markers in the target waypoint advancement direction, and vertical altitude layer markers between adjacent nodes. The configuration relationship feature set is added to the UAV node attribute set according to the correspondence between adjacent UAV nodes, and the UAV node attribute set and the adjacency constraint list are arranged accordingly to generate a configuration constraint planning set.

[0080] S2. Based on the configuration constraint programming set, divide the three-dimensional traversable connected domain and convert the target waypoints that violate the configuration relationship into temporary navigation points to generate the configuration rearrangement navigation set.

[0081] S2.1. Spatial expansion is performed on the obstacle occupancy cells of the 3D occupied grid map according to the safety interval in the configuration constraint planning set to generate a safety envelope grid set; wherein, the 3D occupied grid map is represented by equal-sized spatial cells to represent the 3D airspace, the obstacle occupancy cells represent the spatial cells that the UAV cannot pass through, and the spatial expansion is used to mark the spatial cells around the obstacle occupancy cells that are smaller than the safety interval as safety envelope grids.

[0082] The number of expansion layers for spatial dilation is determined by the safety interval and the grid side length of the 3D occupied raster map, expressed as:

[0083] ;

[0084] in, This indicates the number of grid layers that the obstacle occupies as it expands outward. This represents the safety margin in the configuration constraint programming set. This indicates the grid side length of a 3D-occupied raster map. This indicates rounding up, which ensures that the safety envelope grid set after spatial expansion is not less than the safety interval requirement.

[0085] S2.2. Remove the spatial cells occupied by the safety envelope grid set and retain the spatial cells that meet the flight altitude range of the UAV to generate a passable grid set; wherein, the flight altitude range of the UAV is jointly defined by the UAV flight control constraints and the airspace boundary of the 3D occupied grid map. Spatial cells below the minimum flight altitude, spatial cells above the maximum flight altitude, and spatial cells occupied by the safety envelope grid set are not included in the passable grid set. The passable grid set is used to represent the spatial range in which the UAV can deploy temporary navigation points and 3D candidate trajectory segments.

[0086] Connecting consecutive grids in the accessible grid set according to spatial adjacency relationships generates candidate connected regions. Spatial adjacency relationship means that two accessible grids have a common grid surface, consecutive grids mean accessible grids that can be reached grid by grid through the common grid surface, and candidate connected regions mean three-dimensional spatial regions that UAVs can fly continuously without crossing the safe envelope grid set.

[0087] A three-dimensional traversable connected domain is generated by selecting a region from the candidate connected domains that can cover the current position of the UAV, the target waypoint, and the node order relationship. The grid where the current position of the UAV is located must belong to the three-dimensional traversable connected domain. If the grid where the target waypoint is located belongs to the traversable grid set, it is covered by the three-dimensional traversable connected domain. If the grid where the target waypoint is located falls into the safe envelope grid set, it is handled by the waypoint set destruction in the subsequent configuration. The node order relationship is used to limit the forward and backward travel order of multiple UAV nodes in the same three-dimensional traversable connected domain.

[0088] S2.3. Based on the configuration constraint planning set, detect graph edge length exceeding the limit, node sequence intersection, and altitude layer relationship reversal corresponding to the target waypoint, and generate a configuration-breaking waypoint set; where graph edge length exceeding the limit means that the distance between the target waypoints corresponding to adjacent UAV nodes is less than the safe interval in the configuration constraint planning set, node sequence intersection means that the order of adjacent UAV nodes in the direction of advancement of the target waypoint is inconsistent with the node order relationship in the configuration relationship feature set, and altitude layer relationship reversal means that the vertical height relationship of the target waypoints corresponding to adjacent UAV nodes is inconsistent with the altitude layer relationship in the configuration relationship feature set.

[0089] Project the target waypoints in the configuration-disrupted waypoint set to reachable locations within the 3D traversable connectivity domain to generate a candidate temporary point set. Here, reachable location represents a spatial location within the 3D traversable connectivity domain that can be reached from the current position of the corresponding UAV through continuous traversable grids. When the target waypoint falls into the safe envelope grid set or the target waypoint disrupts the node sequence relationship, select the grid center within the 3D traversable connectivity domain that is closest to the target waypoint and still satisfies the traversability condition as the candidate temporary point.

[0090] S2.4. Correct the candidate temporary point set according to the graph edge length relationship, node order relationship and altitude layer relationship in the cluster 3D configuration diagram, and generate temporary navigation points; wherein, each candidate temporary point in the candidate temporary point set is first determined according to the node order relationship to determine the passage order position, then adjusted to the corresponding vertical level in the 3D passage connectivity domain according to the altitude layer relationship, and finally the safe interval between adjacent UAV nodes is checked according to the graph edge length relationship. Candidate temporary points that satisfy the above three types of relationships are determined as temporary navigation points; if multiple candidate temporary points satisfy the above three types of relationships, the candidate temporary point that is closest to the corresponding target waypoint along the navigation order is selected as the temporary navigation point.

[0091] The temporary navigation points, target waypoints that do not disrupt configuration relationships, and configuration adjustment relationships are arranged in the navigation order to generate a configuration rearrangement navigation set. Among them, the configuration adjustment relationship records the correspondence between target waypoints in the configuration disruption waypoint set and temporary navigation points, as well as the graph edge length relationship, node order relationship, and altitude layer relationship of temporary navigation points relative to adjacent UAV nodes in the cluster's 3D configuration graph.

[0092] S3. Construct cubic spline candidate trajectory segments using configuration rearrangement navigation sets, and correct intermediate control points that do not meet the turning radius and vertical variation constraints to generate candidate trajectory deviation sets.

[0093] S3.1. Arrange the temporary navigation points, target waypoints that do not disrupt the configuration relationship, and configuration adjustment relationships in the configuration rearrangement navigation set according to the node order to generate a navigation point sequence; wherein, the first end of the navigation point sequence corresponds to the current position of the UAV, the middle position of the navigation point sequence corresponds to the temporary navigation point, and the end of the navigation point sequence corresponds to the target waypoint. The configuration adjustment relationship is used to maintain the graph edge length relationship, node order relationship, and altitude layer relationship between adjacent UAV nodes.

[0094] The current position of the UAV, the navigation point sequence, and the safety interval are converted into cubic spline control points to generate a trajectory control point set. The current position of the UAV serves as the starting control point of the cubic spline curve, the temporary navigation points and the target waypoints in the navigation point sequence serve as the subsequent control points of the cubic spline curve, and the safety interval is used to exclude control point positions that fall into the safety envelope grid set. If a control point falls into the safety envelope grid set, it moves along the direction of the adjacent traversable grid in the three-dimensional traversable connected domain to the center of the nearest traversable grid. The moved control point remains in the trajectory control point set.

[0095] S3.2. A cubic spline curve is fitted using a set of trajectory control points to generate a three-dimensional candidate trajectory segment. The cubic spline curve is a piecewise cubic polynomial curve commonly used in the field of three-dimensional path planning, which ensures that adjacent curve segments maintain positional and tangential continuity at the connection point. The starting point of the three-dimensional candidate trajectory segment corresponds to the current position of the UAV, and the ending point of the three-dimensional candidate trajectory segment corresponds to the target waypoint or temporary navigation point. The three-dimensional candidate trajectory segment is used to express the flightable path of the UAV from the current position to the navigation point sequence.

[0096] The curve sampling points of the three-dimensional candidate trajectory segments are arranged according to the speed direction of the UAV, and the UAV node attributes are written into the three-dimensional candidate trajectory segments to generate a candidate trajectory segment set. Among them, the curve sampling points are arranged sequentially along the advancement direction of the UAV's current position to the navigation point sequence, and the UAV node attributes include speed direction, remaining battery power, minimum turning radius, and safety interval.

[0097] S3.3. Calculate the turning radius of adjacent curve segments based on three consecutive curve sampling points in the candidate trajectory segment set, and generate the turning radius test result; the turning radius test result is used to compare with the minimum turning radius in the UAV node attributes; when the vector cross product modulus is zero, the three consecutive curve sampling points are located on the same straight line, and adjacent curve segments are treated as continuous straight lines and are not marked as insufficient turning radius.

[0098] The turning radius calculation originates from the formula in analytic geometry that determines the circumcircle radius using three points. It is derived from the relationship between the vectors of adjacent curve segments and the circumcircle radius, and the expression is:

[0099] ;

[0100] in, Indicates the first The turning radius corresponding to each curve sampling point , and These represent the three-dimensional coordinates of three consecutive curve sampling points. Indicates the magnitude of the vector;

[0101] The vertical change test results are generated by calculating the height difference between adjacent sampling points and the vertical change rate of continuous trajectory segments based on the adjacent curve sampling points in the candidate trajectory segment set. The height difference between adjacent sampling points is obtained from the vertical coordinate difference between adjacent curve sampling points, and the vertical change rate of continuous trajectory segments is obtained from the height difference between adjacent sampling points and the spatial distance between adjacent sampling points. The expression is as follows:

[0102] ;

[0103] ;

[0104] in, Indicates the first The curve sampling point up to the 1st The height difference between adjacent sampling points of each curve sampling point Indicates the first The curve sampling point up to the 1st Vertical rate of change of continuous trajectory segments between each curve sampling point and These represent the vertical coordinates of adjacent curve sampling points, and These represent the three-dimensional coordinates of adjacent curve sampling points.

[0105] S3.4. Calculate the obstacle envelope distance based on the spatial relationship between the curve sampling points in the candidate trajectory segment set and the safety envelope grid set, and calculate the adjacent UAV configuration deviation based on the difference between the adjacent UAV nodes corresponding to the candidate trajectory segment set and the configuration constraint planning set, and generate the trajectory constraint verification set; where the obstacle envelope distance represents the distance from the curve sampling point to the nearest safety envelope grid boundary, and the adjacent UAV configuration deviation represents the deviation of the adjacent UAV nodes corresponding to the three-dimensional candidate trajectory segment in terms of graph edge length relationship, node order relationship and height layer relationship.

[0106] Curve segments that do not meet the requirements for turning radius, height difference between adjacent sampling points, and vertical change rate of continuous trajectory segments in the trajectory constraint test set are marked as trajectory segments to be corrected; among them, the starting point and ending point of the corresponding three-dimensional candidate trajectory segment are retained for the trajectory segments to be corrected.

[0107] Keeping the start and end points of the trajectory segment to be corrected unchanged, the intermediate control point of the trajectory segment to be corrected is moved along the center of the traversable grid in the three-dimensional traversable connected domain to generate a candidate trajectory segment for correction. Among them, the moving direction of the intermediate control point is preferentially directed towards the center of the traversable grid near the line connecting the start and end points of the trajectory segment to be corrected. The moved intermediate control point avoids the safety envelope grid set and makes the turning radius not less than the minimum turning radius in the UAV node attributes, so that the height difference between adjacent sampling points and the vertical change rate of continuous trajectory segments meet the UAV flight control constraints.

[0108] The control point changes and configuration deviation changes between the corrected candidate trajectory segment and the 3D candidate trajectory segment are compared to generate a candidate trajectory deviation set. The control point changes represent the spatial displacement of the intermediate control points before and after correction, and the configuration deviation changes represent the changes in the graph edge length relationship, node order relationship, and height layer relationship of the corrected candidate trajectory segment relative to the configuration constraint planning set.

[0109] S4. Convert the candidate trajectory deviation set into a time-spreading graph, extract the minimum associated subgraph corresponding to the conflict edge using maximum flow and minimum cut, rearrange the adjustable local navigation points, and generate a locked trajectory subgraph set.

[0110] S4.1. Divide the candidate trajectory segments in the candidate trajectory deviation set according to the trajectory update cycle of the flight controller, and generate a time-slice trajectory sequence. The time-slice trajectory sequence is arranged according to three types of information: UAV number, sampling time, and curve sampling point. The sampling time is obtained by sequentially increasing the current planning time along the prediction time window. The curve sampling point is determined by the position of the candidate trajectory segment at the corresponding sampling time. The time-slice trajectory sequence is used to represent the three-dimensional position change of each UAV within the prediction time window.

[0111] To further explain, the prediction window is determined by the time required for speed adjustment, communication negotiation time, and local trajectory recalculation time, and is obtained by rounding up according to the trajectory update cycle of the flight controller.

[0112] The trajectory occupancy unit is determined based on the grid positions traversed by the time slice trajectory sequence in the three-dimensional connected domain, and a trajectory occupancy unit set is generated. The trajectory occupancy unit represents the three-dimensional connected domain grid where the curve sampling point is located and the adjacent grids covered by the safety interval. When the curve sampling point is located at the grid boundary, all adjacent grids intersecting with the curve sampling point are selected. The trajectory occupancy unit set is used to determine whether different UAVs enter the adjacent safety envelope in the same time slice.

[0113] The UAV number, time slice, and trajectory occupancy unit are combined into a time node to generate a time node set. Each time node uniquely represents the position of a UAV in the three-dimensional connected domain occupied in a time slice. The time node set retains the control point change and configuration deviation change in the candidate trajectory deviation set, so that subsequent maximum flow and minimum cut calculations can distinguish between adjustable trajectory segments and non-adjustable trajectory segments.

[0114] S4.2. Connect the time nodes corresponding to adjacent time slices of the same UAV to generate a continuous time edge of the trajectory; wherein, the continuous time edge of the trajectory represents the continuous flight relationship of the same UAV along the correction candidate trajectory segment from the previous time slice to the next time slice. The continuous time edge of the trajectory is used to maintain the temporal order of the UAV trajectory. The continuous time edge of the trajectory is also used to subsequently determine the end time node before the conflicting UAV reaches the next temporary navigation point.

[0115] Connect the time nodes when different UAVs enter adjacent safety envelopes in the same time slice to generate conflict edges. Adjacent safety envelopes indicate that there are common surfaces, common edges, common vertices, or spatial distances between the trajectory occupancy units of different UAVs that are less than the safety interval. Conflict edges indicate that there are path conflicts between different UAVs in the same time slice due to insufficient safety interval.

[0116] The time extension graph is composed of a set of time nodes, continuous time edges of the trajectory, and conflict edges. The time extension graph is a directed graph structure, with continuous time edges of the trajectory connected in the increasing direction of time slices, and conflict edges connecting time nodes in the same time slice that have a safe envelope conflict.

[0117] S4.3. In the time extension graph, select the time nodes at both ends of the conflict edge as the source nodes of the maximum flow minimum cut, and select the end time nodes before the conflicting UAV reaches the next temporary navigation point as the sink nodes of the maximum flow minimum cut to generate a conflict cut graph; where the source nodes represent the set of time nodes where the conflict has already occurred, and the sink nodes represent the set of time nodes before the conflicting UAV must reach the next temporary navigation point while maintaining the current navigation order.

[0118] The locked trajectory segments formed in the previous planning cycle, the drone trajectory segments with remaining battery power below the return-to-home battery power requirement, and the trajectory segments that have entered the target waypoint neighborhood are marked as non-cut edges, generating a non-cut edge set. Among them, the locked trajectory segments formed in the previous planning cycle represent trajectory segments that have been issued to the flight controller and need to be continuously executed; the drone trajectory segments with remaining battery power below the return-to-home battery power requirement represent trajectory segments that need to retain return-to-home capability; and the trajectory segments that have entered the target waypoint neighborhood represent trajectory segments that, if further modification, would disrupt the target waypoint arrival relationship. The non-cut edge set is used to prevent the maximum flow and minimum cut from severing the critical trajectory continuity relationship.

[0119] Adjustable trajectory segments, unlocked local navigation points, and adjacent trajectory segments not marked as non-cut edges are marked as cut edges to generate a cut edge set. Among them, adjustable trajectory segments are derived from the corrected candidate trajectory segments in the candidate trajectory deviation set, unlocked local navigation points are derived from the temporary navigation points in the configuration rearrangement navigation set, and adjacent trajectory segments not marked as non-cut edges represent trajectory segments that can participate in local rearrangement without breaking the configuration constraint planning set.

[0120] S4.4. Determine the edge capacity based on the trajectory modification amount, configuration connectivity, and communication connectivity corresponding to the cutable edge, and generate a capacity-based conflict cut graph. The trajectory modification amount comes from the change in control points in the candidate trajectory deviation set, the configuration connectivity comes from the graph edge length relationship, node order relationship, and height layer relationship in the configuration constraint planning set, and the communication connectivity comes from the graph edges in the cluster 3D configuration graph. The edge capacity represents the trajectory change cost, configuration connectivity effect, and communication cooperation effect caused by cutting the corresponding cutable edge. The smaller the edge capacity, the more suitable the cutable edge is to enter the minimum cut edge set.

[0121] The maximum flow algorithm is used to calculate the flow distribution from the source to the sink in a capacity-scaled conflict-cut graph, thus obtaining the saturation boundary. The maximum flow minimum cut algorithm is a classic algorithm in graph theory. First, the initial flow of each cutable edge in the capacity-scaled conflict-cut graph is set to zero, and a residual graph is constructed based on the difference between the edge capacity and the current flow. An augmenting path is searched from the source to the sink in the residual graph. If an augmenting path exists, the minimum remaining capacity of each edge along the augmenting path is taken as the augmenting flow for this iteration, and the forward and reverse edge flows are updated along the augmenting path. The search for augmenting paths is repeated. Update edge flows until there are no reachable augmenting paths from the source to the sink in the residual graph, and obtain the maximum flow distribution from the source to the sink. Based on the maximum flow distribution, mark the still reachable time nodes in the final residual graph starting from the source. Define the cut edges that point from the reachable time nodes on the source side to the unreachable time nodes on the sink side and have zero remaining capacity as saturation boundaries. The saturation boundary represents the boundary where the capacity between the reachable time nodes on the source side and the time nodes on the sink side has reached its upper limit. It is used to determine the cut edges required to cut the connection between the source and the sink.

[0122] Select cutable edges from the saturated boundary to cut the connection between the source and sink points, and generate a minimum cut edge set. The minimum cut edge set satisfies that after cutting, the source point cannot reach the sink point along the continuous time edge and conflict edge of the trajectory, and the minimum cut edge set only contains cutable edges from the cutable edge set and does not contain non-cutable edges from the non-cutable edge set.

[0123] S4.5. Backtrack along the minimum cut edge set to find the drone nodes, conflict edge nodes, and adjacent nodes that maintain configuration connectivity, and generate a minimum correlation subgraph. The drone nodes are derived from the drone numbers in the time node set, the conflict edge nodes are derived from the time nodes at both ends of the conflict edge in the time extension graph, and the adjacent nodes that maintain configuration connectivity are derived from the drone nodes that are directly connected to the conflicting drone nodes in the cluster's 3D configuration graph.

[0124] The trajectory corresponding to the non-cut edges in the minimum associated subgraph remains unchanged, and the local navigation points corresponding to the cut edges are rearranged along the 3D connected domain to generate a rearranged navigation point set. The trajectory corresponding to the non-cut edges retains its original time-slice trajectory sequence, while the local navigation points corresponding to the cut edges are extracted from the configuration rearranged navigation set. When rearranging the local navigation points, the graph edge length relationships, node order relationships, and height layer relationships in the configuration constraint planning set are maintained. The rearranged navigation point set is used to replace the corresponding local navigation points in the configuration rearranged navigation set. The rearranged navigation point set replaces the corresponding local navigation points in the configuration rearranged navigation set, and trajectory segments that have not entered the minimum associated subgraph are retained, generating a locked trajectory subgraph set.

[0125] S5. By locking the trajectory sub-map set, identify the takeover drone that failed to complete the target waypoint, perform local trajectory recalculation on the takeover trajectory segment, and generate collaborative 3D path planning results.

[0126] S5.1. Based on the UAV nodes, locked trajectory segments, and recalculated trajectory segments in the locked trajectory sub-map, identify the faulty UAVs that have exited the cooperative planning and the unfinished target waypoints, and generate a task compensation waypoint set; where, the faulty UAVs that have exited the cooperative planning refer to UAVs that cannot continue to complete the target waypoints during the execution process corresponding to the locked trajectory sub-map, and the unfinished target waypoints refer to the target waypoints located after the end of the locked trajectory segment of the faulty UAV and that have not yet been reached.

[0127] Within the range of nodes adjacent to the locked trajectory sub-map and the cluster 3D configuration map, select UAVs that still maintain communication connection and have sufficient remaining power to reach the target waypoint, and generate a candidate takeover UAV set; where maintaining communication connection means that there is still a communication adjacency relationship between the candidate UAV node and the adjacent UAV node in the locked trajectory sub-map, and reaching the target waypoint means that the candidate UAV still has the remaining power required for return after reaching the uncompleted target waypoint from the end of the locked trajectory segment.

[0128] S5.2. Determine the takeover drone based on the trajectory modification amount, communication adjacency relationship, and remaining power status of the candidate takeover drone set, and generate the takeover waypoint allocation relationship; wherein, the trajectory modification amount is determined by the spatial distance from the end position of the candidate drone's locked trajectory segment to the uncompleted target waypoint and the length of the replannable segment that needs to be changed after accessing the uncompleted target waypoint, the communication adjacency relationship is used to ensure that the takeover drone is still within the cooperative communication range of the cluster's three-dimensional configuration diagram, and the remaining power status is used to exclude candidate drones that cannot meet the target waypoint arrival requirements; when multiple candidate drones meet the above conditions, select the candidate drone with the smallest trajectory modification amount and still maintaining the communication connection as the takeover drone.

[0129] The unfinished target waypoints in the takeover waypoint allocation relationship are placed into the replannable segment after the locked trajectory segment of the takeover UAV, generating the takeover trajectory segment. The starting point of the takeover trajectory segment is the ending position of the locked trajectory segment of the takeover UAV, and the waypoints of the takeover trajectory segment are the unfinished target waypoints in the takeover waypoint allocation relationship. The takeover trajectory segment continues to maintain the node order relationship and altitude layer relationship in the configuration constraint planning set, and the takeover trajectory segment does not change the locked trajectory segment of the takeover UAV.

[0130] S5.3. Recalculate the candidate trajectory segments of the takeover trajectory segment using cubic spline to generate candidate trajectory segments for takeover. The starting point, unfinished target waypoints, and safety intervals of the takeover trajectory segment are converted into cubic spline control points according to the navigation order in the configuration rearrangement navigation set. The cubic spline control points are fitted to the candidate trajectory segments for takeover. The candidate trajectory segments for takeover are arranged with curve sampling points according to the velocity direction of the takeover UAV, and the trajectory constraint is checked according to the turning radius, the height difference between adjacent sampling points, the vertical change rate of continuous trajectory segments, and the obstacle envelope distance.

[0131] The candidate takeover trajectory segments undergo time-spread graph conflict verification to generate collaborative 3D path planning results. The candidate takeover trajectory segments are divided into time-slice trajectory sequences according to the flight controller's trajectory update cycle. These time-slice trajectory sequences determine trajectory occupancy units, which, together with the locked trajectory segments in the locked trajectory subgraph, constitute a time node set, continuous time edges, and conflict edges. If conflict edges are generated during time-spread graph conflict verification, the minimum correlation subgraph is extracted according to the maximum flow and minimum cut in S4, and the adjustable local navigation points are rearranged until there are no conflict edges between the candidate takeover trajectory segments and the locked trajectory segments in the locked trajectory subgraph. The collaborative 3D path planning results are generated from the conflict-free takeover trajectory segments corresponding to the takeover UAV, the locked trajectory segments in the locked trajectory subgraph, configuration rearrangement records, conflict resolution records, minimum correlation subgraph records, and mission takeover records.

[0132] This embodiment also provides a UAV swarm collaborative 3D path dynamic planning system, including:

[0133] The configuration mapping module maps the UAV status, target waypoints, and communication adjacency relationships into a cluster 3D configuration graph, and extracts the graph edge length, node order, and height layer relationships to generate a configuration constraint planning set;

[0134] The domain rearrangement module divides the three-dimensional traversable connected domains based on the configuration constraint planning set, and converts target waypoints that violate configuration relationships into temporary navigation points to generate configuration rearrangement navigation sets;

[0135] The trajectory correction module constructs cubic spline candidate trajectory segments using configuration rearrangement navigation sets, and corrects intermediate control points that do not meet the constraints of turning radius and vertical variation, generating a candidate trajectory deviation set.

[0136] The subgraph locking module converts the candidate trajectory deviation set into a time-spreading graph, extracts the minimum associated subgraph corresponding to conflict edges using maximum flow and minimum cut, rearranges the adjustable local navigation points, and generates a locked trajectory subgraph set.

[0137] The fault takeover module identifies the takeover drone that failed to complete the target waypoint by locking the trajectory sub-map set, performs local trajectory recalculation on the takeover trajectory segment, and generates collaborative 3D path planning results.

[0138] In summary, this invention maps UAV status, target waypoints, and communication adjacency relationships into a swarm 3D configuration graph. It then forms a configuration constraint planning set using graph edge lengths, node order, and altitude layer relationships. This allows the UAV swarm to move beyond simply relying on fixed formation coordinates and instead use the structural relationships between adjacent nodes to constrain subsequent path planning, improving the swarm's configuration adaptability in complex 3D airspace. Through 3D connectivity components and temporary navigation point conversion mechanisms, this invention can perform local navigation rearrangement when target waypoints disrupt configuration relationships, avoiding direct modification of the entire UAV path and reducing the computational burden caused by expanding the planning range. By using cubic spline candidate trajectory segments and intermediate control point correction, this invention enables paths to simultaneously meet turning radius and vertical change requirements, improving trajectory executability. By extracting the minimum associated subgraph using time-spread graphs and maximum flow minimum cut, this invention can limit multi-UAV conflicts to the necessary local scope of replanning, avoiding repeated participation of unrelated UAVs in negotiations. By locking the trajectory sub-map set to determine the takeover drone for the faulty drone's target waypoint and performing local trajectory recalculation on the takeover trajectory segment, the mission continuity and cluster path stability can be maintained when some drones withdraw from collaboration, thereby improving the real-time performance, security, and mission completion reliability of the drone cluster collaborative 3D path dynamic planning.

[0139] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for dynamic 3D path planning in UAV swarm collaboration, characterized by: include, The UAV status, target waypoints, and communication adjacency relationships are mapped into a cluster 3D configuration graph, and the graph edge length, node order, and height layer relationship are extracted to generate a configuration constraint planning set. The three-dimensional traversable connected domain is divided based on the configuration constraint programming set, and the target waypoints that violate the configuration relationship are converted into temporary navigation points to generate the configuration rearrangement navigation set; The configuration rearrangement navigation set is used to construct cubic spline candidate trajectory segments, and intermediate control points that do not meet the turning radius and vertical variation constraints are corrected to generate a candidate trajectory deviation set; The candidate trajectory deviation set is converted into a time-spreading graph. The minimum correlation subgraph corresponding to conflict edges is extracted using maximum flow and minimum cut. Adjustable local navigation points are then rearranged to generate a locked trajectory subgraph set. The specific steps are as follows: In the time extension graph, the time nodes at both ends of the conflict edge are selected as the source points of the maximum flow minimum cut, and the end time node before the conflicting UAV reaches the next temporary navigation point is selected as the sink point of the maximum flow minimum cut to generate the conflict cut graph. Mark the locked trajectory segments formed in the previous planning cycle, the drone trajectory segments with remaining power below the return power requirement, and the trajectory segments that have entered the neighborhood of the target waypoint as non-cut edges, and generate a set of non-cut edges; Mark the adjustable trajectory segments, the unlocked local navigation points, and the adjacent trajectory segments that are not marked as non-cut edges as cut edges, and generate a cut edge set; The edge capacity is determined based on the trajectory modification amount, configuration connection relationship and communication connection relationship corresponding to the cut edge, and a capacityd conflict cut graph is generated. The saturation boundary is obtained by calculating the flow distribution from the source to the sink in the capacity-scaled conflict cut graph using maximum flow calculation. Select cutable edges from the saturated boundary that sever the connection between the source and sink points, generate the minimum cut edge set, and backtrack along the minimum cut edge set to the UAV nodes, conflict edge nodes, and adjacent nodes that maintain configuration connectivity to generate the minimum associated subgraph. Keep the trajectory corresponding to the non-cut edges in the minimum associated subgraph unchanged, and rearrange the local navigation points corresponding to the cut edges along the three-dimensional traversable connected region to generate a rearranged navigation point set; The corresponding local navigation points in the configuration rearrangement navigation set are replaced by the rearranged navigation point set, and the trajectory segments that have not entered the minimum associated subgraph are retained to generate the locked trajectory subgraph set; By locking the trajectory sub-map, the takeover drone that failed to complete the target waypoint is identified. The takeover trajectory segment is then recalculated locally to generate collaborative 3D path planning results.

2. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 1, characterized in that: The specific steps for generating the configuration constraint programming set are as follows: The current position, target waypoint, speed direction, remaining battery power, minimum turning radius, and safety interval of the drone are labeled as node attributes to generate a drone node attribute set. Connect the drone nodes in the attribute set that can exchange trajectory information according to the communication adjacency relationship, and generate a three-dimensional configuration diagram of the cluster. Extract the edge lengths between adjacent nodes, the node order in the direction of the target waypoint advance, and the vertical height layer relationships from the cluster's 3D configuration graph to generate a configuration relationship feature set; The configuration relationship feature set is added to the UAV node attribute set according to the correspondence between adjacent UAV nodes to generate a configuration constraint planning set.

3. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 2, characterized in that: The specific steps for dividing the three-dimensional accessible connected components are as follows: Based on the safety intervals in the configuration constraint planning set, spatial expansion is performed on the obstacle occupancy cells of the 3D occupied grid map to generate a safe envelope grid set. Remove the spatial cells occupied by the safety envelope grid set and retain the spatial cells that meet the flight altitude range of the UAV to generate a passable grid set; Connect consecutive grids in the traversable grid set according to spatial adjacency to generate candidate connected regions. Then, select regions from the candidate connected regions that can cover the current position of the UAV, the target waypoint, and the node sequence relationship to generate a three-dimensional traversable connected region.

4. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 3, characterized in that: The specific steps for converting target waypoints that disrupt configuration relationships into temporary navigation points and generating configuration rearrangement navigation sets are as follows: Based on the configuration constraint planning set, detect graph edge length out-of-bounds, node sequence intersection, and height layer relationship reversal corresponding to the target waypoint, and generate a configuration-breaking waypoint set; Project the target waypoints in the configuration-breaking waypoint set to reachable locations within the three-dimensional traversable connected domain to generate a candidate temporary point set; The candidate temporary point set is corrected according to the graph edge length relationship, node order relationship and height layer relationship in the cluster 3D configuration diagram, and temporary navigation points are generated. The temporary navigation points, target waypoints that do not disrupt configuration relationships, and configuration adjustment relationships are arranged in navigation order to generate a configuration rearrangement navigation set.

5. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 4, characterized in that: The specific steps for constructing cubic spline candidate trajectory segments and generating a candidate trajectory segment set are as follows: Arrange the temporary navigation points and target waypoints in the configuration rearrangement navigation set according to the node order to generate a navigation point sequence; The current position of the UAV, the sequence of navigation points and the safety interval are converted into cubic spline control points, and three-dimensional candidate trajectory segments are generated by fitting cubic spline curves. The curve sampling points of the three-dimensional candidate trajectory segments are arranged according to the speed direction of the UAV, and the UAV node attributes are written into the three-dimensional candidate trajectory segments to generate a candidate trajectory segment set.

6. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 5, characterized in that: The specific steps for generating the candidate trajectory deviation set are as follows: Based on the candidate trajectory segment set, calculate the turning radius of adjacent curve segments, the height difference between adjacent sampling points, the vertical change rate of continuous trajectory segments, the obstacle envelope distance, and the deviation of adjacent unmanned vehicle configuration, and generate a trajectory constraint test set; The curve segments in the trajectory constraint test set that do not meet the requirements for turning radius, height difference between adjacent sampling points, and vertical change rate of continuous trajectory segments are marked as trajectory segments to be corrected; Keeping the start and end points of the trajectory segment to be corrected unchanged, and moving the corresponding intermediate control points, a candidate trajectory segment for correction is generated. The change in control points and the change in configuration deviation between the candidate trajectory segment for correction and the 3D candidate trajectory segment are compared to generate a candidate trajectory deviation set.

7. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 6, characterized in that: The specific steps for converting the candidate trajectory deviation set into a time-spread map are as follows: The candidate trajectory segments corresponding to the candidate trajectory deviation set are divided according to the prediction time window, the time slice trajectory sequence is generated, and the trajectory occupancy unit is determined according to the grid position passed by the time slice trajectory sequence in the three-dimensional connected domain. Combine the drone number, time slice, and trajectory occupancy unit into a time node to generate a time node set. Connect the time nodes corresponding to adjacent time slices of the same drone to generate a continuous time edge of the trajectory. Connect the time nodes of different drones entering adjacent safety envelopes in the same time slice to generate a conflict edge. The time extension graph is composed of a set of time nodes, continuous time edges of the trajectory, and conflict edges.

8. The UAV swarm collaborative three-dimensional path dynamic planning method as described in claim 1, characterized in that: The specific steps for generating the collaborative 3D path planning results are as follows: Based on the locked trajectory sub-map, identify the faulty UAVs that have exited the collaborative planning and the uncompleted target waypoints, and generate a task compensation waypoint set; Within the range of nodes adjacent to the locked trajectory sub-map and the cluster 3D configuration map, select UAVs that still maintain communication connection and have sufficient remaining power to reach the target waypoint, and generate a candidate takeover UAV set. The takeover drone is determined based on the trajectory modification, communication adjacency, and remaining battery status of the candidate takeover drone set, and the takeover waypoint allocation relationship is generated. The unfinished target waypoints in the takeover waypoint allocation relationship are placed into the replannable segment after the locked trajectory segment of the takeover UAV, thus generating the takeover trajectory segment; The candidate trajectory segments of the takeover trajectory segment are recalculated three times and the time expansion graph conflict is verified to generate collaborative three-dimensional path planning results.

9. A UAV swarm collaborative three-dimensional path dynamic planning system, based on the UAV swarm collaborative three-dimensional path dynamic planning method according to any one of claims 1 to 8, characterized in that: include, The configuration mapping module maps the UAV status, target waypoints, and communication adjacency relationships into a cluster 3D configuration graph, and extracts the graph edge length, node order, and height layer relationships to generate a configuration constraint planning set; The domain rearrangement module divides the three-dimensional traversable connected domains based on the configuration constraint planning set, and converts target waypoints that violate configuration relationships into temporary navigation points to generate configuration rearrangement navigation sets; The trajectory correction module constructs cubic spline candidate trajectory segments using configuration rearrangement navigation sets, and corrects intermediate control points that do not meet the constraints of turning radius and vertical variation, generating a candidate trajectory deviation set. The subgraph locking module converts the candidate trajectory deviation set into a time-spreading graph, extracts the minimum associated subgraph corresponding to conflict edges using maximum flow and minimum cut, rearranges the adjustable local navigation points, and generates a locked trajectory subgraph set. The fault takeover module identifies the takeover drone that failed to complete the target waypoint by locking the trajectory sub-map set, performs local trajectory recalculation on the takeover trajectory segment, and generates collaborative 3D path planning results.

Images