UAV formation control method and system

Abstract

The present invention relates to a drone formation control method and system, and the control method includes the following steps: selecting a drone formation; for each drone, controlling the formation according to the angle formed by two or three drones adjacent to it, achieving expansion or contraction of the formation size by controlling the distance between any two drones, and planning to obtain the control input of each drone; and performing drone formation control according to the control input of each drone. The present invention is based on the local coordinate system of the drone, and senses the neighboring drones to obtain angle measurements, wherein only one drone measures the distance to its neighboring drones, the angle measurement is used to control the desired shape, and the distance measurement adjusts the size of the formation, and the flexible formation size enables the drone team to adapt to changes in corridor traffic and achieve the desired adjustment of formation translation and size.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) intelligent control, and in particular relates to a UAV formation control method and system. Background Art

[0002] For n drones moving in a plane, the index set of the drones is defined as The underlying control of each drone is determined by an outer position loop and an inner attitude loop. Since the trajectory dynamics of each drone has a larger time constant than the attitude dynamics, and the formation only considers position and velocity, the outer position loop is used to drive the drones to complete the formation control.

[0003] At the formation control level, each UAV can be regarded as a particle system, and the dynamics of the i-th UAV can be approximately described as:

[0004]

[0005] Among them, p i represents the position of UAV i in the fixed global coordinate system, is the speed, v i It's speed. is the acceleration, u i is the control input.

[0006] In order to achieve the desired formation control, the following assumptions are made for all UAVs:

[0007] Assumption 1: There is no communication between drones, and each drone is equipped with a sensor for measuring line of sight.

[0008] Assumption 2: The initial positions of the drones are independent and non-collinear;

[0009] According to Assumption 1, the formation does not need to meet any requirements on the communication topology. Each drone needs to measure the direction angle of its neighbors relative to its local coordinate system; Assumption 2 ensures that the formation avoids collisions and remains non-collinear when t>0.

[0010] Based on assumptions 1 and 2, each UAV i can be located in its local coordinate system relative to Measuring angle θ ij ∈[0,π], such as Figure 1 As shown, represents the neighbor set of drone i, and for different i, may be different; the local coordinate system follows the right-hand system; then, at x i -y i Angle α on the plane i It can be expressed as:

[0011] α i =θ ij -θ ik (2)

[0012] Among them, α i ∈(0,π) is the interior angle of the triangle formed by drones j, i and k.

[0013] like Figure 1 As shown, drone i can measure the line of sight angle of its neighbors. Then, the interior angle α i It can be calculated as follows:

[0014]

[0015] in, The unit vector representing the position of drone i pointing to the position of drone j, T is the transpose; The unit vector that represents the position of drone i pointing to the position of drone k.

[0016] set up is the desired steady-state angle, such as Figure 2 (a). The angle-based formation problem becomes to achieve the desired angle constraint:

[0017]

[0018] However, there is a problem with the constraint of (4): it cannot effectively control the size of the formation. Figure 2 (a) and Figure 2 The angles of the configurations in (b) all satisfy equation (4), but they cannot determine the distance between the UAVs. In other words, achieving the desired configuration may result in the UAV formation size being too large or too small. Summary of the invention

[0019] Based on the above-mentioned shortcomings and deficiencies in the prior art, an object of the present invention is to provide a UAV formation control method and system.

[0020] In order to achieve the above-mentioned object of the invention, the present invention adopts the following technical solutions:

[0021] A method for controlling a drone formation comprises the following steps:

[0022] S1. Select the formation of drones;

[0023] S2. For each UAV, control the formation according to the angle formed by two or three adjacent UAVs, expand or shrink the formation size by controlling the distance between any two UAVs, and plan to obtain the control input of each UAV;

[0024] S3. Perform UAV formation control based on the control input of each UAV.

[0025] 2. The UAV formation control method according to claim 1 is characterized in that, in step S1, the formation of the UAVs includes a triangular formation, a quadrilateral formation and a polygonal formation; wherein the number of sides of the polygon is greater than 4.

[0026] As a preferred solution, for the triangle formation, the corresponding control input is:

[0027]

[0028]

[0029]

[0030] Among them, u i is the control input of the i-th UAV in the triangle formation, i is 1, 2 or 3; is the expected acceleration of the triangle formation, v i is the current speed of the i-th UAV in the triangle formation, is the expected speed of the triangle formation, α i is the angle between the i-th UAV in the triangle formation and its two adjacent UAVs, is the expected angle between the ith UAV in the triangle formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector of the angle between the ith UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the ith UAV, and j and k∈[1,3] are integers; k1, k2, k3 are scalar gains, respectively; d 13 is the distance between the first UAV and the third UAV, is the expected distance between the first UAV and the third UAV.

