An unmanned aerial vehicle cluster trajectory planning method based on passable space evaluation and autonomous grouping

Abstract

A kind of unmanned aerial vehicle cluster trajectory planning method based on passable space evaluation and autonomous grouping, comprising the following steps: front-end host receives local environment information collected by each unmanned aerial vehicle, and constructs global environment map;In the global environment map, search multiple topologically inequivalent candidate paths from the current position to the local target point, generate collision-free safety corridor for each candidate path, and calculate the passable volume of each safety corridor;According to the number of candidate paths and the passable volume of each candidate path, trigger the corresponding formation state mechanism.Cluster formation state mechanism includes no-path mechanism, single-path capacity shortage mechanism, multi-path capacity shortage mechanism and overall passable mechanism;According to the triggered formation state mechanism, execute the corresponding grouping strategy;Each unmanned aerial vehicle solves its own trajectory according to the grouping strategy and target assignment result under the condition of meeting obstacle avoidance constraint and dynamics constraint.The present application can improve the robustness and task success rate of cluster formation.

Description

Technical Field

[0001] This invention relates to the field of multi-UAV cooperative control and autonomous navigation technology, and in particular to a method for planning UAV swarm trajectories based on traversable space assessment and autonomous grouping. Background Technology

[0002] Most existing UAV formation planning methods employ a multi-objective weighted optimization framework, which solves for constraints such as smoothness, dynamics, obstacle avoidance, inter-UAV collision avoidance, and formation maintenance in a unified manner. This type of method can achieve good results in obstacle-sparse scenarios, but when there are large or dense obstacles, the formation maintenance objective and the obstacle avoidance objective often conflict strongly, easily leading to trajectory oscillations, severe detours, and planning failures.

[0003] When individual drones are blocked by obstacles, other drones will be affected by the global formation maintenance objective, causing cascading blockages. Therefore, relying solely on fixed-weight optimization is insufficient to reliably handle the dynamic conflict between "safety priority" and "formation maintenance." Thus, a technical solution is needed that can explicitly sense passage capability and proactively adjust the formation organization. Summary of the Invention

[0004] This invention aims to provide a method for planning the trajectory of UAV swarms based on accessible space assessment and autonomous formation. It provides spatial topology guidance information through front-end accessible space assessment and dynamically adjusts the formation constraint scope through a split-merge mechanism, enabling the swarm to prioritize safe passage through complex obstacle environments and then quickly and smoothly restore formation, thereby improving the robustness of swarm formation and mission success rate.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A method for planning the trajectory of a drone swarm based on drivable space assessment and autonomous grouping includes the following steps:

[0007] S1. The front-end host receives local environmental information collected by each drone and constructs a global environmental map;

[0008] S2. In the global environment map, search for multiple candidate paths with topological inequivalence from the current location to the local target point, generate a collision-free safe corridor for each candidate path, and calculate the passable volume of each safe corridor.

[0009] S3. Based on the number of candidate paths and the passable volume of each candidate path, trigger the corresponding formation state mechanism. The formation state mechanism includes a no-path mechanism, a single-path insufficient capacity mechanism, a multi-path insufficient capacity mechanism, and a mechanism that allows overall passage.

[0010] S4. Execute the corresponding grouping strategy based on the triggered formation state mechanism;

[0011] S5. Each UAV solves its own trajectory based on the grouping strategy and target assignment results, while satisfying obstacle avoidance constraints and dynamic constraints.

[0012] Preferably, S2 includes:

[0013] S21. Construct a topological road network graph containing multiple candidate paths using a topological road network generation method; search for candidate short paths in the topological road network graph using depth-first search; filter out redundant paths that are equivalent to existing paths using the homotopy equivalence criterion, and retain candidate paths that are topologically inequivalent.

[0014] S22. Discretize each candidate path to generate a seed point sequence; based on the seed point sequence, use an iterative dilation method to generate a collision-free convex polyhedron safety corridor; calculate the length of the minor semi-axis of the inscribed ellipsoid of the safety corridor, and calculate the passable volume based on the length of the minor semi-axis of the inscribed ellipsoid.

[0015] Preferably, in S22, the generation of the safe corridor satisfies the following constraint: Within a given local region R, solve for the maximum volume convex set C that contains the seed set S and does not intersect with the obstacle set O. The optimization problem is as follows:

[0016]

