An anti-collision method for UAV formation based on a three-dimensional velocity obstacle model and an improved artificial potential field method

Abstract

The present invention discloses a UAV formation anti-collision method based on a three-dimensional velocity obstacle model and an improved artificial potential field method, including: improving the artificial potential field based on the communication topology and communication weight; calculating the collision avoidance velocity field between UAVs based on the improved first artificial potential field; constructing an elastic collision avoidance distance for the UAVs, and introducing the relative motion velocity vector to improve the artificial potential field under the elastic collision avoidance distance; calculating the collision avoidance velocity field of the UAVs based on the improved second artificial potential field; obtaining the total velocity field of the UAVs based on the collision avoidance velocity field and the collision avoidance velocity field; converting the UAV anti-collision instructions for the total velocity field to obtain velocity instructions, pitch angle instructions, and yaw angle instructions; and tracking the above instructions to achieve inter-aircraft anti-collision and obstacle avoidance. The inter-aircraft collision avoidance of the present invention takes into account the communication topology and collision avoidance priority, can dynamically adjust the collision avoidance distance and potential field value, avoid ineffective collision avoidance maneuvers, and has a higher overall efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical field of anti-collision for UAV formations, and particularly relates to an anti-collision method for UAV formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method. Background Art

[0002] The formation flight of UAVs can improve the efficiency of task completion and broaden the application scope of UAVs, and has broad application prospects in both military and civilian fields. If the UAVs in the formation collide with each other or with airspace obstacles, it will not only cause economic losses, but also seriously affect the task execution. Therefore, enhancing the anti-collision ability of UAVs is one of the keys to improving the success rate of formation tasks.

[0003] UAV obstacle avoidance algorithms can be divided into global obstacle avoidance algorithms and local obstacle avoidance algorithms according to whether the global map is known: global obstacle avoidance essentially belongs to one of the constraints of trajectory planning, and the optimal path is found through continuous iteration; local obstacle avoidance algorithms are for unknown or dynamic environments, and only sense the surrounding environment information through on-board sensors to avoid obstacles, and are prone to falling into local optima. Although global obstacle avoidance can be carried out through trajectory planning in a known environment, in the face of sudden and unknown obstacles in a dynamic environment, a more flexible local obstacle avoidance algorithm is needed for trajectory replanning, and complex obstacle environments often easily cause cluster obstacle avoidance failures. Therefore, studying the local dynamic obstacle avoidance algorithm for UAV clusters in complex environments is one of the keys to ensuring the safe flight of the cluster.

[0004] The artificial potential field method was originally a virtual force method proposed by Khatib, which reflects the distribution and shape of obstacles and other information in the potential field value of each point in the environment. According to the magnitude of the potential field value, the UAV decides the traveling direction and speed. At present, when the artificial potential field method is used for anti-collision between aircraft, the actual inter-aircraft communication topology is rarely considered; when used for obstacle avoidance, it usually only avoids obstacles according to the distance between the UAV and the obstacle, and there are problems such as avoiding obstacles too early or ending obstacle avoidance too late, and unnecessary obstacle avoidance maneuvers in some cases resulting in energy consumption and time waste, and the obstacle avoidance efficiency needs to be further improved.

[0005] The velocity obstacle method calculates the magnitude and heading of the velocity required for obstacle avoidance by analyzing the spatial geometric relationship between the UAV and the dynamic obstacle. This method is mostly used for single-UAV obstacle avoidance and is not suitable for inter-aircraft collision avoidance in UAV formations. Moreover, most existing studies only avoid obstacles by adjusting the heading or the magnitude of the velocity. Since the range of change in the magnitude of the velocity is limited, simply changing the magnitude of the velocity cannot achieve the purpose of complete anti-collision, and at the same time, simply changing the heading angle will lead to too long obstacle avoidance time.

[0006] In view of the problems in the prior art, there is an urgent need to propose an anti-collision method for UAV formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method. Summary of the Invention

