CN112198901B

CN112198901B - Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area

Info

Publication number: CN112198901B
Application number: CN202011223941.3A
Authority: CN
Inventors: 王宏伦; 张梦华; 李娜; 吴健发; 纪红霞; 武天才
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2020-05-14
Filing date: 2020-11-05
Publication date: 2022-02-18
Anticipated expiration: 2040-11-05
Also published as: CN112198901A

Abstract

The invention discloses an unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone, and belongs to the technical field of unmanned aerial vehicle navigation guidance and control. Firstly, obtaining the comprehensive situation information of the airspace where the unmanned aerial vehicle is located, if the unmanned aerial vehicle is located in an emergency steering section, continuing to steer emergently until the unmanned aerial vehicle enters a stable flight section. And then calculating the total threat index of each obstacle, sequencing all the obstacles according to the total threat index, judging whether a warning target exists in each threat source if the threat source exists in the obstacles, and calculating the boundary condition of the unmanned aerial vehicle on the unmanned aerial vehicle collision region of the warning target if the threat source exists in the obstacles. And if the warning target is located in the boundary condition, predicting the available escape time and escape distance for the unmanned aerial vehicle to avoid the warning target. If the local complete escape time is longer than the decision time, an emergency target exists in the warning target, and the unmanned aerial vehicle carries out emergency maneuver according to an emergency collision avoidance strategy to finish autonomous collision avoidance. The invention is suitable for complex environment and is easy to realize.

Description

Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area

Technical Field

The invention relates to an unmanned aerial vehicle autonomous collision avoidance decision-making method based on a three-dimensional dynamic collision zone, and belongs to the technical field of unmanned aerial vehicle navigation guidance and control.

Background

Unmanned aerial vehicle has advantages such as use is nimble, strong adaptability and price/performance ratio, and under the dual promotion of actual demand and technical development, its range of application constantly expands. The unmanned aerial vehicle autonomous collision avoidance decision means that the unmanned aerial vehicle decides whether to re-plan a route to avoid danger according to knowledge and understanding of flight situations. With the rapid development of the unmanned aerial vehicle industry, the scarcity of airspace resources is increasingly remarkable. Meanwhile, due to the fact that the anti-collision capacity is automatically avoided, the existing unmanned aerial vehicle at home and abroad is difficult to share an airspace with aircrafts such as a man-machine, and the future development of the unmanned aerial vehicle is severely limited. The realization of the autonomous collision avoidance decision of the unmanned aerial vehicle in the complex environment is one of the prerequisites that the unmanned aerial vehicle reaches the safety level equivalent to that of the unmanned aerial vehicle and is integrated into a shared airspace, and has important significance.

Collision avoidance decisions are typically based on the results of collision detection. Based on data from various airborne sensors and communication links, collision detection provides assessment information for collision collisions in the current situation for collision avoidance decisions by modeling the state estimation and spatial encounter of the local machine and the environment. In the current collision detection research, compared with the traditional collision zone based on a fixed threshold value, the three-dimensional dynamic collision zone can better adapt to the rapid change of a complex environment due to the comprehensive consideration of the real-time state information of the unmanned aerial vehicle and the intrusion machine. Although the three-dimensional dynamic collision area has a great application potential in the field of collision avoidance decision, related researches still leave a great gap at present. Due to the lack of sufficient mining and utilization of time safety information and distance safety information contained in the model, no research has been provided for a collision avoidance decision method based on a three-dimensional dynamic collision zone.

Based on the analysis, the unmanned aerial vehicle autonomous collision avoidance decision method has the following technical problems to be solved:

(1) the unmanned aerial vehicle can flexibly switch maneuvering modes according to the current flight situation, and guarantee is provided for the flight safety of the unmanned aerial vehicle in a complex environment;

(2) the flight safety information on which collision avoidance decision is based has clear physical significance, simple form and reasonability and effectiveness;

(3) the decision method has the advantages of low calculation cost, good real-time performance and easy engineering realization.

Disclosure of Invention

In order to solve the problems, the unmanned aerial vehicle has sufficient flexibility and effectiveness in collision avoidance decision making, the invention provides the unmanned aerial vehicle autonomous collision avoidance decision making method based on the three-dimensional dynamic collision area, the threat assessment result of the barrier in the environment and the flight safety information contained in the three-dimensional dynamic collision area are fully utilized, reasonable mapping from the current situation to a maneuver mode is provided for the unmanned aerial vehicle, the unmanned aerial vehicle adopts appropriate avoidance maneuver at appropriate time, and the balance between the route smoothness and the flight safety of the unmanned aerial vehicle in the complex environment is realized.

The unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone specifically comprises the following steps:

the method comprises the following steps: acquiring comprehensive situation information of an airspace where the current unmanned aerial vehicle is located on a current air route where the unmanned aerial vehicle flies to a target;

the comprehensive situation information specifically comprises: the position and ground speed vector of the unmanned aerial vehicle, the position, shape, size and ground speed vector of each detected obstacle, etc.

Step two: judging whether the unmanned aerial vehicle is in an emergency steering section or not at the current sampling moment, if so, continuing to perform emergency steering until the unmanned aerial vehicle enters a stable flight section; otherwise, the unmanned aerial vehicle continues to fly in a stable flight section;

in the emergency steering section, the unmanned aerial vehicle applies the maximum control force to realize the rapid change of the track inclination angle and/or the track azimuth angle; when the change amount of the ground speed direction reaches the maximum rising angle of the track inclination angle, the unmanned aerial vehicle finishes the emergency steering and enters a stable flight section.

Step three: when the unmanned aerial vehicle is in a stable flight section, calculating the threat index of each obstacle detected at the current sampling moment;

assuming that the target sight line is directed to the obstacle centroid by the unmanned aerial vehicle centroid; total threat index T for ith obstacle_iBy distance threat index T_riAngle threat index T_aiAnd velocity threat index T_viThe three parts are as follows.

The definition of each threat index is as follows:

(1) distance threat index T_ri:

Wherein r is_aFor a warning distance, r_dIs the distance at risk. R_LiIs the relative distance between the unmanned plane and the ith obstacle. When R is_LiGreater than r_aThe distance threat index of the obstacle is zero; when R is_LiBetween r_aAnd r_dWhen R is_LiThe smaller the distance threat index; r_LiLess than r_dAnd in time, the relative distance between the unmanned aerial vehicle and the barrier is too close, and the distance threat index is maximum.

(2) Angular threat index T_ai:

q_rThe included angle between the speed direction of the unmanned aerial vehicle relative to the obstacle and the target direction is positive when the relative speed direction deviates to the left. When in use

In time, the relative distance between unmanned aerial vehicle and the barrier appears the trend that reduces, shows that unmanned aerial vehicle is approaching the barrier, has the collision risk. | q_rThe closer to zero the | is, the greater the collision risk is, and the greater the corresponding angle threat index is;

and when the relative distance is unchanged or increases, the unmanned aerial vehicle is far away from the obstacle, and the angle threat index is zero.

(3) Velocity threat index T_vi:

When the obstacle is a dynamic intrusion machine:

in the formula, V_AIs the ground speed of the unmanned aerial vehicle, V_iThe ground speed of the invader. V_i∈[0,0.6V_A) The speed of the time-lapse invading machine is obviously less than that of the unmanned aerial vehicle, the relative speed is mainly determined by the unmanned aerial vehicle at the time, the unmanned aerial vehicle can change the relative speed more easily by adjusting the self speed, the collision avoidance difficulty is lower, and the speed threat index of the invading machine is smaller; v_i∈[0.6V_A,1.5V_A) When, V_iApproaching or even exceeding V_AThe capability of the unmanned aerial vehicle for changing the relative speed by adjusting the speed of the unmanned aerial vehicle is obviously weakened, the collision avoidance difficulty is improved, and the speed threat index of the intrusion machine is V-shaped_iAnd V_AAn increase in the ratio; v_i∈(1.5V_A, + ∞) the speed threat index of the intruder is maximized.

When the obstacle is a static obstacle, the threat assessment object is a point on the surface of the static obstacle, which is closest to the unmanned aerial vehicle, and V_i＝0。