[0017] Where O represents the set of obstacles; S represents the set of seed points after path discretization; C represents the collision-free convex polyhedron; R represents the local constraint region of the corridor construction; vol(C) represents the volume of the convex polyhedron C;

[0018] Calculate the passable volume:

[0019]

[0020] in, Represents the passable volume of a certain path; Pi is a constant. Let be the minor semi-axis of the inscribed ellipsoid; when When the path is deemed passable, it is added to the path set P. This represents the minimum passable volume threshold for a single machine; at the same time, the passable volume is written into the candidate path passable volume set V.

[0021] Preferably, S3 includes:

[0022] If the number of candidate paths is 0, the no-path mechanism is triggered, and all drones plan independently, abandoning formation to maintain the target;

[0023] If the number of candidate paths is 1 and the passable volume of the shortest path is less than the required volume for array compression, then the single path capacity shortage mechanism is triggered, and the system switches to single-machine independent planning mode.

[0024] If the number of candidate paths is greater than 1 and the passable volume of the shortest path is less than the required volume for formation compression, then the multi-path capacity shortage mechanism is triggered, and subgroup allocation is performed.

[0025] In other cases, the entire process can proceed through the mechanism and enter the merge state.

[0026] Preferably, S4 includes:

[0027] When the multipath capacity shortage mechanism is triggered, the subgroup allocation strategy includes: sorting all candidate paths in ascending order of path length; estimating the maximum number of drones that each path can accommodate based on the passable volume of each candidate path; allocating drones to each candidate path in ascending order of path length until all drones have been allocated; and selecting the drone closest to the candidate path as the criterion for allocation based on the minimum distance from the current location of the drone to the midpoint of the candidate path.

[0028] Preferably, S4 includes: when the overall passability mechanism is triggered, restoring the global formation and grouping all drones into the same formation group; constructing a cost matrix, the elements of which are the distances between the current position of each drone and the position of each target formation; and using the Hungarian algorithm to solve for the optimal matching relationship between drones and groups to obtain the target formation position of each drone.

[0029] As a preferred embodiment, S5 includes:

[0030] Each UAV uses the Minimum Control Quantity (MINCO) parameterization method to parameterize its trajectory; an optimization objective function is constructed, which includes a weighted sum of smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-UAV collision avoidance cost, and formation maintenance cost; each UAV independently solves for the optimal trajectory using a quasi-Newton method (L-BFGS) optimization solver.

[0031] As a preferred option, the objective function in S5 is defined as follows:

[0032]

[0033] in, This indicates a backend trajectory optimization issue; Represents a multi-cost weight vector; , , , , These represent the smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-machine collision avoidance cost, and formation maintenance cost, respectively. Indicates the first The formation of drones maintains its cost, and ; Indicates the first The desired position vector of the drone in the target formation; Used to suppress abrupt changes in trajectory curvature Used to ensure dynamic constraints such as velocity / acceleration. Used for obstacle safety distance penalties Used for inter-machine safety distance constraints Used to constrain formation deviations.

[0034] As a preferred option, S5 also includes: parallel periodic triggering of the front-end host and the back-end drone; the front-end broadcasts group information and path status information every period; the back-end receives the information, reconstructs and optimizes the problem, and performs local replanning; when the front-end host fails, it automatically switches to another drone to continue running the front-end function.

[0035] The aforementioned method for planning drone swarm trajectories based on accessible space assessment and autonomous grouping is used to enable drones to quickly and safely pass through complex obstacle areas.

[0036] Compared with the prior art, the beneficial effects of the present invention are reflected in:

[0037] Different from Figure 2 The traditional technique described above directly weights and unifies the solution for multiple objectives such as smoothness, dynamics, obstacle avoidance, and formation maintenance. This invention adopts a hierarchical architecture of "front-end passable space assessment + back-end distributed trajectory optimization". At the front end, the passable volume of each channel is explicitly calculated through topological path search and safe corridor generation. Based on this, the "whether the whole can pass" is determined at a high level and the formation constraint domain is actively adjusted. This prevents the obstacle avoidance gradient and formation gradient from continuously offsetting each other due to fixed weights in the back-end optimization. This mechanism eliminates the trajectory oscillation, severe detours, and planning failure problems that are common in traditional methods in large and dense obstacle scenarios.