[0007] To solve the above technical problems, the present invention proposes a method for preventing collision of UAV formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method. For the problem of collision avoidance between UAVs, communication topology and collision avoidance priority are introduced, and the artificial potential field method is improved to optimize collision avoidance between UAVs; for the problem of avoiding airspace obstacles, based on the geometric relationship of the three-dimensional velocity obstacle model, the conflict resolution and track recovery reference points of UAVs are quantitatively calculated, and the relative motion velocity vector between the UAV and the obstacle is introduced to dynamically adjust the obstacle avoidance action distance and potential field value of the artificial potential field; the negative gradient of the potential field is directly used as the velocity field to calculate the anti-collision command. To solve the problems existing in the above prior art.

[0008] To achieve the above object, the present invention provides a method for preventing collision of UAV formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method, including the following steps:

[0009] Construct an artificial potential field between UAVs;

[0010] Improve the artificial potential field based on the communication topology and communication weights to obtain the improved first artificial potential field;

[0011] Based on the improved first artificial potential field, calculate the collision avoidance velocity field between UAVs;

[0012] Construct an elastic obstacle avoidance distance for UAVs, and introduce a relative motion velocity vector to improve the artificial potential field under the elastic obstacle avoidance distance to obtain the improved second artificial potential field;

[0013] Based on the improved second artificial potential field, calculate the obstacle avoidance velocity field of UAVs;

[0014] Based on the collision avoidance velocity field and the obstacle avoidance velocity field, obtain the total velocity field of UAVs;

[0015] Convert the total velocity field into UAV anti-collision commands to obtain velocity commands, pitch angle commands and yaw angle commands;

[0016] Track the velocity commands, pitch angle commands and yaw angle commands to achieve collision avoidance between UAVs in the formation and obstacle avoidance.

[0017] Optionally, the expression of the improved first artificial potential field is as follows:

[0018]

[0019] where b and c are both constants, D = (||ρ ij || min ,||ρ ij || max) is the action area of the artificial potential field between machines, ||ρ ij || min >0 is the minimum safe distance between machines, ||ρ ij || max is the maximum action distance of the artificial potential field between machines. The i-th UAV is denoted as U i , and the j-th UAV is denoted as U j , ρ ij is the position vector from U i to U j , a ij represents the communication weight from U j to U i , J ij (||ρ ij ||) is the potential field generated between UAV U i and U j . ρ i is the three-dimensional position coordinate of U i , J i (ρ i ) is the sum of the potential fields generated between UAV U i and U j .

[0020] Optionally, the process of calculating the collision avoidance velocity field between UAVs based on the improved first artificial potential field includes: taking the negative gradient of the improved first artificial potential field as the velocity field, and letting Calculate ||ρ ij ||∈D, the collision avoidance velocity field between UAVs is:

[0021]

[0022] Optionally, the process of constructing the elastic collision avoidance distance of UAVs includes: obtaining the maximum repulsive distance of the obstacle, and constructing the elastic collision avoidance distance of UAVs based on the maximum repulsive distance of the obstacle;

[0023] The expression of the maximum repulsive distance of the obstacle is as follows:

[0024]

[0025] Where, ||ρ io || max is the maximum repulsive distance of the obstacle, c1 and c2 are control coefficients related to velocity and relative angle respectively, α0 is the half apex angle of the spatial velocity obstacle cone, α is the angle between the relative velocity vector and the spatial obstacle cone line, is a set constant value, v i is the flight speed magnitude of the i-th UAV.

[0026] Optionally, the expression of the improved second artificial potential field is as follows:

[0027]

[0028] where V uo is the relative motion velocity vector of the UAV and the obstacle, E = (||ρ io || min , ||ρ io || max ) is the elastic obstacle avoidance distance, ||ρ io || min is the minimum repulsion distance of the obstacle, ||ρ io || max is the maximum repulsion distance of the obstacle, U i (ρ io ) is the improved artificial potential field function, and b o , c o are both adjustable constants.

[0029] Optionally, the process of calculating the obstacle avoidance velocity field of the UAV based on the improved second artificial potential field includes: taking the negative gradient of the improved second artificial potential field as the velocity field, and letting calculate the obstacle avoidance velocity field of the UAV when ||ρ io || ∈ E and 0 ≤ |α| ≤ |α0|:

[0030]

[0031] where J i (||ρ io ||) is the potential field generated between the UAV and the obstacle.

[0032] Optionally, the expressions of the velocity command, pitch angle command, and yaw angle command are as follows:

[0033]