Step four, under the current sampling time, directly and linearly weighting and summing the threat indexes corresponding to the obstacles to obtain the total threat index of each obstacle;

the total threat index for the ith obstacle is calculated as follows:

T_i＝ω_rT_ri+ω_aT_ai+ω_vT_vi (4)

wherein, ω is_rIs a distance threat index T_riCorresponding weight, ω_aAs an angular threat index T_aiCorresponding weight, ω_vIs a velocity threat index T_viCorresponding weight, ω_r+ω_a+ω _v1, and ω_r＞0，ω_a＞0，ω_v＞0。

Step five, sequencing all obstacles according to the sequence of the total threat indexes from large to small, judging whether the total threat index of at least one obstacle is larger than a warning threshold value, if so, indicating that the obstacle is a threat source, and executing step six; otherwise, the unmanned aerial vehicle continues flying according to the original route;

step six, judging whether a warning target exists in each threat source, and if so, calculating the boundary condition of the unmanned aerial vehicle on the unmanned aerial vehicle collision region of the warning target; entering a seventh step; otherwise, carrying out route re-planning;

the alert target refers to an intruder in the threat source.

The boundary conditions of the no-maneuver collision region are: when the unmanned aerial vehicle is closest to the warning target, collision happens, namely the distance between the closest points is just equal to the minimum safety distance;

the specific calculation process is as follows:

step 601, defining a basic coordinate system and a motion variable.

Definition of ground system O_gx_gy_gz_g(S_g) Origin O of_gIs a fixed point on the ground, x_gNorth, y, with axis pointing to ground plane_gWest with axis pointing to ground plane, z_gThe axis is vertically upward according to the right-hand rule;

step 602, assuming that the unmanned aerial vehicle A and the invader B fly linearly at a uniform speed, and searching a position vector and a ground speed vector of the centroids of the unmanned aerial vehicle A and the invader B through comprehensive situation information;

the position vector of the A mass center of the unmanned aerial vehicle is P_AThe position vector of B mass center of the invader is P_BThe ground speed vector of unmanned aerial vehicle A is V_AThe ground speed vector of the invader B is V_B；

Velocity vector V of the earth_AAt the horizontal plane O_gx_gy_gProjection of (2) and x_gThe included angle between the axes is chi_AVelocity vector V of the earth_BAt the horizontal plane O_gx_gy_gProjection of (2) and x_gThe included angle between the axes is chi_BProjection relative to x according to the right hand rule_gThe included angle is positive when the axial direction rotates left; velocity vector V of the earth_AAnd the horizontal plane O_gx_gy_gThe included angle between the two is gamma_AVelocity vector V of the earth_BAnd the horizontal plane O_gx_gy_gThe included angle between the two is gamma_BWhen the velocity vector points to the upper part of the horizontal plane, the included angle is positive;

the relative distance between the unmanned aerial vehicle A and the unmanned aerial vehicle B is R_LRelative velocity is V_r。

Step 603, establishing a space rectangular coordinate system O fixedly connected with the mass center of the unmanned aerial vehicle A by taking the mass center of the unmanned aerial vehicle A as an original point_rx_ry_rz_r。

X defining the coordinate system_rAxial and ground speed vector V_AProjection onto a horizontal plane coincides with z_rAxis perpendicular to x in vertical plane_rThe axis pointing upwards, y_rAxis perpendicular to O_rx_rz_rA plane, the direction of which is determined by the right hand rule;

velocity vector V of the earth_BHorizontal plane projection of (2) and (x)_rThe angle between the axes being psi_BRelative distance R_LHorizontal plane projection of (2) and (x)_rThe angle between the axes being psi_LWhen the projection is relative to x_rPositive when rotating axially and leftwards; velocity vector V of the earth_AAnd O_rx_ry_rThe included angle of the plane is equal to the ground speed vector V_AAnd the horizontal plane O_gx_gy_gAngle gamma therebetween_A(ii) a Velocity vector V of the earth_BAnd O_rx_ry_rThe included angle of the plane is equal to the ground speed vector V_BAnd the horizontal plane O_gx_gy_gAngle gamma therebetween_B(ii) a Relative distance R_LAnd O_rx_ry_rIncluded angle of plane is gamma_L(ii) a The angle is positive when the vector is above the plane.

Build a space straightAngular coordinate system O_rx_ry_rz_rThe other method is as follows: the ground coordinate system S_gAfter the origin point of the robot is translated to the position of the center of mass of the unmanned aerial vehicle, the robot rotates around the z axis by an angle x_A. Therefore, the following transformation relationship is established:

wherein, χ_LIs a relative distance R_LProjection in the horizontal plane and x_gThe angle of the axes.

Step 604, the unmanned aerial vehicle A does not change the current ground speed, and the relative distance R between the unmanned aerial vehicle A and the invader at the moment t is established_L(t) an expression;

relative distance R between unmanned aerial vehicle and invador at time t_L(t) is:

Δ x, Δ y, Δ z are relative distances R_LAlong x_rAxis, y_rAxis and z_rThe component of the axis.

Step 605, according to the relative distance R_L(t) the expression calculates the boundary condition of the no-maneuver collision region;

the boundary condition analytic formula is:

wherein the content of the first and second substances,

R₀is the minimum safe distance of the unmanned aerial vehicle.

Step seven, judging whether the alert target is positioned in the boundary condition of the corresponding passive collision area, if so, entering the step eight, otherwise, performing the route re-planning;

and step eight, predicting the available escape time and escape distance for the unmanned aerial vehicle to avoid the warning target in the no-dynamic collision area.

Firstly, establishing a maximum maneuvering collision area and an unavoidable area;

(1) modeling the maximum maneuver impact region, establishing t and R_LA system of equations in two-fold for unknowns: as follows

R_L(t) ≠ 0, R_L' (t) ═ 0 is equivalent to

Will be provided with

Unfolding and finishing to obtain:

a_hduring emergency steering, the average acceleration of the unmanned aerial vehicle in the horizontal direction and the upward direction; a is_vDuring emergency steering, the average acceleration of the unmanned aerial vehicle in the vertical direction; and a is_h> 0 denotes the mean acceleration edge O_ry_rPositive axial direction, a_v> 0 denotes the mean acceleration edge O_rz_rThe positive direction of the axis.

Corresponding to different emergency steering modes, in the above formula a_hAnd a_vThe values of (A) are respectively as follows:

1) when the unmanned aerial vehicle is pulled up with maximum overload, a_h＝0，a_v＝a₂；

a₂The maximum average acceleration generated by the unmanned aerial vehicle in the horizontal direction is obtained;

2) when the drone rolls to the left and pulls with maximum overload, a_h＝a₁，a_v＝a₂；

3) Unmanned aerial vehicle rolls to right and pulls up with maximum overloadWhen a is_h＝-a₁，a_v＝a₂。

a₁The maximum average acceleration generated by the unmanned aerial vehicle in the vertical direction is obtained.

Solving by adopting an iterative method to obtain the closest time t and the current psi corresponding to the closest time t_LAnd gamma_LBoundary value R of collision region_L. Transversely get psi_L∈[0,2π]，

All boundary values of the maximum manoeuvre collision zone corresponding to the current emergency steering mode are available.

If solved, the obtained R_LIf < 0, it means that the two machines will not collide, and the boundary value at this time is R_L＝R₀。

(2) Modeling the unavoidable region, specifically as follows:

the unavoidable distinction is two types: a one-way non-avoidable area and a completely non-avoidable area.

The one-way unavoidable region is the intersection of an inorganic dynamic collision region and a maximum dynamic collision region corresponding to a certain emergency steering; the completely unavoidable region is the intersection of all the one-way unavoidable regions. And the intersection of the maximum maneuvering collision area and the non-maneuvering collision area corresponding to three types of emergency steering is adopted to represent the completely unavoidable area.

Then, extracting safety information of the collision area from a modeling process of the maximum maneuvering collision area and the unavoidable area, and storing escape time and escape distance;

firstly, aiming at an unmanned aerial vehicle-invader pair, a one-way non-evasive area of a dangerous target j and an emergency maneuver mode i is defined as E_ij(ii) a The set of emergency maneuvering modes is { l, r, u }, and comprises rolling and pulling up to the left, rolling and pulling up to the right, and pulling up to the right;

