Unmanned aerial vehicle formation anti-collision method based on three-dimensional speed obstacle model and improved artificial potential field method

Abstract

The invention discloses an unmanned aerial vehicle formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method. The method comprises the following steps: improving an artificial potential field based on a communication topology and a communication weight; based on the improved first artificial potential field, a collision avoidance speed field between the unmanned aerial vehicles is calculated; constructing an elastic obstacle avoidance distance of the unmanned aerial vehicle, and introducing a relative motion velocity vector to improve an artificial potential field under the elastic obstacle avoidance distance; based on the improved second artificial potential field, calculating an obstacle avoidance speed field of the unmanned aerial vehicle; based on the collision avoidance speed field and the obstacle avoidance speed field, obtaining a total speed field of the unmanned aerial vehicle; performing unmanned aerial vehicle anti-collision instruction conversion on the total speed field to obtain a speed instruction, a pitch angle instruction and a yaw angle instruction; and tracking the instructions to realize inter-machine collision prevention and obstacle avoidance. According to the invention, communication topology and collision avoidance priority are considered in inter-machine collision avoidance, the obstacle avoidance distance and potential field value can be dynamically adjusted, invalid obstacle avoidance maneuver is avoided, and the overall efficiency is higher.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle formation anti-collision, and in particular relates to an unmanned aerial vehicle formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method. Background Art

[0002] UAV formation flight can improve the efficiency of completing tasks and expand the scope of UAV use, 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 execution of the mission. Therefore, enhancing the anti-collision capability of UAVs is one of the keys to improving the success rate of the formation's mission execution.

[0003] The obstacle avoidance algorithm of UAV can be divided into global obstacle avoidance algorithm and local obstacle avoidance algorithm according to whether the global map is known or not: global obstacle avoidance is essentially one of the constraints of trajectory planning, and the optimal path is found through continuous iteration; local obstacle avoidance algorithm is aimed at unknown or dynamic environments, and only avoids obstacles through the surrounding environment information perceived by onboard sensors, which is easy to fall into local optimum. Although global obstacle avoidance can be performed through trajectory planning in known environments, more flexible local obstacle avoidance algorithms are required to re-plan the trajectory in the face of sudden and unknown obstacles in dynamic environments. Complex obstacle environments often easily cause cluster obstacle avoidance failures. Therefore, studying the local dynamic obstacle avoidance algorithm of UAV clusters in complex environments is one of the keys to ensure the safe flight of clusters.

[0004] The artificial potential field method was originally proposed by Khatib as a virtual force method, which makes the distribution and shape of obstacles and other information reflected in the potential field value at each point in the environment. According to the size of the potential field value, the drone determines the direction and speed of travel. At present, the artificial potential field method is rarely used to avoid collisions between drones. When it is used for obstacle avoidance, it is usually only based on the distance between the drone and the obstacle. There are problems such as too early obstacle avoidance or too late obstacle avoidance, and in some cases, unnecessary obstacle avoidance maneuvers cause energy consumption and time waste. The obstacle avoidance efficiency needs to be further improved.

[0005] The speed obstacle method calculates the speed and heading required for obstacle avoidance by analyzing the spatial geometric relationship between the drone and the dynamic obstacle. This method is mostly used for obstacle avoidance of a single drone, and is not suitable for collision avoidance between drone formations. In addition, most existing studies only avoid obstacles by adjusting the heading or speed. Since the speed change range is limited, simply changing the speed cannot achieve the purpose of complete collision avoidance. At the same time, simply changing the heading angle will result in too long obstacle avoidance time.

[0006] In view of the problems in the existing technology, it is urgent to propose a UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method. Summary of the invention

[0007] In order to solve the above technical problems, the present invention proposes a UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method. For the problem of collision avoidance between aircraft, communication topology and collision avoidance priority are introduced, and the artificial potential field method is improved to optimize collision avoidance between aircraft; for the problem of airspace obstacle avoidance, the geometric relationship of the three-dimensional speed obstacle model is used to quantitatively calculate the reference points for UAV conflict resolution and track recovery, and the relative motion speed vector between the UAV and the obstacle is introduced to dynamically adjust the obstacle avoidance 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 instruction. In order to solve the problems existing in the above prior art.