[0031] As a preferred solution, for polygonal formation, based on the expansion of triangle formation, each UAV needs to measure two angles relative to three adjacent UAVs, and the corresponding control input is:

[0032]

[0033] Among them, u i is the control input of the ith UAV in the polygonal formation, v i is the current speed of the i-th UAV in the triangle formation, where i is an integer greater than 3; is the set of drones included in the polygonal formation; αjik is the angle between the i-th UAV in the polygonal formation and its adjacent j-th and k-th UAVs, is the expected angle formed by the i-th UAV in the triangle formation and its adjacent j-th UAV and k-th UAV.

[0034] As a preferred solution, for the quadrilateral formation, the corresponding control input is:

[0035]

[0036]

[0037]

[0038]

[0039] Among them, u i is the control input of the i-th UAV in the quadrilateral formation, i is 1, 2, 3 or 4; is the expected acceleration of the quadrilateral formation, v i is the current speed of the i-th UAV in the quadrilateral formation, is the expected speed of the quadrilateral formation, α i is the angle between the i-th UAV in the quadrilateral formation and its two adjacent UAVs, is the expected angle between the ith UAV in the quadrilateral formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector between the i-th UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the i-th UAV, and j and k∈[1,4] are integers; k1, k2, k3 are scalar gains, respectively; d 32 is the distance between the third UAV and its adjacent second UAV, is the expected distance between the third UAV and its adjacent second UAV; d 34 is the distance between the third UAV and its adjacent fourth UAV, is the expected distance between the third UAV and its adjacent fourth UAV, z 32 is the unit vector of the third drone pointing to the second drone, z 34 The unit vector pointing from the 3rd drone to the 4th drone.

[0040] The present invention also provides a UAV formation control system, comprising:

[0041] Selection module, used to select the formation of drones;

[0042] The planning module is used to control the formation of each drone according to the angle formed by two or three adjacent drones, expand or shrink the size of the formation by controlling the distance between any two drones, and plan to obtain the control input of each drone;

[0043] The control module is used to control the UAV formation according to the control input of each UAV.

[0044] As a preferred solution, the formations of the drones include triangular formations, quadrilateral formations and polygonal formations; wherein the number of sides of the polygon is greater than 4.

[0045] As a preferred solution, for the triangle formation, the corresponding control input is:

[0046]

[0047]

[0048]

[0049] Among them, u i is the control input of the i-th UAV in the triangle formation, i is 1, 2 or 3; is the expected acceleration of the triangle formation, v i is the current speed of the i-th UAV in the triangle formation, is the expected speed of the triangle formation, α i is the angle between the i-th UAV in the triangle formation and its two adjacent UAVs, is the expected angle between the ith UAV in the triangle formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector of the angle between the ith UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the ith UAV, and j and k∈[1,3] are integers; k1, k2, k3 are scalar gains, respectively; d 13 is the distance between the first UAV and the third UAV, is the expected distance between the first UAV and the third UAV.

[0050] As a preferred solution, for polygonal formation, based on the expansion of triangle formation, each UAV needs to measure two angles relative to three adjacent UAVs, and the corresponding control input is:

[0051]

[0052] Among them, u iis the control input of the ith UAV in the polygonal formation, v i is the current speed of the i-th UAV in the triangle formation, where i is an integer greater than 3; is the set of drones included in the polygonal formation; α jik is the angle between the i-th UAV in the polygonal formation and its adjacent j-th and k-th UAVs, is the expected angle formed by the i-th UAV in the triangle formation and its adjacent j-th UAV and k-th UAV.

[0053] As a preferred solution, for the quadrilateral formation, the corresponding control input is:

[0054]

[0055]

[0056]

[0057]

[0058] Among them, u i is the control input of the i-th UAV in the quadrilateral formation, i is 1, 2, 3 or 4; is the expected acceleration of the quadrilateral formation, v i is the current speed of the i-th UAV in the quadrilateral formation, is the expected speed of the quadrilateral formation, α i is the angle between the i-th UAV in the quadrilateral formation and its two adjacent UAVs, is the expected angle between the ith UAV in the quadrilateral formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector between the i-th UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the i-th UAV, and j and k∈[1,4] are integers; k1, k2, k3 are scalar gains, respectively; d 32 is the distance between the third UAV and its adjacent second UAV, is the expected distance between the third UAV and its adjacent second UAV; d 34 is the distance between the third UAV and its adjacent fourth UAV, is the expected distance between the third UAV and its adjacent fourth UAV, z 32 is the unit vector of the third drone pointing to the second drone, z 34 The unit vector pointing from the 3rd drone to the 4th drone.

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

