CN111880574A

CN111880574A - Unmanned aerial vehicle collision avoidance method and system

Info

Publication number: CN111880574A
Application number: CN202010770573.8A
Authority: CN
Inventors: 汤俊; 万宇; 老松杨; 秦婉亭; 陈曦; 卢聪; 李�浩
Original assignee: National University of Defense Technology
Current assignee: National University of Defense Technology
Priority date: 2020-08-04
Filing date: 2020-08-04
Publication date: 2020-11-03
Anticipated expiration: 2040-08-04
Also published as: CN111880574B

Abstract

The invention discloses a collision avoidance method and system for an unmanned aerial vehicle, wherein the method comprises the steps of judging whether a collision exists with a local machine or not according to the information of the local machine and peripheral unmanned aerial vehicles; when a conflict exists, communicating with the potentially threatening unmanned aerial vehicle; determining to threaten the unmanned aerial vehicle and screening two collision avoidance maneuvers; generating a plan track, sending the plan track to the threat unmanned aerial vehicle, and acquiring the plan track or flight information of the threat unmanned aerial vehicle; reconstructing motion tracks of the threat unmanned aerial vehicle on the self-adaptive unmanned aerial vehicle; determining an optimal collision avoidance maneuver and sending the optimal collision avoidance maneuver to the threat unmanned aerial vehicle, wherein the rest collision avoidance maneuvers are used as alternative collision avoidance maneuvers; obtaining an optimal collision avoidance maneuver determined by the threat unmanned aerial vehicle according to the cost function; the time determined by the minimum avoidance interval value and the excitation function in the collision risk excites the optimal collision avoidance maneuver to implement avoidance; and generating and outputting a new avoidance instruction based on the optimized collision avoidance maneuver to finish collision avoidance. The method solves the problems of insufficient processing capacity of the dynamic scene and the like in the prior art, and improves the processing capacity and the fault tolerance rate of the multi-threat collision avoidance scene and the dynamic threat scene.

Description

Unmanned aerial vehicle collision avoidance method and system

Technical Field

The invention relates to the technical field of unmanned aerial vehicles, in particular to an unmanned aerial vehicle collision avoidance method and system.

Background

The collision avoidance strategy based on the principle of 'no interference', the traditional collision avoidance algorithm idea is to carry out collision detection on the unmanned aerial vehicle, once the collision threat exists, the unmanned aerial vehicle immediately generates a strategy and carries out collision avoidance maneuver, and the idea has the following disadvantages: the processing capacity to many threats, dynamic threat is not enough, simultaneously in order to guarantee collision avoidance, need carry out collision avoidance for a long time before the collision takes place to improve the fault-tolerant rate, this also is great to unmanned aerial vehicle's normal motion interference.

In a traditional collision avoidance algorithm, a motion model of an unmanned aerial vehicle is mostly based on a three-degree-of-freedom (3Dof) motion equation, a single machine is simply regarded as a mass point, performance limitation of the single machine is not considered, in a collision avoidance process, a fixed wing unmanned aerial vehicle needs to perform actions such as rolling and climbing, a conventional three-degree-of-freedom-based motion equation cannot accurately describe the motion process of the fixed wing unmanned aerial vehicle, and a complex six-degree-of-freedom (6Dof) motion equation has the defects of large calculated amount and the like.

The traditional geometric algorithm is a collision-free track planning algorithm, has a good collision avoidance effect on a specific collision avoidance scene, has certain limitations, and is particularly difficult to solve the problems of formation collision avoidance, autonomous collision avoidance and the like.

Disclosure of Invention

The invention provides an unmanned aerial vehicle collision avoidance method and system, which are used for overcoming the defects of insufficient processing capability on multi-threat and dynamic threat collision avoidance scenes, large interference on normal motion of an unmanned aerial vehicle and the like in the prior art and improving the processing capability and fault tolerance rate on the multi-threat collision avoidance scenes and the dynamic threat scenes.

In order to achieve the purpose, the invention provides an unmanned aerial vehicle collision avoidance method, which comprises the following steps:

step 1, judging whether the local unmanned aerial vehicle conflicts with the peripheral unmanned aerial vehicle or not according to the acquired navigation information and GPS data of the local unmanned aerial vehicle and the peripheral unmanned aerial vehicle;

step 2, establishing data link communication between the unmanned aerial vehicle and the potential threat unmanned aerial vehicle under the condition that a conflict exists; determining a threat unmanned aerial vehicle according to the threat degree;

screening at least two of a plurality of preset anti-collision machines according to the situation of the threat unmanned aerial vehicle;

step 3, generating a planned track of the self-body corresponding to the screened anti-collision maneuvers respectively by using a track prediction algorithm, sending the planned track to the threat unmanned aerial vehicle to realize the occupation of the airspace corresponding to the planned track by the threat unmanned aerial vehicle, and acquiring related data or flight information corresponding to the planned track of the threat unmanned aerial vehicle;

step 4, reconstructing a planned track of the threat unmanned aerial vehicle according to relevant data or flight information corresponding to the planned track of the threat unmanned aerial vehicle;

step 5, comparing a plurality of combinations of the planned track of the self-body and the planned track of the threat unmanned aerial vehicle through an evaluation algorithm, determining the optimal collision avoidance maneuver of the self-body from the screened collision avoidance maneuvers according to the minimum avoidance interval value, and sending the optimal collision avoidance maneuver to the threat unmanned aerial vehicle through a data link, wherein the rest screened collision avoidance maneuvers are used as alternative collision avoidance maneuvers;

meanwhile, obtaining the optimal collision avoidance maneuver threatening the unmanned aerial vehicle determined according to the cost function;

and 6, judging whether collision risks exist between the self body and the threat unmanned aerial vehicle or not according to the optimal anticollision maneuver of the self body and the optimal anticollision maneuver of the threat unmanned aerial vehicle, and if so, exciting the optimal anticollision maneuver of the self body and the optimal anticollision maneuver of the threat unmanned aerial vehicle to implement avoidance.

