A formation method for hovering UAVs based on intersection trajectory replanning

Abstract

The present invention relates to a formation method of a hovering drone formation based on intersection trajectory replanning, comprising: S1, initialization setting; S2, all non-leader drones send their respective current positions to the leader drone; S3, the leader drone determines whether the current positions sent by all non-leader drones are received within a first preset time period; if so, an initial expected point is allocated to each drone, a corresponding initial maneuvering trajectory is generated, and the initial maneuvering trajectory is replanned to obtain a corresponding optimized trajectory; S4, the leader drone sends the optimized trajectory to the corresponding drone; S5, each drone maneuvers according to the optimized trajectory, and informs the leader drone of the completion of the maneuver after reaching the corresponding target expected point; S6, the leader drone determines whether all drones complete the maneuver within a second preset time period. The present invention can ensure that no collision occurs during maneuvering.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method for forming a hovering unmanned aerial vehicle formation based on intersection trajectory replanning. Background Art

[0002] UAV (Unmanned Aerial Vehicle) plays an increasingly important role in the civil and military fields. However, the individual capabilities of a single UAV are limited and cannot complete complex tasks. UAV formations, due to their functional redundancy and spatial distribution, can enhance their operational capabilities and improve their operational efficiency through mutual collaboration to complete complex tasks. UAV formations need to maintain a certain formation when performing tasks in order to complete the tasks safely and efficiently. The ability of UAV formations to safely and autonomously form the desired formation that needs to be maintained has become one of the key technologies in UAV formation technology. The formation of UAV formation formations refers to the process in which UAV formations form a certain desired geometric formation from an initial randomly distributed, disordered geometric form through information interaction, autonomous planning, and coordinated maneuvers.

[0003] The commonly used method in the study of formation formation is the artificial potential field method, which is to construct a potential field function and then complete the formation by studying the feedback control law. In addition, there is also a method that allows UAVs to autonomously switch control laws to achieve the desired formation. It can also be studied on the graphical theory framework to complete the desired formation, and the rolling time domain method can be used to complete the desired formation. However, the above methods require real-time planning of UAVs, a large amount of information transmission between UAV formation individuals, and as the size of the formation increases, the requirements for communication are getting higher and higher. In this regard, it is particularly important to find a simple and easy-to-implement method to enable UAV formations to autonomously form the desired formation. Summary of the invention

[0004] In order to solve the above technical problems or at least partially solve the above technical problems, the present invention provides a method for forming a formation of a hovering UAV based on intersection trajectory replanning.

[0005] The present invention provides a method for forming a hovering UAV formation based on intersection trajectory replanning, comprising:

[0006] S1. Set the leadership identity of the drone formation and the positions of each expected point in the expected formation, and send the positions of each expected point to the drone with the leadership identity; control all drones to take off and maintain fixed point and orientation, and control each drone to confirm whether it is the leader;

[0007] S2. All non-leader drones send their current locations to the leader drone;

[0008] S3, the UAV with the leadership identity determines whether it has received the current position sent by all the UAVs without the leadership identity within the first preset time period; if so, the UAV with the leadership identity allocates an initial expected point to each UAV, generates a corresponding initial maneuvering trajectory according to the current position of each UAV and the position of the corresponding initial expected point, and replans the initial maneuvering trajectory based on the intersection of each initial maneuvering trajectory to obtain the optimized trajectory corresponding to each UAV; the optimized trajectories of each UAV do not have an intersection;

[0009] S4, the leading drone sends the optimized trajectories corresponding to the respective drones to the corresponding drones;

[0010] S5, each UAV performs maneuvers according to its corresponding optimized trajectory, and after each UAV reaches the corresponding re-planned target expected point, it notifies the UAV with the leadership identity of the completion of the maneuver;

[0011] S6. The leading drone determines whether all drones have completed the maneuver within the second preset time period. If so, a message of successful formation is sent to each drone to enable each drone to maintain a fixed point and orientation, and the formation is completed.

[0012] The method for forming a formation of a hovering UAV based on intersection trajectory replanning provided in this embodiment is first initialized, and then the UAV with a leading identity allocates an initial expected point to each UAV, and then obtains a corresponding initial maneuvering trajectory based on the current position and the initial expected point, and then performs collision planning on each initial maneuvering trajectory based on the intersection to obtain an optimized trajectory, and the optimized trajectory has no intersection, thereby avoiding the occurrence of collisions, and then each optimized trajectory is sent to the corresponding UAV, and the UAV maneuvers according to the optimized trajectory. After reaching the target expected point, it informs the UAV with a leading identity, and then the UAV with a leading identity informs each UAV to maintain a fixed point orientation after determining that the formation is successfully formed. The algorithm of the present invention is simple in design, the control strategy is easy to design, and the UAV track planning speed is fast; and each initial maneuvering trajectory is re-planned based on the intersection to obtain an optimized trajectory. The optimized trajectory can ensure that there is no intersection, so no collision occurs during maneuvering, ensuring that the UAV formation completes the formation task safely and without collision; in addition, according to the characteristics of the hovering UAV being flexible and turning without requiring a large range of rotation, the present invention adopts a method of only one planning, only one optimization, and only one maneuvering, so that there is no staged maneuvering and hovering waiting process during the navigation process of the UAV formation, and the formation formation task can be completed by flying according to the respective planned navigation trajectories, thereby improving the completion efficiency of the formation formation task; in addition, in the present invention, the amount of information exchange between UAV formations is small, the calculation is simple, the planning speed is fast, the coordinated maneuvering logic is clear, and it is easy to implement in engineering. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0014] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0015] Figure 1 It is a flow chart of a method for forming a hovering UAV formation based on intersection trajectory replanning in an embodiment of the present invention;

