UAV formation control method and system based on multi-agent distributed consensus

Abstract

The present invention relates to a drone formation control method and system based on multi-agent distributed consistency. The method comprises the following steps: allocating coordinates of drones and target points to obtain a target allocation relationship, an alignment scale, and a translation distance; obtaining a relative state based on the alignment scale and the translation distance; obtaining a first trajectory based on the relative state between the drones using a consistency algorithm when obstacle avoidance is not required; calculating the degree of violation of the formation by the drones' obstacle avoidance when the degree of violation is large, replacing the formation and obtaining a second trajectory based on the new formation when the degree of violation is small, planning an obstacle avoidance trajectory for the drones that need to avoid obstacles to obtain a third trajectory; and obtaining a target trajectory based on the first, second, and third trajectories when reaching the end point coordinates. The method implements a drone flight trajectory optimization strategy to balance safety and trajectory executability, thereby significantly improving the drone system's ability to cope with changing environments.

Description

Technical Field

[0001] The present invention belongs to the field of drone control, and in particular relates to a drone formation control method and system based on multi-agent distributed consistency. Background Art

[0002] The drone swarm system consists of multiple drones that fly in formation through distributed algorithms and complete complex tasks through collaboration. This can significantly reduce the risk of failure of a single drone and improve the efficiency and quality of the entire group in completing tasks.

[0003] However, in the development of UAV formation flying technology, there are still many challenges and problems to be solved, such as key technical aspects such as communication between UAVs, positioning accuracy, environmental perception, self-management capabilities, and real-time dynamic planning.

[0004] Behavioral control methods focus on the regular design of multi-agent behavior and attempt to achieve formation control by combining priority setting with task requirements. However, this introduces complexity in system design and has limitations in system scalability and robustness. Virtual structure control achieves control objectives by setting a navigator and building a formation around it. Although it has obvious advantages in system stability and implementation accuracy, this method still needs to be improved in terms of flexibility and environmental adaptability. The leader-follower method uses one UAV as the navigator and requires the remaining UAVs to maintain a fixed angle and distance. This method is highly dependent on the navigator. Once the navigator fails, the entire formation is difficult to maintain. In addition, this method faces the problem of excessive communication load when applied in large-scale clusters; it is not conducive to dealing with the changing environment during flight. Summary of the Invention

[0005] Aiming to solve at least one of the technical problems existing in the prior art, the present invention provides a drone formation control method and system based on multi-agent distributed consistency, which improves the response capability of the drone system in the face of changing environments.

[0006] An embodiment of the first aspect of the present invention is a method for controlling a drone formation based on multi-agent distributed consensus, characterized in that it includes the following steps:

[0007] Obtain obstacle coordinates, multiple preset formations, and drone status parameters;

[0008] Performing target point allocation based on the current coordinates of the UAV in the state parameters and the coordinates of the target point in the preset formation to obtain a target allocation relationship between the UAV and the target point, and obtaining an alignment scale and a translation distance of the UAV based on the current coordinates of the UAV, the coordinates of the target point, and the target allocation relationship;

[0009] Obtaining a relative state between the UAVs according to the alignment scale and the translation distance;

[0010] A signal that no obstacle avoidance is required is obtained according to the obstacle coordinates, and a first trajectory of the UAV is obtained according to the relative states between the UAVs using a consistency algorithm;

[0011] Obtaining an obstacle avoidance signal based on the obstacle coordinates, determining a degree of violation of the formation by the obstacle avoidance of the drone based on the current coordinates of the drone and the coordinates of the obstacle, selecting a formation with the least degree of violation among the preset formations as a first target formation when the degree of violation is greater than a preset violation threshold, and obtaining a second trajectory of the drone based on the first target formation; and when the degree of violation is less than or equal to the preset violation threshold, planning an obstacle avoidance trajectory of the drone to be avoided based on the obstacle coordinates while maintaining the current formation, thereby obtaining a third trajectory of the drone;

[0012] When the coordinates of the end point are reached, a target trajectory is obtained according to the first trajectory, the second trajectory and the third trajectory.

[0013] According to some embodiments of the first aspect of the present invention, performing target point allocation based on the current coordinates of the drone in the state parameters and the coordinates of the target point in the preset formation to obtain a target allocation relationship between the drone and the target point includes:

[0014] Solve the first optimization problem to allocate target points according to the current coordinates of the UAVs in the state parameters and the coordinates of the target points in the preset formation, and obtain a target allocation relationship between the UAVs and the target points. The first optimization problem is expressed as: Among them, k is the distribution relationship between the UAV and the target point, pn i is the current coordinate of the i-th UAV, q k(i) is the coordinate of the target point, s * is the optimal alignment scale corresponding to k, d * is the optimal translation distance corresponding to k.

[0015] According to some embodiments of the first aspect of the present invention, obtaining the alignment scale and translation distance of the drone based on the current coordinates of the drone, the coordinates of the target point, and the target allocation relationship includes:

[0016] Solve the second optimization problem to obtain the alignment scale and translation distance of the drone based on the current coordinates of the drone, the coordinates of the target point, and the target allocation relationship. The second optimization problem is expressed as: Among them, s is the current alignment scale, d is the current translation distance, k * is the target allocation relationship between the UAV and the target point, For the corresponding k * The position offset.