[0038] Unlike traditional technologies that only adjust weights or locally deform when the formation encounters obstacles, lacking a proactive decision-making mechanism for formation splitting and reorganization, this invention adopts a "split-subgroup allocation-merge" state switching mechanism triggered by the number of paths and the threshold of passable volume. When the shortest path capacity is insufficient, splitting is triggered to allocate the UAVs to multiple topologically unequal paths for passage. When the passage becomes more spacious, merging is triggered to restore the global formation, which significantly improves the success rate of the cluster in scenarios with large and dense obstacles. At the same time, the Hungarian redistribution during the merging phase reduces trajectory intersections and loops, improving the smoothness of merging. Attached Figure Description

[0039] Figure 1 This is a diagram of the front-end-back-end layered cluster autonomous grouping trajectory planning architecture of the present invention.

[0040] Figure 2 This is a diagram illustrating the trajectory oscillation mechanism in traditional multi-objective formation trajectory planning.

[0041] Figure 3 This document outlines the process for assessing accessible spaces and creating a safe corridor map.

[0042] Figure 4 A diagram showing the state transitions for cluster splitting, subgroup allocation, merging, and reallocation.

[0043] Figure 5 A diagram illustrating the split-merge flight process of a swarm formation in an obstacle environment.

[0044] Figure 6 This is a comparison of the cluster trajectory planning results of various methods in a wall-type obstacle scenario. Detailed Implementation

[0045] To make the technical means, inventive features, objectives, and effects of the invention readily understandable, the invention is further described below with reference to specific illustrations. However, the invention is not limited to the embodiments described below.

[0046] It should be noted that the structures, proportions, sizes, etc., illustrated in the accompanying drawings of this specification are only used to complement the content disclosed in the specification for those skilled in the art to understand and read, and are not intended to limit the conditions under which the present invention can be implemented. Therefore, they have no substantial technical significance. Any modifications to the structure, changes in the proportions, or adjustments to the size, without affecting the effects and objectives that the present invention can produce, should still fall within the scope of the technical content disclosed in the present invention.

[0047] This invention discloses a flexible formation trajectory planning method for UAV swarms in complex obstacle environments, involving splitting and merging. The method constructs a hierarchical architecture consisting of a front-end centralized accessible space assessment and a back-end distributed trajectory optimization. The front-end generates the assessed path accessibility volume through topological path search and safe corridors, and determines whether to split the swarm, assign subgroups, or merge the entire swarm based on the relationship between the number of paths and volume thresholds. During merging, the Hungarian algorithm is used to redistribute the positions of UAVs and the target swarm, reducing trajectory intersections and detours. The back-end solves the joint cost optimization problem based on grouping and assignment information, achieving smooth, feasible, and collision-free real-time trajectory planning. This invention significantly improves the success rate and system resilience of formation tasks in scenarios with large and dense obstacles. While maintaining obstacle avoidance safety and prioritizing task advancement, this method enables online switching between three states of the UAV swarm: "overall formation, splitting subgroups, and re-merging," suitable for tasks such as inspection, monitoring, search and rescue, collaborative transportation, and traversing complex scenarios.

[0048] The core idea of ​​this invention lies in adopting a hierarchical structure of "centralized front-end evaluation + distributed back-end optimization". The front-end module runs on a single host machine, responsible for aggregating point clouds, building maps, searching topological paths, evaluating their passable volumes, and then outputting group sets and redistribution results. The back-end module runs in parallel on each UAV, solving their respective trajectories based on the state information output by the front-end and executing synchronously. Unlike existing methods that only balance costs by weights, this invention first performs high-level passability analysis before deciding whether to maintain formation, split subgroups, or merge them entirely. This strategy can significantly reduce trajectory oscillations and optimization instability caused by the mutual offsetting of multi-objective gradients.

[0049] The functions and algorithm notation involved in this invention are as follows: CreateTopoGraph(·) represents the topology network generation function; DepthFirstSearch(·) represents the depth-first path search function; inEquivalent(·) represents the homotopy inequivalence determination function; DiscretizedPath(·) represents the path discretization function; GenerateCorridor(·) represents the safe corridor generation function; Re-assignment(·) represents the UAV and target position redistribution function; MINCO represents the trajectory parameterization method; L-BFGS represents the unconstrained optimization solver; and Hungarian algorithm represents the Hungarian assignment algorithm.

[0050] Example 1:

[0051] like Figure 1 The method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping, as shown, includes the following steps:

[0052] S1. The front-end host receives local environmental information collected by each drone and constructs a global environmental map;