[0034] where is the velocity command, is the pitch angle command, is the yaw angle command, are the three-axis components of the total velocity.

[0035] Optionally, the process of obstacle avoidance based on the obstacle avoidance velocity field of the UAV includes:

[0036] The drone adjusts its velocity vector based on an improved second artificial potential field to avoid obstacles, and then calculates the relative distance and conflict resolution time at conflict resolution based on the geometric relationship of the drone trajectory recovery with minimum heading adjustment. After reaching the conflict resolution time, the drone performs trajectory recovery and switches to the formation keeping control law until the original trajectory is restored.

[0037] The present invention also provides an electronic device, including: a memory and a processor; the memory is used to store a program; the processor is used to execute the program to implement each step of the method for preventing collision of drone formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method.

[0038] The present invention also provides a readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, each step of the method for preventing collision of drone formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method is implemented.

[0039] Compared with the prior art, the present invention has the following advantages and technical effects:

[0040] The present invention proposes a method for preventing collision of drone formations based on a three-dimensional velocity obstacle model and an improved artificial potential field method. For the problem of collision avoidance between drones, communication topology and collision avoidance priority are introduced, and the artificial potential field method is improved to optimize collision avoidance between drones; for the problem of avoiding airspace obstacles, the relative velocity vector between the drone and the obstacle is introduced to dynamically adjust the obstacle avoidance action distance and potential field value of the artificial potential field; the negative gradient of the potential field is directly used as the velocity field to calculate the anti-collision command.

[0041] Compared with the velocity obstacle method, the method of the present invention is easier to handle the problem of collision avoidance between drones, and adjusts the velocity magnitude, pitch angle, yaw angle, etc. for obstacle avoidance through the artificial potential field method, with higher efficiency and success rate; compared with the traditional artificial potential field method, the collision avoidance between drones takes into account the actual communication topology and collision avoidance priority, can dynamically adjust the obstacle avoidance distance and potential field value, can avoid ineffective obstacle avoidance maneuvers, shorten the obstacle avoidance time, and has higher overall efficiency and success rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The drawings constituting a part of this application are used to provide a further understanding of this application. The schematic embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation to this application. In the drawings:

[0043] Figure 1 is a schematic diagram of the artificial potential field according to an embodiment of the present invention;

[0044] Figure 2 is the three-dimensional velocity obstacle model according to an embodiment of the present invention;

[0045] Figure 3Schematic diagram for constructing the elastic repulsion distance according to an embodiment of the present invention;

[0046] Figure 4 Schematic diagram for adjusting the velocity vector of an unmanned aerial vehicle according to an embodiment of the present invention;

[0047] Figure 5 Schematic diagram for restoring the flight path of an unmanned aerial vehicle based on minimum heading adjustment according to an embodiment of the present invention;

[0048] Figure 6 Schematic diagram of the process of the anti-collision method for an unmanned aerial vehicle formation based on a three-dimensional velocity obstacle model and an improved artificial potential field method according to an embodiment of the present invention. Detailed implementation manners

[0049] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments may be combined with each other. The present application will be described in detail below with reference to the drawings and in conjunction with the embodiments.

[0050] It should be noted that the steps shown in the flowchart of the drawings may be executed in a computer system such as a set of computer-executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than here.

[0051] Embodiment 1

[0052] As Figure 6 shown, in this embodiment, an anti-collision method for an unmanned aerial vehicle formation based on a three-dimensional velocity obstacle model and an improved artificial potential field method is provided, including the following steps:

[0053] Construct an artificial potential field between unmanned aerial vehicles;

[0054] Improve the artificial potential field based on the communication topology and communication weights to obtain the improved first artificial potential field;

[0055] Calculate the collision avoidance velocity field between unmanned aerial vehicles based on the improved first artificial potential field;

[0056] Construct the elastic collision avoidance distance of the unmanned aerial vehicle, and introduce the relative motion velocity vector to improve the artificial potential field under the elastic collision avoidance distance to obtain the improved second artificial potential field;

[0057] Calculate the collision avoidance velocity field of the unmanned aerial vehicle based on the improved second artificial potential field;

[0058] Obtain the total velocity field of the unmanned aerial vehicle based on the collision avoidance velocity field and the collision avoidance velocity field;