[0017] According to some embodiments of the first aspect of the present invention, the relative states between the drones are expressed as: Among them, α and β are adjustable parameters greater than 0, is the component of the relative state between the UAVs in the x, y and z directions, a ij are the elements of the adjacency matrix, is the component of the current coordinate of the i-th drone in the x, y and z directions, is the component of the current coordinate of the j-th UAV in the x, y and z directions, is the component of the current velocity of the i-th drone in the x, y and z directions, are the components of the current velocity of the j-th drone in the x, y, and z directions.

[0018] According to some embodiments of the first aspect of the present invention, obtaining the first trajectory of the drone based on the relative states between the drones using a consistency algorithm includes:

[0019] Obtaining partial derivatives of reference velocities of the UAVs according to the relative states between the UAVs;

[0020] Obtaining a reference speed of the UAV according to a partial derivative of the reference speed of the UAV;

[0021] Obtaining a partial derivative of a reference position of the UAV according to a reference speed of the UAV;

[0022] A first trajectory of the UAV is obtained according to the partial derivative of the reference position of the UAV.

[0023] According to some embodiments of the first aspect of the present invention, obtaining the degree of violation of the formation by the drone's obstacle avoidance based on the drone's current coordinates and the obstacle coordinates includes:

[0024] When the distance between the current coordinates of the drone and the coordinates of the obstacle is less than or equal to the preset distance threshold, the cosine value between the direction vector of the formation and the direction vector of the obstacle avoidance is calculated;

[0025] The degree of violation of the formation by the UAV obstacle avoidance is obtained based on the current coordinates of the UAV and the coordinates of the obstacle, the expected change direction of the formation, and the cosine value between the direction vector of the formation and the direction vector of the obstacle avoidance;

[0026] The degree of violation of the formation by the UAV obstacle avoidance is expressed as: Among them, δ is the degree of violation of the formation by the UAV obstacle avoidance, N is the total number of UAVs, γi is the normalized representation of the cosine value between the formation direction vector and the obstacle avoidance direction vector, u i (t) is the expected change direction of the formation, x i (t) is the current coordinate of the UAV, is the distance between the current coordinates of the drone and the coordinates of the obstacle, d t is the preset distance threshold.

[0027] According to some embodiments of the first aspect of the present invention, while maintaining the current formation, planning an obstacle avoidance trajectory of the drone to be avoided according to the obstacle coordinates to obtain a third trajectory of the drone includes:

[0028] Solve the third optimization problem to plan the obstacle avoidance trajectory of the UAV to be avoided according to the obstacle coordinates while maintaining the current formation, and obtain the third trajectory of the UAV. The third optimization problem is expressed as: Among them, α, β and γ are weight coefficients, a is the acceleration of the drone, g goal is the penalty term for the distance from the drone to the target point, g o is the penalty term for the distance from the drone to the obstacle.

[0029] According to some embodiments of the first aspect of the present invention, the penalty item of the distance from the drone to the target point is obtained based on the current speed of the drone, the current acceleration of the drone, the current coordinates of the drone, the coordinates of the target point, the coordinates of the target point with the minimum distance and the sampling time, and the penalty item of the distance from the drone to the obstacle is obtained based on the minimum distance between the drone and the obstacle, the current speed of the drone, the current acceleration of the drone, the current coordinates of the drone and the sampling time.

[0030] An embodiment of the second aspect of the present invention is a computer-readable storage medium storing program instructions, which, when executed by a processor, implement the drone formation control method based on multi-agent distributed consistency as described above.

[0031] An embodiment of the third aspect of the present invention is a drone formation control system based on multi-agent distributed consensus, comprising:

[0032] A computer device comprising the computer-readable storage medium.

[0033] The beneficial effects of the present invention include: addressing the contradiction between the limited computing power of small drones and flight trajectory planning, realizing a drone flight trajectory optimization strategy to balance key factors such as safety and trajectory feasibility; considering that drones need to maintain a specific formation in an environment with obstacles, a flexible formation control method is used to enable drones to adjust or appropriately change the formation structure according to environmental changes to effectively avoid obstacles; when a specific formation needs to be maintained and there are obstacles around, the formation can be moderately destroyed, which can not only achieve the formation but also ensure safety during flight; the degree of violation of the formation can be calculated based on the location and characteristic information of the obstacle, and the formation can be automatically adjusted to effectively avoid potential conflicts. The formation is measured and flexibly transformed based on the information of the obstacle. The formation is flexibly adjusted according to environmental changes and actual needs, which significantly improves the response capability of the drone system in the face of changing environments.

[0034] In addition, additional aspects and advantages of the present invention will be set forth in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 This is a step diagram of the UAV formation control method based on multi-agent distributed consensus;

[0036] Figure 2 This is a flow chart of the UAV formation control method based on multi-agent distributed consensus;

[0037] Figure 3 It is a schematic diagram of remapping the drone and the target point;

[0038] Figure 4 It is a schematic diagram of the cosine value between the formation's direction vector and the obstacle avoidance direction vector;