the definition of safety information is based on dangerous targets, i.e. warning targets located in the respective passive collision zone. The method comprises the following steps: one-way escape time/distance, and complete escape time.

One-way escape time/distance: there are two categories, local and global. Wherein the content of the first and second substances,the local one-way escape time/distance refers to the situation that a dangerous target j reaches a one-way non-avoidable area E_ijThe remaining time/distance of (c), corresponding to a certain emergency maneuver and the safety information of the drone-intruder pair, is denoted msg_ijI belongs to { l, r, u }, j belongs to {1,2, …, n }, and n is the number of intrusion machines detected by the unmanned aerial vehicle; the global one-way escape time/distance refers to the global minimum local one-way escape time/distance corresponding to the same emergency maneuver mode i and is expressed as

Complete escape time: there are two categories, local and global. Wherein, the local complete escape time refers to the situation that a dangerous target j reaches a completely unavoidable region E_com,jIs expressed as

The global complete escape time refers to the global minimum local complete escape time, expressed as

The 'one-way' indicates that the row subscript of the security information is a given value in a set { l, r, u }, and the 'complete' indicates that the row subscript can be taken from { l, r, u } in a traversal way; "local" indicates that the column index of the security information is a given value in the set {1, …, n }, and "global" indicates that the column index will take values from {1, …, n } in a traversal manner.

Step nine: and judging whether the local complete escape time is greater than the decision time, if so, judging that an emergency target exists in the warning target, and carrying out emergency maneuvering by the unmanned aerial vehicle according to an emergency collision avoidance strategy. Otherwise, carrying out route re-planning;

the decision time is given according to the maneuvering capability of the unmanned aerial vehicle, the measurement error of the sensing equipment, the minimum safe distance between the two unmanned aerial vehicles and other factors.

When the global complete escape time is not greater than the decision time, the collision is considered to be imminent, and the unmanned aerial vehicle should adopt emergency maneuver.

The emergency maneuver comprises an upward emergency maneuver, an upper left emergency maneuver and an upper right emergency maneuver;

according to the local one-way escape distance, an emergency maneuver corresponding to the maximum global one-way escape distance is adopted, and the corresponding emergency maneuver is started, specifically as follows:

1) upward emergency maneuvering: in the emergency steering section, the unmanned plane is pulled up with the maximum overload and has n_x＝sinγ，n_y＝0，n_z＝n_nmax(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters a stable flight section, and tracks χ ═ χ₀，γ＝γ_maxThe steady straight flight path.

n_nmaxThe maximum normal overload of the unmanned aerial vehicle is achieved;

2) emergency maneuvering at the upper left: in the emergency steering section, the unmanned aerial vehicle rolls to the left with maximum overload and is pulled up, and n is provided_x＝sinγ，n_y＝n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters a stable flight section, and tracks χ ═ χ₀-Δχ_max，γ＝γ_maxThe steady straight flight path.

μ_maxAt the maximum allowable roll angle, Δ χ_maxFor a predetermined maximum track azimuth change (Δ χ)_max＞0)。

3) Emergency maneuvering at the upper right: in the emergency steering section, the unmanned plane rolls to the right with maximum overload and is pulled up, and n is provided_x＝sinγ，n_y＝-n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters a stable flight section, and tracks χ ═ χ₀+Δχ_max，γ＝γ_maxThe steady straight flight path.

χ₀Adopting a track azimuth angle before emergency maneuver;

when only a single emergency target exists, the larger the local one-way escape distance is, the larger the distance between the unmanned aerial vehicle and the closest point of the emergency target is after the corresponding emergency maneuvering mode is adopted, the higher the flight safety is, and the maneuvering mode is more advantageous; conversely, the more disadvantageous the maneuvering mode is, the more disadvantageous the avoidance of collisions will be.

Step ten: and after the unmanned aerial vehicle finishes autonomous collision avoidance at the current sampling moment, returning to the step two, and repeatedly performing the unmanned aerial vehicle autonomous collision avoidance decision at the next sampling moment.

The invention has the advantages that:

(1) the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone can enable the unmanned aerial vehicle to adopt avoidance maneuvers matched with the urgency of collision conflict, thereby ensuring the flight safety of the unmanned aerial vehicle in a complex environment and simultaneously reducing the deviation of the unmanned aerial vehicle relative to the original route as much as possible;

(2) the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision area fully extracts and utilizes flight safety information contained in the three-dimensional dynamic collision area, the information has clear and visual physical significance, and reasonable basis is provided for collision avoidance decision;

(3) the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision area has the advantages of low calculation cost and good real-time performance, and is easy for engineering realization.

Drawings

FIG. 1 is an overall flow chart of the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone;

FIG. 2 is a comprehensive situation diagram of an airspace where an unmanned aerial vehicle is located, which is collected in the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone;

FIG. 3 is a spatial geometric relationship of relative motion between an unmanned aerial vehicle and an intruder in the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone;

FIG. 4 is a schematic diagram of an unmanned aerial vehicle autonomous collision avoidance decision-making method based on a three-dimensional dynamic collision zone, wherein the unmanned aerial vehicle autonomous collision avoidance decision-making method is free of a dynamic collision zone;

FIG. 5 is a schematic diagram of a maximum maneuvering collision zone in the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone;

FIG. 6 is a schematic diagram of an unavoidable region in the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision region;

FIG. 7 is a schematic diagram of a local one-way escape distance in the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone;

fig. 8 is a schematic diagram of an emergency encounter between an unmanned aerial vehicle and a single intruder in the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone; wherein, fig. 8(a) shows that the intruder is outside all the one-way non-evasive areas; FIG. 8(b) shows an intruder within a one-way unavoidable region;

fig. 9 is a schematic diagram of an emergency encounter between an unmanned aerial vehicle and multiple intruders in the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone;

fig. 10 is a schematic diagram of an unmanned aerial vehicle adopting an emergency maneuver in the unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone;

FIG. 11 is a schematic diagram of a route re-planning adopted by an unmanned aerial vehicle in the unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone;

fig. 12 is a diagram of a drone airway according to an embodiment of the invention; wherein FIG. 12(a) is a three-dimensional airway; FIG. 12(b) is a projection of the airway in the horizontal plane; figure 12(c) is drone flight height;

fig. 13 is an emergency collision avoidance diagram of the unmanned aerial vehicle in an emergency in the embodiment of the present invention; wherein fig. 13(a) shows before emergency collision avoidance; FIG. 13(b) shows after emergency collision avoidance;

FIG. 14 is a graph illustrating the overload of the drone along the axes of the track set in accordance with an embodiment of the present invention; wherein FIG. 14(a) shows overload along the x-axis of the track system; FIG. 14(b) shows an overload along the y-axis of the track system; FIG. 14(c) shows an overload along the z-axis of the track system;

FIG. 15 is a graph of the distance between the drone and each dynamic obstacle in an embodiment of the present invention;

fig. 16 is a graph of maximum threat indices for a dynamic barrier in an embodiment of the invention.

Detailed Description

The present invention will be described in further detail with reference to the accompanying drawings and examples.

The invention provides an unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision area, which provides reasonable mapping from the current situation to a maneuvering mode for an unmanned aerial vehicle by fully utilizing a threat evaluation result of an obstacle in the environment and flight safety information contained in the three-dimensional dynamic collision area, so that the unmanned aerial vehicle adopts appropriate avoidance maneuvering at appropriate time, and the balance between the route smoothness and the flight safety of the unmanned aerial vehicle in a complex environment is realized.

An unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone is shown in the overall flow chart of fig. 1, and specifically comprises the following steps:

the method comprises the following steps: and acquiring the comprehensive situation information of the airspace where the current unmanned aerial vehicle is located on the air path where the current unmanned aerial vehicle flies to the target.

The comprehensive situation information is shown in fig. 2, and specifically includes: the position and ground speed vector of the unmanned aerial vehicle, the position, shape, size and ground speed vector of each detected obstacle, etc.

Step two: aiming at the current sampling moment, judging whether the unmanned aerial vehicle is in an emergency steering section or not by combining the comprehensive situation information at the moment, if so, continuing to perform emergency steering until the unmanned aerial vehicle enters a stable flight section; otherwise, the unmanned aerial vehicle continues the steady flight segment.