[0059] Convert the anti-collision instruction of the unmanned aerial vehicle for the total velocity field to obtain the velocity instruction, pitch angle instruction, and yaw angle instruction;

[0060] Track the speed command, pitch angle command, and yaw angle command to achieve collision avoidance between drones in a formation and obstacle avoidance.

[0061] The process of constructing an artificial potential field between drones includes:

[0062] Consider a formation consisting of n drones. The i-th drone is denoted as U i . The artificial potential field method is used for collision avoidance control between drones, and the potential field function is only used for obstacle avoidance and not for formation organization. Therefore, only a repulsive potential field function is constructed. Let J i (ρ i ) be the sum of the potential fields J i generated between drone U j and another drone U ij (ρ ij ), and for every position in space, J ij (ρ ij ) is differentiable. Among them, ρ i is the three-dimensional position coordinate of U i , and ρ ij is the position vector from U i to U j . The artificial potential field between drones is as Figure 1 shown.

[0063] Improve the artificial potential field based on the communication topology and communication weights to obtain the improved first artificial potential field. The process of calculating the collision avoidance speed field between drones based on the improved first artificial potential field includes:

[0064] Traditional artificial potential fields assume that drone U i can obtain information about all other drones U j in the formation, which often does not match the actual communication topology; and it does not consider the collision avoidance priority levels of drones in the formation. To overcome these two defects, this embodiment introduces the concept of multi-agent consensus, that is, adding communication topology and communication weight variables to form a new artificial potential field. Let the set of all drones U i that communicate with U j be N i . U i can only obtain information about the drones that communicate with it, such as position, attitude, etc. Let a ij represent the communication weight from U j to U i , which reflects the importance of the information of U j to U i . The relationship between the total potential field and the partial potential fields of drone U i is as follows:

[0065]

[0066] In the formula, ||·|| is the L2 norm. When U i When performing the collision avoidance action between aircraft, the UAV with a high communication weight is given priority to be avoided. When there is a risk of collision between aircraft, the UAV in the secondary position is destroyed first, and the safety of the UAV in the important position is given priority to be ensured.

[0067] The first improved artificial potential field based on the classical Morse function is designed in this embodiment as follows:

[0068]

[0069] In the formula, both b and c are constants, which respectively determine the amplitude and change speed of the repulsive potential, and are both adjustable parameters. D = (||ρ ij || min ,||ρ ij || max ) determines the action area of the artificial potential field between aircraft, ||ρ ij || min > 0 is the minimum safe distance between aircraft. If ||ρ ij || is less than this value, a collision between aircraft will occur. ||ρ ij || max is the maximum action distance of the artificial potential field between aircraft. The UAV cluster maintains formation flight according to the formation-keeping control law. When ||ρ ij || ≤ ||ρ ij || max in some cases, it switches to the collision avoidance mode between aircraft. When ||ρ ij || > ||ρ ij || max , it switches back to the formation flight mode according to the formation-keeping control law. Here, it is required that ||ρ ij || max is less than the inter-aircraft distance set when maintaining stable formation flight and the obstacle detection range of the UAV on-board sensor.

[0070] Taking the negative gradient of the potential field function (2) directly as the velocity field, let When calculating ||ρ ij || ∈ D, the collision avoidance velocity field between UAVs is:

[0071]

[0072] Construct the elastic obstacle avoidance distance of the UAV. Under the elastic obstacle avoidance distance, introduce the relative motion velocity vector to improve the artificial potential field, and obtain the improved second artificial potential field; the process of calculating the obstacle avoidance velocity field of the UAV based on the improved second artificial potential field includes:

[0073] In the process of anti-collision control between drones, drones approaching each other can maneuver to avoid collision, and the relative speed and distance between individual drones are relatively controllable due to the existence of formation control between drones; in the process of obstacle avoidance, only drones can actively avoid obstacles, and obstacles cannot actively avoid drones, and because the speed and direction of drones relative to dynamic obstacles in complex environments are more uncertain, it is necessary to study more flexible and efficient obstacle avoidance algorithms.