[0039] Figure 5 This is another diagram of the cosine value between the formation's direction vector and the obstacle avoidance direction vector;

[0040] Figure 6 This is a schematic diagram of drone communication topology;

[0041] Figure 7 This is the effect diagram of the numerical simulation of the consistency algorithm;

[0042] Figure 8 This is another effect diagram of the numerical simulation run of the consistency algorithm;

[0043] Figure 9 This is the architecture diagram of the drone system. DETAILED DESCRIPTION

[0044] The following will be combined with the embodiments and drawings to clearly and completely describe the concept, specific structure and technical effects of the present invention so as to fully understand the purpose, scheme and effect of the present invention. It should be noted that the embodiments and features in the embodiments of this application can be combined with each other unless there is a conflict.

[0045] It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly fixed or connected to the other feature or indirectly fixed or connected to the other feature. Furthermore, terms such as "upper," "lower," "left," "right," "top," and "bottom" used in this disclosure are intended solely to describe the relative positions of the components of the disclosure as shown in the accompanying drawings.

[0046] In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art. The terms used in this specification are only for describing specific embodiments and are not intended to limit the present invention. The term "and / or" as used herein includes any combination of one or more of the related listed items.

[0047] Embodiments of the present invention provide a UAV formation control method based on multi-agent distributed consensus. This method can calculate trajectory information in discrete space using a fixed step size based on input UAV and environmental information. Therefore, in the distributed consensus formation algorithm, the UAVs only need to know the geometry of the target formation. Under the control of the consensus protocol, they continuously negotiate to achieve a consistent system state and complete the intended formation mission.

[0048] Reference Figure 1 and Figure 2 ,The UAV formation control method includes the following steps:

[0049] Step S100: allocate target points based on the current coordinates of the drone in the state parameters and the coordinates of the target points in the preset formation, and obtain the target allocation relationship between the drone and the target point. Based on the current coordinates of the drone, the coordinates of the target point and the target allocation relationship, the alignment scale and translation distance of the drone are obtained.

[0050] In a distributed consensus formation algorithm, drones only need to know the geometry of the target formation. Under the control of a consensus protocol, they continuously negotiate to achieve a consistent system state and complete the intended formation mission. Information such as the starting point coordinates, end point coordinates, obstacle coordinates, multiple preset formations, and drone state parameters are input into the algorithm model to perform drone formation control calculations.

[0051] Initialize the formation information according to the input of the algorithm model. Initialization parameters include the initial position p0, initial velocity v0, initial acceleration a0 of N drones, and the position information p of the obstacle.obs , the current position of the i-th UAV is pn i =[pn ix , pn iy , pn iz ] T , i=1,...,N, the default formation is q j =[q jx ,q jy ,q jz ] T , j=1,...,N, etc.

[0052] The drone cluster needs to reach the end point in a fixed formation, but different allocation methods of drone target points will lead to different degrees of trajectory conflict, so the drones and target points need to be remapped.

[0053] Solve the first optimization problem to allocate target points based on the current coordinates of the UAV in the state parameters and the coordinates of the target points in the preset formation, and obtain the target allocation relationship between the UAV and the target points. The first optimization problem is expressed as: Among them, k is the distribution relationship between the UAV and the target point, pn i is the current coordinate of the i-th UAV, q k(i) is the coordinate of the target point, s * is the optimal alignment scale corresponding to k, d * is the optimal translation distance corresponding to k.

[0054] Solving the first optimization problem can obtain the optimal distribution relationship k between the UAV and the target point * .

[0055] Reference Figure 3 For example, the original allocation method is that target point 1 is assigned to drone 2, and target point 2 is assigned to drone 1. After remapping, the new allocation method is that target point 1 is assigned to drone 1, and target point 2 is assigned to drone 2.

[0056] Considering that there may be a deviation between the current pose of the drone and the pose of the target point, it is necessary to calculate the appropriate alignment scale and translation distance based on the assigned target point to eliminate the deviation.

[0057] Solve the second optimization problem to obtain the alignment scale and translation distance of the drone based on the current coordinates of the drone, the coordinates of the target point, and the target allocation relationship. The second optimization problem is expressed as: Among them, s is the current alignment scale, d is the current translation distance, k * is the target allocation relationship between the UAV and the target point, For the corresponding k *The position offset.

[0058] The first and second optimization problems are decoupled, and their optimal values ​​can be solved separately. Therefore, the optimal allocation can be solved first, and then the optimal alignment scale and translation distance can be calculated.

[0059] The target point remapping problem can be solved by solving the following integer programming problem: Among them, for x ij , if the position of the i-th UAV is assigned to the j-th position in the formation, then x ij is 1, otherwise x ij is 0,

[0060] There is a closed-form solution for the alignment scale and translation distance. Solve for the following intermediate variables: The following closed-form solution can be obtained:

[0061] Step S200: The relative state between the UAVs is obtained based on the alignment scale and the translation distance; a signal that no obstacle avoidance is required is obtained based on the obstacle coordinates, and a first trajectory of the UAVs is obtained based on the relative state between the UAVs using a consistency algorithm.

