Unmanned aerial vehicle control system and method for formation flight in wind environment

Abstract

The invention relates to an unmanned aerial vehicle control system and method for formation flight in a wind environment. The method comprises the following steps: generating a wind disturbance environment with a specified map scale and containing random obstacles; acquiring external image data and inertial measurement data of the unmanned aerial vehicle, and generating a grid map through point cloud extraction and conversion operation; the method comprises the following steps: modeling a spatial topological structure of an unmanned aerial vehicle cluster by adopting an undirected graph mode, and solving and generating a collision-free initial trajectory by adopting an unconstrained discrete optimization mode; optimizing the initial trajectory by adopting a task redistribution strategy, and re-planning a global target and a local target to obtain a re-planned optimized trajectory; and based on the optimized trajectory, solving by adopting a model prediction control mode to obtain an optimal control quantity for unmanned aerial vehicle bottom layer control so as to correspondingly control the motion state of each unmanned aerial vehicle and realize formation flight. Compared with the prior art, the unmanned aerial vehicle cluster can efficiently and accurately realize formation flight in an obstacle environment with wind influence.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle formation control, and particularly relates to an unmanned aerial vehicle control system and method for formation flight in a wind environment. BACKGROUND

[0002] In search and rescue, exploration, and inspection tasks, unmanned aerial vehicles have shown great potential. Unmanned aerial vehicles can detect disease signs from high altitude, inspect crops, and perform seeding and fertilization according to needs, regardless of the impact of complex muddy roads. Using this technology, land can be managed more effectively, labor costs can be reduced, and farming efficiency can be improved. The widespread use of unmanned aerial vehicles in power line inspection also demonstrates their significant utility in this field, making them an important tool for power system maintenance and having great development potential. With the accelerated advancement of new infrastructure construction and the widespread deployment of 5G networks and artificial intelligence technology, the future potential of power inspection unmanned aerial vehicles has been highly recognized by the industry. China's vast geographical scope, diverse terrain, complex environmental conditions, and changing climate have led to limitations and difficulties in traditional surveying methods. These factors collectively hinder the effectiveness and efficiency of surveying activities, but the introduction of unmanned aerial vehicle technology has significantly improved the situation in land surveying, bringing significant positive impacts to this field. In terms of natural disaster response, it is important to assess disasters, determine the number of affected people and the extent of chaos, and develop rescue plans accordingly. Micro unmanned aerial vehicles can quickly traverse complex and narrow mountain and forest terrain, and can complete precise searches over a large area in a short time, making them outstanding in search and rescue tasks.

[0003] As industry standards continue to improve, independent unmanned aerial vehicles are subject to certain constraints when performing high-difficulty tasks, so multiple unmanned aerial vehicles need to work together. Formation flight is considered a basic ability for unmanned aerial vehicles to achieve collaborative work. Through formation flight, more complex tasks can be effectively solved. Existing formation flight algorithms are mostly conducted in open, interference-free environments and cannot be directly applied to complex actual scenarios. Due to the diversity of constraints and environmental uncertainties, there are certain challenges in achieving formation flight, especially in obstacle environments with wind influence, making it difficult to successfully achieve formation flight.

[0004] Current technology has significant defects in the field of UAV formation flight and trajectory planning. First, most algorithms rely on centralized architecture or global information sharing, resulting in high risk of single point failure, high computational complexity, and difficulty in adapting to dynamic environmental changes. Although the centralized method can provide an optimal solution, it has long computation time and poor real-time performance, making it difficult to respond to sudden obstacles. The distributed method can alleviate this problem, but it still needs to pre-set a virtual structure or rely on a leader-follower model, resulting in information transmission bottlenecks and insufficient formation stability. In addition, existing algorithms perform well in free environments, but in restricted environments, fixed geometric formations are difficult to change formation flexibly, and pre-set desired trajectories or rigid structures cannot adapt to dense dynamic obstacles, with weak response ability to sudden obstacles. Obstacle avoidance methods based on sphere radius or single integrator model are prone to failure in complex continuous obstacles, and their dynamic feasibility is questionable.

[0005] Secondly, the inherent defects of obstacle avoidance strategies further limit the practicality of the technology. Although the artificial potential field method is easy to implement, it is prone to local minimum, resulting in formation oscillation or stagnation; repulsive function or topology reconstruction method is difficult to generate effective obstacle avoidance path in unbounded obstacle, and the safety and energy consumption are difficult to balance. In addition, the linearization assumption of the model under external disturbance reduces the control accuracy, and the adaptive anti-disturbance ability is limited. Some formation maintenance and obstacle avoidance strategies are not coupled with kinematic constraints, and tracking errors are prone to accumulate in actual deployment. In large-scale cluster scenarios, the communication load increases dramatically, and existing consensus algorithms cannot guarantee state consistency. Affine transformation or fixed formation allocation strategy is also difficult to adapt to heterogeneous obstacle scenarios, and the optimal formation selection mechanism is missing. These defects jointly restrict the autonomy, safety and task reliability of UAV clusters in complex dynamic environments. SUMMARY

[0006] The purpose of the present application is to overcome the defects of the prior art and provide a UAV control system and method for formation flight in a wind environment, which can enable UAV clusters to efficiently and accurately achieve formation flight in obstacle environments affected by wind.

[0007] The purpose of the present application can be achieved by the following technical solutions: a UAV control system for formation flight in a wind environment, comprising a task redistribution module, a trajectory tracking module, an automated test and evaluation module, and a distributed trajectory generation module connected in a closed loop, the distributed trajectory generation module being connected with an environment perception module, the environment perception module being used to collect external image data, inertial measurement data and wind data of the UAV;

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

[0009] The task reassignment module is used to solve the optimal distribution and formation to re-plan the global target and local target;

[0010] The trajectory tracking module is used to track the re-planned trajectory to determine the optimal control amount for controlling the motion of the UAV;

[0011] The automatic test and evaluation module is used to automatically generate obstacles and wind forces, form a simulation map, and control the UAV to move in the simulation map based on the optimal control amount to obtain the formation flight test results and perform evaluation.

[0012] A UAV control method for formation flight in a wind environment, comprising the following steps:

[0013] S1, generating a wind disturbance environment with a specified map scale and containing random obstacles for UAV formation flight test;

[0014] S2, obtaining external image data and inertial measurement data of the UAV in the wind disturbance environment, generating a grid map by extracting point clouds and conversion operations;

[0015] S3, modeling the spatial topology of the UAV cluster using an undirected graph, and solving to generate a collision-free initial trajectory using an unconstrained discrete optimization method;

[0016] S4, using a task reassignment strategy to optimize the initial trajectory, and obtaining a re-planned optimized trajectory by re-planning the global target and local target;

[0017] S5, based on the optimized trajectory, using a model predictive control method to solve the optimal control amount for the bottom layer control of the UAV;

[0018] S6, according to the optimal control amount, corresponding control each UAV's motion state, realize formation flight.

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

[0020] S31, modeling the spatial topology of the UAV cluster using an undirected graph;

[0021] S32, using the MINCO trajectory representation method to describe the polynomial trajectory, taking the middle point of each adjacent trajectory and the duration of each trajectory as two decoupled variables, so that the trajectory satisfies various temporal and spatial constraints while ensuring local smoothness;

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

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

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