In order to achieve the above object, the present invention further provides an unmanned aerial vehicle collision avoidance system, which includes a memory and a processor, wherein the memory stores an unmanned aerial vehicle collision avoidance program, and the processor executes the steps of the above method when running the unmanned aerial vehicle collision avoidance program.

The method and the system provided by the invention adopt the collision avoidance thought of 'no interference', namely, the interference on the normal flight of the unmanned aerial vehicle is reduced to the maximum extent, and the collision avoidance maneuver is triggered at the last moment before the collision. The fixed-wing unmanned aerial vehicle firstly screens out candidate anti-collision strategies in a preset strategy set according to threat situation, the strategy set of the unmanned aerial vehicle is set off-line in advance according to past experience, and the anti-collision strategies can be guaranteed to be feasible through inspection. And then, the unmanned aerial vehicle determines an optimal collision avoidance strategy through cooperation, and the corresponding planned flight path is the reservation of the unmanned aerial vehicle to a certain airspace. Through planning unmanned aerial vehicle's escape route in advance, guarantee that unmanned aerial vehicle exists the escape route all the time at the conflict in-process, can enough effectively reduce the collision risk of unmanned aerial vehicle formation, can activate collision avoidance strategy activation time as late as possible again, can reduce the interference to fixed wing unmanned aerial vehicle to the at utmost, can effectively apply to in independently cooperating the flight of fixed wing unmanned aerial vehicle formation.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the structures shown in the drawings without creative efforts.

Fig. 1 is a working schematic diagram of a collision avoidance method for an unmanned aerial vehicle according to an embodiment of the present invention;

FIG. 2 is a schematic view of a planned flight path;

FIG. 3(a) is a first geometric situation diagram of a threat drone appearing around a target aircraft;

FIG. 3(b) is a second geometric situation diagram of a threat to drones around a target aircraft;

FIG. 4 is a graph of trajectory prediction based on a 3DOF model;

FIG. 5 is a schematic view of a planned flight path;

FIG. 6 is a drone control diagram;

FIG. 7 is a schematic diagram of track management processing of multi-source data;

FIG. 8 is a schematic view of a delay frame;

FIG. 9 is a schematic view of a maneuver selection;

FIG. 10 is a schematic diagram of the calculation of the activation time of the crash barrier maneuver excitation control module;

FIG. 11 is a schematic diagram of crash maneuver excitation control module calculations;

fig. 12 is a flow chart of the operation of an automatic air crash system in accordance with an embodiment of the present invention.

The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

It should be noted that all the directional indicators (such as up, down, left, right, front, and rear … …) in the embodiment of the present invention are only used to explain the relative position relationship between the components, the movement situation, etc. in a specific posture (as shown in the drawing), and if the specific posture is changed, the directional indicator is changed accordingly.

In addition, the descriptions related to "first", "second", etc. in the present invention are only for descriptive purposes and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "a plurality" means at least two, e.g., two, three, etc., unless specifically limited otherwise.

In the present invention, unless otherwise expressly stated or limited, the terms "connected," "secured," and the like are to be construed broadly, and for example, "secured" may be a fixed connection, a removable connection, or an integral part; the connection can be mechanical connection, electrical connection, physical connection or wireless communication connection; they may be directly connected or indirectly connected through intervening media, or they may be connected internally or in any other suitable relationship, unless expressly stated otherwise. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.

In addition, the technical solutions in the embodiments of the present invention may be combined with each other, but it must be based on the realization of those skilled in the art, and when the technical solutions are contradictory or cannot be realized, such a combination of technical solutions should not be considered to exist, and is not within the protection scope of the present invention.

Example one

As shown in fig. 1, an embodiment of the present invention provides an unmanned aerial vehicle collision avoidance method, including the following steps:

the step of determining the threat unmanned aerial vehicle in the step 2 comprises the following steps:

step 201, when a threat unmanned aerial vehicle is also loaded with an automatic aerial collision avoidance system, the threat unmanned aerial vehicle is a cooperative threat unmanned aerial vehicle, the threat unmanned aerial vehicle establishes communication with the cooperative threat unmanned aerial vehicle through a specified data link, and self state information sent by the cooperative threat unmanned aerial vehicle is obtained;

step 202, when the threat unmanned aerial vehicle is not equipped with an automatic aerial collision avoidance system, the threat unmanned aerial vehicle is a non-cooperative threat unmanned aerial vehicle, and the state information of the non-cooperative threat unmanned aerial vehicle is obtained by the unmanned aerial vehicle through a radar or other ways;

step 203, sorting the state information of the threat unmanned aerial vehicles passing through different sources, wherein the priority of the special data chain from the automatic air collision avoidance system is the highest, the priority of the data chain from the non-automatic air collision avoidance system is the second priority, and the priority of the data chain from the radar and other unknown data sources is the last;

and step 204, tracking the invading unmanned aerial vehicles according to the priority and calculating the threat degree of the invading unmanned aerial vehicles in a multi-vehicle situation, and selecting three unmanned aerial vehicles with the highest threat degree as threat machines.

The step of screening the collision avoidance maneuver in the step 2 comprises the following steps:

step 21, realizing nine collision avoidance maneuvers in a preset direction by changing a roll angle EA and an overload factor NZ;

step 22, simplifying the motion model of the unmanned aerial vehicle from a six-degree-of-freedom motion model into a three-degree-of-freedom motion model, and constructing a relative motion track threatening the unmanned aerial vehicle to reach a space closest point CPA from the current position according to the flight state of the threatening unmanned aerial vehicle;

and 23, screening at least two of the nine anti-collision mechanisms according to the relative motion tracks.

For example, when the relative motion trajectory threatens that the unmanned aerial vehicle appears above, on the left side and in the rear direction of the self-propelled vehicle, and is passing through from the left to the right and from the front to the rear and from the top to the bottom, the self-propelled vehicle adopts the upward and leftward anticollision maneuver;

