An unmanned aerial vehicle formation control method based on adaptive leader selection and graph theory metrics

Abstract

The application discloses a kind of self-adapting pilot selection and graph theory metric UAV formation control method, comprising the following steps: first, obtain environmental perception data, calculate the nearest obstacle distance and local obstacle density of each UAV;With the nearest obstacle distance and local obstacle density of current pilot as pilot switching criterion, decide whether in follower set selects new pilot;Build formation graph model and utilize symmetric normalized Laplacian matrix to measure formation shape, and calculate the shape similarity error between current formation and target formation;Establish the pilot-follower relationship that follower position is equal to the sum of pilot position and relative displacement;Perform Sim (3) uniformization processing, finally the reference trajectory before and after switching is smoothly spliced.The UAV formation control method of the application can evaluate local risk in real time and adaptively select pilot in the face of complex environment, and suppresses trajectory and control quantity mutation when pilot switches and formation transitions.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for unmanned aerial vehicles (UAVs), and more particularly to a UAV formation control method based on adaptive navigator selection and graph theory metrics. Background Technology

[0002] Unmanned aerial vehicle (UAV) swarm navigation and cooperative control has been a long-term research direction in the field of robotics and autonomous systems. Limited by the perception range and operational efficiency of single-unit systems, multi-unit swarming is considered a key approach to improve mission efficiency and robustness. A relatively systematic methodology has been developed both domestically and internationally for UAVs focusing on formation maintenance and reconfiguration. Early research commonly employed techniques for maintaining formation shape, including virtual structures, navigation functions, reactive behavior, consistency-based local control laws, and leader-follower control laws. However, in dynamic or dense obstacle environments, these methods often face the dual challenges of insufficient global coordination and insufficient adaptability to complex environments. For example, traditional leader-follower strategies are prone to over-reliance on the leader, and formation is difficult to maintain when communication is interrupted or the leader fails. Furthermore, in real-world scenarios, guiding the swarm to safely change its position and then converge back to the target formation after individuals deviate from their preset positions to avoid obstacles is a key challenge in engineering implementation.

[0003] Unmanned aerial vehicle (UAV) formation reconfiguration addresses situations such as sudden changes in mission requirements, unexpected obstacles, or member failures. The goal is to smoothly transition the swarm from its initial formation to the target formation while ensuring collision-free flight and safety. Existing research often decomposes the reconfiguration problem into a two-stage framework of "global target allocation + local trajectory planning." From a system architecture perspective, either a global optimization approach with a central node providing unified solutions or a distributed negotiation approach where each UAV makes autonomous decisions based on neighborhood information can be employed. However, in environments with dense obstacles or limited communication, distributed negotiation based solely on incomplete local information may lead to decision conflicts or deadlocks, making stable reconfiguration difficult.

[0004] Based on the above research findings, at least the following shortcomings and areas for improvement exist:

[0005] (1) Lack of real-time risk quantification and navigator selection criteria for complex environments: Many methods assume that the navigator is fixed or are based solely on simple rule switching, making it difficult to stably select a "safer" navigator in dense obstacle environments.

[0006] (2) Navigator switching is prone to oscillation, leading to reference jumps and control shocks: Under noise, disturbances or rapid changes in the local environment, the lack of vibration suppression mechanism will cause frequent switching, resulting in trajectory discontinuity, thrust abrupt changes or even collision risks.

[0007] (3) Lack of global shape measurement that is insensitive to translation, rotation and scale, the formation shape is prone to drift: When relying solely on local following or local consistency constraints, the formation may experience overall scale drift or slow shape distortion, especially after deviating from obstacle avoidance and it is difficult to "pull back".

[0008] (4) Lack of a unified geometric consistency and smooth transition mechanism during the switching or reconstruction phase: There are scale and attitude differences in the reference poses under different reference templates or different navigator coordinate systems, and direct switching is prone to geometric inconsistencies and discontinuities.