The invention mainly considers two evasive actions possibly taken by the unmanned aerial vehicle: re-planning of airway and emergency maneuvers. The route re-planning enables an unmanned plane to avoid obstacles while moving to a target point according to the change of a dynamic environment, and emergency maneuvers are more severe and are generally used for resolving collision conflicts which are about to occur.

The design of the emergency maneuver needs to comprehensively consider various factors. Firstly, from the viewpoint of flight safety, after the speed direction of the unmanned aerial vehicle is changed to a certain degree, the collision can be avoided by keeping the current speed without steering all the time; secondly, from the perspective of the actual flight performance constraints of the unmanned aerial vehicle, the unmanned aerial vehicle cannot make an emergency turn without limit. E.g. by maximum residual thrust Δ T of the engine_maxThe maximum rising angle of the unmanned aerial vehicle during steady straight-line rising motion is gamma_max＝arcsin(ΔT_maxand/W), wherein W is unmanned aerial vehicle gravity. If the unmanned aerial vehicle is pulled up without limit to cause overlarge track inclination angle, serious problems such as runaway and the like are likely to be caused; finally, from the perspective of route planning, under the condition that the unmanned aerial vehicle can safely avoid obstacles, the negative effects caused by adopting emergency maneuvers should be reduced as much as possible, and the unmanned aerial vehicle is prevented from deviating from the original route too far.

In combination with the above analysis, the present invention divides emergency maneuvers into two phases: an emergency steering section and a smooth flight section. In the emergency steering section, the unmanned aerial vehicle applies the maximum control force to realize the rapid change of the track inclination angle and/or the track azimuth angle; when the change amount of the ground speed direction reaches a certain value, the unmanned aerial vehicle finishes the emergency steering and enters a stable flight section. It should be noted that, given the flyability of the planned route and the flight safety of the drone, the drone does not perform a new action until it exits the current emergency turn segment.

Step three: and when the unmanned aerial vehicle is in a stable flight section, calculating the threat index of each obstacle detected at the current sampling moment.

By quantifying and ordering the threat levels of the obstacles, threat assessment lays a preliminary foundation for collision avoidance decisions. In low-altitude complex environments, a drone may encounter multiple obstacles simultaneously. At the moment, in order to better ensure the safety of the unmanned aerial vehicle, after the unmanned aerial vehicle obtains the comprehensive situation of the airspace, threat assessment needs to be carried out on line, and quantitative recognition of the danger of the current situation is formed, so that a basis is provided for resolution of collision conflicts. In order to realize the measurement of the threat degree of each barrier, the invention adopts a threat index method, decomposes the solving problem of the threat index into a plurality of smaller subproblems by extracting the key factors causing collision conflict based on the concept of a divide-and-conquer method, and finally comprehensively considers the solving result of each subproblem to obtain the threat index of each barrier and sort the threat index, thereby establishing the basis for the subsequent collision avoidance decision.

The intruder is first targeted for threat assessment,in the collision and collision problem, three key factors for determining the threat degree of the intrusion machine comprise the relative distance R between the unmanned aerial vehicle and the intrusion machine_LThe relative velocity vector V of both parties_rDirection and magnitude of. The comprehensive situation of the unmanned aerial vehicle flying in the obstacle environment is shown in fig. 2, wherein the target sight line is directed to the center of mass of the intruder from the center of mass of the unmanned aerial vehicle; q. q.s_rThe included angle between the speed direction of the unmanned aerial vehicle relative to the invader and the direction of the target sight line is positive when the relative speed is deviated to the left. Under the situation, defining the total threat index T of the ith intrusion machine_iBy distance threat index T_riAngle threat index T_aiAnd velocity threat index T_viThe three parts are as follows.

The definition of each threat index is as follows:

(1) distance threat index T_ri:

Wherein r is_aAnd r_dRespectively a warning distance and a dangerous distance. R_LiThe relative distance between the unmanned plane and the ith intrusion machine. When R is_LiGreater than r_aThe distance threat index of the invader is zero; r_LiBetween r_aAnd r_dWhen R is_LiThe smaller the distance threat index; r_LiLess than r_dAnd in time, the relative distance between the unmanned aerial vehicle and the invader is too close, and the distance threat index is maximum.

(2) Angular threat index T_ai:

When in use

When the unmanned aerial vehicle is close to the invading machine, collision risks exist. | q_rThe closer to zero the | is, the collisionThe greater the collision risk, the greater the corresponding angle threat index;

and when the relative distance is unchanged or increases, the unmanned aerial vehicle is far away from the invader, and the angle threat index is zero.

(3) Velocity threat index T_vi:

When the obstacle is a dynamic intrusion machine:

in the formula, V_AAnd V_iThe ground speed of the unmanned aerial vehicle and the ground speed of the intrusion machine are respectively. V_i∈[0,0.6V_A) The speed of the time-lapse invading machine is obviously less than that of the unmanned aerial vehicle, the relative speed is mainly determined by the unmanned aerial vehicle at the time, the unmanned aerial vehicle can change the relative speed more easily by adjusting the self speed, the collision avoidance difficulty is lower, and the speed threat index of the invading machine is smaller; v_i∈[0.6V_A,1.5V_A) When, V_iApproaching or even exceeding V_AThe capability of the unmanned aerial vehicle for changing the relative speed by adjusting the speed of the unmanned aerial vehicle is obviously weakened, the collision avoidance difficulty is improved, and the speed threat index of the intrusion machine is V-shaped_iAnd V_AAn increase in the ratio; v_i∈(1.5V_A, + ∞) the speed threat index of the intruder is maximized.

Step four, under the current sampling time, after threat indexes corresponding to all key factors are obtained according to the method, a direct linear weighting method is adopted, all the threat indexes are respectively multiplied by weights and then summed, and the total threat index of each obstacle is obtained;

the total threat index for the ith obstacle is calculated as follows:

T_i＝ω_rT_ri+ω_aT_ai+ω_vT_vi (4)

the risk degree of the current situation and the target needing to be avoided preferentially can be rapidly determined by sequencing the obstacles, so that the rationality and the effectiveness of collision avoidance decision of the unmanned aerial vehicle are guaranteed.

the alert target refers to an intruder in the threat source.

The no-maneuver collision area is used for judging whether collision and collision are possible under the current situation, and is defined as a set of the current positions of the intrusion machines which inevitably cause collision when the unmanned aerial vehicle does not avoid maneuvers. The critical conditions for this region are: the two machines are closest to each other and just collide, namely the distance between the closest points is just equal to the minimum safe distance. The closest point in collision avoidance flight means that for two objects moving relatively, if their movement paths are determined, two points must exist in the whole movement process, and when the two objects reach the two points respectively at the same time, the distance between the two objects is the closest.

And calculating the non-dynamic collision area of each warning target by the following specific process:

step 601, in order to establish a passive collision area, a basic coordinate system and a motion variable need to be defined first.

Definition of ground system O_gx_gy_gz_g(S_g) Origin O of_gAt a fixed point on the ground, axis x_g、y_gNorth and west, respectively, directed at the ground level, axis z_gPressing the right hand rule to lead the vertical direction to be upward;

Velocity vector V of the earth_AAt the horizontal plane O_gx_gy_gProjection and axis x of_gThe included angle between the two is x_AVelocity vector V of the earth_BAt the horizontal plane O_gx_gy_gProjection and axis x of_gThe included angle between the two is x_BProjection relative to axis x according to the right-hand rule_gThe included angle is positive when the left rotation is carried out; velocity vector V of the earth_AAnd O_gx_gy_gThe included angle between the two is gamma_AVelocity vector V of the earth_BAnd O_gx_gy_gThe included angle between the two is gamma_BWhen the velocity vector points to the upper part of the horizontal plane, the included angle is positive;

Step 603, centering on the center of mass of the unmanned aerial vehicle A, and keeping a minimum safe distance R₀The round ball with the radius is a forbidden zone, and when the intrusion machine is positioned in the forbidden zone, the collision is considered to have occurred.