[0025] Assuming that each drone is able to communicate with all other drones in the swarm, Fig. is fully connected. The symmetric normalized Laplace matrix of is defined as:

[0026]

[0027] where I∈R N×N is the identity matrix, A∈R N×N It's a picture The adjacency matrix, D∈R N×N is the degree matrix, and a differentiable formation similarity error metric is used to evaluate the deviation from the desired formation:

[0028]

[0029] where tr{·} represents the trace of the matrix, is the symmetric normalized Laplace operator of the current group formation, is the corresponding term of the ideal formation defined in advance, 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-1, which is divided into M segments, where the i-th segment is defined as:

[0032]

[0033] Where β(x)=(1,x,…,x N ) T It's the base. is the polynomial coefficient, the state variables used are position, velocity, and acceleration, and the control variable is the derivative of acceleration. At this time, s = 3, and the overall trajectory is given by the coefficient matrix and time vector is described, defined as:

[0034]

[0035] The trajectory p consisting of c and T i (t):

[0036]

[0037] Next, the MINCO trajectory is defined, using is described, defined as:

[0038]

[0039] All trajectories in are compactly parameterized by q and T only, where denotes the middle point of each segment of trajectory p, denotes the duration of each segment of trajectory p.

[0040] Further, the specific process of the step S33 is:

[0041] A distributed local trajectory optimization framework is designed, and the optimization objective is:

[0042]

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

[0044]

[0045] Using the representation form of the MINCO trajectory representation method, the trajectory p(t) is defined, which is parameterized by the variable group q, T;

[0046] The local starting point of the trajectory p(t) is The local ending point of the trajectory p(t) is Both of them together constitute the boundary constraints of the trajectory p [s-1] (t) = (p(t) T , p(t) T ,…, p (s-1) (t) T ) T ∈ R ms denotes the set of high-order derivatives of an s-order dynamic system;

[0047] Various additional constraints or tasks are described, including formation similarity, obstacle avoidance, cluster mutual avoidance and dynamic feasibility;

[0048] Based on MINCO trajectory representation, the boundary constraints of trajectory are eliminated by boundary conditions, and the constraints of additional conditions are eliminated by constraint transcription, and the following is obtained:

[0049]

[0050] Among them The smoothness of the optimization objective and the minimum control cost, i.e. control cost, The minimum time cost of the optimization objective, i.e. total flight time cost, The constraints of various conditions or tasks, subscript * = {f, o, r, d}, f represents formation similarity, o represents obstacle avoidance, r represents cluster mutual avoidance, and d represents dynamic feasibility, i.e. Including formation similarity cost P f , obstacle avoidance cost P o , cluster mutual avoidance cost P r , and dynamic feasibility cost P d , w c , w t , w * Indicates the weight of each cost, δ indicates the fixed sampling time interval, and all costs J are parameterized by variable group {q, T, δ} J = J(q, T, δ);

[0051] The quasi-Newton method is used to solve the unconstrained optimization problem, according to the properties of MINCO trajectory representation, the mathematical expression form of position And its derivative is converted into the form of {c, T}, and {c, T} is converted into the form of {q, T} by mathematical transformation, so that And And The entire optimization process only has two parameter groups {q, T} to optimize, and finally the collision-free initial trajectory is obtained.

[0052] Further, the control cost J c is used to reduce the energy consumption of the system and to strengthen the stability, and needs to ensure that the s-order control input cost of the trajectory is minimized, and the control input cost is defined as:

[0053]

[0054] The total flight time cost J t is used to ensure the aggressiveness of the trajectory and the stability of the execution, and needs to minimize the total flight time between the local starting point and the local ending point: ​

[0055]

[0056] The formation similarity cost P f For the UAV deviating from the formation shape to make a punishment:

[0057]

[0058] The obstacle avoidance cost P o Adopting the Euclidean signed distance field (ESDF) to punish the obstacles that are too close to the UAV, and not processing when the distance is greater than the safety distance:

[0059]

[0060] Wherein, d o,max is a safety threshold set according to the actual situation, represents the distance between the sampling point and the nearest obstacle around it;

[0061] The cluster mutual avoidance cost P r For the UAV that is too close to other UAVs to make a punishment, and Φ represents the set of other robots in the cluster, and the cluster mutual avoidance cost is defined as:

[0062]

[0063] Wherein, d r,max is a safety distance threshold between each UAV;

[0064] The dynamic feasibility cost P d For limiting the speed and acceleration to ensure that the UAV flies stably on the specified trajectory, and the dynamic feasibility cost P d is defined as:

[0065]

[0066] Wherein, v m and a m represent the maximum linear speed and maximum linear acceleration of the UAV.

[0067] Further, the specific process of the step S4 is:

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