[0009] To overcome the shortcomings of existing technologies, there is an urgent need for an adaptive UAV formation control method that can assess local risks in real time and adaptively select a leader in complex environments. At the same time, it should introduce a global shape metric to constrain the formation and suppress abrupt changes in trajectory and control variables during leader switching and formation transition. Summary of the Invention

[0010] The purpose of this invention is to overcome the shortcomings of the prior art and to provide an adaptive UAV formation control method that can assess local risks in real time and adaptively select a navigator in the face of complex environments. At the same time, it introduces a global shape metric to constrain the formation and suppresses abrupt changes in trajectory and control variables during navigator switching and formation transition.

[0011] To achieve the above objectives, the present invention is implemented through the following technical solution:

[0012] An adaptive leader selection and graph theory metric method for UAV formation control includes the following steps:

[0013] (1) First, obtain the environmental perception data of each UAV in the formation, and calculate the nearest obstacle distance and local obstacle density for each UAV;

[0014] (2) Use the distance to the nearest obstacle and the local obstacle density of the current navigator as the navigator switching criteria to decide whether to select a new navigator from the follower set;

[0015] (3) When the formation undergoes deformation or requires reconstruction in a complex environment, the formation shape is measured by constructing a formation graph model and using a symmetric normalized Laplace matrix, and the shape similarity error between the current formation and the target formation is calculated.

[0016] (4) Establish a navigator-follower relationship where the follower position is equal to the sum of the navigator position and the relative displacement. The expected position of each follower is obtained by minimizing the shape similarity error. The difference between the expected position and the current position of the navigator is used as the relative displacement of the corresponding follower, so that each follower can maintain the overall formation shape while keeping track of the navigator.

[0017] (5) When a navigator switch occurs, Sim(3) is performed to make the current formation instance and the desired formation template consistent, and the reference relative pose after the switch is obtained. Finally, the reference trajectories before and after the switch are smoothly spliced ​​together to achieve continuous transition and collision-free reconstruction of the formation during the navigator switch process.

[0018] Furthermore, in step (1), the nearest obstacle distance and local obstacle density are calculated based on the Euclidean signed range field (ESDF), assuming time... Time The center of mass of the drone is located at Then the safety margin Local obstacle density It can be defined in the following integral form:

[0019] (1)

[0020] in, For local receptive domain, The distance is half the diagonal of the body. The sensing radius of the drone, and Voxels are considered "dangerous voxels," and the above integral can be approximated by voxel counting:

[0021] (2)

[0022] in, This represents the number of dangerous voxels within the local sensing domain. The total number of voxels in the local perceptual domain.

[0023] Furthermore, the switching criterion in step (2) adopts a trigger threshold. Hysteresis threshold and minimum stay time The joint mechanism, and meets the requirements , When the local obstacle density of the current navigator is not less than the first density threshold Or, the distance from the current navigator to the nearest obstacle surface is not greater than the first distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. When the local obstacle density of the current navigator is not greater than the second density threshold, a navigator switch is triggered. Or, the distance from the current navigator to the nearest obstacle surface is not less than the second distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. At this time, there is no need to switch navigators.

[0024] Furthermore, in step (2), the new navigator is selected from the current group of followers whose local obstacle density is the lowest.

[0025] Furthermore, in step (3), the formation graph model is to construct an undirected graph from the current UAV formation:

[0026] (3)

[0027] in, Let be the set of vertices in an undirected graph. Let the undirected graph be a set of edges. Each drone in the formation corresponds to a vertex in the undirected graph. The connection between any two drones with a topological relationship corresponds to an edge in the undirected graph. The Euclidean distance between the two drones is used as the weight of the edge, and the edge weight can be the Euclidean distance. ,in and Let i and j represent the three-dimensional coordinates of the UAVs respectively; the symmetric normalized Laplace matrix is:

[0028] (4)