[0053] Each drone collects local depth point clouds in real time and sends them to the front-end host. The front-end unifies the multi-source point clouds into a global coordinate system, constructing a local-global integrated raster map. Map updates use a scrolling window approach, retaining only the planning-related areas to reduce computational overhead.

[0054] In this embodiment, the front-end cycle is 0.5 seconds. Each cycle receives the point cloud and status of all online drones. After completing the map update, it enters the topology evaluation process.

[0055] S2. In the global environment map, search for multiple topologically inequivalent candidate paths from the current location to the local target point. For each candidate path, generate a collision-free safe corridor and calculate the passable volume of each safe corridor. Figure 3 As shown in the figure, P n Let P1, P2, and P3 represent the first, second, and third candidate paths after sorting by length.

[0056] S21. Call the topology network generation function to construct a topology network graph containing multiple candidate paths; search for candidate short paths in the topology network graph using depth-first search; use homotopy equivalence determination to filter out redundant paths that are equivalent to existing paths, and retain candidate paths that are topologically inequivalent.

[0057] The front end calls CreateTopoGraph( , O) Construct a topological road network G, where As the starting point for the current planning, Let O be the local target point and O be the set of obstacles. The topology graph generation uses a sampling and pruning mechanism to retain only topologically inequivalent paths, reducing redundant paths.