[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 starts or automatically defined in the program, and the formation position q' after the formation alignment j is defined as:

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

[0071] where s ∈ R represents the formation scale coefficient, represents the translation coefficient, and the formation alignment is determined by the two parameters s and d;

[0072] When the task is re-assigned, two problems need to be solved, the target assignment and the formation alignment problem, among which the target assignment problem refers to the cost of the current required target completed by the UAV cluster for some individuals is higher, while the cost of these individuals in completing other targets is lower, and the solution to the target assignment problem aims to find the optimal formation alignment parameters s and d:

[0073]

[0074] where σ ∈ S N is the assignment graph of the formation task, S N represents the symmetric group of all permutations from the set 1,..., N to itself, so as to minimize the total transition distance between the current UAV and the aligned local target, and minimize the time of restoring the order of the formation;

[0075] The formation alignment problem refers to the cost of restoring the current formation scale of the UAV cluster is large, and there is a new formation in the three-dimensional space that has a smaller cost than restoring the current ideal formation, and the solution to the problem aims to solve the matching problem between the UAV cluster and the local target set, which is described as:

[0076]

[0077] where σ * represents the optimal assignment of the formation, w i is the task weight of each UAV, and the distance cost weighted according to the formation constraint generates the standard formation most suitable for the current UAV position;

[0078] The target assignment problem and the formation alignment problem are coupled with each other, and have three decision variables: the formation scale coefficient s, the translation coefficient d, and the task assignment σ, and the final goal of the task reassignment problem is to find the optimal combination of variables that satisfy the above formula, and σ * is calculated by decoupling, and s and d are calculated by σ:

[0079]

[0080] where The solution in the above formula is independent of s and d, so the task assignment problem is solved first, and then the optimal task assignment σ is applied * Solve s and d, the solution to the formation alignment problem:

[0081]

[0082] where the parameters are defined as:

[0083]

[0084]

[0085] By centrally remapping the key parameters of the formation, distributing the trajectory replanning, combining the local trajectory replanning for a single UAV and the global remapping of the entire cluster system, the local target and the global target after global remapping are directly returned, the global trajectory is replanned on the on-board computer of each UAV, and finally the trajectory replanning after cluster reorganization is realized.

[0086] Further, the specific process of the step S5 is:

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

[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 is written in the form of discrete state space equation, the step is Δt, and the transition equation of the predicted state at the kth time interval is:

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

[0093] where A d , B d and C d are defined as

[0094]

[0095]

[0096] The transition equation of the predicted state of the kth time slot is simplified as:

[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] In combination with a model predictive control framework of N UAVs, dynamic feasibility constraints and obstacle avoidance constraints of the UAVs are processed, and optimal control input quantities within a certain range in the future are output, wherein the model predictive control planning solver is distributed on each UAV in a decentralized manner, and different UAVs are independent of each other.

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

[0101] During each time slot, each UAV takes the current state variable as the initial value of an iterative optimization problem, determines the optimal control input by solving a local optimization problem based on a model predictive control formula, and the state variable and control variable of the UAV i at the time slot k are x i (k) and u i (k), and the state variable and control input from the time slot k to the future M steps are defined as:

[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 of 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 by the following formula:

[0105]

[0106] At time slot k, the calculated optimal control input Take the first item As the control quantity executed at time [k:k+1], the overall cost function is divided into the tracking cost function and obstacle avoidance cost function And assign different weights, the formula is given by the following formula:

[0107]

[0108] Among them, λ1 represents the weight of the tracking cost, and λ2 represents the weight of the obstacle avoidance cost. The larger the weight of a certain item, the greater the impact of the cost function of this item on the overall cost function.

[0109] Considering the safety of runtime, obstacle avoidance is also taken into account in the cost function, and the cost is tracked. Used to maintain error-free tracking trajectory to meet the requirements of formation keeping and obstacle avoidance:

[0110]

[0111] where r i (k+q|k) represents the set of three-dimensional positions of UAV i at time k+q predicted 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, r = [x, y, z] T ;

[0112] Obstacle avoidance cost The Euclidean Signed Distance Field (ESDF) calculation is used, and the ESDF distance of drone i at time k+q is recorded as d o,i (k+q|k), obstacle avoidance cost Defined as:

[0113]

[0114] in d o,maxThe minimum safe distance from the obstacle, and less than this safe distance indicates a collision risk;

[0115] Based on the definition of the tracking cost and the obstacle avoidance cost, the control optimization goal of the UAV cluster is to approach the predefined trajectory while successfully avoiding obstacles, and the trajectory tracking optimization problem of each UAV i based on model predictive control is described as follows:

[0116]

[0117] s.t.C1X i (k+1)=f a (X i (k),u i (k))

[0118]

[0119] Wherein, C1 represents the state update equation of the UAV i, C2 represents the feasible region of the speed, C3 represents the feasible region of the acceleration, and C2 and C3 determine the dynamic feasibility of the control variable obtained by optimization.

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

[0121] The present application designs a closed-loop connected task reassignment module, a trajectory tracking module, an automatic test and evaluation module, and a distributed trajectory generation module, and the distributed trajectory generation module is connected with an environment perception module, which collects external image data, inertial measurement data and wind data of the UAV by using the environment perception module; the distributed trajectory generation module adopts the MINCO trajectory representation method with independent parameters to generate local targets, update cluster positions and calculate formation errors; the task reassignment module is used to solve and calculate optimal distribution and formation to re-plan global targets and local targets; the trajectory tracking module is used to track the re-planned trajectory to determine the optimal control amount for controlling the movement of the UAV; the automatic test and evaluation module is used to automatically generate obstacles and wind to form a simulation map, and control the UAV to move in the simulation map based on the optimal control amount to obtain formation flight test results and perform evaluation. Thus, a new formation flight framework is realized, which can make the UAV cluster successfully realize formation flight in an obstacle environment with wind influence under the premise of reducing the operation time as much as possible.

[0122] The application designs a double-layer framework: the upper layer generates trajectories adaptively by using a distributed trajectory generation framework, and the lower layer focuses on control decision in a wind disturbance environment. The distributed trajectory generation framework of the upper layer adopts the MINCO trajectory representation method of independent parameters for the trajectory optimization problem of the unmanned aerial vehicle, can meet the needs of different flight tasks, models the spatial topological structure of the unmanned aerial vehicle cluster by using an undirected graph, and quantifies the differences between different formations by using a differentiable metric method. In terms of space and feasibility constraints, an unconstrained discrete optimization method is used to solve the trajectory optimization problem, which can realize high-quality collision-free trajectories at low time complexity, and a task redistribution strategy is proposed for the formation disorder problem in formation flight, which explicitly describes the conflict between keeping formation and obstacle avoidance of the unmanned aerial vehicle, describes the disorder problem as a task allocation and formation alignment problem for the improper scale and allocation, obtains the optimal allocation and formation at extremely low time complexity by decoupling solving, re-plans the global target and local target by using the optimal allocation and formation, and ensures that the unmanned aerial vehicle cluster can quickly converge to the expected formation by performing new path planning in a distributed manner.

[0123] The core of the lower layer is model predictive control. The state space equation of the unmanned aerial vehicle is established for the continuity constraint of the motor, the state is updated for the high-order control input, the ESDF distance field (Euclidean Signed Distance Field) established by using the prior map is used to design a model predictive control framework considering the comprehensive target tracking and obstacle avoidance of the feasibility constraint, the optimal control quantity in a certain time range is generated, the algebraic relationship between the motor speed and the control quantity is given based on the differential flatness characteristic, and thus the bottom layer control of the unmanned aerial vehicle is realized. BRIEF DESCRIPTION OF DRAWINGS

[0124] Figure 1 It is a method flowchart of the application;

[0125] Figure 2 It is an application framework schematic diagram of the embodiment;

[0126] Figure 3 It is an algorithm execution process schematic diagram of the task redistribution strategy in the embodiment;

[0127] Figure 4a It is a local trajectory optimization simulation diagram (starting point) in the embodiment;

[0128] Figure 4b It is a local trajectory optimization simulation diagram (flight process 1) in the embodiment;

[0129] Figure 4c It is a local trajectory optimization simulation diagram (flight process 2) in the embodiment;

[0130] Figure 4d The simulation graph of local trajectory optimization (flight process 3) in the embodiment;

[0131] Figure 4e The simulation graph of local trajectory optimization (flight process 4) in the embodiment;

[0132] Figure 4f The simulation graph of local trajectory optimization (end point) in the embodiment;

[0133] Figure 5 The formation similarity curve of distributed trajectory generation in the embodiment;

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

[0135] Figure 6b The flight effect schematic diagram of the formation after remapping in the embodiment;

[0136] Figure 7 The formation similarity comparison schematic diagram before and after the task reassignment algorithm in the embodiment;

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

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

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

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

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

[0142] Figure 8f The flight effect schematic diagram of the formation in the end point when the wind speed is 1 m / s in the embodiment;

[0143] Figure 9 The formation similarity error curve of the formation flight when the wind speed is 1 m / s in the embodiment. DETAILED DESCRIPTION

[0144] The application will be described in detail below with reference to the drawings and specific embodiments.

[0145] EMBODIMENT

[0146] In order to efficiently generate continuous trajectories and perform stable tracking in an obstacle environment under wind disturbance, the present scheme develops a distributed trajectory generation framework to generate trajectories, and designs model predictive control to make decisions on control quantities to control the movement of the unmanned aerial vehicle. The present scheme first proposes a control system for formation flight of unmanned aerial vehicles in a wind environment, including a task reassignment module, a trajectory tracking module, an automatic test and evaluation module, and a distributed trajectory generation module connected in turn to form a closed loop, wherein the distributed trajectory generation module is connected with an environment perception module, and the environment perception module is used to collect external image data, inertial measurement data and wind data of the unmanned aerial vehicle;

[0147] The distributed trajectory generation module adopts the MINCO trajectory representation method with independent parameters, which is used to generate local targets, update cluster positions and calculate formation errors;

[0148] The task reassignment module is used to solve the calculation of optimal assignment and formation to re-plan global targets and local targets;

[0149] The trajectory tracking module is used to track the re-planned trajectory to determine the optimal control quantity for controlling the movement of the unmanned aerial vehicle;

[0150] The automatic test and evaluation module is used to automatically generate obstacles and wind to form a simulation map, and control the movement of the unmanned aerial vehicle in the simulation map based on the optimal control quantity to obtain formation flight test results and perform evaluation.

[0151] Based on the above system, a control method for formation flight of unmanned aerial vehicles in a wind environment is realized, as shown in Figure 1 The method comprises the following steps:

[0152] S1, a wind disturbance environment with a specified map size and containing random obstacles is generated for unmanned aerial vehicle formation flight test;

[0153] S2, external image data and inertial measurement data of the unmanned aerial vehicle are obtained in the wind disturbance environment, and a grid map is generated by extracting point clouds and conversion operations;

[0154] S3, the spatial topology structure of the unmanned aerial vehicle cluster is modeled in an undirected graph manner, and an unconstrained discrete optimization method is used to solve and generate a collision-free initial trajectory;

[0155] S4, a task reassignment strategy is adopted to optimize the initial trajectory, and the global target and the local target are re-planned to obtain the re-planned optimized trajectory;

[0156] S5, based on the optimized trajectory, the model predictive control method is used to solve the optimal control quantity for the bottom layer control of the unmanned aerial vehicle;

[0157] S6、According to the optimal control amount, the motion state of each unmanned aerial vehicle is controlled to realize formation flight.

[0158] The embodiment applies the above scheme, as shown in the figure, the overall system architecture is composed of four main parts: task reassignment model, distributed trajectory generation model, trajectory tracking model and automatic test and evaluation model. The embodiment uses code generation to specify the map size, random obstacle wind disturbance experiment environment, to ensure stable and unbiased formation flight test, and then uses numerical model to simulate quadrotor unmanned aerial vehicle, to improve the fidelity of formation flight experiment, and make it more close to the actual quadrotor unmanned aerial vehicle model. Figure 2

[0159] In the environment perception part, VINS-Fusion is used to process RGB-D camera and IMU data for self-positioning and mapping of the unmanned aerial vehicle. Specifically, point cloud data is extracted from the output of VINS-Fusion, filtered and processed to generate a sparse point cloud map. The processed point cloud data is converted using an algorithm to calculate the occupancy probability of each grid point and converted into a grid map.

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

[0161] I. Distributed trajectory generation

[0162] Planning a feasible and safe trajectory is crucial for unmanned aerial vehicles. This scheme aims to design a distributed trajectory optimization framework that can consider formation keeping, obstacle avoidance, mutual avoidance and dynamic feasibility. In the path planning process of the unmanned aerial vehicle, a plurality of collision-free global paths are obtained, and a distributed group trajectory optimization strategy is adopted to ensure that the unmanned aerial vehicle formation can continuously navigate during flight.

[0163] This scheme uses MINCO trajectory representation method to describe polynomial trajectory, using intermediate point and time as two variables. First, consider a m-dimensional N=2s-1 order polynomial trajectory, which is divided into M segments, where the i-th segment is defined as:

[0164]

[0165] Where β(x)=(1,x,…,x N ) T is the base, is the polynomial coefficient. In this scheme, the state variables are position, velocity and acceleration, and the control variable is the derivative of acceleration (jerk), so s=3. The overall trajectory is described by the coefficient matrix and the time vector , which is defined as:​

[0166]

[0167] The trajectory p consisting of c and T i (t) :

[0168]

[0169] Next, define the MINCO trajectory, using denoted as:

[0170]

[0171] All trajectories in are compactly parameterized by q and T only. Where denotes the midpoint of each segment of trajectory p, denotes the duration of each segment of trajectory p. The MINCO trajectory representation uses the decoupled variable combination q, T to represent the whole trajectory, which can guarantee the local smoothness of the trajectory while satisfying various spatiotemporal constraints of the task requirements.

[0172] The scheme expects that the UAV cluster can smoothly and safely navigate in the predetermined formation, so it needs a high-efficiency path planner that can be distributed on each UAV. The planner can effectively plan in an unknown environment using limited sensor visibility. Further, to improve the scalability and robustness of the cluster, the scheme uses a distributed local trajectory optimization framework, and the optimization objective is:

[0173]

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

[0175]

[0176] Equation (6-a) defines the representation form of the MINCO trajectory representation, which defines the trajectory p(t) parameterized by the variable combination q, T. Equation (6-b) defines the local starting point Equation (6-c) defines the local ending point of the trajectory p(t) [s-1] (t) = (p(t) T , p(t) T , …, p (s-1)(t) T ) T ∈R ms represents the set of high-order derivatives of the s-order dynamic system. Various additional constraints or tasks are described generically in formula (6-d), and the similarity of the formation, obstacle avoidance, cluster mutual avoidance, and dynamic feasibility are considered in the present scheme.

[0177] Based on the MINCO trajectory representation method, the constraints of (6-b)-(6-c) are eliminated through boundary conditions. For constraint (6-d), the condition of the hard constraint strictly limits the solution of the system, making the solution of the optimization problem very difficult and time-consuming. By constraint transcription, the constraint of the additional condition is eliminated, making the solution more efficient:

[0178]

[0179] where represents the smoothness and minimum control cost of (5), represents the minimum time cost of (5), represents the constraint of each condition or task, subscript * = {f, o, r, d}, f represents formation similarity, o represents obstacle avoidance, r represents cluster mutual avoidance, d represents dynamic feasibility, w c ,w t ,w * represents the weight of each item, δ represents the fixed sampling time interval, and all the costs J are parameterized by the variable group {q, T, δ} J = J(q, T, δ).

[0180] The optimization function used in the present scheme is discontinuous, but the design of the cost function is continuous, and the quasi-Newton method is used to solve the unconstrained optimization problem. According to the properties of the MINCO trajectory representation method, the mathematical expression of position and its derivative can be converted to the form of {c, T} through trajectory representation, and {c, T} can be converted to the form of {q, T} through mathematical transformation, so it can be efficiently obtained from and and so that the whole optimization process only has two parameter groups {q, T} to optimize, which can be more convenient for construction and solution. Next, each cost function J needs to be defined.

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

[0182]

[0183] (2) Total flight time cost J​t In the planning process, the trajectory curve that meets the formation and collision avoidance requirements of the system 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 the execution, the total flight time between the local start point and the local end point needs to be minimized:

[0184]

[0185] (3) Formation similarity cost P f In the invention, an undirected graph is described, in which represents a set of vertices, represents a set of edges. The graph is a time-varying undirected graph, and each vertex i represents the real-time position of the unmanned aerial vehicle. The set of edges ε represents the communication topology structure of the unmanned aerial vehicle cluster. If the edge e ij connects vertex i and vertex j, it means that the i-th unmanned aerial vehicle and the j-th unmanned aerial vehicle can communicate and share information with each other. In this paper, it is assumed that each unmanned aerial vehicle can communicate with all other unmanned aerial vehicles in the cluster, and the graph is fully connected. The symmetric normalized Laplacian matrix of the graph is defined as:

[0186]

[0187] where I ∈ R N×N is the identity matrix, A ∈ R N×N is the adjacency matrix of the graph , and d ∈ R N×N is the degree matrix. In order to evaluate the deviation from the required formation, this paper uses a differentiable formation similarity error metric:

[0188]

[0189] where tr[·} represents the trace of the matrix, is the symmetric normalized Laplacian operator of the current group formation, is the corresponding item of the ideal formation defined in advance. f s is a dimensionless value that reflects the error of formation similarity. This scheme punishes the unmanned aerial vehicles that deviate from the formation shape:

[0190]

[0191] (4) Obstacle avoidance cost P oThe perception module of the UAV can perceive the surrounding environment in real time and establish a point cloud map of the surroundings. The present scheme adopts the Euclidean signed distance field (ESDF) to punish obstacles that are too close to obstacles, and does not process when the distance is greater than the safety distance.

[0192]

[0193] wherein d o,max is a safety threshold value set according to actual conditions, represents the distance between the sampling point and the nearest obstacle around it.

[0194] (5) Cluster mutual avoidance cost P r In the process of trajectory optimization, in addition to the need to avoid obstacles, the trajectory of the UAV will have potential collision danger when it is too close to other UAVs, so it is necessary to punish the UAVs that are too close to other UAVs. Φ represents the set of other robots in the cluster, and the cluster mutual avoidance cost is defined as:

[0195]

[0196] wherein d r,max is a safety distance threshold value between each UAV.

[0197] (6) Dynamic feasibility cost P d In the process of flight, the speed and torque of the motor have upper limits, so the speed and acceleration need to be limited to ensure that the UAV can fly stably on the specified trajectory. The dynamic feasibility cost P d is defined as:

[0198]

[0199] wherein v m and a m represent the maximum linear speed and maximum linear acceleration of the UAV.

[0200] II. Task reassignment model

[0201] There is a certain limitation in local trajectory optimization, and the entire UAV cluster cannot quickly recover from the sudden changes in formation caused by obstacles or dense environments to execute the corresponding tasks. The direct cause of this problem is that the fixed formation size and local path planning cannot cope with disordered formation states. In view of the problem of disordered UAV formation, it is divided into task allocation and formation alignment, and decoupling processing and solution are carried out. The task reassignment strategy is designed to reassign the entire cluster when the formation is disordered, effectively alleviating the impact of dense environments on the formation. ​

[0202] Definition of p i = [p ix , p iy , p iz ] T i = 1,..., N represents the current position of the UAV. Definition of 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 starts or automatically defined in the program. The formation position q j ′ is defined as:

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

[0204] where s e R represents the formation scale coefficient, represents the translation coefficient. The formation alignment is determined by the two parameters s and d. When the task is re-assigned, two problems need to be solved, the target assignment and the formation alignment problem. The target assignment problem refers to the current target required to be completed by the UAV cluster for some individuals is higher in cost, while the cost spent by these individuals in completing other targets is lower, as shown in the example of Figure (4-1a). The solution to this problem aims to find the optimal formation alignment parameters s and d

[0205]

[0206] where σ e S N is the assignment graph of the formation task, S N represents the symmetric group of all permutations from the set 1,..., N to itself. The formula solves the assignment of minimizing the total transition distance between the current UAV and the aligned local target, minimizing the time of restoring the order of the formation. The formation alignment problem refers to the cost required for the UAV cluster to restore the current formation is large, while there is a new formation in the three-dimensional space that has a smaller cost than restoring the current ideal formation. The solution to this problem aims to solve the matching problem between the UAV cluster and the local target set, described as:

[0207]

[0208] where σ * represents the optimal assignment of the formation, w iis the task weight of each UAV. The distance cost weighted by formation constraint generates a standard formation that fits the current UAV position best. The above equations are coupled with each other and have three decision variables: formation scale factor s, translation factor d, and task allocation σ. The final goal of the task re-allocation problem is to find the optimal combination of variables that satisfy the above equations simultaneously. σ can be calculated in a decoupled way * and s and d are calculated by σ:

[0209]

[0210] where The solution of the above equations is independent of s and d, so the task allocation problem is solved first and then the optimal task allocation σ * s and d are solved. The solution of the formation alignment problem is:

[0211]

[0212] where the parameters are defined as:

[0213]

[0214] For the swarm system with only trajectory optimization, it is difficult to quickly recover order when sudden obstacles and formation shape changes occur. Considering the operation speed, the task re-allocation strategy is proposed in this scheme, which can effectively deal with sudden obstacles and make the swarm reach the equilibrium state in the desired formation faster, further improving the coordination and adaptability of the system. For the distributed computing framework, communication between UAVs is crucial. The information of itself can be published to other UAVs in real time and the information of other UAVs can be received to optimize the planning of itself, so that the whole system quickly converges to the desired state. In practical applications, communication delay will cause inaccurate information or circular dependency problems, making it difficult to ensure the consistency of decision-making. Therefore, a centralized re-planner is necessary. The task re-allocation strategy of this scheme centrally remaps the key parameters of the formation and distributes the re-planned trajectory, finally realizing the trajectory re-planning after swarm reorganization.

[0215] The task reassignment strategy adopted in the present solution is to directly return the local target and the global target after global remapping, and to separately perform global trajectory re-planning on the on-board computer of each UAV. The task reassignment strategy combines local trajectory re-planning for a single UAV and global remapping of the entire cluster system. The local re-planned trajectory enables each UAV to formulate a trajectory within its local perception range, while the global remapping is an efficient centralized strategy that re-plans the local target by solving the task reassignment method and reassigns the global target to the re-assigned UAV. The task re-planning strategy performs well in a distributed asynchronous system, and can effectively prevent the cluster system from deadlocking even in the presence of communication delay.

[0216] As shown in Figure 3 , the core operation process of the task re-planning strategy in the present embodiment includes: first, each UAV distributedly evaluates whether an emergency situation occurs. Lines 2-8 in the algorithm describe whether the formation constraint perception exceeds the critical value, and whether the obstacle avoidance and formation keeping of a single UAV seriously conflict, triggering the global remapping strategy flag CallGlobalRemap. Next, it is detected whether the overall formation shape deviates too much from the expected formation, and lines 9-11 quantitatively calculate the formation similarity error e sim , which is compared with the threshold e sim,d . If the similarity error is too large, the global remapping flag CallGlobalRemap is triggered. Unlike the periodically activated local re-planning strategy, the global remapping strategy is triggered only when an emergency situation is detected, i.e. CallGlobalRemap = 1. In lines 13-19, the cluster is centrally remapped, lines 14 and 15 solve the task reassignment problem to obtain the optimal σ, s and d, and in line 16, the local target is remapped. In line 17, the global target \mathbf{G}_i^* is remapped, and the local target and the global target are sent as return values to each UAV. Lines 20-22 represent re-planning of the global trajectory in each UAV, formulating a new global trajectory , which forms a new formation structure. This process is fast and efficient, ensuring the continuity, stability and cooperativeness of the cluster motion.

[0217] III. Trajectory tracking model

[0218] In the present solution, the optimized trajectory is a time-varying function σ T(t) which explicitly contains the position, velocity and acceleration information of the UAV at each time instant, while the attitude variables and angular velocity of the UAV are implicitly contained in the trajectory formulation, which can be represented by the position, velocity and acceleration, and are not directly optimized. Therefore, the UAV system is modeled as a third-order integral model, the state variables are chosen as the three-dimensional position, velocity and acceleration, the input variable is chosen as the derivative of acceleration (jerk), and the influence of wind speed is modeled as a disturbance variable w, which is given by:

[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 is written in the form of discrete state space equation with a step size of At, and the transition equation of the predicted state at the kth time interval is:

[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 are defined as

[0225]

[0226]

[0227] For simplicity, 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 the linear state function.

[0230] This proposal considers a model predictive control framework for a fleet of N unmanned aerial vehicles (UAVs). Model predictive control can effectively handle the UAVs' dynamic feasibility and obstacle avoidance constraints and can provide optimal control inputs within a certain future range, rather than just the current optimal value. In this proposal, the proposed model predictive control can effectively track continuously variable target trajectories. The model predictive control planning solver is distributed across each UAV, making each UAV independent of the others. The proposed coordinated control of the UAV swarm can be simplified to the control problem of multiple individual UAVs.

[0231] During each time slot, each UAV uses the 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. In this scheme, the state variable and control variable of UAV i in time slot k are x i (k) and u i (k), the state variables and control inputs from time slot k to the next M steps are defined as:

[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 of 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 by the following formula:

[0234]

[0235] At time slot k, the calculated optimal control input Take the first item As the control quantity executed at time [k:k+1]. This scheme divides the overall cost function into tracking cost function and obstacle avoidance cost function And assign different weights, the formula is given by the following formula:

[0236]

[0237] λ1 represents the weight of the tracking cost, and λ2 represents the weight of the obstacle avoidance cost. The greater the weight of a certain term, the greater the influence of the cost function represented by this term on the overall cost function.

[0238] First, the significance of the two cost functions is analyzed. For a single unmanned aerial vehicle, as long as the planned trajectory is tracked without error, all the above requirements can be met. However, in a real environment, various unexpected events such as wind influence or sudden movement of obstacles may cause the unmanned aerial vehicle to deviate from its position, thereby failing to quickly approach the trajectory and effectively avoid obstacles. Therefore, considering the safety during operation, the present solution also considers obstacle avoidance in the cost function. The tracking cost and the obstacle avoidance cost will be described in detail below.

[0239] Tracking cost The tracking cost is the most important optimization objective for trajectory tracking. During the progress of the unmanned aerial vehicle, it should be tracked without error as much as possible, thereby meeting the requirements of formation keeping and obstacle avoidance. is defined as:

[0240]

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

[0242] Next is the obstacle avoidance cost The obstacle avoidance cost needs to consider the distance from the obstacle at each time and make a judgment on whether it is in the range of possible collision and make a response. The obstacle avoidance cost uses the Euclidean signed distance field (ESDF) calculation, and the ESDF distance of the unmanned aerial vehicle i at time k+q is denoted as d o,i (k+q|k), and the obstacle avoidance cost is defined as:

[0243]

[0244] where d o,max is the minimum safe distance from the obstacle, and a distance less than this safe distance indicates a risk of collision.

[0245] Based on the definition of tracking cost and obstacle avoidance cost, the control optimization goal of UAV swarm is to approach the predefined trajectory while successfully avoiding obstacles. Based on all the above definitions, the trajectory tracking optimization problem of each UAV i based on model predictive control is described as follows:

[0246]

[0247] s.t.C1X 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, which determines the dynamic feasibility of the control variable optimized.

[0250] To further prove the effectiveness and unbiasedness of the formation flight control method in complex scenarios, the data established in the simulation world are used to conduct comprehensive simulation experiments. Whether the distributed trajectory optimization method can optimize a trajectory that integrates formation keeping, obstacle avoidance, aggressiveness, and dynamic feasibility is verified. First, a dense simulation environment composed of many cylindrical, ring-shaped, and continuous polygonal obstacles is established to verify the effectiveness of the distributed trajectory optimization. The hexagonal formation is selected as a case in this embodiment, mainly because of its typical geometric characteristics, which helps to 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 terminal point is specified in the global map, and a formation-level global path search method is used to perform preliminary collision-free path search, and then the selected sampling points in the global path are used as intermediate points to generate local formation trajectories using a distributed method. The color area of the obstacle in the simulation represents the range in which the UAV can dynamically perceive the environment, and when the UAV leaves a certain range of the detected obstacle, the point cloud information is deleted. Figures 4a to 4f The process of local trajectory optimization of the UAV swarm during the journey is shown. Figures 4a to 4f In the figure, the non-red color solid line represents the trajectory that has been traveled, and the red solid line represents the optimized smooth trajectory. The points on the solid line indicate the control intermediate points, and the quadrotor model shows the current position of the UAV. As shown in Figure 4a The UAV swarm uses a hexagonal formation to travel stably, and at this time, no obstacles are encountered, and the formation is consistent with the expected formation. Figures 4b to 4eThe situation that the UAV cluster crosses the complex dense terrain is described, wherein the UAVs periodically calculate the optimized path within a certain range. In Figure 4b The scene shown in Figure 4e Due to the need for obstacle avoidance, the formation is adjusted during flight. Finally, as shown in Figure 4f The UAV cluster reaches the destination with the expected formation, successfully completing the task. According to the simulation results, the UAV cluster can optimize the smooth trajectory of the comprehensive formation formation, obstacle avoidance, mutual avoidance, and dynamic feasibility in real time during flight. The formation flight framework used in this embodiment can well adapt to dense obstacle constraints and provide safe guidance for formation flight.

[0252] As shown in Figure 5 During formation flight, the formation similarity error of the cluster is always maintained below the predetermined critical value (0.05), meeting the requirements of flight. There are two reasons for the change of formation similarity error. One is that the formation of the UAV cluster is adjusted according to the needs of obstacle avoidance and mutual avoidance during formation flight; the other is that the communication delay caused by the asynchronous communication mechanism of the UAV cluster leads to position deviation, which in turn affects the calculation results. This verifies the effectiveness of the trajectory optimization method in maintaining the formation.

[0253] The formation flight system without using the task redistribution algorithm and the formation flight system using the task redistribution algorithm are tested in the same map with the same end point. The path without using the task redistribution algorithm is shown in Figure 6a There are multiple UAVs' trajectories crossing and colliding, the UAVs are close to each other during flight, which can easily cause collision and confusion, and the distance to the specified target point is longer, which is less efficient. The path using the task redistribution algorithm is shown in Figure 6b After the UAV cluster redistributes its own tasks and remaps the target, it quickly recovers the expected formation, and then the cluster generates feasible intermediate paths according to the distributed trajectory generation method. These intermediate paths have no intersection and are far apart, which is safer.

[0254] The formation similarity error during formation flight is shown in Figure 7 Without using the task redistribution algorithm (blue curve in Figure 7 ), the formation similarity error decreases slowly, and it takes about 28 seconds to converge to the expected formation, which is slow and cannot meet the actual application requirements. However, after introducing the task redistribution algorithm (red curve in Figure 7 ), the formation similarity error decreases rapidly and converges to the predetermined formation in about 2 seconds, and the formation similarity error is always maintained at a low level during the entire flight process. This observation verifies the excellent performance of the task redistribution algorithm in handling disordered formation.

[0255] In order to verify whether the trajectory tracking method based on model predictive control can effectively track the optimized smooth trajectory to realize the flight simulation experiment of UAV formation, the embodiment carries out comprehensive formation flight simulation experiment in the constructed simulation environment, simulates the UAV characteristics through the numerical model, and tracks the optimized trajectory. The execution period of the model predictive control is set to 20Hz, the control execution period is 100Hz, and in each control period, the optimal control amount generated by the MPC with an interval of 50ms is calculated by interpolation, so as to determine the optimal control amount in the current execution period for control calculation.

[0256] The initial state of the robot is randomly arranged, the wind disturbance is set to 1m / s, and the direction is the y-axis direction. Due to the improper initial formation, the cluster calls the task redistribution strategy to redistribute and remap, Figure 8a The whole cluster recovers the expected formation, Figures 8b to 8e The multiple angle views of the UAV cluster successfully crossing the obstacle dense area are shown, and in the crossing process, the UAV cluster does not collide with the obstacles or other UAVs in the cluster. Figure 8c And Figure 8d Due to the continuous obstacles, the cluster changes the formation to adapt to the environment, and then realizes obstacle avoidance. Figure 8f The cluster recovers the expected formation when reaching the terminal point, and each UAV reaches the global target after remapping, and completes the task. In the whole formation flight process, the cluster can maintain the formation to the greatest extent at any time.

[0257] In addition, the formation flight simulation formation similarity error shown in Figure 9 The initial formation similarity error is 0.87, which is higher than the critical value, the task redistribution algorithm is called to redistribute the formation and remap the target, so that the error rapidly decreases to a low level. In the subsequent flight process, due to the factors such as wind speed and obstacle avoidance, it is kept within the critical range, verifying that the trajectory tracking method based on model predictive control can effectively execute the precise control of UAV formation under wind speed disturbance. This result shows that this method is suitable for executing the fully autonomous formation flight task under wind speed disturbance, successfully avoiding obstacles, completing tasks, and ensuring the accuracy of the formation and the efficiency of the execution.

Claims

1. A UAV control system for formation flying in a windy environment, characterized in that: It includes a task reallocation module, a trajectory tracking module, an automated test and evaluation module, and a distributed trajectory generation module, which are sequentially connected to form a closed loop. The distributed trajectory generation module is connected to an environment 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 reallocation module is used to solve and calculate the optimal allocation and formation to re-plan the global and local goals; The trajectory tracking module is used to track the re-planned trajectory to determine the optimal control amount for controlling the movement of the drone; The automated test and evaluation module is used to automatically generate obstacles and wind forces to form a simulation map, and to control the UAVs to move in the simulation map based on the optimal control amount to obtain and evaluate the formation flight test results.

2. A method for controlling drones for formation flying in a windy environment, characterized in that: The following steps are involved: S1. Generate a wind-disturbed environment with a specified map size and random obstacles for UAV formation flight testing. S2, obtain the external image data and inertial measurement data of the UAV in a wind disturbance environment, and generate a raster map by extracting point clouds and performing transformation operations; S3. Use an undirected graph approach to model the spatial topology of the drone cluster and use an unconstrained discrete optimization approach to generate collision-free initial trajectories. S4. Adopt the task reallocation strategy to optimize the initial trajectory and obtain the replanned optimized trajectory by replanning the global and local goals; S5. Based on the optimized trajectory, the model predictive control method is used to solve the optimal control quantity for the underlying control of the UAV; S6. According to the optimal control quantity, the motion state of each UAV is controlled accordingly to realize formation flight.

3. The method for controlling UAVs for formation flying in a windy environment according to claim 2, wherein: Step S3 The following processes are included: S31. Use an undirected graph approach to model the spatial topology of a drone cluster. S32. Use the MINCO trajectory representation method to describe polynomial trajectories, taking 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 required by various tasks 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 initial collision-free trajectory.

4. The method for controlling UAVs for formation flying in a windy environment according to claim 3, wherein: The specific process of step S31 is as follows: Using an undirected graph Describes a cluster formation consisting of N drones, where represents a set of vertices, Represents a set of edges, graph It is a time-varying undirected graph, where each vertex i represents the real-time position of the drone The set of edges ε represents the communication topology of the drone cluster. If the edge e ij ∈ε connects vertex i and vertex j, indicating that the i-th UAV and the j-th UAV can communicate with each other and share information; Assuming that each drone is able to communicate with all other drones in the swarm, Fig. is fully connected, The symmetric normalized Laplace matrix of is defined as: where I∈R N×N is the identity matrix, A∈R N×N It's a picture The adjacency matrix, D∈R N×N is the degree matrix, and a differentiable formation similarity error metric is used to evaluate the deviation from the desired formation: where tr{·} represents the trace of the matrix, is the symmetric normalized Laplace operator of the current group formation, is the corresponding term of the ideal formation defined in advance, f s It is a dimensionless value that reflects the error in formation similarity.

5. The method for controlling UAVs for formation flying in a wind environment according to claim 4, characterized in that: The specific process of step S32 is as follows: First, consider an m-dimensional polynomial trajectory of degree N=2s-1, which is divided into M segments, where the i-th segment is defined as: Where β(x)=(1,x,…,x N ) T It's the base. is the polynomial coefficient, the state variables used are position, velocity, and acceleration, and the control variable is the derivative of acceleration. At this time, s = 3, and the overall trajectory is given by the coefficient matrix and time vector Description, defined as: Combine c and T to form the overall trajectory p i (t): Next, define the MINCO trajectory using It is defined as: All trajectories in are compactly parameterized by only q and T, where represents the midpoint of each adjacent trajectory, Indicates the duration of each trajectory.

6. The method for controlling UAVs for formation flying in a windy environment according to claim 5, characterized in that: The specific process of step S33 is: Design a distributed local trajectory optimization framework with the following optimization objectives: The first term defines the smoothness and minimum control cost, the second term defines the aggressiveness of the system, ρ is the time regularization parameter, T represents the total time from the local starting point to the local end point, and the constraints of this continuous optimization objective are: Using the representation form of the MINCO trajectory representation method, the trajectory p(t) is defined and parameterized by the variable group q,T; The local starting point of the trajectory p(t) is The local end point of the trajectory p(t) is The two together constitute the boundary constraints of the trajectory, p [s-1] (t)=(p(t) T ,p(t) T ,…,p (s-1) (t) T ) T ∈R ms represents the set of higher-order derivatives of an s-order dynamical system; It generally describes various additional constraints or tasks, including formation similarity, obstacle avoidance, cluster mutual avoidance, and dynamic feasibility; Based on the MINCO trajectory representation method, the boundary constraints of the trajectory are eliminated through boundary conditions, and the constraints of additional conditions are eliminated through constraint transcription, resulting in: in Represents the smoothness of the optimization objective and the minimum control cost, i.e., the control cost, represents the minimum time cost of the optimization objective, that is, the total flight time cost, represents the constraints of various conditions or tasks, subscript *={f,o,r,d}, f represents formation similarity, o represents obstacle avoidance, r represents mutual avoidance of clusters, and d represents dynamic feasibility, i.e. 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 * represents the weight of each cost, δ represents the fixed sampling time interval, and all costs J are parameterized by the variable group {q, T, δ} J = J(q, T, δ); The quasi-Newton method is used to solve the unconstrained optimization problem. According to the properties of the MINCO trajectory representation method, the trajectory representation is used to represent the position The mathematical expression of its derivative is converted into the form of {c, T}, and through mathematical transformation, {c, T} is converted into the form of {q, T}, so as to obtain and get and The entire optimization process involves optimizing only two parameter groups {q, T}, and finally obtaining a collision-free initial trajectory.

7. The method for controlling UAVs for formation flying in a windy environment according to claim 6, characterized in that: The control cost J c In order to reduce the energy consumption of the system and enhance stability, it is necessary to ensure that the s-order control input cost of the trajectory is minimized. The control input cost is defined as: The total flight time cost J t To ensure the aggressiveness of the trajectory and the stability of the execution, it is necessary to minimize the total flight time between the local starting point and the local end point: The formation similarity cost P f Used to penalize drones that deviate from the formation shape: The obstacle avoidance cost P o The Euclidean Signed Distance Field (ESDF) is used to penalize obstacles that are too close to the obstacle and not to process them when they are greater than the safe distance: Among them, d o,max It is a safety threshold set according to actual conditions. Indicates sampling point The distance between itself and the nearest obstacle; The cluster mutual avoidance cost P r Used to detect drones that are too close to other drones ι (t), ι∈Φ, t=j is the penalty, Φ represents the set of other robots in the cluster, and the cluster mutual avoidance cost is defined as: where d r,max is the safety distance threshold between each drone; The dynamic feasibility cost P d It is used to limit the speed and acceleration to ensure that the UAV flies stably on the specified trajectory and to set the dynamic feasibility cost P d Defined as: Among them, v m and a m Indicates the maximum linear velocity and maximum linear acceleration of the drone.

8. The method for controlling UAVs for formation flying in a wind environment according to claim 7, characterized in that: The specific process of step S4 is as follows: Define p i =[p ix ,p iy ,p iz ] T ,i=1,…,N represents the current position of the UAV; Define q j =[q jx ,q jy ,q jz ] T ,j=1,…,N represents the ideal formation position template, which is defined before the task starts or automatically defined in the program, and the formation position q′ after the formation is aligned j Defined as: q′ j =s·q j +d, Where s∈R represents the formation size coefficient, It represents the translation coefficient. The formation alignment is determined by two parameters, s and d. When redistributing tasks, two problems need to be solved: target allocation and formation alignment. The target allocation problem refers to the situation where the cost of completing the target that the drone cluster currently needs to achieve is high for some individuals, while these individuals have lower costs when completing other targets. Solving the target allocation problem aims to find the optimal formation alignment parameters s and d: where σ∈S N is the allocation diagram of the formation task, S N represents the symmetric group of all permutations from the set 1,…,N to itself, thereby minimizing the distribution of the overall transition distance between the current drone and the aligned local target, minimizing the time it takes for the formation to return to order; The formation alignment problem refers to the situation where the cost of restoring the current formation size of a UAV swarm is high, while a new formation exists in three-dimensional space that is less costly than restoring the current ideal formation. Solving this problem aims to solve the problem of matching the UAV swarm with the local target set, which can be described as: where σ * represents the optimal allocation of formations, w i is the mission weight of each UAV, and the standard formation that best suits the current UAV position is generated based on the distance cost weighted by the formation constraint; The target allocation problem and the formation alignment problem are coupled with each other and have three decision variables: formation size coefficient s, translation coefficient d, and task allocation σ. The ultimate goal of the task redistribution problem is to find the optimal variable combination that satisfies the above equations at the same time. σ is calculated in a decoupled manner. * And calculate s and d through σ: in The solution in the above formula is independent of s and d, so we first solve the task allocation problem and then apply the optimal task allocation σ * Solve for s and d, the solution to the formation alignment problem: The parameters are defined as: By centrally remapping the key parameters of the formation and distributively replanning the trajectory, combining local trajectory replanning for individual drones with global remapping of the entire cluster system, the globally remapped local and global targets are directly returned, and global trajectory replanning is performed separately on the onboard computer of each drone, ultimately achieving trajectory replanning after cluster reorganization.

9. The method for controlling UAVs for formation flying in a windy environment according to claim 2, wherein: The specific process of step S5 is as follows: The UAV system is modeled as a third-order integral model. The state variables are selected as three-dimensional position, velocity, and acceleration. The input variable is selected as the derivative of acceleration. The influence of wind speed is modeled as the interference variable w, which is given by the following formula: x=[x,y,z,v x ,v y ,v z ,a x ,a y ,a z ] T u=[j x ,j y ,j z ] T in=[in x ,In y ,In z ] T The state prediction of the UAV is written as a discrete state space equation with a step size of Δt. The transfer equation of the predicted state at the kth time slot is: x(k+1)=A d x(k)+B d u(k)+C d w(k), Among them A d 、B d and C d Defined as The transfer equation of the predicted state of the kth time slot is simplified as follows: x i (k+1)=f a 9x i (k),u i (k),w i (k)), where f a (·) represents the linear state function; A model predictive control framework with N drones is combined to handle the dynamic feasibility constraints and obstacle avoidance constraints of the drones and output the optimal control input within a certain range in the future. Among them, the model predictive control planning solver is distributed on each drone, and different drones are independent of each other.

10. The method for controlling UAVs for formation flying in a wind environment according to claim 9, characterized in that: The working process of the model predictive control planning solver is: During each time slot, each UAV uses the 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 x i (k0 and u i (k), the state variables and control inputs from time slot k to the next M steps are defined as: x i [k:k+M-1]=[x i (k|k),x i (k+1|k),…,x i (k+M-1|k)] u i [k:k+M-1]=[u i (k|k),u i (k+1|k),…,u i (k+M-1|k)] For UAV i, the objective function of solving its own model predictive control formula is defined as J i 9x i [k:k+M-1],u i [k:k+M-1]), the optimal control input is defined as Obtained by the following formula: At time slot k, the calculated optimal control input Take the first item As the control quantity executed at time [k:k+1], the overall cost function is divided into the tracking cost function and obstacle avoidance cost function And assign different weights, the formula is given by: Among them, λ1 represents the weight of the tracking cost, and λ2 represents the weight of the obstacle avoidance cost. The larger the weight of a certain item, the greater the impact of the cost function of this item on the overall cost function. Considering the safety of runtime, obstacle avoidance is also taken into account in the cost function, and the cost is tracked. Used to maintain error-free tracking trajectory to meet the requirements of formation keeping and obstacle avoidance: where r i (k+q|k) represents the set of three-dimensional positions of UAV i at time k+q predicted 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, r = [x, y, z] T ; Obstacle avoidance cost The Euclidean Signed Distance Field (ESDF) calculation is used, and the ESDF distance of drone i at time k+q is recorded as d o,i (k+q|k), obstacle avoidance cost Defined as: in d o,max The minimum safe distance from obstacles. If the distance is less than this, there is a risk of collision. Based on the definitions of tracking cost and obstacle avoidance cost, the control optimization goal of the UAV swarm is to approach the predefined trajectory while successfully avoiding obstacles. The trajectory tracking optimization problem of each UAV i based on model predictive control is described as follows: s.t.C1X i (k+1)=f a (X i (k),u i (k)) Among them, C1 represents the state update equation of UAV i, C2 represents the feasible domain of velocity, and C3 represents the feasible domain of acceleration. C2 and C3 determine the dynamic feasibility of the optimized control variables.

Images