[0029] in, For an undirected graph of arrays, Let be the degree matrix of the undirected graph of the array, and with This indicates the error in shape similarity.

[0030] Furthermore, in step (5), Sim(3) is standardized to make the navigator consistent with... Switch to Then, regarding the current formation example With expected template Solve for similarity transformations:

[0031] (5)

[0032] in, Let i be the position coordinates of the drone i in the desired formation. Let it be its position coordinates within the current formation. Let be a rotation matrix. Scaling factor As the translation vector, the expected reference after unification is obtained. The desired relative pose is mapped to a new reference frame to eliminate scale, attitude, and translation differences; smooth stitching is performed on the reference trajectory. Smooth stitching:

[0033] (6)

[0034] in, For drones At any moment Smooth reference position, The reference trajectory before the switch at time The corresponding position This represents the expected position after Sim(3) unification and mapping to the new reference frame. For normalized time variables, Switching time for the navigator To ensure a smooth transition time, It is a short-time sigmoid weight function. It is used to ensure position and velocity continuity and reduce switching shock.

[0035] It also includes an adaptive navigator selection and graph theory metric UAV formation control system, comprising an environment perception module, an environment index calculation module, a navigator selection module, a graph theory shape evaluation module, a desired position generation module, a formation reconstruction and transition module, and a control execution module. Based on an adaptive navigator selection and graph theory metric UAV formation control method, it completes the control transformation of the UAV formation.

[0036] It also includes an electronic device comprising a processor and a memory, wherein the memory stores a computer program, which, when executed by the processor, performs control transformation of the drone formation based on an adaptive navigator selection and graph theory metric drone formation control method.

[0037] It also includes a computer-readable storage medium storing a computer program that, when executed by a processor, performs control transformation of the drone formation based on an adaptive navigator selection and graph theory metric drone formation control method.

[0038] Compared with the prior art, the present invention has the following beneficial effects:

[0039] This invention introduces the nearest obstacle distance and local obstacle density based on ESDF as risk characterization, providing a unified quantitative basis for navigator selection and switching. To avoid frequent navigator switching, a joint mechanism of "trigger threshold + hysteresis threshold + minimum dwell time" is adopted to suppress oscillations, ensuring that switching can be triggered but not frequently. The UAV formation is modeled as a graph and a symmetric normalized Laplacian matrix is ​​constructed to establish a shape similarity metric that is insensitive to translation, rotation, and scale, which is used to continuously constrain and correct the formation, avoiding formation drift or slow distortion. Sim(3) uniformization processing can eliminate scale, attitude, and translation differences, and through The smooth splicing of the system enables a continuous transition of the reference, reducing the risk of control impact and collision. Attached Figure Description

[0040] Figure 1 This is a flowchart of the method of the present invention;

[0041] Figure 2 This is a schematic diagram of local environment density calculation based on ESDF;

[0042] Figure 3 Flowchart for Adaptive Navigator Switching;

[0043] Figure 4 A schematic diagram of undirected graph modeling for formation and Laplacian shape measurement;

[0044] Figure 5 A schematic diagram of Sim(3) consistency after the navigator switch;

[0045] Figure 6 for Level smooth splicing and short-time S-shaped weighting Schematic diagram. Detailed Implementation

[0046] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0047] like Figures 1 to 6 As shown, an adaptive leader selection and graph theory metric method for UAV formation control includes the following steps:

[0048] (1) First, obtain the environmental perception data of each UAV in the formation, and calculate the nearest obstacle distance and local obstacle density for each UAV;

[0049] (2) Use the distance to the nearest obstacle and the local obstacle density of the current navigator as the navigator switching criteria to decide whether to select a new navigator from the follower set;

[0050] (3) When the formation undergoes deformation or requires reconstruction in a complex environment, the formation shape is measured by constructing a formation graph model and using a symmetric normalized Laplace matrix, and the shape similarity error between the current formation and the target formation is calculated.