[0058] when and Continuous sampling and updating, among which, Indicates the cumulative sampling time of the topology graph; This indicates the upper limit of the topology graph sampling time; Indicates the current sampling number; This indicates the upper limit of the number of samples. After obtaining the topological road network G, candidate short paths are searched in the graph using DepthFirstSearch. and adopt inEquivalent ( We perform homotopy equivalence checks on a new path (P), where P represents the set of candidate topological paths. If a new path is equivalent to an existing path but longer, it is not added to the set.

[0059] S22. Discretize each candidate path to generate a seed point sequence; based on the seed point sequence, use an iterative dilation method to generate a collision-free convex polyhedron safety corridor; calculate the length of the minor semi-axis of the inscribed ellipsoid of the safety corridor, and calculate the passable volume of the candidate path based on the minor semi-axis.

[0060] For each path that passes the equivalence check, DiscretizedPath is executed, discretizing it into a sequence of seed points with small step sizes. Then, GenerateCorridor is called on this sequence to construct a collision-free convex corridor, which consists of multiple convex polyhedra.

[0061] Corridor generation satisfies the following constraint: Within a given local region R, solve for the maximum volumetric convex set C that contains a seed set S and does not intersect with the obstacle set O. The optimization problem is shown in the following equation. In implementation, an iterative inflation method (FIRI, fastiterative region inflation) is used, alternately performing constrained inflation and maximum inscribed ellipsoid updates.

[0062]

[0063] Where O represents the set of obstacles; S represents the set of seed points after path discretization; C represents a collision-free convex polyhedron (safe corridor element); R represents the local constraint region of the corridor construction; vol(C) represents the volume of the convex polyhedron C; assuming the path can be constructed using the minor semi-axis of the inscribed ellipsoid of the channel as... The passable volume of the path is defined as follows. When the path is deemed passable, it is added to the path set P, and its volume is written to V.

[0064]

[0065] in, Represents the passable volume of a certain path; This represents the minimum passable volume threshold for a single machine; V represents the set of passable volumes for candidate paths. Pi is a constant.

[0066] The above assessment and The process is repeated cyclically under certain conditions until the time budget or the maximum number of paths is reached. This indicates the cumulative time for assessing accessible space; This indicates the upper limit of the assessment time for accessible space; Indicates the maximum number of paths that can be retained;

[0067] S3. Based on the number of candidate paths and the passable volume of each candidate path, trigger the corresponding formation state mechanism; the formation state mechanism includes no-path mechanism, single-path insufficient capacity mechanism, multi-path insufficient capacity mechanism, and overall passability mechanism, such as... Figure 4 As shown, ℕ represents the current formation group set;

[0068] The front-end triggers four types of mechanisms based on the number and size of paths:

[0069] Mechanism M1 (no path): If If the value is 0, then the local space is too restricted. Set the group to empty and plan all drones independently;

[0070] Mechanism M2 (single path but insufficient capacity): If =1 and Then it will also switch to single-player mode;

[0071] Mechanism M3 (Multi-path and Shortest Path Capacity Insufficient): If >1 and If so, then subgroup allocation will be performed;

[0072] Mechanism M4 (can pass as a whole): In other cases, it enters the merge state and triggers position reallocation.

[0073] in This represents the volume corresponding to the shortest path after sorting by path length. This represents the equivalent passage requirement volume after the current formation is compressed, and is used to determine whether it is necessary to temporarily relax global formation constraints. Indicates the number of candidate paths;

[0074] S4. Execute the corresponding grouping strategy according to the triggered formation state mechanism;

[0075] 1. When the no-path mechanism or single-path capacity shortage mechanism is triggered, each drone will plan independently;

[0076] 2. When the multipath capacity shortage mechanism is triggered, the drones will be assigned to the subgroups corresponding to each candidate path, including:

[0077] First, the paths are sorted in ascending order of length. Then, the number of drones that each path can accommodate is estimated based on its passable volume. The system prioritizes assigning the maximum number of drones that can be accommodated to the shortest path, and then continues to assign them to the next shortest path, until all drones are grouped.

[0078] To reduce the additional maneuver distance introduced by splitting, the allocation is based on the criterion of "minimum distance from the current position of the UAV to the midpoint of the candidate path". During the splitting phase, the original formation assignment set A remains unchanged, only the group set ℕ and the range of formation constraints are changed.

[0079] 3. When the overall passability mechanism is triggered, restore the global formation and perform a redistribution of the drone and target formation positions;

[0080] The system first restores the global grouping. Then execute Re-assignment( , The redistribution objective is defined as follows.

[0081]

[0082] In the specific implementation, the cost matrix is ​​constructed. The Hungarian algorithm is used to find the optimal one-to-one match; This represents the element of the reallocation cost matrix, meaning the first element. The drone and the first The distance cost between the positions of each target formation. N represents the total number of drones; , , Indicates an index variable; Indicates the first The current position vector of the drone; Let the vector represent the desired position of the j-th UAV within the target formation. Indicates the first Current position vector of the drone; Indicates the first The target index of the drone in A; A represents the set of target formation position vectors; A represents the set of assignments from UAVs to target formation positions.

[0083] This process avoids trajectory intersections and large detours caused by "the rear drones having to return to their original index positions," thus shortening the merging and recovery time.

[0084] S5. Each UAV solves its own trajectory based on the grouping strategy and target assignment results, while satisfying obstacle avoidance constraints and dynamic constraints.

[0085] Each UAV uses the MINCO parameterization method to parameterize its trajectory; an optimization objective function is constructed, which includes a weighted sum of smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-UAV collision avoidance cost, and formation maintenance cost; the optimal trajectory is solved using the L-BFGS optimization solver.

[0086] Each UAV independently solves the optimization problem P1, as shown in the following equation. The trajectory is parameterized using MINCO, and the optimizer is L-BFGS. In the split state, formation preservation terms are applied only to members of the same subgroup; in the merge state, formation preservation terms are applied to all UAVs, and the redistributed target points are used.

[0087]

[0088] in, This indicates a backend trajectory optimization issue; Represents a multi-cost weight vector; , , , , These represent the smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-machine collision avoidance cost, and formation maintenance cost, respectively. Indicates the first The formation of drones maintains its cost, and ; Indicates the first The desired position vector of the drone in the target formation; Used to suppress abrupt changes in trajectory curvature Used to ensure dynamic constraints such as velocity / acceleration. Used for obstacle safety distance penalties Used for inter-machine safety distance constraints This is used to constrain formation deviation. By guiding the front-end state, the direction of the back-end cost combination remains consistent, which reduces the oscillations caused by the repeated offsetting of obstacle avoidance gradient and formation gradient.

[0089] Scrolling execution and communication mechanism:

[0090] The front-end and back-end are triggered in parallel cycles. The front-end broadcasts every cycle. The system receives the path status summary (A, B, C, and D), and immediately reconstructs and optimizes the problem, performing local replanning. If the front-end host fails, it can switch to another drone to continue running the front-end functions, ensuring system fault tolerance.

[0091] Under the corresponding implementation parameters Take 20ms. Taking 80ms, the total is 100ms, which is less than the 0.5-second front-end cycle, thus meeting the real-time operation constraints.

[0092] This embodiment corresponds to a dense obstacle area of ​​60m × 40m, where sixteen drones need to complete a formation flight mission, and the result is as follows. Figure 5As shown, the system continuously performs topology evaluation during the scrolling process, dynamically switching between splitting and merging states based on local channel changes. The front end prioritizes outputting the shortest path and passable volume, while the back end reconstructs and optimizes the problem according to the state.

[0093] When the environment becomes more congested, this invention maintains mission feasibility through proactive splitting; when the passage becomes more spacious, it triggers smooth merging through reallocation. This process demonstrates higher arrival efficiency and greater resilience in complex maps.

[0094] Example 2: Eight-plane rectangular formation traversing wall-like obstacles

[0095] In this embodiment, the eight drones initially form a rectangular formation, with the objective of traversing a wall-like obstacle passage 5.0m wide, 0.25m thick, and 3.0m high. To verify the effectiveness of the method of this invention, it is compared with three representative leading-edge formation trajectory planning methods:

[0096] Zhu's method: H. Zhu, J. Juhl, L. Ferranti, and J. Alonso-Mora, “Distributedmultirobot formation splitting and merging in dynamic environments,” in 2019IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 9080–9086.

[0097] Zhou's method: X. Zhou,

[0098] Quan's method: L. Quan, L. Yin, T. Zhang, M. Wang, R. Wang, S. Zhong, X. Zhou, Y.Cao, C. Xu, and F. Gao, “Robust and efficient trajectory planning forformation flight in dense environments,” IEEE Transactions on Robotics, vol.39, no. 6, pp. 4785–4804, 2023.

[0099] The comparison results are as follows Figure 6 As shown. Zhu's method employs a formation split-merge strategy based on corridor intersection regions, which can escape the predicament through splitting after trajectory oscillation. However, due to the lack of a redistribution mechanism in the merging phase, the trajectory exhibits significant detours. Zhou's method constructs an optimization problem by combining different cost functions and uses relative positions to describe formation constraints. Quan's method uses A* search to provide initial values ​​and introduces a graph-based formation representation in the backend trajectory optimization, while using an ESDF map to guide obstacle avoidance.

[0100] In wall-shaped obstacle scenarios, both Zhou's and Quan's methods exhibit performance degradation due to their difficulty in handling the conflict between formation maintenance and obstacle avoidance. Zhou's method suffers from significant trajectory oscillations and is unable to escape this state, ultimately leading to failure to reach the target point. In Quan's method, the formation constraints drive the drone to unfavorable positions close to large obstacles, and the initial values ​​provided by A* search are detrimental to the subsequent trajectory optimization, causing the planning of a single drone to terminate. Although Zhu's method can escape the predicament through splitting, the trajectory detours severely during the merging phase.

[0101] This invention's method promptly determines the split-merge state through accessible space assessment and introduces a Hungarian redistribution mechanism during the merging phase, effectively mitigating trajectory overlap between UAVs. When the shortest path volume is insufficient to accommodate the complete formation, the split mechanism is triggered, and the subgroups traverse different feasible paths. Through the redistribution mechanism, the subgroups reorganize into their original target formation behind the obstacle and continue flying to the destination. Experimental results show that this invention's method generates the smoothest, oscillation-free trajectory among all comparative methods, verifying its adaptability and superiority in complex obstacle environments.

[0102] Key points for project implementation:

[0103] 1) The front end must output the status within a fixed time budget and should not block the back end from replanning; 2) During the subgroup allocation phase, path feasibility and safety should be prioritized before considering maintaining the formation; 3) During the merging phase, reassignment should be performed first and then the formation constraints should be strengthened to avoid forced return to position causing crossover; 4) When the front end is unavailable, host switching should be supported to ensure continuous system operation.