relative motion track appears in target unmanned aerial vehicle below, right side, left direction for threatening unmanned aerial vehicle, and positive relative target unmanned aerial vehicle is from right to left, from back to front, when following up to pass through, and the local takes the anticollision maneuver of upwards, left.

as shown in FIG. 2, the planned flight path is a cone of area that increases in size over time depending on the uncertainty of the predicted flight path.

In step 3, generating the planned trajectories respectively corresponding to the screened crash maneuvers by using a trajectory prediction algorithm, includes:

step 31, outputting a series of predicted positions corresponding to a preset time period according to a preset operation cycle based on a six-degree-of-freedom motion equation;

step 32, predicting the uncertainty distance of the track around each predicted position according to the uncertainty of the flight path, and forming a circular area on the cross section passing through the predicted position;

and step 33, forming a conical area taking the series of predicted positions as a central line according to the condition that the uncertain distance increases along with the time.

The step 3 of obtaining the planned trajectory or flight information threatening the unmanned aerial vehicle comprises the following steps:

step 301, sending a planned trajectory to the threatening unmanned aerial vehicle;

step 302, when the threat unmanned aerial vehicle is a cooperative unmanned aerial vehicle, receiving a predicted track of the threat unmanned aerial vehicle responding to a local vehicle and generating a planned track corresponding to the screening collision avoidance maneuver according to a track prediction algorithm;

and when the threat unmanned aerial vehicle is a non-cooperative unmanned aerial vehicle, receiving acceleration information and speed information of the threat unmanned aerial vehicle.

the step 4 comprises the following steps:

when relevant data corresponding to a planned track threatening the unmanned aerial vehicle comes from a special data chain of an automatic aerial collision avoidance system (namely when the threatening unmanned aerial vehicle is a cooperative unmanned aerial vehicle), reconstructing a motion track of the threatening unmanned aerial vehicle according to the maneuvering flight track information contained in the special data chain;

when the flight information threatening the drone comes from a non-automatic air collision avoidance system data link, radar, or other source of unknown data (i.e., when the threatening drone is a non-cooperative drone):

if the acceleration information is obtained and the target machine moves at low overload acceleration, predicting the movement track of the target machine by using a kinematical Model movement Model;

if the acceleration information can not be obtained, predicting the motion track along a speed vector expansion mode;

if acceleration information is not available and the threat machine speed is in a very low range, then the ballistic model is used to predict its motion trajectory.

the planned trajectory of the local machine and the planned trajectory of the threat drone may form a plurality of combinations that are compared using an evaluation algorithm.

The step 5 comprises the following steps:

step 51, combining a planned track corresponding to the unmanned aerial vehicle anticollision maneuver with a motion track threatening the reconstruction of the unmanned aerial vehicle on the unmanned aerial vehicle one by one;

aiming at any two motion track combinations of the self-propelled vehicle and the threat unmanned aerial vehicle, outputting the difference between the track center distance and the track uncertainty distance UD in the space-time domain as a minimum avoidance distance value AD;

and step 52, determining the optimal collision avoidance maneuver of the local machine according to the motion track combination with the maximum minimum avoidance interval value. Unmanned aerial vehicle is at the collaborative in-process, and communication between unmanned aerial vehicle needs certain time, because unmanned aerial vehicle need reserve latency and guarantee in coordination, threatens constantly to circulate between unmanned aerial vehicle and cooperates at the in-process of calculating minimum avoidance interval value, cooperates each time and all is based on the latest update that produces constantly is mobile.

The step 6 comprises the following steps:

step 6, judging whether collision risks exist between the unmanned aerial vehicle and the threat unmanned aerial vehicle, and if the collision risks exist, exciting the optimal collision avoidance maneuver to implement avoidance according to the minimum avoidance interval value and the time determined by the excitation function; the excitation function targets the anti-collision maneuvering excitation time at the latest on the basis of providing enough time protection intervals;

the step 6 comprises the following steps:

step 61, extracting a minimum avoidance interval between motion tracks corresponding to the two collision avoidance maneuvers according to the optimal collision avoidance maneuver determined by the unmanned aerial vehicle and the optimal collision avoidance maneuver determined by the threat unmanned aerial vehicle;

step 62, calculating an allowable minimum trajectory distance MASD, wherein the MASD is the sum of the semi-span WS of the unmanned aerial vehicle and the threat unmanned aerial vehicle, and the sum of the expected distance DSD input by the system and the uncertainty;

and step 63, when the unmanned aerial vehicle judges that the planned track of the unmanned aerial vehicle overlaps with the planned track of the threatening unmanned aerial vehicle, namely the predicted minimum distance PMR is smaller than the allowable minimum track distance MASD (the minimum avoidance distance is the same), activating the collision avoidance maneuver.

A hybrid motion model of the unmanned aerial vehicle is built, a six-degree-of-freedom motion equation-based track prediction model is built aiming at track prediction with high precision requirement, the collision avoidance strategy of the unmanned aerial vehicle can be accurately converted into a corresponding planned track, and track prediction faster than real time is provided. And for the conventional flight process, a Piccolo model is adopted for representation. By constructing the hybrid motion model of the fixed-wing unmanned aerial vehicle, the accuracy of the flight path of the unmanned aerial vehicle is ensured, the complexity is obviously reduced, and the calculated amount is reduced. The scheme is that automatic anti-collision operation among multiple machines is completed based on a data chain, once the existence of risks is detected, the unmanned aerial vehicle is controlled to perform climbing, rolling and other series of flight actions until the collision threat is eliminated, and the basic principle is shown in figure 1. The automatic aerial collision avoidance system obtains navigation information and GPS data of a local unmanned aerial vehicle and a peripheral unmanned aerial vehicle from a navigation system, once collision with the peripheral unmanned aerial vehicle is judged to possibly exist, data link communication with the potential threat aircraft is established, 3 collision avoidance maneuvers are screened from 9 appointed collision avoidance maneuvers according to the situation of the threat unmanned aerial vehicle, the collision avoidance maneuvers include 1 preferred collision avoidance maneuver and 2 alternative collision avoidance maneuvers, three corresponding plan trajectories are generated based on a modularized trajectory prediction algorithm, meanwhile, plan track information of the local unmanned aerial vehicle is sent to the peripheral unmanned aerial vehicle through the data link, occupation of the airspace is preset, the same type information of other aircrafts is obtained, an evaluation algorithm is used for comparing the planned track combination of the local unmanned aerial vehicle and the threat unmanned aerial vehicle, and the optimal maneuver is determined according to a cost function. In the whole collision avoidance process, the system continuously and circularly calculates the comparison, continuously updates the collision avoidance maneuver, simultaneously judges whether the collision avoidance maneuver is excited, and triggers the autonomous escape flight operation if the collision is judged to occur (namely the planned flight paths of the two unmanned aerial vehicles are crossed in space in a certain time period); if no collision possibility is detected in the escape flight, the collision avoidance maneuver is updated according to the new state information and the planned flight path is transmitted.