[0051] (4) Establish a navigator-follower relationship where the follower position is equal to the sum of the navigator position and the relative displacement. The expected position of each follower is obtained by minimizing the shape similarity error. The difference between the expected position and the current position of the navigator is used as the relative displacement of the corresponding follower, so that each follower can maintain the overall formation shape while keeping track of the navigator.

[0052] (5) When a navigator switch occurs, Sim(3) is performed to make the current formation instance and the desired formation template consistent, and the reference relative pose after the switch is obtained. Finally, the reference trajectories before and after the switch are smoothly spliced ​​together to achieve continuous transition and collision-free reconstruction of the formation during the navigator switch process.

[0053] The control process of this invention is divided into:

[0054] (1) Calculate the local environmental density: such as Figure 2 As shown, the system first perceives the environment and constructs an ESDF (Extended Array of Deployments). Each UAV in the formation is equipped with a LiDAR or visual sensor to acquire environmental point cloud or depth information. Further, distance transformation can be performed on a raster or voxel map to construct the ESDF. The experimental verification platform can be based on ROS and Gazebo simulations, using LiDAR and visual sensors to simulate perception capabilities.

[0055] The current navigator's nearest obstacle distance and local obstacle density are calculated based on the Euclidean signed range field (ESDF), assuming time... Time The center of mass of the drone is located at Then the safety margin Local obstacle density It can be defined in the following integral form:

[0056] (1)

[0057] in, For local receptive domain, The distance is half the diagonal of the body. The sensing radius of the drone can be set according to the sensor range and formation scale. In this example, it is... It is 2m. The external interface box can be taken at a diagonal of 0.5m. And... The voxels are considered "dangerous voxels," and in engineering implementation, the above integral can be approximated by voxel counting:

[0058] (2)

[0059] in, This represents the number of dangerous voxels within the local sensing domain. The total number of voxels in the local perceptual domain.

[0060] (2) Adaptive Navigator Switching: The new navigator is selected from the current group of followers whose local obstacle density is the lowest. For example... Figure 3 As shown, to avoid frequent handovers caused by noise, a trigger threshold is used as the handover criterion. Hysteresis threshold and minimum stay time The joint mechanism, and meets the requirements , When the local obstacle density of the current navigator is not less than the first density threshold Or, the distance from the current navigator to the nearest obstacle surface is not greater than the first distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. At that time, a navigator switch is triggered, and the new navigator is selected. .in, It is a proportional quantity (0–1). , These represent the maximum local obstacle density that triggers the switch and the local obstacle density that causes the switch to hysteresis, respectively, and can be selected and maintained between 0.05 and 0.8. . , These represent the minimum obstacle distance required to trigger the switch and the minimum obstacle distance required for the switch lag, respectively, and can be compared with... Proportional settings, in this example, , . The timeout can be 0.2–10s; this example uses 1s to balance response speed and stability.

[0061] When the local obstacle density of the current navigator is not greater than the second density threshold Or, the distance from the current navigator to the nearest obstacle surface is not less than the second distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. At this time, there is no need to switch navigators.

[0062] (3) Formation diagram model and shape measurement:

[0063] like Figure 4 As shown, the formation is modeled as an undirected graph:

[0064] (3)

[0065] in, Let be the set of vertices in an undirected graph. Let the undirected graph be a set of edges. Each drone in the formation corresponds to a vertex in the undirected graph. The connection between any two drones with a topological relationship corresponds to an edge in the undirected graph. The Euclidean distance between the two drones is used as the weight of the edge, and the edge weight can be the Euclidean distance. ,in and Let i and j represent the three-dimensional coordinates of the UAVs respectively; the symmetric normalized Laplace matrix is:

[0066] (4)