Claims

1. A method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping, characterized in that, Includes the following steps: S1. The front-end host receives local environmental information collected by each drone and constructs a global environmental map; S2. In the global environment map, search for multiple candidate paths with topological inequivalence from the current location to the local target point, generate a collision-free safe corridor for each candidate path, and calculate the passable volume of each safe corridor. S3. Based on the number of candidate paths and the passable volume of each candidate path, trigger the corresponding formation state mechanism; the formation state mechanism includes no path mechanism, single path insufficient capacity mechanism, multi-path insufficient capacity mechanism, and overall passable mechanism; S4. Execute the corresponding grouping strategy based on the triggered formation state mechanism; including: When the multipath capacity shortage mechanism is triggered, the subgroup allocation strategy includes: sorting all candidate paths in ascending order of path length; estimating the maximum number of drones that each path can accommodate based on the passable volume of each candidate path; allocating drones to each candidate path in ascending order of path length until all drones have been allocated; and selecting the drone closest to the candidate path as the criterion for allocation, based on the minimum distance from the current location of the drone to the midpoint of the candidate path. When the overall passability mechanism is triggered, the global formation is restored, and all drones are grouped into the same formation group; a cost matrix is ​​constructed, the elements of which are the distances between the current position of each drone and the position of each target formation; the Hungarian algorithm is used to solve for the optimal matching relationship between drones and groups, and the target formation position of each drone is obtained. S5. Each UAV solves its own trajectory based on the grouping strategy and target assignment results, while satisfying obstacle avoidance constraints and dynamic constraints.