[0074] In three-dimensional space, assume that the radiation radius of the drone is R u , the radiation radius of a dynamic threat obstacle is R o The obstacle model is expanded to increase the radiation radius R of the drone. u Added to the obstacle, so that the drone can be regarded as a mass point relative to the obstacle during the obstacle avoidance process. In addition, considering that the obstacle avoidance process may be affected by uncertain factors such as communication delay and obstacle size, position, speed and other information detection deviation, a safety distance R is further introduced safe To improve the safety margin. Therefore, the adjusted obstacle expansion radius R = R u +R o +R safe At a certain moment, the positions of the drone and the obstacle are P u (x u ,y u ,z u ), P o (x o ,y o ,z o ), the UAV velocity vector is V u , the obstacle velocity vector is V o .

[0075] like Figure 2 As shown, P o As the center of the sphere, R is the radius to make a sphere, with P u Make a cone and a sphere P as the vertex o Tangent. The sphere can better simulate dynamic obstacles with strong maneuverability such as airplanes. Among them, V uo is the relative vector velocity between the two, that is, V uo =V u -V o , the magnitude of the relative velocity is v uo , d is the position of the drone P u and the obstacle center position P o The Euclidean distance between them. If the vector V uoIf the end is within the three-dimensional spatial velocity obstacle cone, the dynamic obstacle poses a threat to the UAV. Whether the obstacle poses a threat to the UAV can also be determined by comparing the magnitudes of the angles α0 and α. Here, α0 is the half vertex angle of the spatial velocity obstacle cone, and α is the angle between the relative velocity V uo and the spatial obstacle cone line. If α > α0, the obstacle is not threatening; if α < α0, the obstacle is threatening.

[0076] The above-mentioned collision avoidance between UAVs uses the classical Morse function to construct the artificial potential field. The distance ||ρ ij || between UAVs is the only variable affecting the magnitude of the potential field, and factors such as the magnitude and direction of the relative velocity between UAVs are not considered. For dynamic obstacles, only the distance ||ρ io || between the UAV and the obstacle is used to construct and adjust the magnitude of the artificial potential field, which is not flexible and efficient enough. Therefore, for obstacle avoidance in the face of obstacles, this embodiment considers improving the artificial potential field function from two aspects: (1) adjusting the action area of the artificial potential field from a fixed area to an elastic area E=(||ρ io || min ,||ρ io || max ), and ||ρ io || max is dynamically adjusted according to the magnitude and direction of the relative velocity between the UAV and the obstacle; (2) introducing the relative motion velocity vector V uo of the UAV and the obstacle into the design of the potential field function to improve the obstacle avoidance efficiency.

[0077] (1) Construction of the elastic obstacle avoidance distance:

[0078] To ensure that the UAV can successfully avoid obstacles in any scenario, such as acceleration, deceleration, etc., it is very difficult to adjust the proportional coefficient in the potential field function and select the obstacle repulsion distance ||ρ io || max . When the UAV is moving at a high flight speed, if the obstacle repulsion distance ||ρ io || maxIf the selection is too small, since the acceleration of the drone has a dynamic upper limit, even if it is subjected to the repulsive force generated by the obstacle within the obstacle repulsion distance, it cannot reach the desired reverse acceleration, resulting in the drone being unable to decelerate in time. At this time, the "braking" distance required by the drone is long while the provided "braking" distance is too short, and finally it collides with the obstacle. Therefore, the best solution is to increase the "braking" distance so that the drone starts to decelerate at a farther distance. If a larger "braking" distance is also used during low-speed movement, it will increase the obstacle avoidance distance, reduce the flight speed, and thus increase the obstacle avoidance time. In addition, according to the three-dimensional velocity obstacle model, the larger the relative motion angle between the drone and the obstacle, the shorter the time to avoid the obstacle by adjusting the drone's heading angle, and the shorter the required obstacle avoidance distance; the smaller the relative motion angle between the drone and the obstacle, the longer the time to avoid the obstacle by adjusting the drone's heading angle, and the longer the required obstacle avoidance distance. Therefore, in this embodiment, using the idea of elastic distance, the obstacle repulsion distance is set as a variable related to the speed and the relative motion direction angle to achieve flexible obstacle avoidance under different velocity vectors. Figure 3 is a schematic diagram for constructing the elastic repulsion distance. Take the relative motion velocity vector V of the drone and the obstacle uo and the cross-sectional view coplanar with the obstacle expansion sphere.