[0060] The drone formation control method and system of the present invention are based on the local coordinate system of the drone and sense neighboring drones to obtain angle measurements, wherein only one drone measures the distance to its neighboring drones. The angle measurement is used to control the desired shape, while the distance measurement adjusts the size of the formation. The flexible formation size enables the drone team to adapt to changes in corridor traffic and achieve desired adjustment of formation translation and size. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 is a schematic diagram of angle measurement between a UAV of the present invention and its adjacent UAVs;

[0062] Figure 2 is a schematic diagram of uncertainty of the formation size based only on angle control of the present invention;

[0063] Figure 3 It is a schematic diagram of the comparison of the triangle formation movement strategy based on distance and angle of the present invention;

[0064] Figure 4 is a schematic diagram of the local direction of the drone of the present invention being inconsistent;

[0065] Figure 5 It is a schematic diagram of the scale of an extended formation team based on the expansion of a triangular formation to a polygonal formation of the present invention;

[0066] Figure 6 It is a schematic diagram of the uniqueness of the formation of the present invention;

[0067] Figure 7 is a schematic diagram of the uncertainty of the co-circular formation of the present invention;

[0068] Figure 8 is a schematic diagram of a square formation configuration of the present invention;

[0069] Fig. 9 is a schematic diagram of the motion direction caused by the distance dynamic error of the present invention;

[0070] Fig.10 It is a schematic diagram of the maneuvering trajectory of the triangular formation when encountering narrow terrain of the present invention;

[0071] Fig.11 It is a schematic diagram of the change in movement direction planning caused by the distance dynamic error of the triangle formation of the present invention;

[0072] Fig.12 It is a schematic diagram of the maneuvering trajectory of the polygonal formation when encountering narrow terrain of the present invention;

[0073] Fig.13 It is a schematic diagram of the change in movement direction planning caused by the distance dynamic error of the polygonal formation of the present invention;

[0074] Fig.14 is a schematic diagram of the maneuvering trajectory of a square formation when encountering narrow terrain according to the present invention;

[0075] Fig.15 It is a schematic diagram of the change in motion direction planning caused by the distance dynamic error of the square formation of the present invention. DETAILED DESCRIPTION

[0076] The technical solution of the present invention is explained in detail below.

[0077] As we all know, a triangle can be uniquely determined by its three corners and any one side. Therefore, for a triangle formation, the formation can be expanded and contracted by controlling the distance between any two drones while maintaining the angle constraint. In this case, the distance d between drone #1 and drone #3 is specifically chosen. 13 .like Figure 3 As shown in (a), when UAV #1 and UAV #2 remain stationary, d 13 will change, and drone #3 moves along the line where the angle bisector of ∠132 lies. Then, Figure 3 In (b), taking the formation contraction as an example, drone #3 moves inward along the angle bisector, resulting in d 13 decreases; however, to ensure the desired interior angle constraints of the triangle, UAV #1 and UAV #2 will also move along the angle bisectors of their respective angles. Therefore, the size of the entire UAV formation will decrease. In order to obtain d 13 , making the following assumptions:

[0078] Assumption 3: Drone #3 can measure the distance between itself and other drones, while other drones do not need to measure distance information.

[0079] Consider three UAVs with double integrator dynamics described by Eq. (1). The goal is to design the control input u for each UAV i as i , to facilitate the expansion and contraction of the formation while ensuring its stability; the results are then extended to n drones. The stability of the entire drone system during the formation maneuver can be described as follows:

[0080]

[0081] in, Having determined the desired formation shape, The desired speed of the formation is determined. represents the expected distance between UAVs i and j.

[0082] The following is an analysis of the UAV formation control input and the stability of the system, with detailed explanations for triangular configurations, polygonal configurations, and square configurations. First, the formation control input for the triangular configuration is planned, and then the angle rigidity theory is used to extend it to the formation control of n UAVs; finally, a formation control input scheme for a square formation is planned.

[0083] 1. Triangle Formation Control

[0084] Considering the desired triangle formation and size, the following formation control inputs are proposed for drones #1 to #3:

[0085]

[0086] Among them, u i is the control input of the i-th UAV in the triangle formation, i is 1, 2 or 3; is the expected acceleration of the triangle formation, v i is the current speed of the i-th UAV in the triangle formation, is the expected speed of the triangle formation, α i is the angle between the i-th UAV in the triangle formation and its two adjacent UAVs, is the expected angle between the ith UAV in the triangle formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector of the angle between the ith UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the ith UAV, and j and k∈[1,3] are integers; k1, k2, k3 are scalar gains, respectively; d 13 is the distance between drone #1 and drone #3, is the expected distance between UAV #1 and UAV #3.

[0087] Before introducing the stability analysis, a lemma is given first:

[0088] For a matrix of the form M:

[0089] Where I is the identity matrix and k is a positive constant. If the submatrix H is a Hurwitz matrix and its eigenvalue λ satisfies the inequality (k 2 Re[λ]+Im 2 [λ])<0, then the matrix M is also Hurwitz.

[0090] Proof: The characteristic polynomial of matrix M can be expressed as

[0091]

[0092] Assume λ1, λ2, ..., λ n are the n eigenvalues ​​of H. For any H, there always exists an n×n unitary matrix U such that

[0093]

[0094] Substituting equation (8) into |λ(λ+k)I n -H|, get

[0095] |λ(λ+k)I n -H|=(λ 2 +kλ-λ1)(λ 2 +kλ-λ2)L(λ 2 +kλ-λ n ) (9)

[0096] For any real eigenvalue λ i ,i∈1,2,...,n, since H is a Hurwitz matrix, λ i is a negative number. Therefore, the equation (λ 2 +kλ-λ i )=0 has a negative real part.

[0097] For any pair of conjugate complex eigenvalues, assuming that λ1 and λ2 are a pair of conjugate complex eigenvalues ​​of H, the polynomial (λ 2 +kλ-λ1)(λ 2 +kλ-λ2) can be expressed as:

[0098]

[0099] Then the Hurwitz determinant is

[0100]

[0101] in

[0102]

[0103] Since k>0 and Re[λ j ]<0, j∈1,2, so we have

[0104] λ1+λ2=2Re[λ j ]<0 (13)

[0105] λ1λ2=Re 2 [λ j ]+Im 2 [λ j ]>0 (14)

[0106] Here, we get that a4, a3, a2, a1, and a0 are all positive numbers. Now, considering the order of principal minors of H1, we have

[0107] △1=a3=2k>0 (15)

[0108]

[0109]

[0110] △4=a0△3 (18)

[0111] According to formula (17), if there is only (k 2 Re[λ j ]+Im 2 [λ j ])<0, where j∈1,2, then the roots of the quartic polynomial in equation (10) have negative real parts. Therefore, if there is only (k 2 Re[λ i ]+Im 2 [λ i ])<0, where i∈1,2,...,n, then the roots of polynomial (9) have negative real parts. So far, the lemma has been proved.

[0112] Below is the theorem for triangle formation.

[0113] Theorem 1: Consider a group of three drones given by equation (1), under assumptions 1-3. If the initial error and For i=1,2,3, Close to the equilibrium point, the formation control input (6) ensures and It converges asymptotically as t→+∞.

[0114] Proof: First, due to Combined with equation (3), the dynamics of the internal angle (i.e., included angle) of each drone can be expressed as

[0115]

[0116] in

[0117] definition Then the angular error dynamics are as follows

[0118]

[0119] in, For all j,i,k∈1,2,3.

[0120] Secondly, The velocity error dynamics can be derived from (6),

[0121]

[0122] Furthermore, since α1+α2+α3=π, the velocity error dynamics of drone #3 can be rewritten as

[0123]

[0124] Finally, the dynamic error of relative distance is analyzed. have

[0125]

[0126]

[0127] Define the state variables of the system:

[0128]

[0129] According to equations (19)-(24), we have the following closed-loop system:

[0130]

[0131]

[0132]

[0133] Then, R1 and R2 are linearized near the equilibrium point X = 0 to analyze the stability of the system, and we get

[0134]

[0135] in, is the linearization of the system matrix around the origin. For convenience, the notation represents the value of the variable at X = 0. Next, by analyzing The eigenvalues ​​of prove that system (29) is stable, and prove that is a Hurwitz matrix. Let λ be An eigenvalue of , then the characteristic equation is as follows

[0136]

[0137] According to formula (30), it is found that -k1 is The triple characteristic root of , and -k1<0. Then, the analysis matrix The eigenvalue of Then we have:

[0138]

[0139] in, yes The i-th column of , i∈1,2,3. By using YesN jik z ik = 0 and N jik z ki =0; then it can be simplified to:

[0140]

[0141]

[0142]

[0143] Further simplification can be achieved by using get:

[0144]

[0145] in, and

[0146] Let p = 1 + cos α3, q = cos α3 - cos α1, then equation (35) can be rewritten as:

[0147]

[0148] Next, let λ1 be An eigenvalue of , then

[0149]

[0150] in,

[0151]

[0152] Now, we focus on the coefficients of the roots of the cubic polynomial of (37) by using the Routh-Hurwitz criterion. Considering p>0, pq>0 and g1g2-f 12 f 21 >0, we can get a3, a2, a1 and a0 are all positive numbers. Then, we get the Hurwitz determinant

[0153]

[0154] get:

[0155] △1=a2>0 (40)

[0156]