Example two

Based on the first embodiment, the present embodiment provides an unmanned aerial vehicle collision avoidance system, which includes a memory and a processor, where the memory stores an unmanned aerial vehicle collision avoidance program, and the processor executes the steps of the method according to any embodiment when running the unmanned aerial vehicle collision avoidance program. As a specific embodiment of the collision avoidance system for an unmanned aerial vehicle, the processor includes an automatic aerial collision avoidance system, and the automatic aerial collision avoidance system includes the following functional modules:

the information receiving and processing module: receiving and processing flight state information of the unmanned aerial vehicle and the target unmanned aerial vehicle through an airborne system, a sensor and a data link;

the collision avoidance maneuver generation module: maneuvers in all directions can be generated through actions such as rolling, climbing and the like, and the scheme comprises nine maneuvers in three types;

the collision avoidance maneuver evaluation module is used for screening 3 collision avoidance maneuvers in advance from 9 collision avoidance maneuvers including 1 preferred collision avoidance maneuver and 2 alternative collision avoidance maneuvers by the target unmanned aerial vehicle based on flight state information of the target unmanned aerial vehicle and the threat aircraft;

a motion trail prediction module: generating 3 planning tracks corresponding to 3 collision avoidance maneuvers by using an embedded track prediction algorithm (Trajectory prediction);

threat unmanned aerial vehicle orbit rebuilds module: the target unmanned aerial vehicle is divided into two conditions of cooperation and non-cooperation, planned flight path information of the cooperative target unmanned aerial vehicle is received, the motion trail of the cooperative target unmanned aerial vehicle is reconstructed on a local computing unit, flight state information of the non-cooperative target unmanned aerial vehicle is received, and the motion trail of the non-cooperative target unmanned aerial vehicle is estimated on the local computing unit;

maneuver evaluation and selection module: evaluating all planned track combinations of the unmanned aerial vehicles and the target unmanned aerial vehicle, determining which combination can delay maneuver as long as possible and provide enough protection interval, and selecting the maneuver finally executed by each unmanned aerial vehicle for the cooperative target through data chain coordination;

the maneuvering activation and control module: and judging whether to activate the collision avoidance maneuver of the local aircraft according to the planned track corresponding to the optimal collision avoidance strategy determined by the local aircraft and the threat aircraft, and recovering normal flight after the threat is eliminated.

The collision avoidance maneuver generation module:

unmanned aerial vehicle has high mobility and flexibility, through rolling, the action such as climbing can produce the maneuver of all directions, changes unmanned aerial vehicle flight attitude (pitch angle, roll angle, yaw angle) and speed, acceleration etc. through changing control parameters such as the angle of elevator, aileron, rudder, spoiler, wing flap, stabilizer to realize the control to unmanned aerial vehicle, finally produces the maneuver of different directions. When the unmanned aerial vehicle is in collision avoidance, maneuvers in all directions can be generated, but in an automatic collision avoidance system, the following definition is generally followed, and the maneuvers in specific directions are realized by changing the roll angle (EA) and the overload factor (NZ), wherein the maneuvers comprise nine kinds of maneuvers. The maneuvering number is limited to 9, the budget amount of the system can be obviously reduced, the system operation speed is improved, the rapid response of the unmanned aerial vehicle is ensured, and meanwhile, the standardization of an automatic collision avoidance system of the unmanned aerial vehicle is facilitated. The nine maneuvers also basically ensure that the unmanned aerial vehicle can maneuver towards all directions, roll and climb seven types, the increment of the roll angle relative to the current side inclination angle is respectively-90 degrees, -60 degrees, -30 degrees, 0 degree, 30 degrees, 60 degrees and 90 degrees, and the climbing overload is 5g absolute acceleration; keeping the roll angle and the standard g acceleration unchanged; the roll angle was kept constant and dropped with an absolute acceleration of-0.5 g. When the aircraft is operated, the roll rate is the maximum roll rate allowed by the performance of the aircraft, and the climbing overload is 5g absolute acceleration. Roll angle and overload can be adjusted according to the scene.

In performing maneuver evaluation and selection, the automated aerial collision avoidance system first pre-screens 3 collision maneuvers from the 9 maneuvers, including 1 preferred collision maneuver and 2 secondary collision maneuvers, due to hardware limitations in computational processing power and data link information transmission. The preselection is made based on the geometry of the threat drone relative to the host computer, including determining whether the threat drone is in the forward or aft direction of the host computer, above or below the host computer, on the left or right side of the host computer, and in a head-on or tail-end configuration, among others. As shown in fig. 3(a), threat unmanned aerial vehicle appears in target unmanned aerial vehicle top, left side, backward, and positive relative target unmanned aerial vehicle turns right from a left side, and from the past backward, from last down pass through, expect to appear in this local below to the right side, should this local should take upwards, flight track left. As shown in fig. 3(b), the threat drone appears below the target drone, right side, left direction, and is right to left relative to the target drone, from back to front, from down to up across, and is expected to appear in the left side behind the local top, and should take an upward, left flying trajectory.