[0008] To achieve the above object, the present invention provides a UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method, comprising the following steps:

[0009] Construct an artificial potential field between drones;

[0010] Improving the artificial potential field based on the communication topology and the communication weight to obtain an improved first artificial potential field;

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

[0012] 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;

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

[0014] Based on the collision avoidance velocity field and the obstacle avoidance velocity field, obtaining a total velocity field of the UAV;

[0015] 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;

[0016] The speed command, pitch angle command and yaw angle command are tracked to achieve anti-collision and obstacle avoidance between drones in the drone formation.

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

[0018]

[0019] 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.

[0020] Optionally, 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:

[0021]

[0022] Optionally, the process of constructing the elastic obstacle avoidance distance of the drone includes: obtaining a maximum obstacle repulsion distance, and constructing the elastic obstacle avoidance distance of the drone based on the maximum obstacle repulsion distance;

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

[0024]

[0025] 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.

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

[0027]

[0028] 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.

[0029] Optionally, based on the improved second artificial potential field, the process of calculating the obstacle avoidance velocity field of the drone 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:

[0030]

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

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

[0033]

[0034] in, is the speed 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 performing obstacle avoidance based on the obstacle avoidance velocity field of the drone includes:

[0036] 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.

[0037] The present invention also provides an electronic device, comprising: 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 UAV formation anti-collision method based on a three-dimensional speed 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 UAV formation anti-collision method based on a three-dimensional speed 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 UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method. For the problem of collision avoidance between aircraft, communication topology and collision avoidance priority are introduced, and the artificial potential field method is improved to optimize the collision avoidance between aircraft. For the problem of airspace obstacle avoidance, the relative motion speed 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 instruction.

[0041] Compared with the speed obstacle method, the method of the present invention is easier to handle the problem of collision avoidance between machines, and uses the artificial potential field method to comprehensively adjust the speed, pitch angle, yaw angle, etc. to avoid obstacles, which is more efficient and has a higher success rate; compared with the traditional artificial potential field method, the collision avoidance between machines takes into account the actual communication topology and collision avoidance priority, can dynamically adjust the obstacle avoidance distance and potential field value, can avoid invalid obstacle avoidance maneuvers, shorten the obstacle avoidance time, and has higher overall efficiency and a higher success rate. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

[0045] Figure 3A schematic diagram of elastic repulsion distance construction according to an embodiment of the present invention;

[0046] Figure 4 A schematic diagram of the speed vector adjustment of a UAV according to an embodiment of the present invention;

[0047] Figure 5 A schematic diagram of a UAV track recovery based on minimum heading adjustment according to an embodiment of the present invention;

[0048] Figure 6 The present invention is a flowchart of a method for avoiding collision of UAV formations based on a three-dimensional speed obstacle model and an improved artificial potential field method. DETAILED DESCRIPTION

[0049] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

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

[0051] Embodiment 1

[0052] like Figure 6 As shown, in this embodiment, a UAV formation anti-collision method based on a three-dimensional speed obstacle model and an improved artificial potential field method is provided, comprising the following steps:

[0053] Construct an artificial potential field between drones;

[0054] Improving the artificial potential field based on the communication topology and the communication weight to obtain an improved first artificial potential field;

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

[0056] 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;

[0057] Based on the improved second artificial potential field, calculating the obstacle avoidance velocity field of the UAV;

[0058] Based on the collision avoidance velocity field and the obstacle avoidance velocity field, obtaining a total velocity field of the UAV;

[0059] 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;

[0060] The speed command, pitch angle command and yaw angle command are tracked to achieve anti-collision and obstacle avoidance between drones in the drone formation.

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