[0067] in, For an undirected graph of arrays, Let be the degree matrix of the undirected graph of the array, and with This represents the shape similarity error. The metric is insensitive to translation, rotation, and scale, and is differentiable, making it suitable for embedded control fine-tuning. E in an undirected graph can be generated from a fixed-communication graph, a k-nearest neighbor graph, a radius-neighbor graph, or a Delaunay triangulation graph. In some embodiments, the graph may be required to remain connected or have a minimum degree of 2 to improve the stability of the shape metric. In addition to distance, squared distance, inverse distance, or exponential kernel weights can also be used to enhance local robustness.

[0068] (4) Navigator-follower control and shape fine-tuning:

[0069] Within the navigator-follower framework, the navigator guides the overall movement of the formation, while the followers adjust their own trajectories based on the navigator's position. To achieve unified control of "following + global shape maintenance," it is possible to... As a fine-tuning target, by minimizing The desired relative displacement or desired position of the follower is calculated. In this embodiment, this can be achieved through gradient descent or distributed iterative computation, wherein... The gradient regarding the position can be obtained using the chain rule. For each control cycle, the desired position can be updated with 1 to 5 lightweight iterations, or... By weight Incorporate it into the cost function of the tracking controller, such as in MPC / least squares tracking.

[0070] (5) Sim(3) consistency and smooth transition after navigator switching:

[0071] like Figure 5 As shown, Sim(3) standardization processing is performed by the navigator from Switch to Then, regarding the current formation example With expected template Solve for similarity transformations:

[0072] (5)

[0073] in, Let i be the position coordinates of the drone i in the desired formation. Let it be its position coordinates within the current formation. Let be a rotation matrix. Scaling factor As the translation vector, the expected reference after unification is obtained. And the desired relative pose is mapped to a new reference frame to eliminate scale, pose, and translation differences. For example... Figure 6 As shown, smooth stitching is performed on the reference trajectory. Smooth stitching:

[0074] (6)

[0075] in, For drones At any moment Smooth reference position, The reference trajectory before the switch at time The corresponding position This represents the expected position after Sim(3) unification and mapping to the new reference frame. For normalized time variables, Switching time for the navigator To ensure a smooth transition time, It is a short-time sigmoid weight function. This is used to ensure position and velocity continuity and reduce switching shock, enabling a seamless transition and collision-free reconfiguration of the formation during leader switching. For example, a cubic polynomial could be used. Its satisfaction , , , When smoothing time When the time is set to 0.5–1.0 s, the reference trajectory can be smoothly transitioned from the old reference to the new reference, thereby suppressing the peak values ​​of thrust and attitude angular velocity.

[0076] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for UAV formation control based on adaptive navigator selection and graph theory metrics, characterized in that: Includes the following steps: (1) First, obtain the environmental perception data of each UAV in the formation, and calculate the nearest obstacle distance and local obstacle density for each UAV; (2) Use the distance to the nearest obstacle and the local obstacle density of the current navigator as the navigator switching criteria to decide whether to select a new navigator from the follower set; (3) When the formation undergoes deformation or requires reconstruction in a complex environment, the formation shape is measured by constructing a formation graph model and using a symmetric normalized Laplace matrix, and the shape similarity error between the current formation and the target formation is calculated. (4) Establish a navigator-follower relationship where the follower position is equal to the sum of the navigator position and the relative displacement. The expected position of each follower is obtained by minimizing the shape similarity error. The difference between the expected position and the current position of the navigator is used as the relative displacement of the corresponding follower, so that each follower can maintain the overall formation shape while keeping track of the navigator. (5) When a navigator switch occurs, Sim(3) is performed to make the current formation instance and the desired formation template consistent, and the reference relative pose after the switch is obtained. Finally, the reference trajectories before and after the switch are smoothly spliced ​​together to achieve continuous transition and collision-free reconstruction of the formation during the navigator switch process.