When the automatic air collision avoidance system judges the geometric situation, in order to simplify the computation amount, the motion model of the unmanned aerial vehicle is simplified from a 6DOF (six DOF motion model) to a 3DOF (three DOF motion model) model, and according to the flight state of the threat unmanned aerial vehicle, including position and speed information, a relative motion track which threatens the unmanned aerial vehicle to reach a space closest point of CPA (closed point of ap) is constructed, so as to perform pre-screening of collision avoidance maneuver.

In the 3DOF model, the state information of the drone includes position information P ═ (xe, ye, ze) and velocity information Ve ═(ue, Ve, we), where Ve is the three-dimensional velocity of the drone in the ground coordinate axis system, where ψ is the yaw angle, θ is the pitch angle, φ is the roll angle, S is the pitch angle_ψθφFor transformation matrix from ground coordinate system to body coordinate systemAnd Vb is the speed of the unmanned aerial vehicle under the coordinate system of the robot body.

Ve＝S_ψθφ.Vb

P_t+τ(A)＝P_t(A)+V_t(A)×τ

P_t+τ(T)＝P_t(T)+V_t(T)×τ

Tau is the time consumed by the two unmanned aerial vehicles to reach the closest point of space according to the set flight state at the current moment t; p_t+τ(A) For the position of the target drone to the closest point of spatial approach, P_t+τ(T) is the position of the threat unmanned aerial vehicle to the closest point in space, and the motion track of the threat unmanned aerial vehicle relative to the target unmanned aerial vehicle is (P)_t(A)-P_t(T),P_t+τ(A)-P_t+τ(T))。

The local motion track prediction module:

the motion trajectory prediction model may predict a trajectory of the drone for a future period of time based on current state parameters and control parameters of the drone. The motion trail prediction model generates three groups of corresponding plan tracks according to the three groups of collision avoidance maneuvers of the unmanned aerial vehicle, and then sends the three groups of corresponding plan tracks to the threat unmanned aerial vehicle. The unmanned plane state parameters (ub, vb, wb, xe, ye, ze, pr, qr, rr, phir, theta, psir) include velocity information, position information, and attitude information. The speed information comprises a machine body longitudinal axis speed ub, a machine body transverse axis speed vb and a machine body vertical axis speed wb; the position information comprises a ground coordinate axis north coordinate xe, a ground coordinate axis east coordinate ye and an altitude ze; the attitude information comprises a ground coordinate shafting roll angle phir, a ground coordinate shafting pitch angle theta, a ground coordinate shafting yaw angle psir, a roll angle speed pr, a pitch angle speed qr and a yaw angle speed rr; the control parameters (dEr, dAr, dRr, dT, dASr, dFr, dSr) of the drone include an elevator yaw angle dEr; aileron declination, rudder declination dRr, engine valve Dt, spoiler declination dASr, flap declination dFr, stabilizer declination dSr.

As shown in fig. 5, in this model, the prediction time of the motion trajectory prediction model is set to 5 seconds, that is, trajectory information of 5 seconds in the future of the local machine is predicted, and the uncertainty of the predicted flight path increases with the increase of time. The operation cycle of the algorithm is 10Hz, the track prediction algorithm outputs 50 predicted positions P (t) (at intervals of 0.1 s) corresponding to 5s to other modules every 0.1s, as shown in FIG. 5, the planned flight path is a conical area, the central line of the conical area is a track generated by the 50 predicted positions, and the section size of the conical area is the track uncertain distance UD (t) corresponding to the predicted positions, and the size of the conical area depends on the uncertainty of the predicted flight path and increases along with time. The robustness of the track prediction algorithm is improved by the uncertain track distance, and 95% of actual deviation can be ensured to fall into the output result of the uncertain model.

When the unmanned aerial vehicle track is predicted, considering that the unmanned aerial vehicle has the characteristics of high speed and flexibility, and the maneuvers of rolling, climbing and the like are added in the collision avoidance process, a Track Prediction Algorithm (TPA) depicts the motion state of the unmanned aerial vehicle based on 6DOF (six degrees of freedom). The six-degree-of-freedom motion equation of the unmanned aerial vehicle comprises two parts, namely a centroid motion equation (force equation) and a rotation equation (moment equation) around the centroid.

1. Centroid motion equation (force equation):

wherein V_EFor unmanned aerial vehicles relative to the ground coordinate system O_gx_gy_gz_gAbsolute velocity of, V_BIs a coordinate system O of the unmanned aerial vehicle relative to the body_Bx_By_Bz_BIs the rotational angular velocity of the drone.

V_B＝i u_b+j v_b+k w_b

ω＝i p+j q+k r

Wherein i, j, k are x under the coordinate axis of the machine body_B,y_B,z_BUnit vector on axis

Thrust F of engine_TAircraft gravity mg, lift

Resistance force

And gravity

In the body coordinate system O_Bx_By_Bz_BProjection is performed to obtain the following equation

Wherein the lift force

Resistance force

Side force

The mean air density ρ (density of airflow), wing area S (wing area), wing span b (wing span), and mean aerodynamic chord length

Angle of attack (angle of attack) α, sideslip angle (slide) β, control surface deflections, angular velocities p, q,_r(angularrate)。

2. equation of rotation around the centroid (moment equation):

H＝Iω

wherein I_x，I_y，I_z，I_xzRespectively, the moment of inertia around the longitudinal axis, the vertical axis and the transverse axis of the airframe and the product of inertia of the aircraft to the longitudinal axis and the vertical axis of the airframe are approximate to I in consideration of the fact that the aircraft has a longitudinal symmetry plane_xy＝0、I_yz＝0。

Wherein, the external moment M_x，M_y，M_zThe projection vectors of M on the longitudinal axis, the horizontal axis and the vertical axis of the machine body are respectively. External moments are generated from aerodynamic moments and engine thrust, where L_T,M_T,N_TIs a projection vector of the thrust moment of the airplane on a longitudinal axis, a horizontal axis and a vertical axis,

is the projection vector of the aerodynamic moment on the vertical axis, the horizontal axis and the vertical axis.

