An unmanned aerial vehicle control system and method for formation flight in a wind environment

Abstract

The application relates to a kind of unmanned aerial vehicle control system and method for formation flight in wind environment, the method comprises: generating wind disturbance environment with specified map scale and containing random obstacles;Obtain the external image data and inertial measurement data of unmanned aerial vehicle, generate grid map by extracting point cloud and conversion operation;Using undirected graph, the spatial topology of unmanned aerial vehicle cluster is modeled, and unconstrained discrete optimization is used to solve the initial trajectory without collision;Using task reassignment strategy, the initial trajectory is optimized, and the global target and local target are re-planned to obtain the optimized trajectory after re-planning;Based on the optimized trajectory, the optimal control amount for the bottom layer control of unmanned aerial vehicle is obtained by using model predictive control method, to control the motion state of each unmanned aerial vehicle, realize formation flight.Compared with the prior art, the unmanned aerial vehicle cluster can efficiently and accurately realize formation flight in the obstacle environment with wind influence.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) formation control technology, and in particular to a UAV control system and method for formation flight in windy conditions. Background Technology

[0002] Drones have demonstrated immense potential in search and rescue, exploration, and inspection missions. They can overcome the challenges posed by complex and muddy roads, detecting signs of disease from high altitudes, inspecting crops, and sowing and fertilizing as needed. This technology enables more efficient land management, reduces labor costs, and improves agricultural efficiency. The widespread application of drones in power transmission line inspections also demonstrates their significant utility in this field, making them an important tool for power system maintenance with enormous development potential. With the accelerated construction of new infrastructure and the widespread deployment of 5G networks and artificial intelligence technologies, the future potential of power line inspection drones is highly recognized by the industry. my country's vast territory, diverse terrain, complex environmental conditions, and variable climate have led to limitations and difficulties in traditional surveying methods. These factors collectively hinder the effectiveness and efficiency of surveying activities. However, the introduction of drone technology has significantly improved the state of land surveying, bringing significant positive impacts to this field. In responding to natural disasters, timely assessment of disasters, determination of the number of affected people and the degree of disruption, and the development of relief plans accordingly are crucial. Micro drones can quickly traverse complex and narrow mountain and forest terrain, completing large-scale precise searches in a short time, making them particularly outstanding in search and rescue missions.

[0003] As operational standards across industries continue to rise, independent drones face limitations when performing challenging tasks, necessitating collaborative work among multiple drones. Formation flight is considered a fundamental capability for collaborative drone operations, effectively addressing more complex tasks. However, most existing formation flight algorithms operate in open, undisturbed environments and cannot be directly applied to complex real-world scenarios. Due to diverse constraints and environmental uncertainties, achieving formation flight presents significant challenges, particularly in obstacle-prone environments affected by wind.

[0004] Current technologies for drone formation flying and trajectory planning have significant shortcomings. First, most algorithms rely on centralized architectures or global information sharing, leading to high single-point failure risks, high computational complexity, and difficulty adapting to dynamic environmental changes. While centralized methods can provide optimal solutions, they suffer from long computation times, poor real-time performance, and difficulty handling sudden obstacles. Distributed methods can alleviate this problem, but still require pre-set virtual structures or rely on leader-follower models, resulting in information transmission bottlenecks and insufficient formation stability. Furthermore, existing algorithms perform well in free environments, but in constrained environments, fixed geometric formations struggle to flexibly change formations, and pre-set desired trajectories or rigid structures cannot adapt to dense dynamic obstacles, exhibiting weak responsiveness to sudden obstacles. Obstacle avoidance methods based on sphere radii or single integrator models are prone to failure in complex continuous obstacles, raising questions about their dynamic feasibility.

[0005] Secondly, the inherent flaws of obstacle avoidance strategies further limit the practicality of the technology. While artificial potential field methods are easy to implement, they are prone to getting trapped in local minima, leading to formation oscillations or stagnation. Repulsion functions or topology reconstruction methods struggle to generate effective obstacle avoidance paths in unbounded obstacles, making it difficult to balance safety and energy consumption. Furthermore, the linearization assumption of the model under external disturbances reduces control accuracy, limiting adaptive disturbance rejection capabilities. In some formations, kinematic constraints are not coupled with the obstacle avoidance strategy, leading to accumulated tracking errors during actual deployment. In large-scale swarm scenarios, communication load increases dramatically, existing consensus algorithms struggle to guarantee state consistency, and affine transformations or fixed formation allocation strategies are ill-suited to heterogeneous obstacle scenarios. An optimal formation selection mechanism is also lacking. These flaws collectively constrain the autonomy, safety, and mission reliability of UAV swarms in complex dynamic environments. Summary of the Invention

[0006] The purpose of this invention is to overcome the defects of the prior art and provide a UAV control system and method for formation flight in windy environments, which enables UAV swarms to achieve efficient and accurate formation flight in obstacle environments affected by wind.

[0007] The objective of this invention can be achieved through the following technical solution: a UAV control system for formation flight in windy conditions, comprising a task redistribution module, a trajectory tracking module, an automated testing and evaluation module, and a distributed trajectory generation module connected in sequence to form a closed loop, wherein the distributed trajectory generation module is connected to an environmental perception module, and the environmental perception module is used to collect external image data, inertial measurement data, and wind data of the UAV.

[0008] The distributed trajectory generation module uses the MINCO trajectory representation method with independent parameters to generate local targets, update cluster positions, and calculate formation errors.

[0009] The task redistribution module is used to solve for the optimal allocation and formation in order to replan the global and local objectives;

[0010] The trajectory tracking module is used to track the replanned trajectory in order to determine the optimal control quantity for controlling the movement of the UAV.

[0011] The automated testing and evaluation module is used to automatically generate obstacles and wind conditions, form a simulation map, and control the UAV to move in the simulation map based on the optimal control quantity in order to obtain formation flight test results and conduct evaluation.

[0012] A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions includes the following steps:

[0013] S1. Generate a wind-disrupted environment with a specified map size and random obstacles for drone formation flight testing;

[0014] S2. Acquire external image data and inertial measurement data of the UAV in a windy environment, and generate a raster map by extracting point cloud data and performing transformation operations.

[0015] S3. The spatial topology of the UAV swarm is modeled using an undirected graph approach, and the initial collision-free trajectory is generated by unconstrained discrete optimization.

[0016] S4. Adopt a task redistribution strategy to optimize the initial trajectory. By replanning the global and local objectives, the optimized trajectory is obtained.

[0017] S5. Based on the optimized trajectory, the optimal control quantity for the underlying control of the UAV is obtained by using model predictive control.

[0018] S6. Based on the optimal control quantity, control the motion state of each UAV accordingly to achieve formation flight.

[0019] Furthermore, step S3 includes the following process:

[0020] S31. Model the spatial topology of the UAV swarm using an undirected graph approach;

[0021] S32. Use the MINCO trajectory representation method to describe the polynomial trajectory, and use the midpoint of each adjacent trajectory segment and the duration of each trajectory segment as two decoupled variables, so that the trajectory can meet the spatiotemporal constraints of various task requirements while ensuring local smoothness.

[0022] S33. Design a distributed local trajectory optimization framework and use the quasi-Newton method to solve the unconstrained optimization problem to obtain the collision-free initial trajectory.

[0023] Furthermore, the specific process of step S31 is as follows:

[0024] Using undirected graphs Describe a swarm formation consisting of N drones, where Represents the set of vertices. A graph represents the set of edges. It is a time-varying undirected graph, where each vertex i represents the real-time position of the UAV. The set of edges ε represents the communication topology of a drone swarm. If edge e ij ∈ε connects vertex i and vertex j, indicating that the i-th drone and the j-th drone can communicate with each other and share information;

[0025] Assuming each drone can communicate with all other drones in the cluster, Figure It is fully connected. (Figure) The symmetric normalized Laplace matrix is ​​defined as:

[0026]

[0027] Where I∈R N×N It is an identity matrix, A∈R N×N It is a picture The adjacency matrix, D∈R N×N It is a degree matrix, using a differentiable formation similarity error metric to assess deviation from the desired formation:

[0028]

[0029] Where tr{·} represents the trace of the matrix. It is the symmetric normalized Laplace operator for current group formation. It is the corresponding term of the predefined ideal formation, f s It is a dimensionless value used to reflect the error in formation similarity.

[0030] Furthermore, the specific process of step S32 is as follows:

[0031] First, consider an m-dimensional polynomial trajectory of degree N = 2s⁻¹, dividing it into M segments, where the i-th segment is defined as:

[0032]

[0033] Where β(x)=(1,x,…,x) N ) T It is a base. These are polynomial coefficients, using position, velocity, and acceleration as state variables, and the derivative of acceleration as the control variable. In this case, s = 3, and the overall trajectory is determined by the coefficient matrix. and time vector Description, defined as:

[0034]

[0035] The trajectory p that combines c and T as a whole i (t):

[0036]

[0037] Next, define the MINCO trajectory, using... It is represented and defined as:

[0038]

[0039] All trajectories are compactly parameterized only by q and T, where This represents the midpoint of each adjacent segment of the trajectory. This indicates the duration of each segment of the trajectory.

[0040] Furthermore, the specific process of step S33 is as follows:

[0041] Design a distributed local trajectory optimization framework with the following optimization objective:

[0042]

[0043] The first term defines smoothness and minimum control cost, the second term defines the system's aggressiveness, ρ is the time regularization parameter, and T represents the total time from the local starting point to the local ending point. The constraints of this continuous optimization objective are:

[0044]

[0045] Using the MINCO trajectory representation method, the trajectory p(t) is defined and parameterized by the variable set q, T;

[0046] The local starting point of the trajectory p(t) is The local endpoint of the trajectory p(t) is Together, they form the boundary constraints of the trajectory, p [s-1] (t)=(p(t) T ,p(t) T ,…,p (s-1) (t) T ) T ∈R ms This represents the set of higher-order derivatives of an s-order dynamical system.

[0047] It then generalizes the description of various additional constraints or tasks, including formation similarity, obstacle avoidance, cluster mutual avoidance, and dynamic feasibility.

[0048] Based on the MINCO trajectory representation method, boundary constraints of the trajectory are eliminated through boundary conditions, and additional constraints are eliminated through constraint transcription, resulting in:

[0049]

[0050] in This represents the smoothness of the optimization objective and the minimum control cost, i.e., the control cost. This represents the minimum time cost of the optimization objective, i.e., the total flight time cost. This represents the constraints of various conditions or tasks, with the subscript * = {f, o, r, d}, where f represents formation similarity, o represents obstacle avoidance, r represents mutual avoidance among clusters, and d represents dynamic feasibility. Including formation similarity cost P f Obstacle avoidance cost P o Cluster mutual avoidance cost P r Dynamic feasibility cost P d w c ,w t ,w * The weights of each cost are represented by δ, which represents a fixed sampling time interval. All costs J are parameterized by the variable set {q,T,δ}, J = J(q,T,δ).

[0051] The quasi-Newton method is used to solve the unconstrained optimization problem. Based on the properties of the MINCO trajectory representation method, the position is represented by the trajectory. The mathematical expression of its derivative is transformed into the form {c,T}, and through mathematical transformation, {c,T} is transformed into the form {q,T}, so as to... and get and The entire optimization process only optimizes the two parameter sets {q,T}, and finally obtains the initial trajectory without collision.

[0052] Furthermore, the control cost J c To reduce system energy consumption and enhance stability, it is necessary to minimize the s-order control input cost of the trajectory. The control input cost is defined as:

[0053]

[0054] The total flight time cost J t To ensure the aggressiveness of the trajectory and the stability of its execution, the total flight time between the local starting point and the local ending point needs to be minimized.

[0055]

[0056] The formation similarity cost P f Used to penalize drones that deviate from formation:

[0057]

[0058] The obstacle avoidance cost P o Euclidean Symbolic Distance Field (ESDF) is used to penalize obstacles that are too close to them, while no action is taken if the distance is greater than the safe distance.

[0059]

[0060] Where, d o,max The safety threshold is set based on the actual situation. Indicates sampling point The distance between it and the nearest surrounding obstacle;

[0061] The cluster mutual avoidance cost P r Used to detect drones that are too close to other drones As a penalty, Φ represents the set of other robots in the cluster, and the cluster mutual avoidance cost is defined as:

[0062]

[0063] Where d r,max It is the safe distance threshold between each drone;

[0064] The dynamic feasibility cost P d Used to limit speed and acceleration to ensure stable flight of the drone on a specified trajectory, the dynamic feasibility cost P is... d Defined as:

[0065]

[0066] Among them, v m and a m This indicates the maximum linear velocity and maximum linear acceleration of the drone.

[0067] Furthermore, the specific process of step S4 is as follows:

[0068] Define p i =[p ix ,p iy ,p iz ] T i = 1, ..., N represents the current position of the drone;

[0069] Define q j=[q jx ,q jy ,q jz ] T j = 1, ..., N represents the ideal formation position template, which is defined in advance before the task begins or automatically defined in the program, and the formation position q′ after the formation is aligned. j Defined as:

[0070] q′ j =s·q j +d,

[0071] Where s∈R represents the formation size coefficient. The translation coefficient is represented by s, and the alignment of the formation is determined by both parameters s and d.

[0072] During task redistribution, two problems need to be addressed: target allocation and formation alignment. The target allocation problem refers to the situation where the current objective of a drone swarm is costly for some individuals, while these individuals can achieve other objectives at a lower cost. Solving the target allocation problem aims to find the optimal formation alignment parameters s and d.

[0073]

[0074] where σ∈S N This is a diagram showing the allocation of formation tasks, S N Let represent the symmetric group of all permutations from set 1, ..., N to itself, thereby minimizing the distribution of the overall transition distance between the current drone and the aligned local target, and minimizing the time for the formation to return to order;

[0075] The formation alignment problem refers to the high cost required for a drone swarm to restore its current formation size, while a new formation in three-dimensional space exists with a lower cost than restoring the current ideal formation. Solving this problem aims to address the matching between the drone swarm and a local set of targets, described as follows:

[0076]

[0077] Where σ * The optimal allocation of the representative formation, w i It is the task weight of each drone, and the standard formation that best suits the current drone position is generated based on the distance cost weighted by the formation constraints.

[0078] The target allocation problem and the formation alignment problem are coupled and involve three decision variables: formation size coefficient s, translation coefficient d, and task allocation σ. The ultimate goal of the task reassignment problem is to find the optimal combination of variables that simultaneously satisfies the above equation. σ is calculated using a decoupled approach. * And calculate s and d using σ:

[0079]

[0080] in The solution in the above equation is independent of s and d, therefore we first solve the task allocation problem, and then apply the optimal task allocation σ. * Find the solutions for s and d in the formation alignment problem:

[0081]

[0082] The parameters are defined as follows:

[0083]

[0084]

[0085] By centrally remapping the key parameters of the formation and distributively replanning the trajectory, combined with local trajectory replanning for individual UAVs and global remapping of the entire cluster system, the local and global targets after global remapping are directly returned. Global trajectory replanning is then performed individually on the onboard computer of each UAV, ultimately achieving trajectory replanning after cluster reorganization.

[0086] Furthermore, the specific process of step S5 is as follows:

[0087] The UAV system is modeled as a third-order integral model, with the state variables being three-dimensional position, velocity, and acceleration, and the input variable being the derivative of acceleration. The influence of wind speed is modeled as a disturbance variable w, given by the following equation:

[0088] x=[x,y,v,v x ,v y ,v z ,a x ,a y ,a z ] T

[0089] u = [j x ,j y ,j z ] T

[0090] w = [w x ,w y ,w z ] T

[0091] The state prediction of the UAV can be written in discrete state-space equation form with a step size of Δt. The transition equation for the predicted state in the k-th time slot is as follows:

[0092] x(k+1)=Ad x(k)+B d u(k)+C d w(k),

[0093] Where A d B d and C d Defined as

[0094]

[0095]

[0096] The simplified transition equation for the predicted state in the k-th time slot is as follows:

[0097] x i (k+1)=f a (x i (k),u i (k),w i (k)),

[0098] Where f a (·) represents a linear state function;

[0099] A model predictive control framework with N UAVs is used to handle the dynamic feasibility constraints and obstacle avoidance constraints of the UAVs and output the optimal control input within a certain range in the future. The model predictive control programming solver is distributed on each UAV and the different UAVs are independent of each other.

[0100] Furthermore, the working process of the model predictive control programming solver is as follows:

[0101] During each time slot, each UAV uses its current state variable as the initial value for an iterative optimization problem. It determines the optimal control input by solving a local optimization problem based on model predictive control formulas. The state variable and control variable of UAV i in time slot k are x and x, respectively. i (k) and u i (k), the state variables and control inputs from time slot k to the next M steps are defined as follows:

[0102] x i [k:k+M-1]=[x i (k|k),x i (k+1|k),…,x i (k+M-1|k)]

[0103] u i [k:k+M-1]=[u i (k|k),u i (k+1|k),…,ui (k+M-1|k)]

[0104] For UAV i, the objective function for solving its own model predictive control formula is defined as J. i (x i [k:k+M-1],u i [k:k+M-1]), the optimal control input is defined as Obtained from the following formula:

[0105]

[0106] At time slot k, the calculated optimal control input will be... Take the first item from As the control variable executed between [k:k+1], the overall cost function is divided into tracking cost functions. and obstacle avoidance cost function Different weights are assigned, and the formula is given by the following equation:

[0107]

[0108] Where λ1 represents the weight of tracking cost and λ2 represents the weight of obstacle avoidance cost. The larger the weight of a certain term, the greater the impact of that term's cost function on the overall cost function.

[0109] To ensure runtime safety, obstacle avoidance is also incorporated into the cost function, and the cost is tracked. Used to maintain an error-free tracking trajectory to meet the requirements of formation maintenance and obstacle avoidance:

[0110]

[0111] Where r i (k+q|k) represents the set of three-dimensional positions of UAV i predicted at time k+q in time slot k, r i,des (k+q|k) represents the set of three-dimensional positions of UAV i in its ideal trajectory at time k+q, where r = [x, y, z]. T ;

[0112] Obstacle Avoidance Costs The Euclidean Symbolic Distance Field (ESDF) was used for calculation, and the ESDF distance of UAV i at time k+q was denoted as d. o,i (k+q|k), obstacle avoidance cost Defined as:

[0113]

[0114] in d o,maxThe minimum safe distance between the object and the obstacle; if the distance is less than this safe distance, there is a risk of collision.

[0115] Based on the definitions of tracking cost and obstacle avoidance cost, the control optimization objective of a drone swarm is to successfully avoid obstacles while approaching a predefined trajectory. The trajectory tracking optimization problem for each drone i based on model predictive control is described by the following equation:

[0116]

[0117] stC1X i (k+1)=f a (X i (k),u i (k))

[0118]

[0119] Where C1 represents the state update equation of UAV i, C2 represents the feasible region of velocity, and C3 represents the feasible region of acceleration. C2 and C3 determine the dynamic feasibility of the optimized control variables.

[0120] Compared with the prior art, the present invention has the following advantages:

[0121] This invention designs a closed-loop connected task redistribution module, trajectory tracking module, automated testing and evaluation module, and distributed trajectory generation module. The distributed trajectory generation module is connected to an environmental perception module, which collects external image data, inertial measurement data, and wind data from the UAV. The distributed trajectory generation module uses the MINCO trajectory representation method with independent parameters to generate local targets, update cluster positions, and calculate formation errors. The task redistribution module solves for optimal allocation and formation to replan global and local targets. The trajectory tracking module tracks the replanned trajectory to determine the optimal control variables for controlling the UAV's movement. The automated testing and evaluation module automatically generates obstacles and wind conditions to form a simulation map, and controls the UAVs to move within the simulation map based on the optimal control variables to obtain and evaluate formation flight test results. This achieves a novel formation flight framework that enables UAV swarms to successfully achieve formation flight in obstacle-prone environments with wind influence while minimizing computation time.

[0122] This invention designs a two-layer framework: the upper layer uses a distributed trajectory generation framework to adaptively generate trajectories, while the lower layer focuses on control decisions under wind disturbance conditions. Specifically, the upper-layer distributed trajectory generation framework addresses the trajectory optimization problem for UAVs by employing the MINCO trajectory representation method with independent parameters, which can meet the needs of different flight missions. It models the spatial topology of the UAV swarm using an undirected graph and quantifies the differences between different formations using a differentiable metric. Regarding spatial and feasibility constraints, an unconstrained discrete optimization method is used to solve the trajectory optimization problem, achieving high-quality, collision-free trajectories with low time complexity. Furthermore, to address the formation disorder problem during formation flight, a task redistribution strategy is proposed, explicitly describing the conflict between maintaining UAV formation and obstacle avoidance. For inappropriate scale and allocation, the disorder problem is described as a task allocation and formation alignment problem. Through decoupled solution, the optimal allocation and formation are obtained with extremely low time complexity. Using the optimal allocation and formation, global and local objectives are replanned. For the replanned objectives, the UAV swarm performs distributed new path planning to ensure rapid convergence to the desired formation.

[0123] The core of the lower layer is model predictive control. Considering the continuity constraint of the motor, the state-space equation of the UAV is established, and the state is updated for the higher-order control input. Using the ESDF (Euclidean Signed Distance Field) established by the prior map, a model predictive control framework that considers feasibility constraints for integrated target tracking and obstacle avoidance is designed, and the optimal control quantity within a certain time range is generated. Based on the differential flatness characteristic, the algebraic relationship between motor speed and control quantity is given, thereby realizing the low-level control of the UAV. Attached Figure Description

[0124] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0125] Figure 2 This is a schematic diagram of the application framework for an example.

[0126] Figure 3 This is a schematic diagram illustrating the algorithm execution process of the task redistribution strategy in the embodiment;

[0127] Figure 4a The example image shows a local trajectory optimization simulation (starting point).

[0128] Figure 4b The simulation diagram for local trajectory optimization in the example is shown (flight process 1).

[0129] Figure 4c The simulation diagram for local trajectory optimization in the example is shown in flight process 2.

[0130] Figure 4d The simulation diagram for local trajectory optimization in the example is shown in flight process 3.

[0131] Figure 4e The simulation diagram for local trajectory optimization in the example is shown in flight process 4.

[0132] Figure 4f The image shown is a simulation diagram of the local trajectory optimization in the embodiment (end point);

[0133] Figure 5 The example shows the formation similarity curve generated by the distributed trajectory.

[0134] Figure 6a This is a schematic diagram of the formation flight effect before remapping in the embodiment;

[0135] Figure 6b This is a schematic diagram illustrating the formation flight effect after remapping in the embodiment;

[0136] Figure 7 This is a schematic diagram comparing the formation similarity before and after the task reassignment algorithm in the embodiment;

[0137] Figure 8a This is a schematic diagram of the formation flight effect after remapping and restoring the formation when the wind speed is 1 m / s in the embodiment.

[0138] Figure 8b This is a schematic diagram of the formation flight effect during flight process 1 in the embodiment when the wind speed is 1m / s;