[0062] A signal indicating that no obstacle avoidance is required is obtained based on the coordinates of the obstacle. Specifically, the distance between the drone and the obstacle is obtained based on the coordinates of the drone and the coordinates of the obstacle. When the distance between the drone and the obstacle is greater than or equal to a preset distance threshold, that is, the drone is not close to the obstacle, the drone does not need to avoid the obstacle and a signal indicating that no obstacle avoidance is required is obtained.

[0063] Based on the trajectory tracking control of a single UAV, a linear reference model is designed for each UAV, and the linear reference model takes the relative state of the UAV as input.

[0064] In order to ensure that the system state of the drone cluster is consistent and that collisions do not occur, a fixed position offset is set to complete the formation task. The input of the linear reference model is designed using the relative position and velocity of the drones. The relative state between drones is expressed as: in, is the position offset of the i-th UAV after remapping, which is determined by the desired formation; α and β are adjustable parameters greater than 0, which determine the stability and convergence of the system; is the component of the relative state between the UAVs in the x, y and z directions, a ij are the elements of the adjacency matrix, is the component of the current coordinate of the i-th drone in the x, y and z directions, is the component of the current coordinate of the j-th UAV in the x, y and z directions, is the component of the current velocity of the i-th drone in the x, y and z directions, are the components of the current velocity of the j-th drone in the x, y, and z directions.

[0065] Reference Figure 6 For the adjacency matrix, a directed graph is used to model the communication topology of the six controlled quadcopters. The numbers in the circles represent the serial numbers of the drones, and the arrows indicate the direction of information transmission. For example, drone No. 2 can obtain the position and speed information of drone No. 6. The adjacency matrix corresponding to this topology is:

[0066] Each drone forms a formation by tracking the output of its corresponding linear reference model. The linear reference model uses a consistency algorithm to obtain the first trajectory of the drone based on the relative state of the drones. The following steps are included: obtaining the partial derivative of the drone's reference velocity based on the relative state of the drones; obtaining the drone's reference velocity based on the partial derivative of the drone's reference velocity; obtaining the partial derivative of the drone's reference position based on the drone's reference velocity; and obtaining the drone's first trajectory based on the partial derivative of the drone's reference position. The above process is expressed by the following formula: is the relative state between drones, is the partial derivative of the reference position of the UAV, is the partial derivative of the reference velocity of the UAV.

[0067] After using the consistency algorithm to form a formation, the process returns to the steps of remapping the target points of the drone and calculating the alignment scale and translation distance; during the flight of the drone, the coordinates of the end point are used to determine whether it has reached the end point. When it reaches the end point, the target trajectory is output, otherwise the drone continues to fly forward.

[0068] The stability of the above consistency formation algorithm is proved below. Since the trajectory tracking of UAVs is relatively mature, it is assumed that a single UAV can track a given reference trajectory, that is,

[0069]

[0070] Next, it is proved that the reference trajectory given by tracking can enable multiple UAVs to form a target formation.

[0071] The error of the trajectory is defined as follows:

[0072]

[0073] The input of the linear referencing model can be expressed as:

[0074]

[0075] definition:

[0076] According to the previous assumption, Therefore, we only need to prove that the designed control algorithm can control the reference quantity and achieve formation control. The following is the consistency proof of the algorithm.

[0077] Lemma 1: For the system If the function f(x(t), u(t)) satisfies the local Lipschitz condition with respect to x and is continuous with respect to u. And when u(t) = 0, x = 0 is a uniformly asymptotically stable equilibrium point. Then if there exists a K L Class functions β and K ∞ Class function γ, such that for any initial state x(t0) and bounded input u(t)∈L ∞ , t>t0 has ‖x(t)‖≤β(‖x(t0)‖, t-t0)+γ(‖u(t)‖ ∞ ), the system is said to be input-state stable.

[0078] Since the position offset in the formula is a fixed value, it can be simplified by a simple translation transformation, that is, The above formula can be equivalently written as follows:

[0079]

[0080]

[0081] make It can be restated as: in, is the Kronecker product.

[0082] Note that the formula is linear, and From input to state stability, we only need to examine The stability is enough.

[0083] Definition 1: In a multi-agent system, if under any initial conditions,

[0084]

[0085] Then second-order consistency is achieved.

[0086] Lemma 2: If G n is an n-order directed graph, is the corresponding asymmetric Laplacian matrix, if Gn Contains a directed spanning tree, then the matrix L n There is a single zero characteristic root and all other characteristic roots have positive real parts.

[0087] For the model described in the formula, the eigenvalue of the matrix L is very important for the stability analysis of the system. Here, stability means consistency in state. Assume λ ij (i=1,2,...,N,j=1,2), μi(i=1,2,...,N) are and the eigenvalues ​​of L, then their eigenvalues ​​have the following relationship:

[0088]

[0089] Therefore, the calculation is The eigenvalues ​​of are:

[0090] From the eigenvalue relation, we can get that if the algebraic multiplicity of eigenvalue 0 in L is m, then The geometric multiplicity of the eigenvalue 0 is 2m.