Aerodynamic moment may be expressed in a similar manner to aerodynamic force.

Suppose that the thrust point of the engine lies in the xz plane of the body coordinate system and is within_zThe axial direction being displaced from the center of gravity by a distance z_T。

To sum up, the moment equation of the unmanned aerial vehicle is as follows:

thus, in combination with the force equation and the moment equation of the aircraft, a state equation of the aircraft can be obtained:

wherein, c₄＝I_xz，

c₂＝(I_x-I_y+I_z)I_xz，

c₃＝I_z，

c₉＝I_x，

When the unmanned aerial vehicle anti-collision system controls the aircraft to fly, the aerodynamic parameters of the aircraft are changed by controlling the elevators, the flaps and the like, and the state parameters of the unmanned aerial vehicle, such as speed, attitude angle and the like, are further changed, so that corresponding maneuvering actions are completed. The response model of the drone is shown in fig. 6, and the system can input control parameters to control the flight, and can also predict the flight path of the drone based on the current state of the drone and the set control parameters.

TABLE 2 units of input control parameters and their ranges

Control of	Unit of	Minimum size	Maximum of	Limiting
					Lift rudder	deg	-25	25	60deg/s
Aileron	deg	-21.5	21.5	80deg/s
					Rudder	deg	-30	30	120deg/s
Flap	deg	0	25	25deg/s

Remarking: the elevator is a 'control surface' for controlling the aircraft to ascend and descend, and has the function of pitching the aircraft and changing the pitch angle; the ailerons are main operation control surfaces of the airplane and can generate rolling torque to enable the airplane to make rolling motion and change a rolling angle; the rudder is a movable airfoil part for realizing the control of the aircraft course and is used for controlling the aircraft course and changing a course angle; a flap is an airfoil-shaped movable device for an edge portion of an aircraft wing, the basic effect of which is to increase lift in flight.

The target machine track management module:

the target aircraft track management module is responsible for receiving and processing input data threatening the unmanned aerial vehicle. Threat drones fall into two categories: 1) the cooperative threat unmanned aerial vehicle is also provided with an automatic collision avoidance system, establishes communication with the local machine through a specified data link and sends self state information to the local machine; 2) non-cooperative threat unmanned aerial vehicle, this local obtains threat unmanned aerial vehicle state information through other approaches such as radar.

And gathering the current state information of the target unmanned aerial vehicle, and summarizing the current state information into a list with a unified structure. Data for the same threat drone may come from different sources, prioritized by data source. The automatic air collision avoidance system dedicated data link has the highest priority, the non-automatic air collision avoidance system data link has the second priority, and finally, data from radar and ambiguous data sources. FIG. 7 is a track management process multi-source data schematic that eliminates/merges duplicate threat target information and maps the remaining tracks to a unified "relevant output threat list". Therefore, the time is unified, and whether the information of the target machine and the local machine has collision risks or not is conveniently detected. When the 3s threshold is reached, which times out from the last valid packet, the corresponding target will be removed from the "relevant outgoing threat list" due to the unreliability of the data.

The special data chain of the automatic air anti-collision system comprises maneuvering flight track information followed by the target machine, but the real track of the target machine of other data sources is difficult to obtain, and the track management module of the target machine is required to construct the motion track according to the obtained information:

(1) if the acceleration information is obtained and the target machine moves at low overload acceleration, predicting the motion track of the target machine by using a kinematical model motion model;

(2) if the acceleration information can not be obtained, predicting the motion track along a speed vector expansion mode;

(3) if acceleration information cannot be obtained and the speed of the threat machine is extremely low, a ballistic model ballisticmodel is used for predicting the motion track of the threat machine.

In order to save processing time, the relevant output threat list needs to be subjected to simplified sorting, a target machine with higher threat degree should be processed preferentially, and threat degree J is introduced, wherein the threat degree is a weighting function of the slant distance and the distance rate:

wherein R is a linear distance, R_dotTo approximate the rate, a weighting factor V_critIs an adjustable weighting constant with priority over linear distance and rate of approach to achieve optimal threat assessment. When the approaching speed is positive (the moving direction is far away from the machine), the formula is simplified, and R is removed_dot。

In a multi-aircraft situation, especially when large-scale unmanned aerial vehicles are clustered, the unmanned aerial vehicles can simultaneously encounter the invasion and threat of multiple unmanned aerial vehicles in the flight process. The target unmanned aerial vehicle tracks the invading unmanned aerial vehicle and calculates the Threat degree thereof, a Threat degree table (thread table) is established, the threats of different unmanned aerial vehicles are sequenced, and the Threat table is updated every 1 second. However, due to hardware limitations of unmanned aerial vehicle computing processing capacity and data link information transmission, the target unmanned aerial vehicle cannot respond to all threat unmanned aerial vehicles at the same time, the system can select three unmanned aerial vehicles with the highest threat degree as threat machines, and takes collision avoidance maneuvers for the three unmanned aerial vehicles with the highest threat degree. Each drone considers only the three threatening drones with the highest degree of threat, and the system is updated every 0.25 second. This applies to unmanned clusters.

5. Anti-collision strategy generation module

When encountering other airplane threats, no one can select 3 groups from 9 groups of set maneuvering modes as collision avoidance maneuvers. Next, in the collision strategy generation module, the drone screens out the preferred collision maneuver and the alternative collision maneuver from the 3 sets of collision maneuvers and generates a planned trajectory as the airspace predetermined by the drone, along which the drone will fly according to the preferred collision maneuver when the collision maneuver is activated. In the anti-collision strategy module, the unmanned aerial vehicle firstly sends 3 groups of planned track information of the unmanned aerial vehicle to the threatening unmanned aerial vehicle through a data link, meanwhile obtains planned track information of the threatening unmanned aerial vehicle, then compares the planned track combination of the unmanned aerial vehicle and the threatening unmanned aerial vehicle by using an evaluation algorithm, determines the optimal avoidance strategy, and finally sends the updated optimal anti-collision maneuver and the alternative anti-collision maneuver to other threatening unmanned aerial vehicles through the data link.