[0139] Figure 8c This is a schematic diagram of the formation flight effect during flight process 2 in the embodiment when the wind speed is 1m / s;

[0140] Figure 8d This is a schematic diagram of the formation flight effect during flight process 3 in the embodiment when the wind speed is 1m / s;

[0141] Figure 8e This is a schematic diagram of the formation flight effect during flight process 4 in the embodiment when the wind speed is 1 m / s;

[0142] Figure 8f This is a schematic diagram of the formation flight effect at the endpoint when the wind speed is 1 m / s in the embodiment.

[0143] Figure 9 The figure shows the formation similarity error curve for formation flight when the wind speed is 1 m / s in the example. Detailed Implementation

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

[0145] Example

[0146] To efficiently generate continuous trajectories and perform stable tracking in obstacle-prone environments with wind interference, this solution develops a distributed trajectory generation framework to generate trajectories and designs model predictive control to make decisions on control variables, thereby controlling the movement of the UAV. This solution first proposes a UAV control system for formation flight in windy environments, comprising a task redistribution module, a trajectory tracking module, an automated testing and evaluation module, and a distributed trajectory generation module connected in a closed loop. The distributed trajectory generation module is connected to an environmental perception module, which collects external image data, inertial measurement data, and wind data from the UAV.

[0147] The distributed trajectory generation module uses the MINCO trajectory representation method with independent parameters to generate local targets, update cluster positions, and calculate formation errors.

[0148] The task redistribution module is used to solve for the optimal allocation and formation in order to replan the global and local objectives;

[0149] The trajectory tracking module is used to track the replanned trajectory in order to determine the optimal control quantity for controlling the movement of the UAV;

[0150] The automated testing and evaluation module is used to automatically generate obstacles and wind conditions, form a simulation map, and control the UAV to move in the simulation map based on the optimal control variables in order to obtain formation flight test results and conduct evaluation.

[0151] Based on the above system, a method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions is implemented, such as... Figure 1 As shown, it includes the following steps:

[0152] S1. Generate a wind-disrupted environment with a specified map size and random obstacles for drone formation flight testing;

[0153] S2. Acquire external image data and inertial measurement data of the UAV in a windy environment, and generate a raster map by extracting point cloud data and performing transformation operations.

[0154] S3. The spatial topology of the UAV swarm is modeled using an undirected graph approach, and the initial collision-free trajectory is generated by unconstrained discrete optimization.

[0155] S4. Adopt a task redistribution strategy to optimize the initial trajectory. By replanning the global and local objectives, the optimized trajectory is obtained.

[0156] S5. Based on the optimized trajectory, the optimal control quantity for the underlying control of the UAV is obtained by using model predictive control.

[0157] S6. Based on the optimal control quantity, control the motion state of each UAV accordingly to achieve formation flight.

[0158] This embodiment applies the above-described solution, such as Figure 2 As shown, the overall system architecture consists of four main parts: a task redistribution model, a distributed trajectory generation model, a trajectory tracking model, and an automated testing and evaluation model. This embodiment utilizes code to generate a wind-disrupted experimental environment with a specified map size and random obstacles to ensure stable and unbiased formation flight testing. Then, a numerical model is used to simulate the quadcopter UAV, improving the realism of the formation flight experiment and making it closer to a real quadcopter UAV model.

[0159] In the environmental perception section, VINS-Fusion is used to process RGB-D camera and IMU data for UAV self-localization and mapping. Specifically, point cloud data is extracted from the VINS-Fusion output, filtered and processed to generate a sparse point cloud map. An algorithm is then used to transform the processed point cloud data, calculate the occupancy probability of each grid point, and convert it into a raster map.

[0160] The specific application process of this embodiment includes:

[0161] I. Distributed Trajectory Generation

[0162] Planning a feasible and safe trajectory is crucial for unmanned aerial vehicles (UAVs). This solution aims to design a distributed trajectory optimization framework that comprehensively considers formation maintenance, obstacle avoidance, mutual avoidance, and dynamic feasibility. During the UAV path planning process, several collision-free global paths are obtained. To further ensure that these paths conform to the dynamics and kinematic constraints of the UAVs, a distributed swarm trajectory optimization strategy is adopted to ensure that the UAV formation can continuously navigate during flight.

[0163] This scheme uses the MINCO trajectory representation method to describe polynomial trajectories, employing intermediate points and time as two variables. First, consider an m-dimensional polynomial trajectory of degree N = 2s⁻¹, dividing it into M segments, where the i-th segment is defined as:

[0164]

[0165] Where β(x) = (1, x, ..., x) N ) T It is a base. These are polynomial coefficients. In this scheme, the state variables are position, velocity, and acceleration, and the control variable is the derivative of acceleration (jerk), where s = 3. The overall trajectory is determined by the coefficient matrix. and time vector Description, defined as:

[0166]

[0167] The trajectory p that combines c and T as a whole i (t):

[0168]

[0169] Next, define the MINCO trajectory, using... It is represented and defined as:

[0170]

[0171] All trajectories are compactly parameterized only by q and T. This represents the midpoint of each adjacent segment of the trajectory. This represents the duration of each trajectory segment. The MINCO trajectory representation method uses a decoupled combination of variables q and T to represent the entire trajectory, which can ensure the local smoothness of the trajectory while meeting the spatiotemporal constraints of various task requirements.

[0172] This solution aims to enable the drone swarm to navigate smoothly and safely in a predetermined formation. Therefore, it requires a highly efficient path planner that can be distributed across all drones. This planner should be able to perform effective path planning in unknown environments using limited sensor field of view. Furthermore, to improve the scalability and robustness of the swarm, this solution employs a distributed local trajectory optimization framework with the following optimization objective:

[0173]

[0174] The first term defines smoothness and minimum control cost, while the second term defines the system's aggressiveness, where ρ is the time regularization parameter and T represents the total time from the local starting point to the local ending point. The constraints of this continuous optimization objective are:

[0175]

[0176] Equation (6-a) defines the representation form of the MINCO trajectory representation method, defining the trajectory p(t) and parameterizing it using the variable set q and T. Equation (6-b) defines the local starting point of the trajectory p(t). Equation (6-c) defines the local endpoint of the trajectory p(t). Together, they form the boundary constraints of the trajectory, p [s-1] (t)=(p(t) T ,p(t) T ,…,p (s-1)(t) T ) T ∈R ms This represents the set of higher-order derivatives of an s-order dynamic system. Equation (6-d) generalizes various additional constraints or tasks, and in this scheme, similarity of formations, obstacle avoidance, cluster mutual avoidance, and dynamic feasibility are considered.

[0177] Based on the MINCO trajectory representation method, the constraints (6-b)-(6-c) are eliminated through boundary conditions. For constraint (6-d), the hard constraint imposes strict restrictions on the system's solution, making the optimization problem very difficult and time-consuming. Constraint transcription eliminates the additional constraints, making the solution more efficient.

[0178]

[0179] in This indicates the smoothness and minimum control cost of (5). This represents the minimum time cost of (5). This represents the constraints of various conditions or tasks, with the subscript * = {f, o, r, d}, where f represents formation similarity, o represents obstacle avoidance, r represents cluster mutual avoidance, d represents dynamic feasibility, and w... c ,w t ,w * The weights of each item are represented by δ, which represents a fixed sampling time interval. All costs J are parameterized by the variable set {q,T,δ}, J = J(q,T,δ).

[0180] The optimization function used in this scheme is discontinuous, but the cost function is designed to be continuous. A quasi-Newton method is employed to solve the unconstrained optimization problem. Based on the properties of the MINCO trajectory representation method, regarding position... The mathematical expression of its derivative can be transformed into the form of {c,T} through trajectory representation, and {c,T} can be transformed into the form of {q,T} through mathematical transformation. Therefore, it is possible to efficiently transform from... and get and This allows the entire optimization process to focus on only two parameter sets, {q, T}, making construction and solution much easier. Next, we need to define the various cost functions J.