[0157] △3 = a0△2 > 0 (42) Calculate the inequality (41) in the appendix. By checking the above Routh - Hurwitz conditions, it is obtained that all the characteristic roots of equation (37) have negative real parts. Therefore, is Hurwitz.

[0158] Finally, according to the lemma, is Hurwitz, and thus it can be concluded that when the initial error approaches the equilibrium point, the UAV formation system has exponential stability.

[0159] Remark 1: It is worth pointing out that for the proposed control input, the number of sensors measuring local information is minimized.

[0160] Remark 2: Given that Figure 4 , note that there may be misalignments in the local directions of each UAV. Even if a UAV loses its global sense of direction in some complex environments, it will not affect the completion of the formation. The misalignment of local directions does not affect the measurement of the interior angle α i and the distance d ij . Regardless of the direction, the formation movement of the UAVs always heads towards the angle bisector z i of the interior angle α ij + z ik , and the direction of z ij + z ik remains unchanged.

[0161] II. Polygonal Formation Control

[0162] Consider a formation consisting of n UAVs and expand the scale of the formation by the method shown in Figure 5 . For UAV i, i > 3, each UAV needs to measure two angles and relative to three adjacent UAVs j1, j2, j3 < i where there is an angle set

[0163] Assumption 4: When expanding the scale of the formation using the above method, it is assumed that the desired positions of UAVs i > 3, UAV j1, UAV j2, and UAV j3 are not on a circle, and are not zero or π.

[0164] Remark 3: When Assumption 4 holds and the formation configuration is constructed in the manner shown in Figure 5 , the formation has angular stiffness.

[0165] The formation control input planning is:

[0166]

[0167] Where k1 and k2 are positive scalar gains, and k3 is a positive constant, which is a positive constant if i=3 and zero otherwise.

[0168] Theorem 2: Assume that there exists a set of n UAVs with dual-integrator dynamics, whose dynamics are given by equation (1), under the conditions of assumptions 1-4. If the initial error and are close to the equilibrium point, then the formation control input formula (43) ensures that when t→+∞, and converges asymptotically.

[0169] The system state variables are defined as

[0170]

[0171] After the previous preparation, the closed-loop dynamics are given directly.

[0172]

[0173]

[0174]

[0175] Then, linearizing S1 and S2 to approximate the equilibrium point X = 0 to analyze the stability of the system, we get:

[0176]

[0177] The characteristic equation obtained is as follows:

[0178]

[0179] And in formula (49)

[0180]

[0181] Among them, the matrix is a block lower triangular matrix.

[0182]

[0183]

[0184]

[0185]

[0186]

[0187] Let λ1 and λ2 be The two eigenvalues ​​of .

[0188]

[0189] get is Hurwitz. Then, substituting (50) into (49), the characteristic polynomial can be rewritten as:

[0190]

[0191] because It is Hurwitz. Theorem 2 is directly proved according to the lemma.

[0192] Remark 4: It should be noted that there is angular rigidity under Assumption 4. However, the size of the formation is not rigid. Below we explain why the change in the distance between a pair of drones alone can determine the size of the entire formation.

[0193] like Figure 2 As shown, according to the sine theorem

[0194]

[0195] Where R represents the radius of the circumscribed circle of Δ123. 13 shrink times, the radius of the circumscribed circle will also shrink by k times.

[0196] Therefore, since a chord and an angle can uniquely determine a circumcircle, when the angle is determined, the radius of the circumcircle depends only on the length of the chord. like Figure 6 (a) and Figure 6 (b) Therefore, the size of the formation of four UAVs can be completely determined by the distances on both sides of the first triangle Δ123.

[0197] According to mathematical induction, we can deduce n drones.

[0198] 3. Square Formation Control

[0199] The following discusses the square formation control of four UAVs in a circle:

[0200] like Figure 6 As shown in (a), the expected formation positions of the fourth UAV and the first three UAVs are not on the same circle, which is consistent with Assumption 4. This constitutes the condition for the validity of the angular stiffness theory. Therefore, the expected position of the fourth UAV is at α 142and α 243 The constraints are unique. When the four drones are on a circle, that is, when Figure 6 When the red circle and the blue circle in the figure coincide, the formation configuration satisfies the expected angle and distance. However, this configuration is not unique. Figure 7 shown.

[0201] Drone #4 measures the sight angles with drones #1, #2, and #3 respectively to form angle constraints, where like Figure 7 (a). Obviously, the position of UAV #4 is not uniquely determined, because UAV #4 can move arbitrarily along arc 143, as shown in Figure 7 (b) as shown.