[0091] Lemma 3: If and only if If the algebraic multiplicity of the characteristic root that is 0 is 2 and the other characteristic roots all have negative real parts, the second-order consistency of the multi-agent system can be achieved.

[0092] Definition 2: If a directed graph contains a directed spanning tree and satisfies Then the second-order consistency is achieved. i is the Laplace matrix L n The non-zero eigenvalues ​​of R(μ i ), F(μ i ) are the real and imaginary parts of the eigenvalues, then the system is uniformly stable, that is,

[0093] From Lemma 2, we know that L has a single zero characteristic root and the other characteristic roots all have positive real parts. If there is a directed spanning tree in a directed graph, then from Lemma 3, we only need to prove that R(μ i )>0(i=2,3,...,N), and the conditions in Definition 2 are satisfied if and only if R(λ ij )<0 (i=2, 3,..., N; j=1, 2).

[0094] Where R(μ i )>0(i=2,3,…,N), which can be easily obtained from Lemma 2.

[0095] make Where c and d are real numbers, then if R(λ ij)<0(i=2,3,...,N;j=1,2), as long as -βR(μ i ) <c<βR(μ i ), equivalent to c 2 <β 2 R 2 (μ i )(i=2,3,...,N), we can easily get

[0096] Separating the real and imaginary parts, we have:

[0097] Through simple calculation, we can get c 4 -{β 2 [R 2 (μ i )-l 2 (μ i )]-4αR(μ i )}c 2 -l 2 (μ i )[β 2 R(μ i )-2α] 2 =0. From the above formula, we can find that c 2 <β 2 R 2 (μ i )(i=2,3,...,N) the system reaches uniform stability.

[0098] The above proves the stability of the second-order consistency algorithm. That is, through the consistency algorithm, the speeds of drones can be consistent, the positions can be kept at a fixed offset, and formation control can be completed.

[0099] In step S300, an obstacle avoidance signal is obtained based on the obstacle coordinates, and the degree of violation of the formation by the obstacle avoidance of the drone is obtained based on the current coordinates of the drone and the obstacle coordinates. When the degree of violation is greater than the preset violation degree threshold, the formation with the smallest degree of violation among the preset formations is selected as the first target formation, and the second trajectory of the drone is obtained based on the first target formation. When the degree of violation is less than or equal to the preset violation degree threshold, while maintaining the current formation, the obstacle avoidance trajectory of the drone to be avoided is planned based on the obstacle coordinates to obtain the third trajectory of the drone.

[0100] Due to the complexity of the environment, drones may encounter obstacles during formation flight. Therefore, it is necessary to enable drones to avoid obstacles while flying in formation, and automatically switch to another formation to circumvent the obstacle when the current formation is unable to avoid it.

[0101] The first-order integral consistency algorithm is used to express the expected change direction to complete the formation: Among them, N i (t) is the neighbor set of the i-th drone, a ij (t) is the element of the adjacency matrix, x i (t) is the position of the i-th UAV, is the target formation assigned to the i-th UAV.

[0102] By searching the drone's sensing range, it determines whether there are obstacles near the drone. The obstacle coordinates are used to determine the obstacle avoidance signal. Specifically, the distance between the drone and the obstacle is determined based on the drone's coordinates and the obstacle's coordinates. When the distance between the drone and the obstacle is less than a preset distance threshold, meaning the drone is close to the obstacle, the drone is prompted to avoid the obstacle and an obstacle avoidance signal is generated.

[0103] The obstacle avoidance signal can be expressed as: is the distance between the current coordinates of the drone and the coordinates of the obstacle, d t is the preset distance threshold.

[0104] Reference Figure 4 and Figure 5 When the distance between the current coordinates of the drone and the coordinates of the obstacle is less than or equal to the preset distance threshold, the cosine value between the formation direction vector and the obstacle avoidance direction vector is calculated using the cosine theorem; if the calculated result is closer to 1, the degree of conflict is smaller, and if it is closer to -1, the degree of conflict is greater. The cosine value between the formation direction vector and the obstacle avoidance direction vector is expressed as: Normalize the cosine value between the formation direction vector and the obstacle avoidance direction vector, that is, transfer the cosine value to the interval [0, 1] to obtain the conflict metric. The conflict metric is expressed as:

[0105] The degree of violation of the formation by the drone’s obstacle avoidance is obtained based on the drone’s current coordinates, the obstacle coordinates, the expected change direction of the formation, and the cosine value between the formation’s direction vector and the obstacle avoidance direction vector. The degree of violation of the formation by the drone’s obstacle avoidance is expressed as: Among them, δ is the degree of violation of the formation by the UAV obstacle avoidance, N is the total number of UAVs, γ i is the normalized representation of the cosine value between the formation direction vector and the obstacle avoidance direction vector, u i (t) is the expected change direction of the formation, x i (t) is the current coordinate of the UAV.

[0106] When the violation degree exceeds a preset violation degree threshold, a formation change is required. The formation with the least violation degree among the preset formations is selected as the first target formation for subsequent change, so that the drone can successfully avoid the obstacle. The second trajectory of the drone is obtained based on the first target formation.