For researching the relative motion relation between the unmanned aerial vehicle and the intrusion machine, the mass center of the unmanned aerial vehicle A is used as the original point again, and a space rectangular coordinate system O fixedly connected with the mass center of the unmanned aerial vehicle A is established_rx_ry_rz_rAs shown in fig. 3. X defining the coordinate system_rAxial and ground speed vector V_AProjection onto a horizontal plane coincides with z_rAxis perpendicular to x in vertical plane_rThe axis pointing upwards, y_rAxis perpendicular to O_rx_rz_rA plane, the direction of which is determined by the right hand rule;

Establishing a spatial rectangular coordinate system O_rx_ry_rz_rThe other method is as follows: the ground coordinate system S_gAfter the origin point of the robot is translated to the position of the center of mass of the unmanned aerial vehicle, the robot rotates around the axis z by an angle x_A. Therefore, the following transformation relationship is established:

a schematic of the passive collision zone is shown in fig. 4. It can be seen that the boundary line of the passive collision zone is tangential to the no-entry zone of the drone and parallel to the current relative velocity V_rOf (2) is performed. Assuming that the unmanned aerial vehicle of the same party does not change the current ground speed, if the invader is located at nothingOutside the maneuvering collision area, collision cannot occur; otherwise, a collision must occur. Setting the moment of starting to calculate the collision area as an initial moment, and recording the initial moment as 0, wherein the relative distance between the unmanned aerial vehicle and the invader at the initial moment is R_L(0) Is denoted as R_L(ii) a Known as V_A，V_B，γ_A，ψ_B，γ_B，R₀Ignoring the influence of sideslip angle and the like possibly existing in flight, and keeping the relative distance R between the unmanned aerial vehicle and the invading machine at the moment t_L(t) is:

the critical conditions of the passive collision zone are: the nearest point distance between the unmanned aerial vehicle and the invading machine is equal to the minimum safe distance R of the unmanned aerial vehicle₀. When the relative distance between the unmanned aerial vehicle and the invading machine is nearest, R exists_L' (t) — 0 holds. Due to R_L(t) ≠ 0, there is R_L' (t) ═ 0 is equivalent to

Thus to R_L(t) derivation can be simplified to pair

The derivation is as follows:

wherein

For H in formula (8), it is necessary to discuss it as follows:

(1) when H is greater than or equal to 0, let

Obtaining the closest time of the two machines:

according to the critical condition R_L(t)＝R₀Obtaining the critical relative distance:

(2) when the H is less than 0, the content of the compound,

this is always true. Namely, at the initial moment, the two machines are closest to each other, and then the two machines are far away from each other; setting the boundary value of the no-power collision region at the time as R₀。

To sum up, get ψ ergodically_L∈[0,2π]，

All boundary values of the motorless collision zone are obtained. The analytic formula of the passive collision zone is as follows:

step eight: and predicting the available escape time and escape distance for the unmanned aerial vehicle to avoid the warning target in the passive collision area.

Before defining the escape time and the escape distance, a maximum maneuvering collision area and an unavoidable area need to be established.

The maximum maneuvering collision area is used for predicting potential collision situations after corresponding maneuvering of the unmanned aerial vehicle;

the unavoidable region is used for predicting the set of the current positions of the intrusion machines, which cannot avoid collision, no matter whether the unmanned aerial vehicle maneuvers or adopts any maneuver.

The maximum maneuvering collision area is used for predicting potential collision and collision situations after corresponding maneuvering of the unmanned aerial vehicle, and the unavoidable area is used for predicting a set of current positions of the intrusion machine, wherein collision cannot be avoided no matter whether the unmanned aerial vehicle maneuvers or takes any maneuver.

(1) modeling a maximum maneuvering collision area;

in three dimensions, when a drone encounters a threat that requires emergency avoidance, maneuvers commonly employed include horizontal turning maneuvers, vertical lift maneuvers, and combinations of both. In the invention, the calculation of the maximum maneuvering collision area is based on the maneuvering avoidance mode possibly adopted by the unmanned aerial vehicle, and three emergency steering modes of rolling and pulling the unmanned aerial vehicle leftwards, rolling and pulling rightwards and pulling the unmanned aerial vehicle are analyzed.

Defining the average acceleration of the unmanned plane in the horizontal direction during emergency steering as a_hAverage acceleration in the vertical direction is a_vAnd a is a_h> 0 denotes the mean acceleration edge O_ry_rPositive axial direction, a_v> 0 denotes the mean acceleration edge O_rz_rThe positive direction of the axis.

Suppose that the maximum average acceleration that the drone can produce in the horizontal plane is a₁Maximum average acceleration a producible in the vertical direction₂. Corresponding to different emergency steering modes, in the above formula a_hAnd a_vThe values of (A) are respectively as follows:

1) when the unmanned plane is lifted by maximum overload，a_h＝0，a_v＝a₂；

3) When the drone rolls to the right and pulls with maximum overload, a_h＝-a₁，a_v＝a₂。

The maximum maneuvering collision area refers to a current position set of the intrusion machine, wherein the current position set cannot avoid collision just because the unmanned aerial vehicle adopts a certain emergency steering mode with maximum overload from the current moment. Similar to the passive collision zone, the criticality of this zone is still: when the unmanned aerial vehicle is closest to the invader, the relative distance between the unmanned aerial vehicle and the invader is just equal to the minimum safe distance R of the unmanned aerial vehicle₀. According to R_LR when (t) ≠ 0_L' (t) ═ 0 and

can be established with t and R_LA system of equations in two-fold for unknowns:

R_L(t) ≠ 0, R_L' (t) ═ 0 is equivalent to

Will be provided with

Unfolding and finishing to obtain:

solving the above binary equation set by iteration method to obtain the closest time t and the current phi_LAnd gamma_LBoundary value R of collision region_L. Transversely get psi_L∈[0,2π]，

Taking the left maneuver of the unmanned aerial vehicle in the horizontal plane as an example, a schematic diagram of the maximum maneuver collision zone is shown in fig. 5. P₀At the initial moment, the unmanned aerial vehicle is forbidden to enter a point on the boundary of the zone, and after the time of delta t, P is₀To P₁And (4) point. P₁' from P₁Speed V of the intruder along the initial moment_BDirection translation V_BΔ t. If the invader is at P at the initial moment₁', the intruder just reaches P after the time of delta t₁A point, i.e. a collision, happens. It can be seen that P₁' is a point on the boundary line of the maximum maneuver collision area. In the same way, P₂Also on the zone boundary line. By analogy, each point on the boundary line can be obtained, and the maximum maneuvering collision area can be obtained.

(2) Modeling an unavoidable region;

the unavoidable regions can be divided into two categories: a one-way non-avoidable area and a completely non-avoidable area.

And defining the one-way unavoidable region as the intersection of the non-dynamic collision region and the maximum dynamic collision region corresponding to a certain emergency steering, and defining the completely unavoidable region as the intersection of all the one-way unavoidable regions. The difference between the two is that the one-way non-evasive zone corresponds to only a single applicable emergency steering mode, while the completely non-evasive zone corresponds to all applicable emergency steering modes. Obviously, when the invader is located in the totally inaccessible area, no matter whether the unmanned aerial vehicle is mobile or not, what kind of possible mobile is adopted, the collision of the two machines can not be avoided. And the intersection of the maximum maneuvering collision area and the non-maneuvering collision area corresponding to three types of emergency steering is adopted to represent the completely unavoidable area.

A schematic diagram of the unavoidable region is shown in fig. 6, taking a two-dimensional plane case as an example. In the figure, the intersection of the leftward and rightward maneuvering collision areas and the non-maneuvering collision area respectively form a leftward maneuvering unavoidable area and a rightward maneuvering unavoidable area, and the intersection of the two one-way unavoidable areas is a completely unavoidable area.

One-way escape time/distance: there are two categories, local and global. Wherein, the local one-way escape time/distance refers to the arrival of a dangerous target j at the one-way non-avoidable area E_ijThe remaining time/distance of (c) is expressed in msg, in the form of table 1, corresponding to a certain emergency maneuver mode and the safety information of the drone-intruder pair_ijI belongs to { l, r, u }, j belongs to {1,2, …, n }, and n is the number of intrusion machines detected by the unmanned aerial vehicle;