[0062] Consider a fleet of n drones, with the i-th drone denoted as U i The artificial potential field method is used for anti-collision control between machines, and the potential field function is only used for obstacle avoidance, not for organizing formations, so only the repulsive potential field function is constructed. Let J i (ρ i ) is the UAV i With other machines j The potential field J generated between ij (ρ ij ), and for each position in space, J ij (ρ ij ) are all differentiable. Among them, ρ i For U i The three-dimensional position coordinates, ρ ij For U i To U j The artificial potential field between the UAVs is as follows: Figure 1 shown.

[0063] The artificial potential field is improved based on the communication topology and the communication weight to obtain an improved first artificial potential field; and the process of calculating the collision avoidance velocity field between unmanned aircraft based on the improved first artificial potential field includes:

[0064] Traditional artificial potential field assumes that the UAV U i Can obtain all other aircraft in the formation j This is often inconsistent with the actual communication topology; and the anti-collision priority of the UAVs in the formation is not considered. In order to overcome these two defects, this embodiment introduces the concept of multi-agent consistency, that is, adding communication topology and communication weight variables to form a new artificial potential field. i Communication UAV j The set formed is N i , U i It can only obtain information about the drone that communicates with it, such as position, attitude, etc. ij Indicates that from U j To U i The communication weight reflects the j Information for U i The importance of UAV i The relationship between the total potential field and each component potential field is as follows:

[0065]

[0066] In the formula, ||·|| is the L2 norm. i When performing collision avoidance maneuvers between aircraft, priority is given to avoiding UAVs with high communication weights. When there is a risk of collision between aircraft, the secondary UAVs are destroyed first, giving priority to ensuring the safety of the important UAVs.

[0067] This embodiment designs the following improved first artificial potential field based on the classical Morse function:

[0068]

[0069] In the formula, b and c are constants, which respectively determine the amplitude and change speed of the repulsive potential, and are both adjustable parameters. ij || min ,||ρ ij || max ) determines the area of ​​action of the artificial potential field between machines, ||ρ ij || min >0 is the minimum safe distance between machines, ||ρ ij || is less than this value, a collision between machines occurs, ||ρ ij || max is the maximum range of the artificial potential field between drones. The drone cluster maintains formation flight according to the formation maintenance control law. When ||ρ ij ||≤||ρ ij || max Switch to the collision avoidance mode when ||ρ ij ||>||ρ ij || max Then switch to the formation flight mode according to the formation keeping control law. Here, it is required that ||ρ ij || max Smaller than the distance between aircrafts set to maintain stable formation flight and the obstacle detection range of the UAV's onboard sensors.

[0070] The negative gradient of the potential field function (2) is directly used as the velocity field, Calculate ||ρ ij ||∈D when the UAV collision avoidance velocity field is:

[0071]

[0072] The elastic obstacle avoidance distance of the UAV is constructed. Under the elastic obstacle avoidance distance, a relative motion velocity vector is introduced to improve the artificial potential field to obtain an improved second artificial potential field. Based on the improved second artificial potential field, the process of calculating the obstacle avoidance velocity field of the UAV 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 space speed obstacle cone, the dynamic obstacle poses a threat to the drone. Whether the obstacle poses a threat to the drone can also be determined by comparing the angle α0 and α. Among them, α0 is the half-apex angle of the space speed obstacle cone, and α is the relative speed V uo The angle between the obstacle and the cone line of the space obstacle. If α>α0, the obstacle is not threatening; if α<α0, the obstacle is threatening.

[0076] The above-mentioned collision avoidance between machines uses the classic Morse function to construct the artificial potential field, and the distance between machines ||ρ ij || is the only variable that affects the potential field size, and does not take into account factors such as the relative speed and direction between drones. For dynamic obstacles, only the distance between the drone and the obstacle ||ρ io || to construct and adjust the size of the artificial potential field is not flexible and efficient enough. Therefore, in the face of obstacle avoidance, this embodiment considers improving the artificial potential field function from two aspects: (1) adjusting the artificial potential field action area from a fixed area to an elastic area E = (||ρ io || min ,||ρ io || max ),||ρ io || max Dynamically adjust the relative speed between the UAV and the obstacle according to its magnitude and direction; (2) The relative speed vector V between the UAV and the obstacle is uo The potential field function design is introduced to improve the obstacle avoidance efficiency.

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