[0107] After switching the formation, return to the steps of remapping the target point of the drone and calculating the alignment scale and translation distance; during the flight of the drone, determine whether it has reached the end point based on the coordinates of the end point. If it reaches the end point, the target trajectory is output, otherwise the drone continues to fly forward.

[0108] When the violation degree is less than or equal to the preset violation degree threshold, there is no need to change the formation, and the quadratic programming method can be used to make the drone avoid obstacles.

[0109] If there is no need to change the formation, the trajectory of the drone is optimized using the dynamic window method while taking into account the feasibility of the trajectory and obstacle information. If there is no obstacle information within the dynamic window, there is no need to consider the obstacle information and the formation can be completed under the dynamic constraints. The speed and acceleration constraints of the quadrotor are expressed as follows:

[0110] If there is obstacle information in the dynamic window, solve the third optimization problem to obtain the optimal control quantity G(a) for the next step, and we have Among them, α, β and γ are weight coefficients, a is the acceleration of the drone, g goal is the penalty term for the distance from the drone to the target point, g o is the penalty term for the distance from the drone to the obstacle.

[0111] a 2 The goal is to minimize the control variable. By finding the minimum value of G(a), we can obtain the optimal control variable a, which is the acceleration for the next step.

[0112] Since we hope that the target can reach the target as quickly as possible, but we do not want to guide too much when it is too far away from the target point, we get the penalty term of the distance from the drone to the target point according to the current speed of the drone, the current acceleration of the drone, the current coordinates of the drone, the coordinates of the target point, the coordinates of the target point with the smallest distance and the sampling time. Among them, p e is the coordinate of the target point, p min is the coordinate of the target point with the smallest distance, p[k] is the current coordinate of the UAV, v[k] is the current speed of the UAV, a[k] is the current acceleration of the UAV, and Δt is the sampling time.

[0113] Moreover, the penalty term of the distance from the UAV to the obstacle is obtained according to the minimum distance between the UAV and the obstacle, the current speed of the UAV, the current acceleration of the UAV, the current coordinates of the UAV and the sampling time, and we have Among them, d min is the minimum distance between the drone and obstacles.

[0114] Finally, the above problem is expressed as: That is, under the condition of satisfying the dynamic constraints, the minimum value of G(a) is solved to plan the obstacle avoidance trajectory of the UAV that needs to avoid obstacles according to the obstacle coordinates while maintaining the current formation. After obtaining the next acceleration, the next speed and position can be obtained by integration, and then the third trajectory of the UAV can be obtained.

[0115] After using the quadratic programming method to avoid obstacles, the coordinates of the end point are returned to determine whether the end point has been reached. If the end point is reached, the target trajectory is output; otherwise, the drone continues to fly forward.

[0116] In step S400, when the UAV reaches the coordinates of the end point, the target trajectory is obtained according to the first trajectory, the second trajectory and the third trajectory.

[0117] Another embodiment of the present application provides a computer-readable storage medium storing program instructions, which, when executed by a processor, implement the above-described method for controlling a drone formation based on multi-agent distributed consensus.

[0118] Another embodiment of the present application provides a drone formation control system based on multi-agent distributed consensus. The drone formation control system includes: a computer device; the computer device includes the computer-readable storage medium described above.

[0119] In a specific embodiment, referring to Figure 9 The drone system can be implemented using multiple Crazyflie 2.1 drones, a CrazyRadio PA wireless communication module 3, and a Vicon motion capture system 2. Multiple Maker reflective balls 1 are attached to the drones and their frames to form different rigid body shapes, allowing the Vicon motion capture system 2 to accurately obtain the drones' position information as input to the computer device. The formation algorithm then calculates the desired target trajectory and transmits it to each drone via the CrazyRadio PA wireless communication module 3 at 2.4GHz. The drone's onboard controller then calculates thrust and attitude angle as the system's output, controlling the drone to fly according to specified commands. If the drone system malfunctions, meaning the drone deviates from the intended trajectory, the Xbox joystick 4 is used to bring the drone to an emergency stop to minimize damage.

[0120] The sampling time of the UAV system is set to every 0.1 seconds, which ensures that the system can update and respond to changes in real time. The total number of UAVs is 6, ensuring sufficient data volume to evaluate the performance of the formation control algorithm.

[0121] Each drone has a set starting point at the beginning, which is the coordinate [0,0,1]m, and their initial velocity and acceleration are set to zero, which means that the drone starts moving from a stationary state. The target position is set at [14,14,1]m, which provides the drone with a precise location of the end point, and the target velocity and acceleration are also zero, indicating that the drone will eventually come to rest at the target position. In order to control the movement of drones in the formation, limits are set for the maximum velocity and acceleration, with a maximum velocity of 1m and a maximum acceleration of 2m / s. 2 , which helps ensure that the drone's movements are within a safe and controllable range.

[0122] Three tuning parameters, α, β, and γ, were introduced, set to 0.1, 0.3, and 1, respectively. These parameters play a key role in the control algorithm, helping to adjust the responsiveness and sensitivity of the drone's behavior. Finally, for flight safety, a minimum distance threshold of 1 meter and a minimum safety distance of 2 meters were set to prevent collisions between drones.