2. The UAV formation control method based on adaptive navigator selection and graph theory metric as described in claim 1, characterized in that: In step (1), the nearest obstacle distance and local obstacle density are calculated based on the Euclidean signed range field (ESDF), assuming time... Time The center of mass of the drone is located at Then the safety margin Local obstacle density It can be defined in the following integral form: （1） in, For local receptive domain, The distance is half the diagonal of the body. The sensing radius of the drone, and Voxels are considered "dangerous voxels," and the above integral can be approximated by voxel counting: （2） in, This represents the number of dangerous voxels within the local sensing domain. The total number of voxels in the local perceptual domain.

3. The UAV formation control method based on adaptive navigator selection and graph theory metric as described in claim 1, characterized in that: In step (2), the switching criterion uses a trigger threshold. Hysteresis threshold and minimum stay time The joint mechanism, and meets the requirements , When the local obstacle density of the current navigator is not less than the first density threshold Or, the distance from the current navigator to the nearest obstacle surface is not greater than the first distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. When the local obstacle density of the current navigator is not greater than the second density threshold, a navigator switch is triggered. Or, the distance from the current navigator to the nearest obstacle surface is not less than the second distance threshold. And at the current moment Switching points with the last Navigator The time interval between them shall not be less than the minimum stay time. At this time, there is no need to switch navigators.

4. The UAV formation control method based on adaptive navigator selection and graph theory metric as described in claim 3, characterized in that: In step (2), the new navigator is selected from the current group of followers whose local obstacle density is the lowest.

5. The UAV formation control method based on adaptive navigator selection and graph theory metric as described in claim 1, characterized in that: In step (3), the formation graph model is to construct an undirected graph from the current UAV formation: （3） in, Let be the set of vertices in an undirected graph. Let the undirected graph be a set of edges. Each drone in the formation corresponds to a vertex in the undirected graph. The connection between any two drones with a topological relationship corresponds to an edge in the undirected graph. The Euclidean distance between the two drones is used as the weight of the edge, and the edge weight can be the Euclidean distance. ,in and Let i and j represent the three-dimensional coordinates of the UAVs respectively; the symmetric normalized Laplace matrix is: （3） in, For an undirected graph of arrays, the adjacency matrix is... Let be the degree matrix of the undirected graph of the array, and with This indicates the error in shape similarity.

6. The UAV formation control method based on adaptive navigator selection and graph theory metric as described in claim 1, characterized in that: In step (5), Sim(3) is standardized to make the navigator consistent with the navigator. Switch to Then, regarding the current formation example With expected template Solve for similarity transformations: （4） in, Let i be the position coordinates of the drone i in the desired formation. Let it be its position coordinates within the current formation. Let be a rotation matrix. Scaling factor As the translation vector, the expected reference after unification is obtained. The desired relative pose is mapped to a new reference frame to eliminate scale, attitude, and translation differences; smooth stitching is performed on the reference trajectory. Smooth stitching: （5） in, For drones At any moment Smooth reference position, The reference trajectory before the switch at time The corresponding position This represents the expected position after Sim(3) unification and mapping to the new reference frame. For normalized time variables, Switching time for the navigator To ensure a smooth transition time, It is a short-time sigmoid weight function. It is used to ensure position and velocity continuity and reduce switching shock.

7. An adaptive navigator selection and graph theory metric-based UAV formation control system, comprising an environmental perception module, an environmental index calculation module, a navigator selection module, a graph theory shape evaluation module, a desired position generation module, a formation reconstruction and transition module, and a control execution module, characterized in that: Based on the adaptive navigator selection and graph theory metric UAV formation control method according to any one of claims 1-6, the control transformation of the UAV formation is completed.

8. An electronic device, characterized in that, It includes a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, it performs control transformation of the UAV formation based on the UAV formation control method of adaptive navigator selection and graph theory metric as described in any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the following features are defined: Based on the adaptive navigator selection and graph theory metric UAV formation control method according to any one of claims 1-6, the control transformation of the UAV formation is completed.

Images