[0078] In order to ensure that the drone can successfully avoid obstacles in any scenario, such as acceleration and deceleration, the adjustment of the proportional coefficient in the potential field function and the obstacle repulsion distance ||ρ io || max The selection of is very difficult. When the UAV moves at a higher flight speed, if the obstacle repulsion distance ||ρ io || maxIf the value is too small, since the acceleration of the drone has a dynamic upper limit, even if the drone is subjected to the repulsive force generated by the obstacle within the obstacle repulsion distance, it cannot achieve the desired reverse acceleration, resulting in the drone being unable to decelerate in time. At this time, the drone requires a long "braking" distance and the provided "braking" distance is too short, and eventually collides with the obstacle. For this reason, 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 under low-speed motion, the obstacle avoidance distance will be increased, the flight speed will be reduced, and the obstacle avoidance time will be increased. In addition, according to the three-dimensional speed 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 heading angle of the drone, 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 heading angle of the drone, and the longer the required obstacle avoidance distance. Therefore, this embodiment uses the idea of ​​elastic distance to set the obstacle repulsion distance as a variable related to the speed and the angle of relative motion direction, so as to achieve flexible obstacle avoidance under different speed vectors. Figure 3 This is a schematic diagram of the elastic repulsion distance. Take the relative motion velocity vector V between the drone and the obstacle. uo Cross-section view coplanar with the obstacle expansion sphere.

[0079] Figure 3 In the UAV i The obstacle detection sensing range is R d ,||ρ io || max is the elastic repulsion distance. Combined with the three-dimensional speed obstacle avoidance model, the elastic repulsion distance ||ρ io || max The mathematical expression is:

[0080]

[0081] in, Speed ​​for drones The maximum obstacle repelling distance during flight, c1, c2 are control coefficients related to speed and relative angle respectively. α∈[0,α0) and the smaller α is, the more repulsion distance needs to be increased to gain more deceleration space for obstacle avoidance. Because the drone cannot avoid obstacles outside the obstacle sensing range, when ||ρ io || max >R d When it is greater than R d In other cases, such as Or when α>α0, the difficulty of obstacle avoidance of the UAV will not increase, so it is only necessary to take Obstacle avoidance calculations are performed within the range.

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

[0083] Dynamically adjusting the obstacle avoidance distance according to the relative speed and direction can improve the success rate and efficiency of obstacle avoidance. To further optimize the obstacle avoidance efficiency, the obstacle avoidance potential field can also be adjusted according to the relative speed and direction. Just as in daily driving, not only the braking distance is determined based on the speed and direction of the vehicle, but also the braking force is adjusted accordingly. When the vehicle is driving too fast and the angle with the obstacle is small, increase the braking distance and force; when the vehicle is driving slowly and the angle with the obstacle is large, appropriately reduce the braking distance and force.

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

[0085]

[0086] When the drone is within the obstacle range R d Inside, when ||V uo ||→0, When ||V uo When ||→∞, It can be seen that the greater the relative 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 is worth noting that when |α|>|α0|, according to the speed obstacle model, the drone 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 relative speed and direction in the potential field function can make the drone avoid obstacles more efficiently.

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

[0088]

[0089] Furthermore, the aforementioned improved second artificial potential field method improves the obstacle avoidance efficiency by introducing elastic obstacle avoidance distance and relative motion velocity vector, and by ||ρ io ||∈E and 0≤|α|≤|α0| are used as constraints to enable artificial potential field obstacle avoidance. The complete flow and calculation process from conflict resolution to track recovery are described in detail below:

[0090] Assumption 1: The UAV flies in a straight line at a constant speed when it detects an obstacle, and returns to its original track as soon as possible after avoiding the obstacle. The dynamic obstacle is approximately regarded as a straight line with constant speed motion, and the motion state of the obstacle does not change during the conflict resolution process.

[0091] (1) Conflict resolution