[0123] Reference Figure 7 and Figure 8 , Figure 7 and Figure 8 The simulation results clearly depict the information exchange between drones and their current direction of movement. In the figure, the six drones are represented by black circles, while the direction of information exchange between them is indicated by red arrows. The arrows point in the direction of information outflow, and the actual direction of movement of the drones is represented by short black lines pointing outward from the center of the drones. Obstacles are represented by bold black circles, including 20 randomly arranged obstacles and a narrow gap formed by multiple obstacles. This demonstrates the complexity and unpredictability of the actual operating environment and the adaptability of the formation method to narrow gaps that the original formation cannot pass through.

[0124] The results demonstrate that these multi-agent systems demonstrate strong flexibility and adaptability when performing formation flight missions. When maintaining a specific formation while encountering obstacles, they can moderately disrupt the formation, maintaining both formation and flight safety. When faced with extreme obstacles such as narrow gaps, the drones can calculate the degree of violation of their specific formation based on the location and characteristics of the obstacles, automatically adjusting their formation and effectively avoiding potential conflicts. This feature highlights the potential for formation algorithms in dynamic environments, particularly those requiring high levels of coordination and adaptability. Furthermore, the results confirm the advantages of formation algorithms in real-time response and environmental adaptability, demonstrating their significant potential for practical application. The drone formation was able to efficiently transform into a formation, successfully navigating the narrow gap. This highlights the advantages of formation algorithms in adaptability to dynamic environments. Furthermore, the formation is measured based on obstacle information and can be flexibly altered. During operation, the drone system does not need to maintain a fixed formation. Instead, the drone formation flexibly adjusts to environmental changes and actual needs. This adaptability is a significant advantage of the improved formation algorithm, which significantly improves the ability of the UAV system to cope with changing environments.

[0125] Those skilled in the art will appreciate that all or some of the steps and systems in the method disclosed above can be implemented as software, firmware, hardware, and appropriate combinations thereof. Some physical components or all physical components can be implemented as software executed by a processor, such as a central processing unit, a digital signal processor, or a microprocessor, or implemented as hardware, or implemented as an integrated circuit, such as an application-specific integrated circuit. Such software can be distributed on a computer-readable medium, and the computer-readable medium can include computer storage media (or non-transitory media) and communication media (or temporary media). As known to those skilled in the art, the term computer storage media is included in any method or technology for storing information (such as computer-readable instructions, data structures, program modules, or other data) and is volatile and non-volatile, removable, and non-removable. Computer storage media includes, but is not limited to, RAM, ROM, EEPROM, flash memory, or other memory technology, CD-ROM, digital versatile disks (DVD), or other optical disk storage, magnetic cassettes, magnetic tapes, disk storage, or other magnetic storage devices, or any other medium that can be used to store desired information and can be accessed by a computer. In addition, it is well known to those skilled in the art that communication media generally contain computer-readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transmission mechanism, and may include any information delivery medium. In the above description of this specification, the reference terms "one embodiment / example", "another embodiment / example" or "certain embodiments / examples" and the like are intended to mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0126] Those skilled in the art will appreciate that all or some of the steps in the methods, systems, and functional modules / units in the devices disclosed above may be implemented as software, firmware, hardware, or appropriate combinations thereof.

[0127] The units described above as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0128] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0129] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes multiple instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: various media that can store programs, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0130] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of the above-mentioned units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms. Although the embodiments of the present application have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and purpose of the present application, and the scope of the present application is defined by the claims and their equivalents.

[0131] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the aforementioned embodiments. As long as the technical effects of the present invention are achieved by the same means, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present disclosure shall be included within the scope of protection of the present disclosure. Within the scope of protection of the present invention, its technical solutions and / or implementation methods may be modified and varied in various ways.

Claims

1. A UAV formation control method based on multi-agent distributed consensus, characterized by: The following steps are involved: Obtain obstacle coordinates, multiple preset formations, and drone status parameters; Performing target point allocation based on the current coordinates of the UAV in the state parameters and the coordinates of the target point in the preset formation to obtain a target allocation relationship between the UAV and the target point, and obtaining an alignment scale and a translation distance of the UAV based on the current coordinates of the UAV, the coordinates of the target point, and the target allocation relationship; Obtaining a relative state between the UAVs according to the alignment scale and the translation distance; A signal that no obstacle avoidance is required is obtained according to the obstacle coordinates, and a first trajectory of the UAV is obtained according to the relative states between the UAVs using a consistency algorithm; Obtaining an obstacle avoidance signal based on the obstacle coordinates, determining a degree of violation of the formation by the obstacle avoidance of the drone based on the current coordinates of the drone and the coordinates of the obstacle, selecting a formation with the least degree of violation among the preset formations as a first target formation when the degree of violation is greater than a preset violation threshold, and obtaining a second trajectory of the drone based on the first target formation; and when the degree of violation is less than or equal to the preset violation threshold, planning an obstacle avoidance trajectory of the drone to be avoided based on the obstacle coordinates while maintaining the current formation, thereby obtaining a third trajectory of the drone; When the coordinates of the end point are reached, a target trajectory is obtained according to the first trajectory, the second trajectory and the third trajectory.