TABLE 1 storage form of collision zone safety information

The larger the local one-way escape distance is, the larger the distance between the unmanned aerial vehicle and the closest point of the intrusion machine is after the unmanned aerial vehicle adopts a corresponding emergency maneuvering mode. The proof of the proposition is as follows: as shown in FIG. 7, assume that the radius of the forbidden zone is R₀The local one-way escape distance from the invader to the one-way non-evasive area is d_e. Since any point in space can be represented as the intersection of a free initial relative velocity vector and the boundary line of the one-way unavoidable region corresponding thereto and established according to different minimum safe distances,so that Δ R is present₀And the + delta R is the minimum safety distance, a corresponding virtual forbidden zone, a virtual inorganic dynamic collision zone and a virtual one-way non-avoidable zone can be established, and the intersection point of the boundary of the virtual one-way non-avoidable zone and the initial relative speed is just the current position of the intruder. According to equations (6) and (13), the closest point distance between the intruder and the drone is R₀+ Δ R, and Δ R and d_eAnd are in positive correlation. Thus, d_eThe larger the distance between the intruder and the nearest point of the drone.

When only a single emergency target exists, the local one-way escape distance can be used as an index for evaluating the quality of a corresponding emergency maneuvering mode, wherein the larger the local one-way escape distance is, the larger the distance between the unmanned aerial vehicle and the closest point of the emergency maneuvering mode is after the unmanned aerial vehicle adopts the corresponding emergency maneuvering mode is, the higher the flight safety is, and the more the maneuvering mode is advantageous; conversely, the more disadvantageous the maneuvering mode is, the more disadvantageous the avoidance of collisions will be.

According to the definition of the non-evaluable area, when the invader is located in the completely non-evaluable area, the collision is considered to occur. When the intruder is located outside the completely unavoidable region, that is, at least outside one-way unavoidable region, the unmanned aerial vehicle is likely to avoid collision. Under this premise, a schematic diagram of an emergency encounter between a drone and a single intruder is shown in fig. 8.

In fig. 8(a), the intruder is located outside all the one-way unavoidable regions. And the ray along the initial time relative speed direction with the position of the invading machine as a starting point is intersected with the left maneuvering unavoidable region and the right maneuvering unavoidable region in sequence. This indicates that the intruder flies in the current direction and will reach the left maneuver unavoidable region first and then reach the right maneuver unavoidable region. Setting the local one-way escape distance as d₁And d₂(0＜d₁＜d₂) The maximum local one-way escape distance d_max＝max(d₁,d₂)＝d₂. At this moment, no matter unmanned aerial vehicle is maneuvered leftwards or rightwards, all can avoid the collision, but the local one-way escape distance of the former is less, consequently maneuvers rightwards and is more favorable to guaranteeing unmanned aerial vehicle's safety.

In the figureIn 8(b), the intruder is located outside the totally unavoidable area, but has entered into the left maneuver unavoidable area. At this time, there is d₁＜0＜d₂I.e. the local one-way escape distances of the two are negative and positive respectively, and d is still true_max＝max(d₁,d₂)＝d₂. If the unmanned aerial vehicle maneuvers leftwards, collision with the invading machine is inevitable, and the unmanned aerial vehicle maneuvers rightwards, collision can be avoided. Thus, a right maneuver is still the optimal choice at this time.

In combination with the above analysis, the following conclusions can be drawn: for the situation of avoiding a single intrusion machine in an emergency, the unmanned aerial vehicle should adopt an emergency maneuvering mode corresponding to the maximum local one-way escape distance.

The global one-way escape time/distance refers to the global minimum local one-way escape time/distance corresponding to the same emergency maneuver mode i and is expressed as

The difficulty of avoiding multi-machine strategy design in emergency is how to handle conflicts among different 'optimal' emergency maneuvering modes compared with a single-machine situation on the assumption that a plurality of emergency targets exist in the current scene at the same time. A schematic diagram of an unmanned aerial vehicle encountering a multiple intruder in emergency is shown in fig. 9. According to the emergency evasion strategy of the single unmanned aerial vehicle, the optimal maneuvering modes of the unmanned aerial vehicle for evading the

intruders

1 and 2 are right maneuvering and left maneuvering respectively, and the unmanned aerial vehicle cannot adopt two different maneuvering modes simultaneously. In order to obtain a globally optimal maneuver mode, the multi-machine scenario needs to be further analyzed.

In the present case, the use of only a local one-way escape distance does not sufficiently evaluate the merits of the maneuver. The global one-way escape distance is the popularization of the local one-way escape distance under the scene of multiple emergency targets, and the smaller the distance is, the smaller the minimum value of the closest point distances between the unmanned aerial vehicle and all the emergency targets is, the greater the risk of collision is, and the lower the safety of adopting the maneuvering mode is. Therefore, the global one-way escape distance is used as an index for evaluating the quality of the maneuvering mode under the condition of multiple machines. When only a single emergency target exists, the global one-way escape distance is the local one-way escape distance of the single machine.

Assume that the respective local one-way escape distances in the case shown in fig. 9 are as shown in table 2. The global one-way escape distances corresponding to the left and right maneuvers of the unmanned aerial vehicle are 90 meters and 150 meters respectively, and then the right maneuver is the optimal maneuver mode at the moment.

TABLE 2 local one-way escape distances under multi-machine conditions

In summary, the strategy of emergency evasion of multiple intrusion machines or single intrusion machine has a uniform form despite the different number of intrusion machines encountered in emergency.

Finally, the available emergency collision avoidance maneuver strategies are: an emergency maneuver corresponding to the maximum global one-way escape distance is taken. The strategy is suitable for the scene of a single invader and the scene of multiple invaders.

The unmanned aerial vehicle makes collision avoidance maneuvers too early, too late, or in an inappropriate manner, which may result in unnecessary maneuvers or an inability to avoid obstacles. Therefore, it is very important to utilize the established three-dimensional dynamic collision zone to help the drone select collision avoidance maneuvers and to introduce a time threshold to decide the collision avoidance timing. In addition, in order to avoid unnecessary calculation expense and improve the real-time performance of the collision avoidance decision method, threat assessment is firstly carried out on each intrusion machine before a collision area is calculated. If the evaluation results are all smaller than the warning threshold value, the risk that the unmanned aerial vehicle collides with each intrusion machine is small at the moment, and emergency maneuvering is not needed, so that the step of calculating the collision area can be omitted.

Each of the three-dimensional dynamic collision zones described above corresponds to a particular drone-intruder pair. To distinguish the safety information obtained from the collision zone, the information corresponding to one drone-intruder pair is said to be local, while the information corresponding to all drone-intruder pairs is said to be global. By combining the definitions of the one-way unavoidable region and the completely unavoidable region, it can be seen that the safety information in the dynamic collision region can be stored in a two-dimensional array form. Assuming that the current unmanned aerial vehicle detects n intruders in total, the collision zone safety information is stored in the form shown in table 1. Since the drone needs to avoid collision with any intruder, the most conservative value should be taken when considering the global security information.

an emergency goal refers to a dangerous goal where the local total escape time is no greater than the decision time. The decision time is given according to the maneuvering capability of the unmanned aerial vehicle, the measurement error of the sensing equipment, the minimum safe distance between the two unmanned aerial vehicles and other factors.

When the global complete escape time is not longer than the decision time, the collision is considered to be about to occur, and the unmanned aerial vehicle adopts emergency maneuver to rapidly change the self track speed and direction, so that the flight track is changed, and the collision conflict is resolved. Before designing an emergency collision avoidance maneuver strategy, a basic emergency maneuver mode needs to be determined first. Combining the three-degree-of-freedom particle model, the assumptionThe maximum normal overload of the unmanned aerial vehicle is n_nmaxThe maximum allowable roll angle is mu_maxThe unmanned plane turns to positive by rolling to the right around the velocity vector; under the ideal condition, the unmanned aerial vehicle keeps the current speed unchanged during maneuvering, and the track azimuth angle before taking emergency maneuvering is X₀The preset maximum track azimuth angle variation is delta x_max(Δχ_max＞0)。

according to the local one-way escape distance, an emergency maneuver corresponding to the maximum global one-way escape distance is adopted, and the corresponding emergency maneuver is started, as shown in fig. 10, specifically as follows:

When only a single emergency target exists, the established collision area model simultaneously comprises dynamic distance safety information and time safety information, so that clear and effective judgment basis can be provided for emergency collision avoidance decision. The following scene that the unmanned aerial vehicle and the emergency target meet with emergently is analyzed based on the collision zone model, so that an emergency collision avoidance strategy is designed, the strain capacity of the unmanned aerial vehicle under extreme conditions is enhanced, and further guarantee is provided for safe flight of the unmanned aerial vehicle in a shared airspace.

The larger the local one-way escape distance is, the larger the distance between the unmanned aerial vehicle and the closest point of an emergency target is after the corresponding emergency maneuvering mode is adopted, the higher the flight safety is, and the more advantageous the maneuvering mode is; conversely, the more disadvantageous the maneuvering mode is, the more disadvantageous the avoidance of collisions will be.

The route re-planning is shown in fig. 11, and means that the unmanned aerial vehicle moves to a target point and avoids obstacles by using an artificial potential field method and a rolling time domain optimization method according to changes of a dynamic environment.

Examples

The unmanned aerial vehicle autonomous collision avoidance decision effect under the complex environment adopting the method of the invention is shown in fig. 12-16. The flight path of the unmanned aerial vehicle is a solid line, and the dynamic barriers respectively perform uniform linear motion along the corresponding dotted line directions. The unmanned aerial vehicle has the advantages that the unmanned aerial vehicle can avoid static and dynamic obstacles and finally safely reach a target point by timely adopting emergency maneuver after the proposed collision avoidance maneuver strategy is adopted, as shown in fig. 12, the three-dimensional air route is shown in fig. 12(a), the projection of the air route on the horizontal plane is shown in fig. 12(b), and the flight height of the unmanned aerial vehicle is shown in fig. 12 (c).

Emergency collision avoidance for unmanned aerial vehicles in emergency situations is shown in fig. 13. Before an emergency encounter occurs, as shown in fig. 13(a), the unmanned aerial vehicle avoids static and dynamic obstacles while flying to a target point through airway re-planning; when the simulation is carried out for 126s, the unmanned aerial vehicle and the dynamic barrier are in emergency encounter, and the unmanned aerial vehicle adopts the upper left emergency maneuver according to the emergency collision avoidance strategy. The emergency steering process lasts for 5s, during which the track azimuth angle of the drone changes from 42.95 ° to 92.72 °, and the track inclination angle changes from-5.02 ° to 26.03 °; after the emergency steering segment is finished, as shown in fig. 13(b), collision conflict between the unmanned aerial vehicle and the dynamic obstacle is resolved, and the unmanned aerial vehicle continues to fly in an airway re-planning mode until reaching a target point.

The overload of the drone along the trajectory during flight is shown in figure 14. FIG. 14(a) shows overload along the x-axis of the track system, FIG. 14(b) shows overload along the y-axis of the track system, and FIG. 14(c) shows overload along the z-axis of the track system, and it can be seen that each axis of overload is within the constraint. Furthermore, at 126s to 131s, the overload of the drone is maximized both along the y-axis and the z-axis, due to the adoption of the upper left emergency maneuver.

The distance between the unmanned aerial vehicle and each dynamic obstacle is as shown in fig. 15, and the distances of the closest points are 328.5m, 484.5m and 247.3m, which are all larger than the minimum safe distance 150 m.

The threat index of the most threatening dynamic obstacle to the drone at each moment during flight is shown in fig. 16, and the case where the index is higher than the warning threshold occurs at 49s to 51s and 123s to 127s, respectively. In a first time period, although the relative distance between the No. 1 dynamic barrier and the unmanned aerial vehicle is smaller, the condition that emergency collision avoidance maneuver needs to be adopted is not met, the unmanned aerial vehicle can avoid collision by adopting a route re-planning strategy, and the maximum threat index is reduced to be below a warning threshold value; in the second time period, due to the occurrence of an emergency encounter situation, the unmanned aerial vehicle needs to adopt an emergency collision avoidance maneuver to ensure flight safety and reduce the maximum threat index.

Claims

1. An unmanned aerial vehicle autonomous collision avoidance decision method based on a three-dimensional dynamic collision zone is characterized by specifically comprising the following steps:

assuming that the target sight line is directed to the obstacle centroid by the unmanned aerial vehicle centroid;

total threat index T for ith obstacle_iBy distance threat index T_riAngle threat index T_aiAnd velocity threat index T_viThe three parts are as follows;

the definition of each threat index is as follows:

(1) distance threat index T_ri:

Wherein r is_aFor a warning distance, r_dIs the distance at risk; r_LiThe relative distance between the unmanned aerial vehicle and the ith obstacle; when R is_LiGreater than r_aThe distance threat index of the obstacle is zero; when R is_LiBetween r_aAnd r_dWhen R is_LiThe smaller the distance threat index; r_LiLess than r_dWhen the distance between the unmanned aerial vehicle and the obstacle is too short, the distance threat index is maximum;

(2) angular threat index T_ai:

q_rThe included angle between the speed direction of the unmanned aerial vehicle relative to the barrier and the target direction is defined, and the left deviation of the relative speed direction is positive; when in use

When the unmanned aerial vehicle approaches the obstacle, the relative distance between the unmanned aerial vehicle and the obstacle tends to decrease, which indicates that the unmanned aerial vehicle approaches the obstacle and a collision risk exists; | q_rThe closer to zero the | is, the greater the collision risk is, and the greater the corresponding angle threat index is;

when the relative distance is unchanged or increases, the unmanned aerial vehicle is far away from the obstacle, and the angle threat index is zero at the moment;

(3) velocity threat index T_vi:

When the obstacle is a dynamic intrusion machine:

in the formula, V_AIs the ground speed of the unmanned aerial vehicle, V_iThe ground speed of the invader is the size; v_i∈[0,0.6V_A) The speed of the time-lapse invading machine is obviously less than that of the unmanned aerial vehicle, the relative speed is mainly determined by the unmanned aerial vehicle at the time, the unmanned aerial vehicle can change the relative speed more easily by adjusting the self speed, the collision avoidance difficulty is lower, and the speed threat index of the invading machine is smaller; v_i∈[0.6V_A,1.5V_A) When, V_iApproaching or even exceeding V_AThe capability of the unmanned aerial vehicle for changing the relative speed by adjusting the speed of the unmanned aerial vehicle is obviously weakened, the collision avoidance difficulty is improved, and the speed threat index of the intrusion machine is V-shaped_iAnd V_AAn increase in the ratio; v_i∈(1.5V_A, + ∞), the speed threat index of the intruder reaches a maximum;

when the obstacle is a static obstacle, the threat assessment object is a point on the surface of the static obstacle, which is closest to the unmanned aerial vehicle, and V_i＝0；

the total threat index for the ith obstacle is calculated as follows:

T_i＝ω_rT_ri+ω_aT_ai+ω_vT_vi (4)

wherein, ω is_rIs a distance threat index T_riCorresponding weight, ω_aAs an angular threat index T_aiCorresponding weight, ω_vIs a velocity threat index T_viCorresponding weight, ω_r+ω_a+ω_v1, and ω_r＞0，ω_a＞0，ω_v＞0；

the alert target is an intruder in the threat source;

the boundary conditions of the no-maneuver collision zone refer to: when the unmanned aerial vehicle is closest to the warning target, collision happens, namely the distance between the closest points is just equal to the minimum safety distance;

the specific calculation process is as follows:

step 601, defining a basic coordinate system and a motion variable;

the position vector of the A mass center of the unmanned aerial vehicle is P_AInvasion machineThe position vector of B centroid is P_BThe ground speed vector of unmanned aerial vehicle A is V_AThe ground speed vector of the invader B is V_B；

the relative distance between the unmanned aerial vehicle A and the intrusion machine B is R_LRelative velocity is V_r；

Step 603, establishing a space rectangular coordinate system O fixedly connected with the mass center of the unmanned aerial vehicle A by taking the mass center of the unmanned aerial vehicle A as an original point_rx_ry_rz_r；