[0092] like Figure 4 As shown in the figure, after the UAV detects an obstacle, it avoids the obstacle according to the improved second artificial potential field method. Under the action of the velocity field (6), the radial velocity component, i.e., along P u P o The velocity component in the direction of the UAV decreases continuously, while the vertical velocity component remains unchanged. Therefore, the velocity magnitude of the UAV is |v uo |Adjust to |v′ uo | and |v′ uo |<|v uo |, the angle α is adjusted to α′ and α′<α, and during the obstacle avoidance process, its relative angle continuously deviates from the obstacle until α′≥α o , and then turn off the obstacle avoidance algorithm and use V′ uo Maintain a straight and constant speed.

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

[0094]

[0095] Then we can get

[0096]

[0097] When α>α o When the obstacle avoidance algorithm is turned off, the drone continues to maintain a constant speed V′ uo Fly forward, resolve the conflict when reaching point C, and then enter the track recovery phase.

[0098] (2) Track recovery

[0099] By adjusting the UAV velocity vector, the conflict can be resolved, but the original flight track is changed. Therefore, it is necessary to restore the original track as soon as possible after the UAV conflict is resolved. Figure 5 As shown in the figure, the UAV completes the conflict resolution at the relative heading point C, where point B is the tangent point between the relative route AC and the obstacle safety circle. When the UAV reaches point C, it starts to recover the track. AC and A′C are symmetric about OC, and |AC|=|A′C|. Point F is the tangent point between the relative route A′C and the obstacle safety circle.

[0100] according to Figure 5 The geometric relationship in the figure shows that the relative distance S when the UAV conflict is resolved is:

[0101]

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

[0103]

[0104] Where V′ uo The speed of the drone when the obstacle avoidance algorithm is turned off.

[0105] Under the action of the velocity field (6), the UAV keeps moving away from the obstacle until the angle between the UAV and the obstacle is α = α o Turn off the obstacle avoidance algorithm and then move at a speed of V′ uo After a straight uniform flight time t, the drone reaches the track recovery point C, and then switches to the formation keeping control law to quickly restore the original track. Since AC|+|A′C|>|AA′| and |AC|=|A′C|, under the premise of assumption 1, after 2t, the desired position of the drone is A″ and satisfies |AC|+|A″C|>|AC|+|A′C|>AA′|. The drone has achieved conflict resolution at point C. At this time, it switches to the formation keeping control law and can quickly restore to the original formation flying formation keeping control state.

[0106] Based on the collision avoidance speed field and the obstacle avoidance speed field, a total speed field of the UAV is obtained; the total speed field is converted into a UAV collision avoidance instruction to obtain a speed instruction, a pitch angle instruction, and a yaw angle instruction; the speed instruction, the pitch angle instruction, and the yaw angle instruction are tracked to achieve collision avoidance and obstacle avoidance between UAV formations, including:

[0107] Combining the collision avoidance velocity field (3) and the obstacle avoidance velocity field (6), when ||ρ ij ||∈D,||ρ io When ||∈E and 0≤|α|≤|α0|, we get the drone U i The total velocity field is:

[0108]

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

[0110]

[0111] Where V i For UAV i speed.

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

[0113]

[0114]

[0115]

[0116] Where V xi 、V yi 、V zi V i The three-axis components of for The three-axis components. This is then converted into a series of UAV collision avoidance instructions, including speed instructions. Pitch angle command Yaw angle command The specific form of solution is as follows:

[0117]

[0118] Formula (16) is the anti-collision control instruction. Further, according to the UAV model, the corresponding underlying flight control law is designed to implement the above speed instruction Pitch angle command Yaw angle command With the tracking of the drones, the drones in the formation can avoid collisions and obstacles.

[0119] The present invention also provides an electronic device, comprising: 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 UAV formation anti-collision method based on a three-dimensional speed obstacle model and an 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 a three-dimensional speed obstacle model and an improved artificial potential field method is implemented.

[0121] The above are only preferred specific implementations 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 a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on 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; The speed command, pitch angle command and yaw angle command are tracked to achieve anti-collision and obstacle avoidance between drones in the drone formation.

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 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.

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: 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.

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 5, 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.

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 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.

8. 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.

9. 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-8.

10. 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 8 are implemented.

Images