[0079] Figure 3 In, the drone U i has an obstacle detection and perception range of R d , ||ρ io || max is the elastic repulsion distance. Combining with the three-dimensional velocity obstacle avoidance model, the mathematical expression of the elastic repulsion distance ||ρ io || max is:

[0080]

[0081] Among them, is the maximum repulsion distance of the obstacle when the drone flies at a speed of , and c1 and c2 are control coefficients related to the speed and the relative angle respectively. When α ∈ [0, α0) and the smaller α is, the repulsion distance needs to be increased to gain more deceleration space for obstacle avoidance. It should be noted that since the drone cannot avoid obstacles outside the obstacle perception range, so when ||ρ io || max > R d when it is greater, it takes the value of R d . In other cases, such as or α > α0, the difficulty of the drone avoiding obstacles will not increase, so only take for obstacle avoidance calculation within the range.

[0082] (2) Adjust the size of the potential field based on the velocity vector:

[0083] Dynamically adjusting the obstacle avoidance distance according to the magnitude and direction of the relative motion speed can improve the success rate and efficiency of obstacle avoidance. To further optimize the obstacle avoidance efficiency, the size of the obstacle avoidance potential field can also be adjusted according to the magnitude and direction of the relative motion speed. Just like in daily driving, not only the distance of activating the "brake" function is determined according to the driving speed magnitude and direction, but also the "brake" force is adjusted accordingly. When the driving speed is too fast and the angle with the obstacle is small, increase the "brake" distance and force; when the driving speed is slow and the angle with the obstacle is large, appropriately reduce the "brake" distance and force.

[0084] The improved second artificial potential field function after introducing the relative motion speed vector is:

[0085]

[0086] When the UAV is within the influence range R of the obstacle d and ||V uo || → 0, When ||V uo || → ∞, It can be seen that the greater the relative motion speed, the greater the corresponding potential field function value. In addition, when the relative angle |α| < |α0| and |α| → |α0|, sin(|α0| - |α|) → 0. It can be seen that the smaller the angle α, the greater the corresponding potential field function value. It should be noted that when |α| > |α0|, according to the velocity obstacle model, the UAV will not collide with the obstacle and there is no need to avoid the obstacle, and the corresponding potential field function is 0. In short, introducing the magnitude and direction of the relative motion speed into the potential field function can enable the UAV to avoid obstacles more efficiently.

[0087] Based on the improved second artificial potential field function, taking the negative gradient of the potential field directly as the velocity field, let Calculate the obstacle avoidance velocity field of the UAV when ||ρ io || ∈ E and 0 ≤ |α| ≤ |α0|:

[0088]

[0089] Furthermore, the aforementioned improved second artificial potential field method improves the obstacle avoidance efficiency by introducing the elastic obstacle avoidance distance and the relative motion speed vector, and starts the artificial potential field obstacle avoidance with ||ρ io || ∈ E and 0 ≤ |α| ≤ |α0| as the constraint conditions. The following details the complete process and calculation process from conflict resolution to track recovery:

[0090] Assumption 1: The UAV flies straight and uniformly at a constant speed when detecting the front edge of an obstacle. After avoiding the obstacle, it resumes the original flight path as soon as possible. The dynamic obstacle is approximated as a straight-line uniform motion, and the motion state of the obstacle does not change during the conflict resolution process.

[0091] (1) Conflict resolution

[0092] As Figure 4 shown, after the UAV detects the obstacle, it avoids the obstacle according to the improved second artificial potential field method above. Under the action of the velocity field (6), the radial velocity component, that is, the velocity component along the P u P o direction, continuously decreases, and the vertical velocity component remains unchanged. Therefore, the magnitude of the UAV velocity |v uo | is adjusted to |v′ uo | and |v′ uo | < |v uo |, and the included angle α is adjusted to α′ and α′ < α. During the obstacle avoidance process, its relative included angle continuously deviates from the obstacle until α′ ≥ α o , and then the obstacle avoidance algorithm is turned off and it flies straight and uniformly at a constant speed with V′ uo .

[0093] When an obstacle is detected, the following calculations are performed:

[0094]

[0095] Furthermore, it can be obtained that

[0096]