[0202] exist Figure 8 In the configuration shown in (a), when UAV #3 only measures d 32 When 34 There may be ambiguity. To solve the above problem, the constraints can be changed, such as Figure 8 (b) UAV #1, UAV #2, UAV #3 and UAV #4 measure the inner angles α1, α2, α3 and α4 with their adjacent UAVs respectively. Then UAV #3 measures the distance d 32 and d 34 . Therefore, the problem can be stated as follows:

[0203]

[0204] The square formation control input for drones #1 to #4 is:

[0205]

[0206] Theorem 3: Consider a group of four drones whose double integrator dynamics are given by (1), which satisfies Assumptions 1-3. If the initial error and For i=1,2,3,4, Close to the equilibrium point, the square formation control input (56) ensures and It converges asymptotically when t→+∞.

[0207] Proof: Using the control input (56), the angular dynamic error of each drone can be expressed as

[0208]

[0209] The velocity dynamic error is given as follows:

[0210]

[0211] The range dynamic error is given as follows:

[0212]

[0213] Since α1+α2+α3+α4=2π, the system state variables are defined as:

[0214]

[0215] Then we get the closed-loop system linearized at the equilibrium point as follows:

[0216]

[0217]

[0218] Therefore, the characteristic equation can be obtained:

[0219]

[0220] in It is given by equation (64). Let The determinant |λ(λ+k1)I5-H| is equivalent to the matrix The relevant characteristic equation. Therefore, by the lemma, if H is Hurwitz, then It is also Hurwitz's method that makes the drone formation system exponentially stable.

[0221]

[0222] Note 5: Note that in the control input (56), the direction of motion of the convergent part of the range error has changed. In the previous control input (6), the direction of motion of the range dynamic error is the angle bisector z ij +z ik , that is, the dynamic error of the distance to z ij and z ik are equal in weight. Here, z ij and z ik The weights on become the respective distance dynamic errors. Fig. 9 As shown, the direction of movement becomes in

[0223] 4. Simulation test

[0224] The embodiment of the present invention uses a simplified model of a quadcopter to verify the effectiveness of the above control input.

[0225] All quadcopters move in the XY plane, and the altitude and yaw angle of each quadcopter are kept constant. The outer loop dynamics is controlled by the proposed formation controller, while the attitude loop is controlled by the PD controller.

[0226] (1) Triangle Formation Maneuvers

[0227] The initial formation of the three quadcopters is an isosceles right triangle, where the initial position coordinates and velocity are chosen to be p1(0) = [0,0] T m,p2(0)=[4,0] T m,p3(0)=[0,4] T m,v1(0)=[0,0] T m / s,v2(0)=[0,0] T m / s,v3(0)=[0,0] T m / s. Then, the desired formation configuration is set as an equilateral triangle, that is,

[0228] Applying the control input formula (6), the formation maneuver simulation results are as follows: Fig.10 As shown. The selected control gains are k1 = 0.2, k2 = 0.1, k3 = 0.02. The desired translation speed is selected as Expected distance Given by formula (65)

[0229]

[0230] exist Fig.10 In the figure, the dotted lines depict the trajectory of the quadcopter. First, it shows the evolution from the initial formation to an equilateral triangle formation of the desired size. Then, when the formation flies over narrow terrain, the desired distance is changed. Enabled the formation to maintain an equilateral triangle formation through terrain. Finally, restored the size of the formation after passing through terrain. Fig.11 (a) and Fig.11 (b) shows the evolution trajectory of angle error and distance error during the entire formation flight process. Fig.11 (c) shows the evolution of the actual distance. The simulation results show the stability of the control input.

[0231] (2) Polygonal Formation Maneuvers

[0232] Consider the desired formation configuration of six quadcopters. The initial configuration of the formation is given arbitrarily. The initial position coordinates and velocities are chosen to be p1(0) = [0,0] T m,p2(0)=[2,2] T m,p3(0)=[-1,4] Tm,p4(0)=[5,3] T m,p5(0)=[4,0] T m,p6(0)=[4,5] T m,v1(0)=[0,0] T m / s,v2(0)=[0,0] T m / s,v3(0)=[0,0] T m / s,v4(0)=[0,0] T m / s,v5(0)=[0,0] T m / s,v6(0)=[0,0] T m / s. Then, let the desired formation be a square, that is,

[0233] By applying the control input formula (43), the formation maneuver simulation results are as follows: Fig.12 As shown. The control gains are selected as k1 = 0.3, k2 = 0.4, k3 = 0.03. The desired translation speed is selected as Expected distance It is given by formula (65).

[0234] Fig.13 (a) and Fig.13 (b) shows the evolution trajectory of angle error and distance error. Fig.13 (c) shows the evolution of the actual distance.

[0235] (3) Square Formation Maneuvers