In an automatic air collision avoidance system, the drone generates new manoeuvres and collision avoidance strategies at a frequency of four times per second, i.e. the manoeuvre selection is updated every 0.25s based on the newly generated manoeuvres, and the selected manoeuvres each update the flight status information at a frequency of 20 times per second. From the combination of the selected maneuvers into the generation of a collision avoidance strategy, the collision avoidance algorithm will introduce a delay wait to send the local message and receive information from other drones. If there is no delay, the collision avoidance algorithm takes action without receiving the latest message, which may cause false positives or failures. In the time delay framework, the collision avoidance algorithm updates the trajectory with current flight status information, but does not allow maneuvers or commands to change so that all participants have reasonable time to receive information and make decisions. A latency waiting mechanism is critical to ensure maneuver paralleling/synchronization and to perform safe, anticipatory maneuvers. If no new maneuver selections are received from other drones after the delay, then the previous preferred collision avoidance maneuver is executed. As shown in fig. 8, in the time delay frame, the last collision avoidance strategy is continued from the new 3 sets of collision avoidance maneuvers generated by the drone, that is, the last preferred collision avoidance maneuver is still used as the preferred collision avoidance maneuver of the newly generated 3 sets of collision avoidance maneuvers, and is sent to the threat drone, and then the combination comparison is performed and the collision avoidance strategy is updated according to the new collision avoidance maneuver of the threat drone. And the new combination comparison judges whether to update the collision avoidance strategy. When the collision avoidance algorithm selects a new preferred collision avoidance maneuver, the automated air collision avoidance system needs to evaluate whether all threat machines are able to receive this change before allowing the new maneuver to be performed.

The process of maneuvering combination is as shown in fig. 9, at a certain moment, it is assumed that several unmanned aerial vehicles threaten each other in the airspace, the automatic collision avoidance system comprehensively considers each collision avoidance maneuver threatening the unmanned aerial vehicles, combines the planned tracks corresponding to the unmanned aerial vehicle collision avoidance maneuvers one by one, calculates the AD between each pair of unmanned aerial vehicles, and obtains the cost of all combinations. Every two cooperative airplanes have 9 combinations (three types of target airplanes and three types of local airplanes, and the two combinations are matched); for non-cooperative targets, there are only 4 combinations because the threat machine has no automated maneuver; if the situation is three cooperative target machines, 81 track combinations are considered to determine the optimal track combination. And (3) evaluating the single-computer and multi-computer threat scenes by adopting a cost function, wherein the selected maneuvering combinations are as follows:

the principle of selecting the optimal combination of the local machine and the target machine is as follows: the optional maneuvers for each drone are compared and a combination is selected that can delay as much as possible the activation of the collision maneuver. The collision avoidance algorithm selects the track combination with the largest AD value of the minimum avoidance distance so as to delay the activation of the maneuver. The predicted minimum avoidance distance calculation mode is that the minimum distance (track center distance) of two tracks in a space-time domain subtracts a track uncertainty distance UD. M_i(m) represents a collision maneuver m taken by drone i; AD (M)_i(m),M_j(n)) represents the minimum avoidance distance between two drones when drone i takes maneuver m and drone j takes maneuver n.

ω_ij(m,n)＝1/AD_ij(m,n)

When the unmanned aerial vehicle adopts the appointed collision avoidance maneuver, the cost function of the whole unmanned aerial vehicle cluster is as follows:

in the maneuvering combination mode, the maneuvering mode that the unmanned aerial vehicle should adopt is as follows:

the collision avoidance maneuver excitation control module:

the design principle of the anti-collision algorithm is that interference is avoided, interference is minimized, meanwhile, collision can still be achieved, the automatic aerial anti-collision system needs to activate maneuvering as late as possible, the unmanned aerial vehicle A can activate anti-collision maneuvering before entering the unmanned aerial vehicle B to avoid an airspace sphere, the earlier the activation time is, the more maneuvering modes that can be selected by the unmanned aerial vehicle are, the less possibility of collision is, the later the activation time is, the smaller the maneuvering range that can be selected by the unmanned aerial vehicle is, and after the activation time is later than a certain time, no matter what maneuvering mode is adopted, two unmanned aerial vehicles cannot be prevented from colliding. As shown in fig. 10, if the unmanned aerial vehicle a takes maneuver at the maneuver point 1, both the preferred maneuver and the standby maneuver can avoid collision between the two unmanned aerial vehicles, but the activation time is too early, which has a large impact on the normal flight of the unmanned aerial vehicle; if action is taken at the maneuver point 3, no matter the optimal maneuver or the standby maneuver, the unmanned aerial vehicle A cannot be prevented from entering the collision avoidance airspace sphere of the unmanned aerial vehicle B, and collision cannot be avoided; if action is taken at the maneuvering point 2, the standby maneuvering enters into the collision avoidance airspace sphere threatening the unmanned aerial vehicle, but the optimal maneuvering is just tangent to the collision avoidance airspace sphere to achieve collision avoidance, and the maneuvering point 2 is the latest moment of collision avoidance of the unmanned aerial vehicle and also is the optimal activation time of the automatic collision avoidance system. The collision avoidance maneuver excitation control module searches for an optimal collision avoidance position similar to the maneuver point 2 in the flight process of the unmanned aerial vehicle, so that the collision avoidance between the unmanned aerial vehicles can be realized and the minimum interference is achieved. Meanwhile, when collision avoidance is carried out, the radius of a collision avoidance airspace sphere in the automatic collision avoidance system can be adjusted according to actual conditions so as to achieve the optimal collision avoidance effect.

As shown in fig. 11, the collision avoidance maneuver excitation control module compares the planned flight path corresponding to the local optimal maneuver provided by the trajectory prediction algorithm with the received planned trajectory of the optimal maneuver corresponding to the threat machine, and calculates a predicted Minimum distance pmr (predicted Minimum range) to determine the optimal time for maneuvering activation.