[0097] When α > α o , the obstacle avoidance algorithm is turned off, and the UAV continues to fly forward at a constant speed V′ uo . When it reaches a certain point C, conflict resolution is achieved, and then it enters the flight path recovery stage.

[0098] (2) Flight path recovery

[0099] By adjusting the UAV velocity vector, conflict resolution can be achieved, but the original flight path is changed. Therefore, after the UAV conflict is resolved, the original flight path needs to be restored as soon as possible. As Figure 5 shown, at point C on the relative course, the UAV completes conflict resolution. Point B is the tangent point of the relative route AC and the obstacle safety circle. When the UAV reaches point C, it starts to restore the flight path. AC and A′C are symmetric about OC, and |AC| = |A′C|. Point F is the tangent point of the relative route A′C and the obstacle safety circle.

[0100] According to Figure 5 the geometric relationship in

[0101]

[0102] At this time, the conflict resolution time t is

[0103]

[0104] In the formula, V′ uo is the speed of the UAV with the obstacle avoidance algorithm turned off.

[0105] Under the action of the velocity field (6), the UAV continuously deviates from the obstacle until the angle α between the UAV and the obstacle is α = α o at which point the obstacle avoidance algorithm is turned off, and then it flies straight at a constant speed for a time t at a speed of V′ uo to reach the UAV trajectory recovery point C, and then switches to the formation keeping control law, and the original trajectory can be quickly restored. Since |AC| + |A′C| > |AA′| and |AC| = |A′C|, under the premise of assumption 1, after 2t, the expected position of the UAV is A″ and satisfies |AC| + |A″C| > |AC| + |A′C| > AA′|. The UAV achieves conflict resolution at point C. At this time, under the action of switching to the formation keeping control law, it can quickly return to the original formation flight formation keeping control state.

[0106] Based on the collision avoidance velocity field and the obstacle avoidance velocity field, obtain the total velocity field of the UAV; perform UAV anti-collision instruction conversion on the total velocity field to obtain a velocity instruction, a pitch angle instruction, and a yaw angle instruction; track the velocity instruction, the pitch angle instruction, and the yaw angle instruction, and the process of realizing inter-aircraft anti-collision and obstacle avoidance of the UAV formation includes:

[0107] Combining the inter-aircraft collision avoidance velocity field (3) and the obstacle avoidance velocity field (6), when ||ρ ij || ∈ D, ||ρ io || ∈ E and 0 ≤ |α| ≤ |α0|, the total velocity field of the UAV U i is:

[0108]

[0109] The anti-collision control algorithm synthesizes the information of the own aircraft, other aircraft, and obstacles, calculates through the artificial potential field, generates a velocity instruction, a pitch angle instruction, and a yaw angle instruction, and sends these anti-collision instructions to the flight controller. Anti-collision control is achieved by adjusting the velocity vectors of each UAV. Therefore, the desired velocity vector can be defined as

[0110]

[0111] In the formula, V i is the speed of the UAV U i

[0112] ​In the ground coordinate system, Equation (12) can be expressed as

[0113]

[0114]

[0115]

[0116] wherein, V xi 、V yi 、V zi are the three-axis components of V i , and is the three-axis component of . Subsequently, it is converted into a series of UAV anti-collision instructions, including speed instructions pitch angle instructions yaw angle instructions The specific form is solved as follows:

[0117]

[0118] Equation (16) is the anti-collision control instruction. Further, according to the UAV model, the corresponding underlying flight control law is designed to achieve the tracking of the above speed instructions pitch angle instructions yaw angle instructions . The UAVs in the formation can then achieve inter-aircraft anti-collision and obstacle avoidance.

[0119] The present invention also provides an electronic device, including: a memory and a processor; the memory is used for storing a program; the processor is used for executing the program to implement each step of the UAV formation anti-collision method based on the three-dimensional velocity obstacle model and the improved artificial potential field method.

[0120] The present invention also provides a readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, each step of the UAV formation anti-collision method based on the three-dimensional velocity obstacle model and the improved artificial potential field method is implemented.