[0181] (1) Control cost J c To reduce system energy consumption and enhance stability, it is necessary to minimize the s-order control input cost of the trajectory. The control input cost is defined as:

[0182]

[0183] (2) Total flight time cost Jt During the planning process, the trajectory curve that satisfies the system's formation and collision avoidance requirements is not unique. Some trajectories are more complex and take longer to complete the task. In order to ensure the aggressiveness of the trajectory and the stability of its execution, it is necessary to minimize the total flight time between the local starting point and the local ending point.

[0184]

[0185] (3) Formation similarity cost P f In the invention, undirected graphs are used. Describe a swarm formation consisting of N drones, where Represents the set of vertices. Represents the set of edges. (Graph) It is a time-varying undirected graph, where each vertex i represents the real-time position of the UAV. The set of edges ε represents the communication topology of a drone swarm. If edge e ij The ∈ε connects vertex i and vertex j, indicating that the i-th drone and the j-th drone can communicate with each other and share information. In this paper, it is assumed that each drone can communicate with all other drones in the cluster. It is fully connected. (Figure) The symmetric normalized Laplace matrix is ​​defined as:

[0186]

[0187] Where I∈R N×N It is an identity matrix, A∈R N×N It is a picture The adjacency matrix, d∈R N×N It is a degree matrix. To evaluate the deviation from the desired formation, this paper uses a differentiable formation similarity error metric:

[0188]

[0189] Where tr[·} represents the trace of the matrix. It is the symmetric normalized Laplace operator for current group formation. It is the corresponding term for a predefined ideal formation. s It is a dimensionless value reflecting the error in formation similarity. This scheme penalizes drones that deviate from the formation shape:

[0190]

[0191] (4) Obstacle avoidance cost P oThe drone's perception module can perceive the surrounding environment in real time and build a point cloud map of the surroundings. This solution uses Euclidean Symbolic Distance Field (ESDF) to penalize obstacles that are too close to them, while no action is taken if the distance is greater than the safe distance.

[0192]

[0193] Where d o,max The safety threshold is set based on the actual situation. Indicates sampling point The distance between it and the nearest obstacle in its vicinity.

[0194] (5) Cluster mutual avoidance cost P r In the trajectory optimization process, in addition to the need to avoid obstacles, there is a potential collision hazard if the drone's trajectory gets too close to other drones. Therefore, it is necessary to optimize the trajectory for drones that get too close to other drones. As a penalty, Φ represents the set of other robots in the cluster, and the cluster mutual avoidance cost is defined as:

[0195]

[0196] Where d r,max It is the safe distance threshold between each drone.

[0197] (6) Dynamic feasibility cost P d During flight, the motor's speed and torque have upper limits, therefore speed and acceleration need to be limited to ensure the drone can fly stably along a prescribed trajectory. This solution will consider the dynamic feasibility cost P. d Defined as:

[0198]

[0199] Where v m and a m This indicates the maximum linear velocity and maximum linear acceleration of the drone.

[0200] II. Task Reassignment Model

[0201] Local trajectory optimization has certain limitations. The entire UAV swarm cannot quickly recover from sudden obstacles or drastic changes in formation caused by dense environments to execute the corresponding tasks. The direct cause of this problem is that fixed formation size and local path planning cannot cope with disordered formation states. To address the problem of disordered UAV formations, this solution divides it into two parts: task allocation and formation alignment, and decouples and solves them accordingly. This scheme designs a task redistribution strategy to redistribute tasks across the entire swarm when the formation is disordered, effectively mitigating the impact of dense environments on formation.

[0202] Define p i =[p ix ,p iy ,p iz ] T Let i = 1, ..., N represent the current position of the drone. Define q. j =[q jx ,q jy ,q jz ] t ,j=1,,N represents the ideal formation position template, which is defined in advance before the task begins or automatically defined in the program. The formation position q after alignment. j ′ Defined as:

[0203] q′ j =s·q j +d, (16)

[0204] Where s∈R represents the formation size coefficient. This represents the translation coefficient. Formation alignment is determined by two parameters, s and d. During task redistribution, two problems need to be addressed: target allocation and formation alignment. The target allocation problem refers to the situation where the current objective of the drone swarm is costly for some individuals, while these individuals can achieve other objectives at a lower cost, as shown in the example in Figure (4-1a). Solving this problem aims to find the optimal formation alignment parameters s and d.

[0205]

[0206] where σ∈S N This is a diagram showing the allocation of formation tasks, S N Let represent the symmetric group of all permutations from set 1, ..., N to itself. The formula solves for minimizing the distribution of the overall transition distance between the current drone and the aligned local targets, thus minimizing the time required to restore the formation to its ordered state. The formation alignment problem refers to the situation where the cost of restoring the current formation size of a drone swarm is high, while a new formation exists in three-dimensional space with a lower cost than restoring the current ideal formation. Solving this problem aims to address the matching between the drone swarm and the set of local targets, described as:

[0207]

[0208] Where σ * The optimal allocation of the representative formation, w iThis represents the task weight for each drone. A standard formation best suited to the current drone's position is generated based on the distance cost weighted by the formation constraints. The above equations are coupled and have three decision variables: formation size coefficient s, translation coefficient d, and task assignment σ. The ultimate goal of the task reassignment problem is to find the optimal combination of variables that simultaneously satisfies the above equations. σ can be calculated in a decoupled manner. * And calculate s and d using σ:

[0209]

[0210] in The solution in the above equation is independent of s and d, therefore we first solve the task allocation problem, and then apply the optimal task allocation σ. * Solve for s and d. Solution for the formation alignment problem:

[0211]

[0212] The parameters are defined as follows:

[0213]

[0214] To address the issue that swarm systems relying solely on trajectory optimization struggle to quickly restore order when faced with sudden obstacles or changes in formation, and considering computational speed, this solution proposes a task redistribution strategy. This strategy effectively handles unexpected obstacles and enables the swarm to reach equilibrium more quickly within the desired formation, further improving system coordination and adaptability. For distributed computing frameworks, communication between individual UAVs is crucial. They can publish their own information to and receive information from other UAVs in real time to optimize their own planning, leading to rapid convergence of the entire system to the desired state. In practical applications, communication latency can lead to inaccurate information or circular dependencies, making it difficult to guarantee consistent decision-making. Therefore, a centralized replanner is necessary. This solution's task redistribution strategy centrally remaps key formation parameters and distributes trajectory replanning, ultimately achieving trajectory replanning after swarm reorganization.

[0215] This solution employs a task reassignment strategy that directly returns the remapped local and global targets, and performs global trajectory replanning independently on each UAV's onboard computer. The task reassignment strategy combines local trajectory replanning for individual UAVs with global remapping for the entire swarm system. Local trajectory replanning allows each UAV to formulate a trajectory within its local perception range; while global remapping is an efficient centralized strategy that replans local targets by solving the task reassignment method and then reallocates global targets to the reassigned UAVs. The task replanning strategy performs well in distributed asynchronous systems and effectively prevents deadlocks in the swarm system even with communication latency.

[0216] like Figure 3 As shown, the core operation flow of the task replanning strategy in this embodiment includes: First, each UAV distributes and assesses whether an emergency has occurred. Lines 2-8 of the algorithm describe whether the formation constraint perception exceeds the critical value. If a serious conflict occurs between the obstacle avoidance and formation maintenance of a single UAV, the global remapping strategy flag CallGlobalRemap is triggered. Next, it detects whether the overall formation shape differs too much from the expected formation. Lines 9-11 quantify and calculate the formation similarity error e. sim and threshold e sim,d The comparison is performed, and if the similarity error is too large, the global remapping flag CallGlobalRemap is triggered. Unlike the periodically activated local replanning strategy, the global remapping strategy is only triggered when an emergency is detected, i.e., CallGlobalRemap = 1. Lines 13-19 centrally remap the cluster, lines 14 and 15 solve the task redistribution problem to obtain the optimal σ, s, and d, and line 16 remaps the local target. Line 17 remaps the global target \mathbf{G}_i^* and the local target and global goals This is sent as a return value to each drone. Lines 20-22 indicate that the global trajectory is replanned in each drone to formulate a new global trajectory. A new formation structure was formed. This process is fast and efficient, ensuring the continuity, stability, and coordination of the group's movements.