velocity vector V of the earth_BHorizontal plane projection of (2) and (x)_rThe angle between the axes being psi_BRelative distance R_LHorizontal plane projection of (2) and (x)_rThe angle between the axes being psi_LWhen the projection is relative to x_rPositive when rotating axially and leftwards; velocity vector V of the earth_AAnd O_rx_ry_rThe included angle of the plane is equal to the ground speed vector V_AAnd the horizontal plane O_gx_gy_gAngle gamma therebetween_A(ii) a Velocity vector V of the earth_BAnd O_rx_ry_rThe included angle of the plane is equal to the ground speed vector V_BAnd the horizontal plane O_gx_gy_gAngle gamma therebetween_B(ii) a Relative distance R_LAnd O_rx_ry_rIncluded angle of plane is gamma_L(ii) a The included angle is positive when the vector is above the plane;

Δ x, Δ y, Δ z are relative distances R_LAlong x_rAxis, y_rAxis and z_rA component of the axis;

the boundary condition analytic formula is:

wherein the content of the first and second substances,

R₀the minimum safe distance for the drone;

step seven, judging whether the alert target is positioned in the boundary condition of the corresponding passive collision area, if so, entering the step eight, and performing route re-planning;

step eight, predicting the available escape time and escape distance for the unmanned aerial vehicle to avoid the warning target in the no-dynamic collision area;

R_L(t) ≠ 0, R_L' (t) ═ 0 is equivalent to

Will be provided with

Unfolding and finishing to obtain:

a_hduring emergency steering, the average acceleration of the unmanned aerial vehicle in the horizontal direction; a is_vDuring emergency steering, the average acceleration of the unmanned aerial vehicle in the vertical direction; and a is_h> 0 denotes the mean acceleration edge O_ry_rPositive axial direction, a_v> 0 denotes the mean acceleration edge O_rz_rThe positive direction of the axis;

(2) modeling the unavoidable region, specifically as follows:

the unavoidable distinction is two types: a one-way unavoidable region and a completely unavoidable region;

the one-way unavoidable region is the intersection of an inorganic dynamic collision region and a maximum dynamic collision region corresponding to a certain emergency steering; the completely unavoidable region is the intersection of all the one-way unavoidable regions; the intersection of the maximum maneuvering collision area and the non-maneuvering collision area corresponding to the three types of emergency steering is adopted to represent a completely unavoidable area;

firstly, aiming at an unmanned aerial vehicle-invader pair, a one-way non-evasive area of a dangerous target j and an emergency maneuver mode i is defined as E_ij(ii) a The set of emergency maneuvers is { l, r, u }, including rolling leftRotating and pulling up, rolling and pulling up to the right;

the definition of safety information is based on dangerous targets, i.e. alert targets located in the respective passive collision zones; the method comprises the following steps: one-way escape time/distance, and complete escape time;

one-way escape time/distance: the method is divided into local and global categories;

wherein, the local one-way escape time/distance refers to the arrival of a dangerous target j at the one-way non-avoidable area E_ijThe remaining time/distance of (c), corresponding to a certain emergency maneuver and the safety information of the drone-intruder pair, is denoted msg_ijI belongs to { l, r, u }, j belongs to {1,2, …, n }, and n is the number of intrusion machines detected by the unmanned aerial vehicle;

Complete escape time: the method is divided into local and global categories;

wherein, the local complete escape time refers to the situation that a dangerous target j reaches a completely unavoidable region E_com,jIs expressed as

The 'one-way' indicates that the row subscript of the security information is a given value in a set { l, r, u }, and the 'complete' indicates that the row subscript can be taken from { l, r, u } in a traversal way; the local column index indicates that the column index of the security information is a given value in the set {1, …, n }, and the global column index indicates that the column index takes values from {1, …, n } in a traversal way;

step nine: judging whether the local complete escape time is greater than the decision time, if so, judging that an emergency target exists in the warning target, and carrying out emergency maneuvering by the unmanned aerial vehicle according to an emergency collision avoidance strategy; otherwise, carrying out route re-planning;

when the global complete escape time is not more than the decision time, the collision is considered to be about to occur, and the unmanned aerial vehicle should adopt emergency maneuver;

1) upward emergency maneuvering: in the emergency steering section, the unmanned plane is pulled up with the maximum overload and has n_x＝sinγ，n_y＝0，n_z＝n_nmax(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters a stable flight section, and tracks χ ═ χ₀，γ＝γ_maxThe steady straight-line flight trajectory of (1);

n_nmaxthe maximum normal overload of the unmanned aerial vehicle is achieved;

2) emergency maneuvering at the upper left: in the emergency steering section, the unmanned aerial vehicle rolls to the left with maximum overload and is pulled up, and n is provided_x＝sinγ，n_y＝n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters a stable flight section, and tracks χ ═ χ₀-Δχ_max，γ＝γ_maxThe steady straight-line flight trajectory of (1);

μ_maxat the maximum allowable roll angle, Δ χ_maxFor a predetermined maximum track azimuth change (Δ χ)_max＞0)；

3) Emergency maneuvering at the upper right: in the emergency steering section, the unmanned plane rolls to the right with maximum overload and is pulled up, and n is provided_x＝sinγ，n_y＝-n_nmaxsinμ_max，n_z＝n_nmaxcosμ_max(ii) a When the track inclination angle reaches the maximum rising angle gamma_maxIn time, the unmanned aerial vehicle enters stablyFlight path, tracking χ ═ χ >₀+Δχ_max，γ＝γ_maxThe steady straight-line flight trajectory of (1);

χ₀adopting a track azimuth angle before emergency maneuver;

when only a single emergency target exists, the larger the local one-way escape distance is, the larger the distance between the unmanned aerial vehicle and the closest point of the emergency target is after the corresponding emergency maneuvering mode is adopted, the higher the flight safety is, and the maneuvering mode is more advantageous; on the contrary, the maneuvering mode has disadvantages and is more unfavorable for avoiding collision;

2. The unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone as claimed in claim 1, wherein the comprehensive situation information specifically includes: the position and ground speed vector of the unmanned aerial vehicle, the position, shape, size and ground speed vector of each obstacle detected.

3. The unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone as claimed in claim 1, wherein in the second step, in the emergency steering section, the unmanned aerial vehicle applies the maximum control force to realize the rapid change of the track inclination angle and/or the track azimuth angle; when the change amount of the ground speed direction reaches the maximum rising angle of the track inclination angle, the unmanned aerial vehicle finishes the emergency steering and enters a stable flight section.

4. The unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision zone as claimed in claim 1, wherein in step 603, with the unmanned aerial vehicle a centroid as an origin, a spatial rectangular coordinate system O fixedly connected with the unmanned aerial vehicle a centroid is established_rx_ry_rz_rThe other method is as follows: the ground coordinate system S_gAfter the origin point of the robot is translated to the position of the center of mass of the unmanned aerial vehicle, the robot rotates around the z axis by an angle x_A(ii) a Due to the fact thatHere, the following transformation relationship is established:

wherein, χ_LIs a relative distance R_LProjection in the horizontal plane and x_gThe included angle of the axes; velocity vector V of the earth_BAt the horizontal plane O_gx_gy_gProjection of (2) and x_gThe included angle between the axes is chi_B。

5. The unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision zone as claimed in claim 1, wherein in step eight, modeling is performed on the maximum maneuvering collision zone corresponding to different emergency steering modes, a_hAnd a_vThe values of (A) are respectively as follows:

3) When the drone rolls to the right and pulls with maximum overload, a_h＝-a₁，a_v＝a₂；

a₁The maximum average acceleration generated by the unmanned aerial vehicle in the vertical direction is obtained;

solving by adopting an iterative method to obtain the closest time t and the current psi corresponding to the closest time t_LAnd gamma_LBoundary value R of collision region_L(ii) a Transversely get psi_L∈[0,2π]，

All boundary values of the maximum maneuvering collision zone corresponding to the current emergency steering mode can be obtained;

6. The unmanned aerial vehicle autonomous collision avoidance decision method based on the three-dimensional dynamic collision zone as claimed in claim 1, wherein in the ninth step, the decision time is given according to the maneuvering capability of the unmanned aerial vehicle, the measurement error of the sensing device, and the minimum safety distance between the two devices.