[0121] The above is only a preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method, characterized in that: The following steps are involved: Construct an artificial potential field between drones; Improving the artificial potential field based on the communication topology and the communication weight to obtain an improved first artificial potential field; Based on the improved first artificial potential field, calculating the collision avoidance velocity field between the UAVs; Constructing an elastic obstacle avoidance distance of the UAV, and introducing a relative motion velocity vector to improve the artificial potential field under the elastic obstacle avoidance distance to obtain an improved second artificial potential field; Based on the improved second artificial potential field, calculating the obstacle avoidance velocity field of the UAV; Based on the collision avoidance velocity field and the obstacle avoidance velocity field, obtaining a total velocity field of the UAV; The total velocity field is converted into a UAV collision avoidance instruction to obtain a velocity instruction, a pitch angle instruction, and a yaw angle instruction; Tracking the speed command, pitch angle command and yaw angle command to achieve anti-collision and obstacle avoidance between drones in the drone formation; The process of constructing the elastic obstacle avoidance distance of the UAV includes: obtaining the maximum repulsion distance of the obstacle, and constructing the elastic obstacle avoidance distance of the UAV based on the maximum repulsion distance of the obstacle; The expression of the maximum repulsion distance of the obstacle is as follows: Among them, ||ρ io || max is the maximum repulsive distance of the obstacle, c1 and c2 are control coefficients related to speed and relative angle, α0 is the semi-apex angle of the space speed obstacle cone, α is the angle between the relative speed vector and the space obstacle cone line, is a set constant value, v i is the flight speed of the i-th UAV, ||ρ io || is the distance between the drone and the obstacle.

2. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 1 is characterized in that: The improved expression of the first artificial potential field is as follows: Among them, b and c are constants, D=(||ρ ij || min ,||ρ ij || max ) is the action area of ​​the artificial potential field between machines, ||ρ ij || min >0 is the minimum safe distance between machines, ||ρ ij || max is the maximum action distance of the artificial potential field between drones, and the i-th drone is represented by U i , the jth UAV is represented by U j ,ρ ij For U i To U j The position vector, a ij Indicates that from U j To U i The communication weight, J ij (||ρ ij ||) is the drone U i with U j The potential field generated between i For U i The three-dimensional position coordinates, J i (ρ i ) is the UAV i with U j The sum of the potential fields generated between them.

3. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 2 is characterized in that: Based on the improved first artificial potential field, the process of calculating the collision avoidance velocity field between unmanned aircraft includes: taking the negative gradient of the improved first artificial potential field as the velocity field, and setting Calculate ||ρ ij ||∈D when the UAV collision avoidance velocity field is:

4. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 1, characterized in that: The improved expression of the second artificial potential field is as follows: Among them, V uo is the relative motion velocity vector between the drone and the obstacle, E=(||ρ io || min ,||ρ io || max ) is the elastic obstacle avoidance distance, ||ρ io || min is the minimum obstacle repulsion distance, ||ρ io || max is the maximum obstacle repulsion distance, U i (ρ io ) is the improved artificial potential field function, b o 、c o All are adjustable constants.

5. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 4 is characterized in that: Based on the improved second artificial potential field, the process of calculating the obstacle avoidance velocity field of the UAV includes: taking the negative gradient of the improved second artificial potential field as the velocity field, Calculate ||ρ io When ||∈E and 0≤|α|≤|α0|, the obstacle avoidance velocity field of the drone is: Among them, J i (||ρ io ||) is the potential field generated between the UAV and the obstacle.

6. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 1, characterized in that: The expressions of the speed command, pitch angle command and yaw angle command are as follows: in, is the speed command, is the pitch angle command, is the yaw angle command, are the three-axis components of the total velocity.

7. The UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method according to claim 1, characterized in that: The process of obstacle avoidance based on the obstacle avoidance velocity field of the drone includes: The UAV adjusts the velocity vector based on the improved second artificial potential field to avoid obstacles, and then calculates the relative distance and conflict resolution time when the conflict is resolved based on the geometric relationship of the UAV track recovery adjusted based on the minimum heading. After reaching the conflict resolution time, the UAV performs track recovery and switches to the formation keeping control law until the original track is restored.

8. An electronic device, characterized in that: include: Memory and processor; The memory is used to store programs; The processor is used to execute the program to implement the various steps of the UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method as described in any one of claims 1-7.

9. A readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by the processor, the steps of the UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method as described in any one of claims 1 to 7 are implemented.

Images