2. The UAV formation control method based on multi-agent distributed consensus according to claim 1 is characterized in that: The target point allocation is performed according to the current coordinates of the UAV in the state parameters and the coordinates of the target point in the preset formation to obtain a target allocation relationship between the UAV and the target point, including: Solve the first optimization problem to allocate target points according to the current coordinates of the UAVs in the state parameters and the coordinates of the target points in the preset formation, and obtain a target allocation relationship between the UAVs and the target points. The first optimization problem is expressed as: in, is the distribution relationship between the UAV and the target point, pn i is the current coordinate of the i-th UAV, is the coordinate of the target point, s * To correspond The optimal alignment scale, d * To correspond The optimal translation distance.

3. The UAV formation control method based on multi-agent distributed consensus according to claim 2 is characterized in that: The method of obtaining the alignment scale and translation distance of the drone based on the current coordinates of the drone, the coordinates of the target point, and the target allocation relationship includes: Solve the second optimization problem to obtain the alignment scale and translation distance of the drone based on the current coordinates of the drone, the coordinates of the target point, and the target allocation relationship. The second optimization problem is expressed as: Among them, s is the current alignment scale, d is the current translation distance, is the target allocation relationship between the UAV and the target point, To correspond The position offset.

4. The UAV formation control method based on multi-agent distributed consensus according to claim 3 is characterized in that: The relative states between the drones are expressed as: ; Among them, α and β are adjustable parameters greater than 0, is the component of the relative state between the UAVs in the x, y and z directions, a ij are the elements of the adjacency matrix, is the component of the current coordinate of the i-th drone in the x, y and z directions, is the component of the current coordinate of the j-th UAV in the x, y and z directions, is the component of the current velocity of the i-th drone in the x, y and z directions, are the components of the current velocity of the j-th drone in the x, y, and z directions.

5. The UAV formation control method based on multi-agent distributed consensus according to claim 4 is characterized in that: The method of obtaining a first trajectory of the UAVs according to the relative states of the UAVs by using a consistency algorithm includes: Obtaining partial derivatives of reference velocities of the UAVs according to the relative states between the UAVs; Obtaining a reference speed of the UAV according to a partial derivative of the reference speed of the UAV; Obtaining a partial derivative of a reference position of the UAV according to a reference speed of the UAV; A first trajectory of the UAV is obtained according to the partial derivative of the reference position of the UAV.

6. The UAV formation control method based on multi-agent distributed consensus according to claim 1 is characterized in that: The method of obtaining the degree of violation of the formation by the drone's obstacle avoidance according to the drone's current coordinates and the obstacle coordinates includes: When the distance between the current coordinates of the drone and the coordinates of the obstacle is less than or equal to the preset distance threshold, the cosine value between the direction vector of the formation and the direction vector of the obstacle avoidance is calculated; The degree of violation of the formation by the UAV obstacle avoidance is obtained based on the current coordinates of the UAV and the coordinates of the obstacle, the expected change direction of the formation, and the cosine value between the direction vector of the formation and the direction vector of the obstacle avoidance; The degree of violation of the formation by the UAV obstacle avoidance is expressed as: Among them, δ is the degree of violation of the formation by the UAV obstacle avoidance, N is the total number of UAVs, γ i is the normalized representation of the cosine value between the formation direction vector and the obstacle avoidance direction vector, u i (t) is the expected change direction of the formation, x i (t) is the current coordinate of the UAV, is the distance between the current coordinates of the drone and the coordinates of the obstacle, d t is the preset distance threshold.

7. The UAV formation control method based on multi-agent distributed consensus according to claim 1 is characterized in that: The method of planning an obstacle avoidance trajectory of the UAV to be avoided according to the obstacle coordinates while maintaining the current formation to obtain a third trajectory of the UAV includes: Solve the third optimization problem to plan the obstacle avoidance trajectory of the UAV to be avoided according to the obstacle coordinates while maintaining the current formation, and obtain the third trajectory of the UAV. The third optimization problem is expressed as: Among them, α, β and γ are weight coefficients, a is the acceleration of the drone, g goal is the penalty term for the distance from the drone to the target point, g o is the penalty term for the distance from the drone to the obstacle.

8. The UAV formation control method based on multi-agent distributed consensus according to claim 7 is characterized in that: The penalty term for the distance from the drone to the target point is obtained based on the current speed of the drone, the current acceleration of the drone, the current coordinates of the drone, the coordinates of the target point, the coordinates of the target point with the smallest distance, and the sampling time. The penalty term for the distance from the drone to the obstacle is obtained based on the minimum distance between the drone and the obstacle, the current speed of the drone, the current acceleration of the drone, the current coordinates of the drone, and the sampling time.

9. A computer-readable storage medium, characterized in that Program instructions are stored, and when the program instructions are executed by a processor, a drone formation control method based on multi-agent distributed consistency as described in any one of claims 1 to 8 is implemented.

10. The UAV formation control system based on multi-agent distributed consensus is characterized by: include: A computer device comprising the computer-readable storage medium according to claim 9.

Images