The collision avoidance maneuver excitation control module also needs to calculate the minimum allowable trajectory distance MASD (minimum allowed separation distance), which is the sum of the local machine and the target machine semi-span ws (wind span), and the desired distance dsd (desired separation distance) of the system input and the summation of the following Uncertainty (uncertaintiy):

(1) navigation uncertainty (Navigation uncertainty);

(2) uncertainty in trajectory prediction;

(3) trajectory reconstruction/fit uncertainty;

(4) data chain transmission uncertainty;

(5) the track data calculates uncertainty.

MASD＝DSD+U+∑WS

When the unmanned aerial vehicle judges that the planned track of the unmanned aerial vehicle overlaps with the planned track of the threatening unmanned aerial vehicle, namely when the predicted minimum distance PMR is smaller than the allowed minimum track distance MASD, the collision avoidance maneuver is activated, the unmanned aerial vehicle acts according to the optimized collision avoidance maneuver, wherein the expected distance DSD is a fixed value and is input in advance by the system.

The algorithm structure is as follows:

the operation flow of the automatic collision avoidance operation algorithm in the automatic air collision avoidance system is shown in fig. 12.

Initializing the state of the airplane, and setting the initial state parameters and control parameters of the unmanned aerial vehicle

And calculating and storing parameters such as a roll angle (EA) and an overload factor (NZ) of the unmanned aerial vehicle, generating a flying track of 20 seconds in the future according to the flight state parameters of the unmanned aerial vehicle, and sending the flying route of the unmanned aerial vehicle to other unmanned aerial vehicles in the surrounding air through a data link.

Receiving flight routes of other unmanned aerial vehicles in the surrounding airspace, comparing the flight routes with the flight routes, judging whether the flight routes conflict with the unmanned aerial vehicles, if so, calculating the threat degree of the unmanned aerial vehicles, synthesizing the threat degrees of all the surrounding unmanned aerial vehicles, generating a threat table, and simultaneously listing three unmanned aerial vehicles with the highest threat degree to the target unmanned aerial vehicle as threat unmanned aerial vehicles

Based on the current position CP (t0) and the speed V of the unmanned aerial vehicle and the threatening unmanned aerial vehicle, estimating the position CP (t0+ delta t) of the aircraft at the time (t0+ delta t), selecting 3 groups of maneuvers from 9 groups of maneuvers according to the flight trajectory plan of the threatening unmanned aerial vehicle relative to the target unmanned aerial vehicle, and obtaining the roll angle delta EA and the overload factor NZ which need to be compensated;

generating a planned track of the target unmanned aerial vehicle according to the compensated roll angle delta EA and the overload factor NZ;

combining the planned flight paths of the target unmanned aerial vehicle and the threat unmanned aerial vehicle, calculating cost functions of different combinations, and determining optimal maneuver;

whether collision risks exist between the target unmanned aerial vehicle and the threat unmanned aerial vehicle is detected, and if the risks are detected, avoidance operation is triggered.

(8) In the avoidance operation, the roll angle (EA) and the overload factor (NZ) are adjusted, and a new flight operation command is calculated from the compensated roll angle Δ EA and overload factor Δ NZ.

The above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention, and all modifications and equivalents of the present invention, which are made by the contents of the present specification and the accompanying drawings, or directly/indirectly applied to other related technical fields, are included in the scope of the present invention.

Claims

1. An unmanned aerial vehicle collision avoidance method is characterized by comprising the following steps:

2. The collision avoidance method for unmanned aerial vehicles according to claim 1, wherein the step of determining threat to the unmanned aerial vehicle in step 2 comprises:

3. The collision avoidance method for unmanned aerial vehicles according to claim 1, wherein the step of screening collision avoidance maneuvers in step 2 comprises:

4. The collision avoidance method for unmanned aerial vehicles according to claim 2, wherein in step 3, the generating of the planned trajectories respectively corresponding to the screened collision avoidance maneuvers by using a trajectory prediction algorithm comprises:

5. The collision avoidance method for unmanned aerial vehicles according to claim 4, wherein the step of obtaining the planned trajectory or flight information threatening the unmanned aerial vehicle in step 3 comprises:

6. The collision avoidance method for unmanned aerial vehicles according to claim 5, wherein the step 4 comprises:

when relevant data corresponding to the planned track threatening the unmanned aerial vehicle come from a special data chain of the automatic aerial collision avoidance system, reconstructing a motion track of the threatening unmanned aerial vehicle according to the maneuvering flight track information contained in the special data chain;

when the flight information threatening the unmanned aerial vehicle comes from a non-automatic aerial collision avoidance system data chain, radar or other unknown data sources:

if acceleration information cannot be obtained and the threat machine speed is in a very low range, the ballistic model ballisticmodel is used for predicting the motion track of the threat machine.

7. The collision avoidance method for unmanned aerial vehicles according to claim 1, wherein the step 5 comprises:

and step 52, determining the optimal collision avoidance maneuver of the local machine according to the motion track combination with the maximum minimum avoidance interval value.

8. The collision avoidance method for unmanned aerial vehicles according to claim 1, wherein the step 6 comprises:

and step 63, when the unmanned aerial vehicle judges that the planned track of the unmanned aerial vehicle is overlapped with the planned track of the threatening unmanned aerial vehicle, namely the predicted minimum distance PMR is smaller than the allowed minimum track distance MASD, activating the collision avoidance maneuver.

9. The unmanned aerial vehicle collision avoidance method of claim 8, wherein the uncertainty in step 62 comprises: navigation uncertainty, trajectory prediction uncertainty, trajectory reconstruction/fitting uncertainty, data link transmission uncertainty, and trajectory data computation uncertainty.

10. An unmanned aerial vehicle collision avoidance system, comprising a memory and a processor, wherein the memory stores an unmanned aerial vehicle collision avoidance program, and the processor executes the steps of any one of the methods of claims 1 to 9 when running the unmanned aerial vehicle collision avoidance program.