2. The method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping according to claim 1, characterized in that, S2 include: S21. Construct a topological road network graph containing multiple candidate paths using a topological road network generation method; search for candidate short paths in the topological road network graph using depth-first search; filter out redundant paths that are equivalent to existing paths using the homotopy equivalence criterion, and retain candidate paths that are topologically inequivalent. S22. Discretize each candidate path to generate a seed point sequence; based on the seed point sequence, use an iterative dilation method to generate a collision-free convex polyhedron safety corridor; calculate the length of the minor semi-axis of the inscribed ellipsoid of the safety corridor, and calculate the passable volume based on the length of the minor semi-axis of the inscribed ellipsoid.

3. The method for planning UAV swarm trajectory based on drivable space assessment and autonomous grouping according to claim 2, characterized in that, In S22, the generation of safe corridors satisfies the following constraint: Within a given local region R, find the maximum volume convex set C that contains the seed set S and does not intersect with the obstacle set O. The optimization problem is as follows: Where O represents the set of obstacles; S represents the set of seed points after path discretization; C represents the collision-free convex polyhedron; R represents the local constraint region of the corridor construction; vol(C) represents the volume of the convex polyhedron C; Calculate the passable volume: in, It represents the passable volume of a certain path; π represents the constant pi. Let be the minor semi-axis of the inscribed ellipsoid; when When the path is deemed passable, it is added to the path set P. This represents the minimum passable volume threshold for a single machine; at the same time, the passable volume is written into the candidate path passable volume set V.

4. The method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping according to claim 1, characterized in that, S3 include: If the number of candidate paths is 0, the no-path mechanism is triggered, and all drones plan independently, abandoning formation to maintain the target; If the number of candidate paths is 1 and the passable volume of the shortest path is less than the required volume for array compression, then the single path capacity shortage mechanism is triggered, and the system switches to single-machine independent planning mode. If the number of candidate paths is greater than 1 and the passable volume of the shortest path is less than the required volume for formation compression, then the multi-path capacity shortage mechanism is triggered, and subgroup allocation is performed. In other cases, the entire process can proceed through the mechanism and enter the merge state.

5. The method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping according to claim 1, characterized in that, S5 include: Each UAV uses a minimum control parameterization method to parameterize its trajectory; an optimization objective function is constructed, which includes a weighted sum of smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-UAV collision avoidance cost, and formation maintenance cost; each UAV uses a quasi-Newton method optimization solver to independently solve for the optimal trajectory.

6. The method for planning the trajectory of a UAV swarm based on drivable space assessment and autonomous grouping according to claim 1, characterized in that, In S5, the objective function is defined as follows: Where P1 represents the back-end trajectory optimization problem; w represents the multi-cost weight vector; , , , , These represent the smoothness cost, dynamic feasibility cost, obstacle avoidance cost, inter-machine collision avoidance cost, and formation maintenance cost, respectively. This indicates the cost of maintaining the formation of the i-th drone, and ; Represents the expected position vector of the i-th UAV in the target formation; This represents the current position vector of the i-th drone; Used to suppress abrupt changes in trajectory curvature Used to ensure dynamic constraints including velocity and acceleration, Used for obstacle safety distance penalties Used for inter-machine safety distance constraints Used to constrain formation deviations.

7. The method for planning UAV swarm trajectory based on drivable space assessment and autonomous grouping according to claim 1, characterized in that, S5 also includes: parallel periodic triggering of the front-end host and back-end drones; the front-end broadcasts group information and path status information every period; the back-end receives the information, reconstructs and optimizes the problem, and performs local replanning; when the front-end host fails, it automatically switches to other drones to continue running the front-end functions.

Images