[0236] The desired formation configuration of four quadcopters in a circle is considered. The initial configuration of the formation is given arbitrarily. The initial position coordinates and velocity are chosen to be p1(0) = [0,0] T m,p2(0)=[4,0] T m,p3(0)=[6,2] T m,p4(0)=[0,4] T m,v1(0)=[0,0] T m / s,v2(0)=[0,0] T m / s,v3(0)=[0,0] T m / s,v4(0)=[0,0] T m / s. Then, let the desired formation configuration be a square, i.e.

[0237] Applying the control input formula (56), the simulation results of the formation maneuver are as follows: Fig.14As shown. The control gains are selected as k1 = 0.5, k2 = 0.5, k3 = 0.1. The desired translation speed is selected as Desired distance and Given as follows:

[0238]

[0239] Fig.15 (a) and Fig.15 (b) shows the evolution trajectory of angle error and distance error. Fig.15 (c) shows the evolution of the actual distance.

[0240] from Fig.10 , Fig.12 and Fig.14 It can be seen that after deformation and expansion, the direction of the entire formation relative to the global coordinate system is uncertain, precisely because the embodiment of the present invention uses local information. However, overall, the scaling goals of deformation and formation have been achieved.

[0241] In summary, the embodiments of the present invention study the formation control problem in the UAV system and design a control input that uses only local information to adjust the deformation and formation scaling. This method effectively solves the ambiguity when the formation size is controlled only by angle information. For the triangular formation configuration, the desired shape is determined by the three UAVs achieving the desired angle, and the formation scaling is determined by the distance between any two UAVs. Moreover, the applicability of the control input is extended from the triangular formation to the polygonal formation, and the angle rigidity theory is used to determine the formation shape and the distance between any pair of UAVs to achieve the overall scaling of the formation. This design minimizes the number of sensors required for local information measurement. In addition, the embodiments of the present invention also perform a square formation configuration, in which each UAV measures an internal angle, and any UAV measures the distance between itself and the other two adjacent UAVs. The results show that the UAV formation can flexibly pass through narrow terrain.

[0242] V. Appendix

[0243] Referring to inequality (41), substituting a3, a2, a1 and a0 into inequality (41) and rearranging them, we have:

[0244]

[0245] In equation (66), for every term with a negative sign, a term with a positive sign can be found, and by continuing to use p>0, pq>0 and g1g2-f 12 f 21 >0 ensures that their addition is positive, except for the last term, which has an undefined sign.

[0246] Now analyze the positivity or negativity of the last term. Consider The following cases determine the positivity of Equation (66).

[0247] Case 1: If d 13 < d 12 < d 23 or d 12 < d 13 < d 23 or d 12 < d 23 < d 13 ; then: α3 < α1 and cosα3 > cosα1. Therefore, q > 0, and then △2 > 0.

[0248] Case 2: If d 13 < d 23 < d 12 or d 23 < d 13 < d 12 ; then:

[0249] α3 > α1 and cosα3 < cosα1, then q < 0. But by using the sine theorem (54), there is:

[0250]

[0251] - f 13 f 23 q > - f 12 f 23 q

[0252] - f 13 f 23 q + f 12 f 23 q > 0

[0253]

[0254] Therefore, △2 > 0.

[0255] Case 3: If d 23 < d 12 < d 13 ; then:

[0256]

[0257] There are two cases:

[0258] When α1, α2, and α3 are all acute angles, there is

[0259] p + q = 1 + cosα3 + cosα3 - cosα1 > 0

[0260] When α2 is an obtuse angle and α1 and α3 are acute angles, we also have

[0261] p+q>0

[0262] get:

[0263] f 12 f 23 p+f 12 f 23 q>0

[0264] f 32 f 23 p+f 12 f 23 q>0

[0265] By using g2=f 32 +f 12 ,have:

[0266]

[0267] We get △2>0.

[0268] Therefore, it is proved that △2>0 is true.

[0269] The above description is only a detailed description of the preferred embodiments and principles of the present invention. For ordinary technicians in this field, according to the ideas provided by the present invention, there will be changes in the specific implementation methods, and these changes should also be regarded as the protection scope of the present invention.

Claims