[0016] Figure 2 Schematic diagram of the change process of four UAVs from an initial state to a desired formation in an embodiment of the present invention;

[0017] Figure 3 is a schematic diagram of a desired rectangular formation in an embodiment of the present invention;

[0018] Figure 4 Schematic diagram of initial desired points and initial maneuvering trajectories corresponding to four UAVs in an embodiment of the present invention;

[0019] Figure 5 A schematic diagram of an approaching turning point and an exiting turning point of an intersection in an embodiment of the present invention;

[0020] Figure 6 is a schematic diagram of all inflection points corresponding to each intersection point in an embodiment of the present invention;

[0021] Figure 7 is a schematic diagram of each optimized trajectory after replanning in an embodiment of the present invention;

[0022] Figure 8 It is a flow chart of a method for forming a formation of hovering UAVs based on intersection trajectory replanning in an embodiment of the present invention. DETAILED DESCRIPTION

[0023] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0024] In a first aspect, an embodiment of the present invention provides a method for forming a hovering UAV formation based on intersection trajectory replanning, such as Figure 1 and 8 As shown, the method includes:

[0025] S1. Set the leadership identity of the drone formation and the positions of each expected point in the expected formation, and send the positions of each expected point to the drone with the leadership identity; control all drones to take off and maintain fixed point and orientation, and control each drone to confirm whether it is the leader;

[0026] For example, Figure 2 As shown in the figure, a UAV formation consisting of 4 UAV members is introduced. In the initial state, the geometry of the UAV formation is randomly distributed and disordered, and each UAV is in a standby state, waiting for the formation formation start instruction. The expected geometric formation is Figure 2 When the formation begins, all UAVs maintain fixed points and orientations. After information exchange, autonomous planning, and coordinated maneuvers between the UAV formations, they form the desired rectangular formation without any collisions. Figure 3As shown, the desired geometric formation is a rectangle, and the desired points 1, 2, 3, and 4 form the desired rectangular formation. After the UAV formation forms a rectangular formation, each UAV is located at a desired point.

[0027] It is understandable that initialization is performed in S1, the leader identity instruction of the UAV formation is set, and the desired formation instruction is set, that is, the position information of each desired point m is the number of each expected point, m=1,2,···,N num_UAV Of course, some formation parameters can also be included, for example, l UAV 、N num_UAV , l UAV is the length of UAV, N num_UAV is the number of UAVs.

[0028] It is understandable that step S1 can be executed by the controller. After initialization, the relevant setting information is sent to the leader, that is, the drone with leadership status, and each drone is controlled to start taking off and hover in the air to maintain a fixed point and orientation. Only after the leader sends the corresponding optimized trajectory to each drone, each drone starts to maneuver.

[0029] It is understandable that the following process does not require controller parameters and is automatically executed by the drone.

[0030] S2. All non-leader drones send their current locations to the leader drone;

[0031] That is to say, after each drone is launched, through communication between drones, each drone can determine whether it is the leader and which drone is the leader, and then send its current location to the leader. When each non-leader drone sends its current location to the leader, the leader can know the current location of all drones: Where i is the number corresponding to each drone, i = 1, 2, ···, N num_UAV .

[0032] S3, the UAV with the leadership identity determines whether it has received the current position sent by all the UAVs without the leadership identity within the first preset time period; if so, the UAV with the leadership identity allocates an initial expected point to each UAV, generates a corresponding initial maneuvering trajectory according to the current position of each UAV and the position of the corresponding initial expected point, and replans the initial maneuvering trajectory based on the intersection of each initial maneuvering trajectory to obtain the optimized trajectory corresponding to each UAV; the optimized trajectories of each UAV do not have an intersection;

[0033] That is, the leader first determines whether it has received the current positions sent by all non-leader drones within a certain period of time (for example, 60 seconds). If so, it proceeds to the next step, otherwise it considers that the formation has failed. After receiving the current positions of all non-leaders, the leader assigns an initial expected point to each drone (including the leader). The result of the assignment is that each drone obtains a corresponding expected point.

[0034] In specific implementation, the process of the leader assigning initial expected points to each drone (including the leader) may include:

[0035] a. Determine the corresponding objective function based on the principle of shortest distance and least intersection points;

[0036] b. Solve the objective function according to the particle swarm optimization algorithm to obtain the initial expected points corresponding to each UAV.

[0037] That is to say, when allocating the initial expected points, the principle of shortest distance and least intersection is considered, and the optimization goal of the objective function is to minimize distance and least intersection. By solving the objective function (also called fitness function) through the particle swarm optimization algorithm, the initial expected points that meet the above optimization goals can be obtained.

[0038] In the specific implementation, the initial expected points corresponding to each UAV obtained by solving the particle swarm optimization algorithm meet the first condition and the second condition;

[0039] (1) The first condition includes: the initial expected points corresponding to any two drones are different; that is, PS0 ID (i)≠PS0 ID (j),i≠j,i=1,2,···,N num_UAV , j=1,2,···,N num_UAV .PS0 ID (i) The label of the initial desired point assigned to the i-th UAV.

[0040] (2) The second condition includes: if the slopes of the initial maneuvering trajectories of any two UAVs are the same or the slopes do not exist, then one of the first and second conditions must be true;

[0041] The first situation is:

[0042] (x UAV (j)-x UAV (i))(x Aim (PS0 ID (j))-x Aim (PS0 ID(i)))+ (y UAV (j)-y UAV (i))(y Aim (PS0 ID (j))-y Aim (PS0 ID (i)))>0

[0043] The second case is: using the rapid rejection test, at least one of the following four inequalities holds true:

[0044]

[0045] Where, PS0 ID (i) is the number of the initial expected point corresponding to the i-th UAV, PS0 ID (j) is the number of the initial expected point corresponding to the jth UAV; is the current position of the ith UAV, is the position of the initial expected point corresponding to the i-th UAV, is the current position of the jth UAV, is the position of the initial expected point corresponding to the jth UAV.

[0046] Among them, max represents the operation of taking the maximum value, and min represents the operation of taking the minimum value.

[0047] The initial maneuvering trajectory of any two UAVs includes a first line segment and a second line segment, the first line segment is the line segment between the initial position of the ith UAV and the position of the initial expected point, and the second line segment is the line segment between the initial position of the jth UAV and the position of the initial expected point.

[0048] In a specific implementation, if any two drones satisfy the first formula, it can be considered that the slopes of the initial maneuvering trajectories of the two drones are the same or the slopes do not exist, that is, the slopes of the first line segment and the second line segment are the same or the slopes do not exist. The first formula includes:

[0049]

[0050] For example, after the above steps, the initial expected points are assigned to 4 drones. The initial expected points of the 4 drones can be found in Figure 4 In the figure, the position of the light-colored UAV indicates the initial allocation position, and the position of the dark-colored UAV indicates the initial position, that is, the current position. Figure 4 As shown, UAV No. 1 is assigned to the desired point 1, UAV No. 2 is assigned to the desired point 2, UAV No. 3 is assigned to the desired point 4, and UAV No. 4 is assigned to the desired point 3, that is, PS0 ID (1) = 1, PS0ID (2) = 2, PS0 ID (3) = 4, PS0 ID (4) = 3. The result of the initial allocation ensures that each UAV is allocated to a desired point, and the desired points of each UAV are different.

[0051] from Figure 4 It can be seen from the figure that after allocating the initial expected points, the initial maneuvering trajectory is the line segment between the current position and the corresponding initial expected point. There is a high probability that there will be intersections between the initial maneuvering trajectories of each UAV. If there are intersections, each UAV may collide during maneuvering. In order to avoid collisions, the initial maneuvering trajectory needs to be adjusted. The adjustment process is based on each intersection. After the adjustment, the optimized trajectory is obtained. There is no intersection in the optimized trajectory of each UAV, so no collision will occur.

[0052] In a specific implementation, the UAV with the leadership status described in S2 allocates initial expectation points to each UAV, which may include:

[0053] S21, determining the intersection points between the initial maneuvering trajectories of each UAV;

[0054] In specific implementation, S21 may determine all intersection points in the following manner:

[0055] S211. For any two UAVs, determine whether the slopes of the two corresponding initial maneuvering trajectories are the same;

[0056] For example, whether the slopes of the initial maneuvering trajectory of the i-th UAV and the initial maneuvering trajectory of the j-th UAV are the same can be judged by the following formula. If one of the following two equations is not true, it means that L i With L j The slopes are not the same:

[0057] If the above equation determines that the slopes of the two initial maneuvering trajectories are different, perform the following steps:

[0058] S212. Determine whether the following four inequalities are simultaneously true:

[0059]

[0060] Specifically, a rapid repulsion test can be used to determine whether the above four inequalities are true at the same time. If so, the two initial maneuvering trajectories may intersect.

[0061] S213. If the four inequalities are true at the same time, then judge J A ≤0 and J B ≤0 whether it is true at the same time:

[0062] J A =((x UAV (j)-x UAV (i))(y Aim (PS0 ID (i)-y UAV (i))- (y UAV (j)-y UAV (i))(x Aim (PS0 ID (i)-x UAV (i)))× ((x Aim (PS0 ID (j)-x UAV (i))(y Aim (PS0 ID (i)-y UAV (i))- (y Aim (PS0 ID (j)-y UAV (i))(x Aim (PS0 ID (i)-x UAV (i)))

[0063] J B =((x UAV (i)-x UAV (j))(y Aim (PS0 ID (j)-y UAV (j))- (y UAV (i)-y UAV (j))(x Aim (PS0 ID (j)-x UAV (j)))× ((x Aim (PS0 ID (i)-x UAV (j))(y Aim (PS0 ID (j)-y UAV (j))- (y Aim (PS0 ID (i)-y UAV (j))(x Aim (PS0 ID (j)-x UAV (j)))

[0064] Specifically, we can use the straddle test to determine J A ≤0 and J B ≤0. If they are true at the same time, it means that the two initial maneuvering trajectories have an intersection.

[0065] S214, if J A ≤0 and J B ≤0 are both true, it is determined that the two initial maneuvering trajectories have an intersection; and then the position of the intersection is calculated using the following formula.

[0066] make:

[0067] a1=y Aim (PS0 ID (i))-y UAV (i)

[0068] b1=-(x Aim (PS0 ID (i))-x UAV (i))

[0069] c1=x UAV (i)×y Aim (PS0 ID (i))-y UAV (i)×x Aim (PS0 ID (i))

[0070] a2=y Aim (PS0 ID (j))-y UAV (j)

[0071] b2=-(x Aim (PS0 ID (j))-x UAV (j))

[0072] c2=x UAV (j)×y Aim (PS0 ID (j))-y UAV (j)×x Aim (PS0 ID (i))

[0073] d=a1×b2-a2×b1

[0074] Then the coordinates of the intersection of the two initial maneuvering trajectories are:

[0075]

[0076] Where P jd is the position of the intersection, PS0 ID (i) is the number of the initial expected point corresponding to the i-th UAV, PS0 ID (j) is the number of the initial desired point corresponding to the jth UAV; is the current position of the ith UAV, is the position of the initial expected point corresponding to the i-th UAV, is the current position of the jth UAV, is the position of the initial expected point corresponding to the jth UAV.

[0077] It can be seen that the coordinates of all intersection points can be solved by the above method

[0078] See also Figure 4 , the initial assigned trajectory of UAV No. 1 is L 1 , the initial assigned trajectory of UAV No. 2 is L 2 , the initial assigned trajectory of UAV No. 3 is L 3 , the initial assigned trajectory of UAV No. 4 is L 4 , through the above calculation, the positions of the three intersection points can be obtained.

[0079] S22, calculating an entry turning point set and an exit turning point set corresponding to each intersection; each entry turning point in the entry turning point set is a point on an initial trajectory passing through the intersection and heading toward the intersection, and a distance between each entry turning point and the intersection is a preset safety distance; each exit turning point in the exit turning point set is a point on an initial trajectory passing through the intersection and heading away from the intersection, and a distance between each exit turning point and the intersection is a preset safety distance;

[0080] like Figure 5 As shown in the figure, the initial maneuver trajectory containing 4 UAVs is used as an example. For the convenience of description, some definitions are explained:

[0081] (1) Define the point where the trajectories intersect as the intersection point. Figure 5 In the figure, point O is the intersection point.

[0082] (2) Define the preset safety distance as the distance from the turning point to the intersection. Figure 5 In, l safe For a safe distance.

[0083] (3) Define the turning point as the intersection point on the trajectory, and the distance from the intersection point is l safe location point. Figure 5 In the figure, A, B, H, and G are turning points.

[0084] (4) Define the departure turning point as the departure intersection point on the trajectory, and the distance from the intersection point is l safe location point. Figure 5 In the figure, C, D, E, and F are the departure turning points.

[0085] For each intersection, all corresponding entry and exit inflection points are calculated to form an entry inflection point set and an exit inflection point set. For example, the entry inflection point set of the nth intersection is: P SR (n) = [p SR_n (1),p SR_n (2),……,p SR_n (k n )], the set of departure turning points of the nth intersection is: P SL (n) = [p SL_n (1),p SL_n (2),……,p SL_n (k n )],p SR_n (k n ) represents the turning point on the trajectory passing through the nth intersection, p SL_n (k n ) represents the departure turning point on the trajectory passing through the nth intersection point, k n Represents the number of all initial maneuvering trajectories passing through the nth intersection point.

[0086] In a specific implementation, the calculation of the set of entry turning points and the set of exit turning points corresponding to each intersection may include:

[0087] For the nth intersection point, the initial maneuvering trajectory of the i-th UAV passes through the intersection point, let:

[0088]

[0089]

[0090] For the nth intersection point, the positions of the entry and exit inflection points on the initial maneuvering trajectory of the i-th UAV are:

[0091]

[0092]

[0093] Where P sr is the position of the turning point, P sl is the position of the turning point, l safe Indicates the preset safety distance.

[0094] That is, if we pass through the nth intersection The starting point of the initial maneuver trajectory is The end point is The turning point P on the initial maneuvering trajectory can be calculated by the above formula: sr and leaving the turning point P sl .

[0095] The above method can be used to calculate all the entry and exit turning points passing through each intersection. Figure 6 , passing through the intersection point P jd There are two initial maneuvering trajectories in (1), and four inflection points near the intersection. 1 and A 2 To enter the turning point, the coordinates are expressed as p SR_1 (1) and p SR_1 (2) indicates that A 3 and A 4 To leave the turning point, the coordinates are expressed as p SL_1 (1) and p SL_1 (2). Similarly, through the intersection point P jd (2) Four inflection points B 1 , B 2 , B 3 and B 4 The coordinates of SR_2 (1) p SR_2 (2) p SL_2 (1) and p SL_2 (2) indicates that the intersection point P jd (3) The four inflection points C 1 , C 2 , C 3 and C 4 The coordinates of SR_3 (1) p SR_3 (2) p SL_3 (1) and p SL_3 (2) is expressed. Thus, a set of entry turning points and a set of exit turning points corresponding to each intersection are formed.

[0096] S23. According to the set of entry turning points and the set of exit turning points corresponding to each intersection point, the initial maneuvering trajectory of each UAV is adjusted so that there is no intersection point between the obtained optimized trajectories.

[0097] In a specific implementation, S23 may include:

[0098] S231. Set a virtual line for each intersection:

[0099] If the number of initial maneuvering trajectories passing through an intersection is even, a virtual line passing through the intersection is designed so that the number of entry turning points and the number of exit turning points on either side of the virtual line are equal; if the number of initial maneuvering trajectories passing through an intersection is odd, one of the initial maneuvering trajectories passing through the intersection is selected as the virtual line so that the number of entry turning points and the number of exit turning points on either side of the virtual line are equal;

[0100] S232, rotating the virtual line along a preset direction with the intersection as the center, numbering the entry turning point and the exit turning point of the intersection in the order in which the virtual line and the initial maneuvering trajectory overlap; re-pairing each entry turning point and the exit turning point in the order of numbers from small to large; if the virtual line is the initial maneuvering trajectory, pairing the entry turning point and the exit turning point on the virtual line;

[0101] For example, the virtual line L xn Rotate clockwise to overlap the initial maneuvering trajectories passing through the intersection, and number the entry and exit turning points on each initial maneuvering trajectory according to the order of overlap ID = [1, 2, ..., k id ]. Among them, k id is the number of initial maneuvering trajectories on one side of the virtual line.

[0102] When re-pairing the entry and exit turning points on the trajectory according to the numbers, the pairing principles are as follows: (1) only the entry and exit turning points can be paired; (2) each entry and exit turning point is paired in order from small to large numbers; (3) after each pairing is successful, the relative positions of the numbers are kept unchanged, the successfully paired numbers are removed, and the subsequent pairing process is no longer performed. The remaining numbers are used to form a number combination that can participate in the pairing, and pairing is performed again until all the entry and exit turning points are paired; (4) if the virtual line L xn is the initial maneuvering trajectory passing through the intersection, then the entry turning point and the exit turning point on the virtual line are paired.

[0103] In specific implementation, the following pairing method can be used:

[0104] (a) Select the smallest ID from the set of IDs mid (rank), where rank represents the ID in the numbered set mid Position in

[0105] (b) Determine ID mid (rank) inflection point and ID mid Whether the inflection point of (rank+1) can be paired. If the two inflection points are paired successfully, the number of the successful pairing is removed to update the number set and return to (a) for the next pairing; if the pairing is not successful, set rank = rank+1 and re-execute (b) for pairing.

[0106] The above method is a method for re-pairing the inflection points of an intersection. The above method is used to re-pair the inflection points of all intersections. Figure 7 , the result of re-pairing each inflection point: A 1 and A 4, A 2 and A 3 , B 1 and B 4 , B 2 and B 3 , C 1 and C 4 , C 2 and C 3 There are six pairs in total.

[0107] S233: After all turning points in the entry turning point set and the exit turning point set of all intersections are paired, an optimized trajectory is generated according to the pairing results.

[0108] In specific implementation, generating an optimized trajectory according to the pairing result in S233 may include:

[0109] If there is an intersection on the initial maneuvering trajectory corresponding to a UAV, the optimized trajectory of the UAV is: starting from the current position, moving along the initial maneuvering trajectory, passing through at least one turning point pair in sequence, and reaching the target expected point; an turning point pair includes an entry turning point and a departure turning point re-paired with the entry turning point, and the target expected point is the expected point on the initial maneuvering trajectory where the last departure turning point is located; the optimized trajectory corresponding to the UAV includes the positions of each target point that the UAV maneuvers through, and the target points include the entry turning point, departure turning point and target expected point experienced.

[0110] It is understandable that after some drones start from their initial positions, they may reach the corresponding target desired point after passing through one inflection point pair. For example, drone No. 2 can reach the fourth desired point after passing through an inflection point pair at the third intersection, drone No. 1 can reach the first desired point after passing through an inflection point pair at the second intersection, and drone No. 4 can reach the third desired point after passing through an inflection point pair at the first focus. Some drones may need to pass through multiple inflection point pairs to reach the target desired point. For example, drone No. 3 reaches the second desired point after passing through an inflection point pair at the first intersection, an inflection point pair at the second intersection, and an inflection point pair at the third intersection.

[0111] It can be seen that although the initial maneuvering trajectory has changed after re-pairing, it is still on the initial maneuvering trajectory without adding new line segments. Since the initial maneuvering trajectory is calculated based on the principle of short distance and few intersections, the optimized trajectory obtained after re-pairing still has the advantage of short distance.

[0112] For example, the trajectory planned by the i-th UAV is from the starting point Start with the expected point of initial distribution Maneuver; if there is an intersection on the initial maneuver trajectory, it will maneuver from the entry turning point on the trajectory to the exit turning point paired with the entry turning point, and finally reach the target desired point. i -1 target point C pos (i) = [p i (1),p i (2),···,p i (k i -1)] is the paired entry and exit turning points. The i-th drone passes the last exit turning point p i (k i -1) if the departure point is and The line segment L formed by two points m On, then p i (k i )=P Aim (PS0 ID (m)), that is, the target expected point of the i-th UAV is the expected point on the initial maneuvering trajectory where the last turning point is. Finally, all the target points that the i-th UAV passes through are obtained: C pos (i) = [p i (1),p i (2),···,p i (k i )].

[0113] like Figure 7 As shown, all the target points passed by UAV No. 1 are

[0114] C pos (1) = [p SR_2 (2),p SL_2 (1),p SR_3 (1),p SL_3 (2),P Aim (2)], all the target points that UAV No. 2 passes through are

[0115] C pos (2) = [p SR_3 (2),p SL_3 (1),P Aim (4)], all the target points that UAV No. 3 passes through are

[0116] C pos (3) = [p SR_1 (1),p SL_1 (2),P Aim (3)], all the target points that UAV No. 4 passes through are

[0117] C pos(4) = [p SR_1 (2),p SL_1 (1),p SR_2 (1),p SL_2 (2),P Aim (1)], the trajectory formed by each target point is the optimized trajectory. Figure 7 As shown in the figure, the new trajectories of the four UAVs after replanning are L 1 ′, L 2 ′, L 3 ′ and L 4 ′, and it can be seen from the figure that each UAV replanned its trajectory near the intersection point to avoid the possibility of collision during the maneuvering process.

[0118] It can be seen that the above process is to optimize the initial maneuver trajectory generated by the leader based on the expected points initially allocated, and obtain the coordinates C of all target points that each UAV passes through pos (i) = [p i (1),p i (2),···,p i (k i )], represents the coordinates of the target point that the i-th UAV passes by, k i Represents the number of coordinates of all target points passed by the i-th UAV.

[0119] S4, the leading drone sends the optimized trajectories corresponding to the respective drones to the corresponding drones;

[0120] That is to say, after calculating the optimized trajectory of each drone, the leader can send the position of each target point in the optimized trajectory of each drone to the drone. For the leader, after calculating the position of each of its own target points, it does not need to send it, and can directly maneuver according to these target points.

[0121] S5, each UAV performs maneuvers according to its corresponding optimized trajectory, and after each UAV reaches the corresponding re-planned target expected point, it notifies the UAV with the leadership identity of the completion of the maneuver;

[0122] That is to say, after obtaining its own optimized trajectory, each drone maneuvers according to the position of each target point in the optimized trajectory. After the maneuver is completed, the message of the completion of the maneuver is sent to the leader.

[0123] S6. The leading drone determines whether all drones have completed the maneuver within the second preset time period. If so, a message of successful formation is sent to each drone to enable each drone to maintain a fixed point and orientation, and the formation is completed.

[0124] That is to say, after the leader sends the position of each target point in the optimized trajectory to each UAV, it starts timing. If the leader receives the message of maneuver completion returned by all UAVs within a certain period of time, it means that the formation of the formation is successful. At this time, the leader can send a successful command to each UAV. After receiving the successful command, each UAV maintains a fixed-point and directional state, and the formation process of the entire UAV formation is completed.

[0125] In a specific implementation, the second preset time period may be calculated using the following formula:

[0126]

[0127] Wherein, t2 is the second preset time period, N num_UAV is the number of drones, l max is the maximum distance between the current position of all UAVs and all desired points, l UAV is the length of the drone.

[0128] in, Indicates a round-up operation.

[0129] Of course, if the leader does not receive the maneuver completion message from all drones within a certain period of time, it means that some drones may have failed to maneuver, which means that the formation process of the formation has failed.

[0130] In summary, the method provided by the embodiment of the present invention has the following advantages:

[0131] 1. The present invention only needs the initial position of the UAV formation and the position of the desired point of the desired formation, and uses the particle swarm optimization algorithm to plan the initial maneuvering trajectory of the UAV formation. It does not need to establish a complex mathematical model, and the formed initial maneuvering trajectory has the advantages of short distance and few intersections.

[0132] 2. The algorithm of the present invention is simple in design, the control strategy is easy to design, and the UAV trajectory planning speed is fast;

[0133] 3. According to the results obtained by the particle swarm optimization algorithm, the present invention replans each initial maneuvering trajectory based on the intersection to obtain an optimized trajectory. The optimized trajectory can ensure that there is no intersection, so no collision occurs during maneuvering, ensuring that the UAV formation completes the formation task safely without collision;

[0134] 4. The present invention adopts the method of only one planning, only one optimization and only one maneuvering according to the characteristics of hovering UAVs, which are flexible and do not need a large range of rotation to turn. This makes it possible for the UAV formation navigation process to not have staged maneuvers and hovering waiting processes. The formation formation task can be completed by flying according to the planned navigation trajectory, thereby improving the completion efficiency of the formation formation task.

[0135] 5. The present invention requires less information exchange between UAV formations, simple calculations, fast planning, clear coordination and maneuvering logic, and is easy to implement in engineering.

[0136] An embodiment of the present invention further provides a system for forming a hovering UAV formation based on intersection trajectory replanning, comprising a controller and a plurality of UAVs, wherein:

[0137] The controller is used to perform: setting the leadership status of the drone formation and the positions of each desired point in the desired formation, sending the positions of each desired point to the drone with the leadership status; controlling all drones to take off and maintain a fixed point and orientation, and controlling each drone to confirm whether it is the leader;

[0138] All non-leader drones are used to: send their respective current locations to the leader drone;

[0139] The leader drone is used to: determine whether the current position sent by all non-leader drones is received within a first preset time period; if so, the leader drone assigns an initial expected point to each drone, generates a corresponding initial maneuvering trajectory according to the current position of each drone and the position of the corresponding initial expected point, replans the initial maneuvering trajectory based on the intersection of each initial maneuvering trajectory, and obtains the optimized trajectory corresponding to each drone; the optimized trajectories of each drone do not have an intersection; and sends the optimized trajectory corresponding to each drone to the corresponding drone;

[0140] Each UAV is used to: perform maneuvers according to its corresponding optimized trajectory, and notify the UAV with the leadership status of the completion of maneuvering after each UAV reaches the corresponding re-planned target expected point;

[0141] The leader drone is also used to determine whether all drones have completed maneuvers within a second preset time period. If so, a message of successful formation is sent to each drone to enable each drone to maintain a fixed point and orientation, and the formation is completed.

[0142] An embodiment of the present invention further provides an electronic device, which may be a controller or a drone, wherein the drone includes a leader drone and a non-leader drone, and the electronic device includes at least one memory and at least one processor;

[0143] The at least one memory is used to store a machine-readable program;

[0144] The at least one processor is used to call the machine-readable program and execute corresponding steps; the step executed by the controller is S1, the UAV with a leadership identity is S3, S4, S5 and S6, and the steps executed by the UAV without a leadership identity are S2 and S5. The controller and the UAVs jointly realize the formation system of the formation.

[0145] An embodiment of the present invention also provides a computer-readable medium, which can be a computer-readable medium in a controller or a drone. The computer-readable medium stores computer instructions. When the computer instructions are executed by a processor, the processor performs corresponding steps. The processor of the controller performs step S1, the processor of the drone with a leadership identity performs steps S3, S4, S5 and S6, and the processor of the drone with a non-leadership identity performs steps S2 and S5. The controller and the drone jointly implement a formation system.

[0146] An embodiment of the present invention provides a computer-readable medium storing instructions for causing a machine to execute the method for controlling the speed of a rod and wire mill as described herein. Specifically, a system or device equipped with a storage medium may be provided, on which a software program code for implementing the functions of any of the above embodiments is stored, and a computer (or CPU or MPU) of the system or device reads and executes the program code stored in the storage medium.

[0147] In this case, the program code itself read from the storage medium can realize the function of any one of the above-mentioned embodiments, and thus the program code and the storage medium storing the program code constitute a part of the present invention.

[0148] The storage medium embodiments for providing the program code include a floppy disk, a hard disk, a magneto-optical disk, an optical disk (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD+RW), a magnetic tape, a non-volatile memory card, and a ROM. Alternatively, the program code can be downloaded from a server computer via a communication network.

[0149] In addition, it should be clear that the functions of any of the above embodiments can be implemented not only by executing the program code read by the computer, but also by enabling an operating system operating on the computer to complete part or all of the actual operations based on instructions from the program code.

[0150] In addition, it can be understood that the program code read from the storage medium is written to a memory provided in an expansion board inserted into the computer or to a memory provided in an expansion module connected to the computer, and then based on the instructions of the program code, a CPU installed on the expansion board or expansion module is enabled to perform part or all of the actual operations, thereby realizing the functions of any of the above-mentioned embodiments.

[0151] It is understandable that the explanations, specific implementation methods, beneficial effects, examples, etc. of the relevant contents in the system, electronic device, and computer-readable medium provided in the embodiments of the present invention can be found in the corresponding parts of the above-mentioned bar and wire rolling mill speed control method, and will not be repeated here.

[0152] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of more restrictions, an element defined by the sentence "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0153] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0154] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as RON / RAN, a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0155] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation modes, which are merely illustrative rather than restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A method for forming a hovering UAV formation based on intersection trajectory replanning, characterized in that: include: S1. Set the leader status of the drone formation and the positions of each expected point in the expected formation, and send the positions of each expected point to the drone with the leader status; control all drones to take off and maintain fixed point and orientation, and control each drone to confirm whether it is the leader status; S2. All non-leader drones send their current locations to the leader drone; S3, the UAV with the leadership identity determines whether it has received the current position sent by all the UAVs without the leadership identity within the first preset time period; if so, the UAV with the leadership identity allocates an initial expected point to each UAV, generates a corresponding initial maneuvering trajectory according to the current position of each UAV and the position of the corresponding initial expected point, and replans the initial maneuvering trajectory based on the intersection of each initial maneuvering trajectory to obtain the optimized trajectory corresponding to each UAV; the optimized trajectories of each UAV do not have an intersection; S4, the leading drone sends the optimized trajectories corresponding to the respective drones to the corresponding drones; S5, each UAV performs maneuvers according to its corresponding optimized trajectory, and after each UAV reaches the corresponding re-planned target expected point, it notifies the UAV with the leadership identity of the completion of the maneuver; S6, the leading drone determines whether all drones have completed the maneuver within the second preset time period, and if so, sends a message of successful formation to each drone, so that each drone maintains a fixed point and orientation, and the formation is completed; The replanning of the initial maneuvering trajectory based on the intersection points of each initial maneuvering trajectory to obtain an optimized trajectory corresponding to each UAV includes: Determine the intersection points between the initial maneuvering trajectories of each UAV; Calculate the set of entry turning points and the set of exit turning points corresponding to each intersection; each entry turning point in the set of entry turning points is a point on the initial trajectory passing through the intersection and heading toward the intersection, and the distance between each entry turning point and the intersection is a preset safety distance; each exit turning point in the set of exit turning points is a point on the initial trajectory passing through the intersection and heading away from the intersection, and the distance between each exit turning point and the intersection is a preset safety distance; According to the set of entry inflection points and the set of exit inflection points corresponding to each intersection point, the initial maneuvering trajectory of each UAV is adjusted so that there is no intersection between the obtained optimized trajectories; The step of determining the intersection points between the initial maneuvering trajectories of the respective UAVs comprises: For any two drones, determine whether the slopes of the two corresponding initial maneuvering trajectories are the same; if the slopes are different, perform the following steps: Determine whether the following four inequalities hold simultaneously: If the four inequalities are true at the same time, then judge J A ≤0 and J B ≤0 whether it is true at the same time: J A =((x UAV (j)-x UAV (i))(y Aim (PS0 ID (i)-y UAV (i))- (y UAV (j)-y UAV (i))(x Aim (PS0 ID (i)-x UAV (i)))× ((x Aim (PS0 ID (j)-x UAV (i))(y Aim (PS0 ID (i)-y UAV (i))- (y Aim (PS0 ID (j)-y UAV (i))(x Aim (PS0 ID (i)-x UAV (i))) J B =((x UAV (i)-x UAV (j))(y Aim (PS0 ID (j)-y UAV (j))- (y UAV (i)-y UAV (j))(x Aim (PS0 ID (j)-x UAV (j)))× ((x Aim (PS0 ID (i)-x UAV (j))(y Aim (PS0 ID (j)-y UAV (j))- (y Aim (PS0 ID (i)-y UAV (j))(x Aim (PS0 ID (j)-x UAV (j))) If J A ≤0 and J B ≤0 is true at the same time, it is determined that the two initial maneuvering trajectories have an intersection; make: a1=y Aim (PS0 ID (i))-y UAV (i) b1=-(x Aim (PS0 ID (i))-x UAV (i)) c1=x UAV (i)×y Aim (PS0 ID (i))-y UAV (i)×x Aim (PS0 ID (i)) <h2 style=";text-align:left;direction:ltr">a2=y<h2 style=";text-align:left;direction:ltr"> Aim <h2 style=";text-align:left;direction:ltr"> (PS0<h2 style=";text-align:left;direction:ltr"> ID <h2 style=";text-align:left;direction:ltr"> (j))-y<h2 style=";text-align:left;direction:ltr"> UAV <h2 style=";text-align:left;direction:ltr"> (j) b2=-(x Aim (PS0 ID (j))-x UAV (j)) c2=x UAV (j)×y Aim (PS0 ID (j))-y UAV (j)×x Aim (PS0 ID (i)) d=a1×b2-a2×b1 Then the coordinates of the intersection of the two initial maneuvering trajectories are: Where P jd is the position of the intersection, PS0 ID (i) is the number of the initial expected point corresponding to the i-th UAV, PS0 ID (j) is the number of the initial expected point corresponding to the jth UAV; is the current position of the ith UAV, is the position of the initial expected point corresponding to the i-th UAV, is the current position of the jth UAV, is the position of the initial expected point corresponding to the jth UAV; The calculating of the set of entry turning points and the set of exit turning points corresponding to each intersection point includes: For the nth intersection point, the initial maneuvering trajectory of the i-th UAV passes through the intersection point, let: For the nth intersection point, the positions of the entry and exit inflection points on the initial maneuvering trajectory of the i-th UAV are: Where P sr is the position of the turning point, P sl is the position of the turning point, l safe Indicates the preset safety distance; The initial maneuvering trajectory of each UAV is adjusted according to the set of entry inflection points and the set of exit inflection points corresponding to each intersection point, including: A virtual line is set for each intersection: if the number of initial maneuvering trajectories passing through an intersection is even, a virtual line passing through the intersection is designed so that the number of entry turning points and the number of exit turning points on either side of the virtual line are equal; if the number of initial maneuvering trajectories passing through an intersection is odd, one of the initial maneuvering trajectories passing through the intersection is selected as the virtual line so that the number of entry turning points and the number of exit turning points on either side of the virtual line are equal; The virtual line is rotated along a preset direction with the intersection as the center, and the entry turning point and the exit turning point of the intersection are numbered in the order in which the virtual line and the initial maneuvering trajectory overlap; each entry turning point and the exit turning point are re-paired in the order of numbers from small to large; if the virtual line is the initial maneuvering trajectory, the entry turning point and the exit turning point on the virtual line are paired; After all the inflection points in the entry inflection point set and the exit inflection point set of all intersections are paired, an optimized trajectory is generated according to the pairing results; The step of generating an optimized trajectory according to the pairing result includes: If there is an intersection on the initial maneuvering trajectory corresponding to a UAV, the optimized trajectory of the UAV is: starting from the current position, moving along the initial maneuvering trajectory, passing through at least one turning point pair in sequence, and reaching the target expected point; an turning point pair includes an entry turning point and a departure turning point re-paired with the entry turning point, and the target expected point is the expected point on the initial maneuvering trajectory where the last departure turning point is located; the optimized trajectory corresponding to the UAV includes the positions of each target point that the UAV maneuvers through, and the target points include the entry turning point, departure turning point and target expected point experienced.

2. The method according to claim 1, characterized in that The leading drone allocates initial expectation points to each drone, including: According to the principle of shortest distance and least intersection points, determine the corresponding objective function; The objective function is solved according to the particle swarm optimization algorithm to obtain the initial expected points corresponding to each UAV.

3. The method according to claim 2, characterized in that The initial expected points corresponding to each UAV obtained by the particle swarm optimization algorithm satisfy the first condition and the second condition; the first condition includes: the initial expected points corresponding to any two UAVs are different; the second condition includes: if the slopes of the initial maneuvering trajectories of any two UAVs are the same or the slopes do not exist, then one of the first and second conditions must be true; The first situation is: (x UAV (j)-x UAV (i))(x Aim (PS0 ID (j))-x Aim (PS0 ID (i)))+ (y UAV (j)-y UAV (i))(y Aim (PS0 ID (j))-y Aim (PS0 ID (i)))>0 The second situation is that at least one of the following four inequalities holds true: Where, PS0 ID (i) is the number of the initial expected point corresponding to the i-th UAV, PS0 ID (j) is the number of the initial expected point corresponding to the jth UAV; is the current position of the ith UAV, is the position of the initial expected point corresponding to the i-th UAV, is the current position of the jth UAV, is the position of the initial expected point corresponding to the jth UAV.

4. The method according to claim 3, characterized in that If any two UAVs satisfy the first formula, the slopes of the initial maneuvering trajectories of any two UAVs are the same or the slopes do not exist. The first formula includes:

5. The method according to claim 1, characterized in that The second preset time period is calculated using the following formula: Wherein, t2 is the second preset time period, N num_UAV is the number of drones, l max is the maximum distance between the current position of all UAVs and all desired points, l UAV is the length of the drone.

Images