[0217] III. Trajectory Tracking Model

[0218] In this scheme, the optimized trajectory is a time-varying function σ. TThe state variable (t) explicitly contains the position, velocity, and acceleration information of the UAV at each moment, while the attitude variables and angular velocity of the UAV are implicitly included in the trajectory formula and can be represented by position, velocity, and acceleration, and are not the direct optimization target. Therefore, this scheme models the UAV system as a third-order integral model, with the state variables being three-dimensional position, velocity, and acceleration, and the input variable being the derivative of acceleration (jerk). The influence of wind speed is modeled as a disturbance variable w, given by the following equation:

[0219] x = [x, y, z, v] x ,v y ,v z ,a x ,a y ,a z ] T

[0220] u = [j x ,j y ,j z ] T

[0221] w = [w x ,w y ,w z ] T

[0222] The state prediction of the UAV can be written in discrete state-space equation form with a step size of Δt. The transition equation for the predicted state in the k-th time slot is as follows:

[0223] x(k+1)=A d x(k)+B d u(k)+C d w(k),(22)

[0224] Where A d B d and C d Defined as

[0225]

[0226]

[0227] For simplification, equation (22) is expressed as:

[0228] x i (k+1)=f a (x i (k),u i (k),w i (k)),(23)

[0229] Where fa (·) represents a linear state function.

[0230] This proposal considers a model predictive control (MDC) framework with N UAVs. MDC can effectively handle the dynamic feasibility and obstacle avoidance constraints of the UAVs, and can provide the optimal control input within a certain future range, not just the current optimal. In this proposal, the MDC can effectively track continuously variable target trajectories. The MDC planning solver is distributed across each UAV, and the different UAVs operate independently. The proposed cooperative control of the UAV swarm can be simplified to a problem of controlling multiple individual UAVs.

[0231] During each time slot, each UAV uses its current state variable as the initial value for an iterative optimization problem, and determines the optimal control input by solving the local optimization problem based on the model predictive control formula. In this scheme, the state variable and control variable of UAV i in time slot k are x and x, respectively. i (k) and u i (k), the state variables and control inputs from time slot k to the next M steps are defined as follows:

[0232] x i [k:k+M-1]=[x i (k|k),x i (k+1|k),…,x i [(k+M-1|k)] and u i [k:k+M-1]=[u i (k|k),u i (k+1|k),…,u i (k+M-1|k)].

[0233] For UAV i, the objective function for solving its own model predictive control formula is defined as J. i (x i [k:k+M-1],u i [k:k+M-1]), the optimal control input is defined as It can be obtained from the following formula:

[0234]

[0235] At time slot k, the calculated optimal control input will be... Take the first item from This serves as the control variable executed between times [k:k+1]. This scheme divides the overall cost function into a tracking cost function. and obstacle avoidance cost function Different weights are assigned, and the formula is given by the following equation:

[0236]

[0237] λ1 represents the weight of tracking cost, and λ2 represents the weight of obstacle avoidance cost. The larger the weight of a certain term, the greater the impact of that term's cost function on the overall cost function.

[0238] First, we analyze the significance of these two cost functions. For a single drone, simply tracking the planned trajectory without error is sufficient to meet all the above requirements. However, in real-world environments, various unexpected events can occur, such as wind effects or sudden obstacle movement, which may cause the drone's position to deviate, making it unable to quickly approach the trajectory and effectively avoid obstacles. Therefore, considering operational safety, this solution also incorporates obstacle avoidance into the cost function. The tracking cost and obstacle avoidance cost will be described in detail below.

[0239] Tracking Costs The most important optimization goal for trajectory tracking is to maintain error-free tracking of the trajectory as much as possible during the drone's movement, thereby meeting the requirements of formation maintenance and obstacle avoidance. Defined as:

[0240]

[0241] Where r i (k+q|k) represents the set of three-dimensional positions of UAV i predicted at time k+q in time slot k, r i,des (k+q|k) represents the set of three-dimensional positions of UAV i in its ideal trajectory at time k+q, where r = [x, y, z]. T .

[0242] Next is the cost of obstacle avoidance. Obstacle avoidance cost requires considering and judging the distance to the obstacle at every moment, determining whether it is within the potential collision range, and reacting accordingly. The obstacle avoidance cost is calculated using the Euclidean Symbolic Distance Field (ESDF), where the ESDF distance of UAV i at time k+q is denoted as d. o,i (k+q|k), obstacle avoidance cost Defined as:

[0243]

[0244] in d o,max The minimum safe distance between the object and the obstacle; if the distance is less than this safe distance, there is a risk of collision.

[0245] Based on the definitions of tracking cost and obstacle avoidance cost, the control optimization objective of a drone swarm is to successfully avoid obstacles while approaching a predefined trajectory. Combining all the above definitions, the trajectory tracking optimization problem for each drone i based on model predictive control is described as follows:

[0246]

[0247] stC1X i (k+1)=f a (X i (k),u i (k))

[0248]

[0249] Where C1 represents the state update equation of UAV i, C2 represents the feasible region of velocity, and C3 represents the feasible region of acceleration. C2 and C3 determine the dynamic feasibility of the optimized control variables.

[0250] To further demonstrate the effectiveness and unbiasedness of the formation flight control method in complex scenarios, this embodiment conducted comprehensive simulation experiments using data established in a simulated world. The experiment verified whether the distributed trajectory optimization method could optimize a trajectory that integrates formation maintenance, obstacle avoidance, attack capability, and dynamic feasibility. First, a dense simulation environment was established, consisting of numerous cylindrical, circular, and continuous polygonal obstacles, to verify the effectiveness of the distributed trajectory optimization. This embodiment selected a hexagonal formation as a case study primarily due to its typical geometric characteristics, which help highlight its spatial efficiency, structural stability, and consistency in formation flight. However, the proposed method is not limited to hexagonal formations but is applicable to any formation structure.

[0251] A destination is specified on the global map, and a preliminary collision-free path search is performed using a formation-level global path search method. Then, sampling points are selected from the global path as intermediate points, and a distributed method is used to generate the local formation trajectory. In the simulation, the colored areas of obstacles represent the range within which the UAV can dynamically perceive the environment. When the UAV leaves a certain range of the detected obstacles, the point cloud information is deleted. Figures 4a-4f This demonstrates the process of local trajectory optimization by a drone swarm during its movement. Figures 4a-4f In the diagram, non-red solid lines represent the already traveled trajectory, while red solid lines indicate an optimized, smoothed trajectory. Points on the solid lines mark control midpoints, and the quadcopter model displays the drone's current position. For example... Figure 4a As shown, the drone swarm moves forward steadily in a hexagonal formation without encountering any obstacles, and the formation remains consistent with the desired formation. Figures 4b-4eThis describes a scenario where a swarm of drones traverses complex, dense terrain, with the drones periodically calculating optimized paths within a certain range. Figure 4b and Figure 4e In the scenario shown, the formation was adjusted during flight due to obstacle avoidance requirements. Ultimately, as... Figure 4f As shown, the drone swarm reaches its destination in the desired formation and successfully completes the mission. Simulation results demonstrate that the drone swarm can optimize its flight path in real time, incorporating comprehensive formation, obstacle avoidance, mutual avoidance, and dynamic feasibility into a smooth trajectory. The formation flight framework used in this embodiment can effectively adapt to dense obstacle constraints, providing safe guidance for formation flight.

[0252] like Figure 5 As shown, during formation flight, the formation similarity error of the swarm consistently remained below the predetermined critical value (0.05), meeting the flight requirements. There are two reasons for the variation in formation similarity error. First, during formation flight, the formation of the UAV swarm adjusts according to obstacle avoidance and mutual avoidance requirements; second, the asynchronous communication mechanism used by the UAV swarm may cause communication delays, leading to positional shifts and thus affecting the calculation results. This verifies the effectiveness of the trajectory optimization method in maintaining formation.

[0253] The formation flight systems using and without the mission reassignment algorithm were tested on the same map with the same destination. The path without the mission reassignment algorithm is shown below. Figure 6a As shown, the trajectories of multiple drones intersect and conflict. The drones are close together during flight, easily leading to collisions and chaos. Furthermore, the distances required to reach the designated target point are longer, resulting in lower efficiency. The path using the task redistribution algorithm is as follows: Figure 6b As shown, after the drone swarm redistributes its tasks and remaps its targets, it quickly restores the desired formation. Then, the swarm generates feasible intermediate paths according to the distributed trajectory generation method. These intermediate paths do not intersect, are far apart, and have strong security.

[0254] Formation similarity error during formation flight, such as Figure 7 As shown, when the task redistribution algorithm is not used ( Figure 7 (Middle blue curve) The formation similarity error decreases slowly, taking approximately 28 seconds to converge to the desired formation. This slow convergence speed fails to meet practical application requirements. However, after introducing the task reassignment algorithm ( Figure 7 As shown by the red curve in the image, the formation similarity error decreased rapidly, converging to the predetermined formation in just about 2 seconds, and remained at a low level throughout the entire flight. This observation verifies the excellent performance of the task reassignment algorithm in handling unordered formations.

[0255] To verify whether the trajectory tracking method based on model predictive control can effectively track along an optimized smooth trajectory, a flight simulation experiment of UAV formation was conducted in this embodiment. A comprehensive formation flight simulation experiment was performed in a constructed simulation environment. The characteristics of the UAVs were simulated using a numerical model to track the optimized trajectory. The execution cycle of model predictive control was set to 20Hz, and the control execution cycle was 100Hz. Within each control cycle, the optimal control quantity generated by MPC at 50ms intervals was interpolated to determine the optimal control quantity for the current execution cycle.

[0256] The robots were initially randomly arranged, and wind disturbance was set to 1 m / s along the y-axis for simulation. Due to the improper initial formation, the cluster invoked the task reallocation strategy to perform reallocation and remapping. Figure 8a This demonstrates the entire process of the cluster restoring the desired formation. Figures 8b-8e The video shows multiple views of a drone swarm successfully navigating an area densely packed with obstacles, without colliding with any obstacles or other drones in the swarm during the journey. Figure 8c and Figure 8d Due to continuous obstacles, the swarm changes its formation to adapt to the environment, thereby achieving obstacle avoidance. Figure 8f The demonstration showed that the swarm restored the desired formation upon reaching the destination, with each drone reaching the remapped global objective and completing the mission. Throughout the formation flight, the swarm maintained its formation to the greatest extent possible at any point in the flight.

[0257] In addition, observation Figure 9 The formation similarity error in the simulated formation flight shown in the figure was initially 0.87, which was higher than the critical value. The task reassignment algorithm was invoked to reassign the formation and remap the target, rapidly reducing the error to a low level. During subsequent flight, the error remained within the critical range despite minor fluctuations due to wind speed and obstacle avoidance factors. This verifies that the model predictive control trajectory tracking method can effectively perform precise control of UAV formations under wind disturbances. This result indicates that the method is suitable for performing fully autonomous formation flight missions under wind disturbances, successfully avoiding obstacles and completing the mission, ensuring both formation accuracy and high execution efficiency.

Claims

1. A control system for unmanned aerial vehicles (UAVs) used in formation flight under wind conditions, characterized in that, It includes a task redistribution module, a trajectory tracking module, an automated testing and evaluation module, and a distributed trajectory generation module connected in sequence to form a closed loop. The distributed trajectory generation module is connected to an environmental perception module, which is used to collect external image data, inertial measurement data, and wind data of the UAV. The distributed trajectory generation module uses the MINCO trajectory representation method with independent parameters to generate local targets, update cluster positions, and calculate formation errors. The task redistribution module is used to solve for the optimal allocation and formation in order to replan the global and local objectives; The trajectory tracking module is used to track the replanned trajectory in order to determine the optimal control quantity for controlling the movement of the UAV. The automated testing and evaluation module is used to automatically generate obstacles and wind conditions, form a simulation map, and control the UAV to move in the simulation map based on the optimal control quantity in order to obtain formation flight test results and conduct evaluation.

2. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions, applied to the UAV control system for formation flight under wind conditions as described in claim 1, characterized in that, Includes the following steps: S1. Generate a wind-disrupted environment with a specified map size and random obstacles for drone formation flight testing; S2. Acquire external image data and inertial measurement data of the UAV in a windy environment, and generate a raster map by extracting point cloud data and performing transformation operations. S3. The spatial topology of the UAV swarm is modeled using an undirected graph approach, and the initial collision-free trajectory is generated by unconstrained discrete optimization. S4. Adopt a task redistribution strategy to optimize the initial trajectory. By replanning the global and local objectives, the optimized trajectory is obtained. S5. Based on the optimized trajectory, the optimal control quantity for the underlying control of the UAV is obtained by using model predictive control. S6. Based on the optimal control quantity, control the motion state of each UAV accordingly to achieve formation flight.

3. The method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 2, characterized in that, Step S3 The process includes the following: S31. Model the spatial topology of the UAV swarm using an undirected graph approach; S32. The MINCO trajectory representation method is used to describe the polynomial trajectory. The midpoint of each adjacent trajectory segment and the duration of each trajectory segment are used as two decoupled variables, so that the trajectory can meet the spatiotemporal constraints of various task requirements while ensuring local smoothness. S33. Design a distributed local trajectory optimization framework and use the quasi-Newton method to solve the unconstrained optimization problem to obtain the collision-free initial trajectory.

4. The unmanned aerial vehicle control method for formation flight in a wind environment according to claim 3, wherein, The specific process of step S31 is as follows: Using undirected graphs describe A swarm of drones, including Represents the set of vertices. A graph represents the set of edges. It is a time-varying undirected graph, with each vertex... Indicates the real-time location of the drone The set of edges This represents the communication topology of a drone swarm. If the edges... Connecting vertices and vertex , indicating the first The drone and the first The drones communicate with each other and share information. Assuming each drone can communicate with all other drones in the cluster, Figure It is fully connected, graph The symmetric normalized Laplace matrix is ​​defined as: in It is the identity matrix. It is a picture The adjacency matrix, It is a degree matrix, using a differentiable formation similarity error metric to assess deviation from the desired formation: in Represents the trace of a matrix. It is the symmetric normalized Laplace operator for current group formation. It is the counterpart to the predefined ideal formation. It is a dimensionless value used to reflect the error in formation similarity.

5. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 4, characterized in that, The specific process of step S32 is as follows: First consider one dimension The polynomial trajectory of degree M is divided into M segments, where the first segment is the polynomial trajectory of degree M. The segment is defined as: in, It is a base. These are polynomial coefficients, using position, velocity, and acceleration as state variables, and the derivative of acceleration as the control variable. In this case, s=3, and the overall trajectory is determined by the coefficient matrix. and time vector Description, defined as: Will and The trajectory that makes up the whole : Next, define the MINCO trajectory, using... It is represented and defined as: All trajectories are only by and Perform compact parameterization, where This represents the midpoint of each adjacent segment of the trajectory. This indicates the duration of each segment of the trajectory.

6. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 5, characterized in that, The specific process of step S33 is as follows: Design a distributed local trajectory optimization framework with the following optimization objective: The first term defines smoothness and minimum control cost, while the second term defines the system's aggressiveness. It is the time regularization parameter. Let represent the total time from the local starting point to the local ending point. The constraints of the optimization objective are: Using the representation form of the MINCO trajectory representation method, the trajectory is defined. , by variable group Perform parameterization; trajectory The local starting point is trajectory The local endpoint is Together, these two elements constitute the boundary constraints of the trajectory. This represents the set of higher-order derivatives of an s-order dynamical system. Various additional constraints or tasks are generically described, including similarity of formations, obstacle avoidance, flocking mutual avoidance, and dynamic feasibility; Based on the MINCO trajectory representation method, boundary constraints of the trajectory are eliminated through boundary conditions, and additional constraints are eliminated through constraint transcription, resulting in: in This represents the smoothness of the optimization objective and the minimum control cost, i.e., the control cost. This represents the minimum time cost of the optimization objective, i.e., the total flight time cost. Subscript indicates constraints on various conditions or tasks. , f Indicating formation similarity, o Indicates obstacle avoidance. r This indicates that the clusters avoid each other. d Indicates dynamic feasibility, that is Including formation similarity cost Obstacle avoidance costs Cluster mutual avoidance cost Dynamic feasibility cost , This indicates the weight of each cost. This represents a fixed sampling time interval, and all costs... J By variable group Parameterization ; The quasi-Newton method is used to solve the unconstrained optimization problem. Based on the properties of the MINCO trajectory representation method, the position is represented by the trajectory. The mathematical expression of its derivative is converted to Through mathematical transformation, the form of is transformed into Convert to In the form of, from get and The entire optimization process only has The two parameter sets are optimized to obtain the initial trajectory without collision.

7. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 6, characterized in that, The control cost To reduce the energy consumption of the system and enhance the stability, it is required to ensure that the s-order control input cost of the trajectory is minimum, and the control input cost is defined as: the total flight time cost To guarantee the aggressiveness of the trajectory and the stability of the execution, it is required to minimize the total flight time between the local start and the local end: the formation similarity cost for penalizing the drones that deviate from the formation shape: The obstacle avoidance cost Euclidean Signed Distance Fields (ESDF) are used to penalize obstacles that are too close, and leave them untouched when they are at a safe distance: wherein, is a safety threshold set according to actual situation, denotes the distance between the sampling point and its nearest obstacle around. the swarm mutual evasion cost for penalizing over-closeness to other drones, representing a collection of other robots in the swarm, the swarm mutual evasion cost is defined as: wherein is a safety distance threshold between each drone; The dynamic feasibility cost Used to limit speed and acceleration, ensuring stable flight of drones along a prescribed trajectory, and considering dynamic feasibility costs. Defined as: wherein, and denotes the maximum linear velocity and the maximum linear acceleration of the drone.

8. The method of claim 7, wherein, The specific process of step S4 is as follows: Definitions represents the current position of the drone; ideal formation position template, defined in advance before the task starts or automatically in the program, to which the formation is aligned after alignment is defined as: in Indicates the formation size coefficient. Indicates the translation coefficient; formation alignment is determined by... and The two parameters determine the outcome. During task redistribution, two problems need to be addressed: target allocation and formation alignment. The target allocation problem refers to the situation where the current objective of a drone swarm is costly for some individuals, while these individuals can achieve other objectives at a lower cost. Solving the target allocation problem aims to find the optimal formation alignment parameters. and : in It is a diagram showing the allocation of formation tasks. Indicates from set The distribution of the overall transition distance between the current drone and the aligned local target is minimized by the symmetry group of all its permutations, thereby minimizing the time required for the formation to return to order. The formation alignment problem refers to the high cost required for a drone swarm to restore its current formation size, while a new formation in three-dimensional space exists with a lower cost than restoring the current ideal formation. Solving this problem aims to address the matching between the drone swarm and a local set of targets, described as follows: in The optimal allocation of representative formations. It is the task weight of each drone, and the standard formation that best suits the current drone position is generated based on the distance cost weighted by the formation constraints. The target assignment problem and the formation alignment problem are coupled and have three decision variables: formation size coefficient. Translation coefficient Task allocation The ultimate goal of the task redistribution problem is to find the optimal combination of variables that simultaneously satisfies the above equation, and to calculate it using a decoupled approach. and through calculate and : in The solution in the above formula and and Since it is irrelevant, we will first solve the task allocation problem, and then apply the optimal task allocation. Solve and The solution to the formation alignment problem: The parameters are defined as follows: By centrally remapping the key parameters of the formation and distributively replanning the trajectory, combined with local trajectory replanning for individual UAVs and global remapping of the entire cluster system, the local and global targets after global remapping are directly returned. Global trajectory replanning is then performed individually on the onboard computer of each UAV, ultimately achieving trajectory replanning after cluster reorganization.

9. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 2, characterized in that, The specific process of step S5 is as follows: The UAV system is modeled as a third-order integral model, with state variables being three-dimensional position, velocity, and acceleration, and the input variable being the derivative of acceleration. The influence of wind speed is modeled as a disturbance variable. w It is given by the following formula: The state prediction of the UAV is expressed as a discrete state-space equation with a step size of . In its first The transition equation for the predicted state of each time slot is: in , and Defined as The first The simplified expression for the predicted state of each time slot is: in Represents a linear state function; Combine a The model predictive control framework for UAVs is used to handle the dynamic feasibility constraints and obstacle avoidance constraints of UAVs and output the optimal control input within a certain range in the future. The model predictive control programming solver is distributed on each UAV and is independent of each other.

10. A method for controlling unmanned aerial vehicles (UAVs) in formation flight under wind conditions according to claim 9, characterized in that, The working process of the model predictive control programming solver is as follows: During each time slot, each UAV uses its current state variable as the initial value of the iterative optimization problem, and determines the optimal control input by solving the local optimization problem based on the model predictive control formula. The state variable and control variable of UAV i in time slot k are respectively... and From time slot From the Beginning to the Future The state variables and control inputs of each step size are defined as follows: For drones The objective function for solving the model predictive control formula is defined as follows: The optimal control input is defined as It is obtained from the following formula: In the time slot At this point, the optimal control input will be calculated. Take the first item from As in The control variables executed at any given time divide the overall cost function into tracking cost functions. and obstacle avoidance cost function Different weights are assigned to them, and the formula is given by the following equation: in The weights represent the tracking costs. The weights of the obstacle avoidance costs represent the overall cost. The larger the weight of a particular term, the greater its impact on the overall cost function. To ensure runtime safety, obstacle avoidance is also incorporated into the cost function, and the cost is tracked. Used to maintain an error-free tracking trajectory to meet the requirements of formation maintenance and obstacle avoidance: in Indicates drone exist Time slot prediction The set of three-dimensional positions at any given time. Indicates drone exist The set of three-dimensional positions in the ideal trajectory at any given time. ; Obstacle Avoidance Costs The Euclidean Symbolic Distance Field (ESDF) was used for calculation, and the ESDF distance of UAV i at time k+q was denoted as... Obstacle avoidance cost Defined as: in The minimum safe distance between the object and the obstacle; if the distance is less than this safe distance, there is a risk of collision. Based on the definitions of tracking cost and obstacle avoidance cost, the control optimization objective of a drone swarm is to successfully avoid obstacles while approaching a predefined trajectory. The trajectory tracking optimization problem for each drone i based on model predictive control is described by the following equation: in, Indicates drone The state update equation, The feasible region representing velocity. Describes the feasible region of acceleration. and This determines the dynamic feasibility of the control variables obtained through optimization.

Images