1. A method for controlling a drone formation, characterized in that: The following steps are involved: S1. Select the formation of drones; S2. For each UAV, control the formation according to the angle formed by two or three adjacent UAVs, expand or shrink the formation size by controlling the distance between any two UAVs, and plan to obtain the control input of each UAV; S3, performing UAV formation control according to the control input of each UAV; In step S1, the formation of the drones includes a triangle formation, a quadrilateral formation and a polygonal formation; wherein the number of sides of the polygon is greater than 4; For the triangle formation, the corresponding control input is: Among them, u i is the control input of the i-th UAV in the triangle formation, i is 1, 2 or 3; is the expected acceleration of the triangle formation, v i is the current speed of the i-th UAV in the triangle formation, is the expected speed of the triangle formation, α i is the angle between the i-th UAV in the triangle formation and its two adjacent UAVs, is the expected angle between the ith UAV in the triangle formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector of the angle between the ith UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the ith UAV, and j and k∈[1,3] are integers; k1, k2, k3 are scalar gains, respectively; d 13 is the distance between the first UAV and the third UAV, is the expected distance between the first UAV and the third UAV; For polygonal formation, based on the triangle formation, each UAV needs to measure two angles relative to three adjacent UAVs, and the corresponding control input is: Among them, u i is the control input of the ith UAV in the polygonal formation, v i is the current speed of the i-th UAV in the triangle formation, where i is an integer greater than 3; is the set of drones included in the polygonal formation; α jik is the angle between the i-th UAV in the polygonal formation and its adjacent j-th and k-th UAVs, is the expected angle formed by the i-th UAV in the triangle formation and its adjacent j-th UAV and k-th UAV; For the quadrilateral formation, the corresponding control input is: Among them, u i is the control input of the i-th UAV in the quadrilateral formation, i is 1, 2, 3 or 4; is the expected acceleration of the quadrilateral formation, v i is the current speed of the i-th UAV in the quadrilateral formation, is the expected speed of the quadrilateral formation, α i is the angle between the i-th UAV in the quadrilateral formation and its two adjacent UAVs, is the expected angle between the ith UAV in the quadrilateral formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector between the i-th UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the i-th UAV, and j and k∈[1,4] are integers; k1, k2, k3 are scalar gains, respectively; d 32 is the distance between the third UAV and its adjacent second UAV, is the expected distance between the third UAV and its adjacent second UAV; d 34 is the distance between the third UAV and its adjacent fourth UAV, is the expected distance between the third UAV and its adjacent fourth UAV, z 32 is the unit vector of the third drone pointing to the second drone, z 34 The unit vector pointing from the 3rd drone to the 4th drone.

2. A UAV formation control system, characterized in that: include: Selection module, used to select the formation of drones; The planning module is used to control the formation of each drone according to the angle formed by two or three adjacent drones, expand or shrink the size of the formation by controlling the distance between any two drones, and plan to obtain the control input of each drone; A control module for controlling the formation of drones based on the control input of each drone; The formations of the drones include triangular formations, quadrilateral formations and polygonal formations; wherein the number of sides of the polygon is greater than 4; For the triangle formation, the corresponding control input is: Among them, u i is the control input of the i-th UAV in the triangle formation, i is 1, 2 or 3; is the expected acceleration of the triangle formation, v i is the current speed of the i-th UAV in the triangle formation, is the expected speed of the triangle formation, α i is the angle between the i-th UAV in the triangle formation and its two adjacent UAVs, is the expected angle between the ith UAV in the triangle formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector of the angle between the ith UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the ith UAV, and j and k∈[1,3] are integers; k1, k2, k3 are scalar gains, respectively; d 13 is the distance between the first UAV and the third UAV, is the expected distance between the first UAV and the third UAV; For polygonal formation, based on the triangle formation, each UAV needs to measure two angles relative to three adjacent UAVs, and the corresponding control input is: Among them, u i is the control input of the ith UAV in the polygonal formation, v i is the current speed of the i-th UAV in the triangle formation, where i is an integer greater than 3; is the set of drones included in the polygonal formation; α jik is the angle between the i-th UAV in the polygonal formation and its adjacent j-th and k-th UAVs, is the expected angle formed by the i-th UAV in the triangle formation and its adjacent j-th UAV and k-th UAV; For the quadrilateral formation, the corresponding control input is: Among them, u i is the control input of the i-th UAV in the quadrilateral formation, i is 1, 2, 3 or 4; is the expected acceleration of the quadrilateral formation, v i is the current speed of the i-th UAV in the quadrilateral formation, is the expected speed of the quadrilateral formation, α i is the angle between the i-th UAV in the quadrilateral formation and its two adjacent UAVs, is the expected angle between the ith UAV in the quadrilateral formation and its two adjacent UAVs; z ij +z ik is the unit vector of the angle bisector between the i-th UAV in the triangle formation and its two adjacent UAVs, j and k are the numbers of the two UAVs adjacent to the i-th UAV, and j and k∈[1,4] are integers; k1, k2, k3 are scalar gains, respectively; d 32 is the distance between the third UAV and its adjacent second UAV, is the expected distance between the third UAV and its adjacent second UAV; d 34 is the distance between the third UAV and its adjacent fourth UAV, is the expected distance between the third UAV and its adjacent fourth UAV, z 32 is the unit vector of the third drone pointing to the second drone, z 34 The unit vector pointing from the 3rd drone to the 4th drone.

Images