CN-122020015-A - Unmanned aerial vehicle conflict detection method based on track prediction and speed obstacle method

CN122020015ACN 122020015 ACN122020015 ACN 122020015ACN-122020015-A

Abstract

S1, acquiring input data required by track prediction, inputting the input data into a track prediction module to predict the flight track of each unmanned aerial vehicle in a prediction period, and outputting a structure array of the flight track prediction of each unmanned aerial vehicle; S2, inputting the structural body array into a collision detection module for collision detection among unmanned aerial vehicles, wherein the collision detection module comprises the steps of firstly adopting a threshold method for coarse screening, secondly carrying out secondary screening on the unmanned aerial vehicle with no altitude change in the flight stage by adding route altitude overlapping judgment, and finally adopting an improved speed obstacle method combined with the structural body array for collision judgment. The method can reduce the false alarm rate and the false alarm rate of unmanned aerial vehicle conflict judged by the traditional speed obstacle method.

Inventors

BAI CHEN
CUI KE
LI DEHONG
ZHANG LIPENG
CHEN JUNJIE
LI DANGYI
ZHANG GUOCUI

Assignees

卡斯柯信号(成都)有限公司

Dates

Publication Date: 20260512
Application Date: 20251216

Claims (10)

1. The unmanned aerial vehicle conflict detection method based on the track prediction and the speed obstacle method is characterized by comprising the following steps of: S1, acquiring input data required by track prediction, inputting the input data into a track prediction module to predict the flight track of each unmanned aerial vehicle in a prediction period, and outputting a structure array predicted by the flight track of each unmanned aerial vehicle; S2, inputting the structural body array into a collision detection module for collision detection among unmanned aerial vehicles, wherein the collision detection module comprises the steps of firstly adopting a threshold method for coarse screening, secondly carrying out secondary screening on the unmanned aerial vehicle with no altitude change in the flight stage by adding route altitude overlapping judgment, and finally adopting an improved speed obstacle method combined with the structural body array for collision judgment.
2. The unmanned aerial vehicle collision detection method based on the track prediction and speed obstacle method according to claim 1, wherein in the step S1, the structure body array comprises an unmanned aerial vehicle serial number, a track serial number, a predicted track type and a structure body, the predicted track type comprises a line segment, a circle and a point, and the structure body comprises a line segment structure body, a circle structure body and a point structure body; The line segment structure body represents that the unmanned aerial vehicle does uniform linear motion, the round structure body represents that the fixed wing unmanned aerial vehicle does turning maneuver, and the point structure body represents that the multi-rotor unmanned aerial vehicle does turning maneuver; If the unmanned aerial vehicle is a fixed-wing unmanned aerial vehicle, the track prediction output is a line segment+circular structure combination, and if the unmanned aerial vehicle is a multi-rotor unmanned aerial vehicle, the track prediction output is a line segment+point structure combination.
3. The unmanned aerial vehicle collision detection method based on the track prediction and the speed obstacle method according to claim 1, wherein in the step S1, the predicting the flight track of each unmanned aerial vehicle in the space within the prediction period, and outputting a structure array predicted by the flight track of each unmanned aerial vehicle, comprises: s101, judging whether the unmanned aerial vehicle turns, if so, entering a step S102, otherwise, entering a step S103; S102, calculating the residual time and turning end time of the current turning state, updating a turning stage track prediction output structure body array, updating the initial position, the initial time, the track prediction residual time, the target point number and the track serial number of the next time period, and then entering the step S103; S103, calculating the time t i when the unmanned aerial vehicle reaches the next target point/turning area, judging whether t i is more than or equal to the track prediction residual time delta t, if yes, entering a step S104, otherwise, entering a step S105; S104, calculating a uniform linear motion track prediction result from the starting time of the current period to the track prediction ending time, updating a track prediction output structure array and outputting the track prediction output structure array; S105, calculating a track prediction result from the starting time of the current period to the ending time of the uniform linear motion, updating a track prediction output structure array of the uniform linear motion stage, updating the initial position, the initial time, the track prediction residual time and the track sequence number of the next period, and then entering the step S106; S106, inquiring turning maneuver time t ti of the unmanned aerial vehicle under the current route turning angle, judging whether the track prediction residual time delta t is less than or equal to the turning maneuver time t ti , if yes, updating the track prediction output structure array and outputting, otherwise, entering the step S107; and S107, updating the turning stage track prediction output structure body array, updating the initial position, the initial time, the track prediction residual time, the target point number and the track serial number of the next time period, and returning to the step S104.
4. The unmanned aerial vehicle collision detection method based on the track prediction and speed obstacle method according to claim 1, wherein in the step S1, for a multi-rotor unmanned aerial vehicle, each unmanned aerial vehicle in the prediction space has a flight track within a prediction period, and a structure array predicted by the flight track of each unmanned aerial vehicle is output, and the method comprises: S111, judging whether the current state is turning maneuver, namely judging whether the distance between the current position (x 0 ,y 0 ,z 0 ) of the unmanned aerial vehicle and the current target point (x 1 ,y 1 ,z 1 ) is smaller than a multi-rotor turning judgment distance L t , if so, judging that the unmanned aerial vehicle is turning maneuver and entering the step S112, otherwise, judging that the unmanned aerial vehicle is in uniform linear motion and entering the step S113; S112, acquiring a historical moment when the unmanned aerial vehicle starts to enter a point state, calculating historical time t th of turning maneuver of the unmanned aerial vehicle by calculating a difference value between the current moment and the historical moment, inquiring and acquiring turning maneuver time t t1 of the unmanned aerial vehicle under a turning angle, calculating turning maneuver remaining time t tr = t t1 -t th and turning ending moment, updating a turning stage track prediction output structure body array, updating the initial position, the initial moment, the track prediction remaining time, a target point number and a track serial number of the next time period, and then entering the step S113; S113, if the real-time state of the unmanned aerial vehicle is turning maneuver, taking the cruising speed of the unmanned aerial vehicle as the constant-speed linear motion speed, otherwise taking the real-time speed V now of the current telemetry data as the constant-speed linear motion speed, calculating the time t i required by the unmanned aerial vehicle to fly to the current target point from the initial position at a constant speed according to the constant-speed linear motion speed, and judging whether t i is greater than or equal to the track prediction residual time Deltat, if so, entering the step S114, otherwise entering the step S115; s114, the unmanned aerial vehicle does uniform linear motion in the track prediction residual time, the unmanned aerial vehicle is predicted to fly to the direction of a target point in uniform linear motion from the current position according to the real-time speed/cruising speed of the current telemetry data, the position coordinate (x e ,y e ,z e ) reached after the flight delta t time is updated, and the track prediction output structure array is updated and output; S115, firstly making uniform linear motion and then turning maneuver on the unmanned aerial vehicle in the track prediction residual time, calculating a track prediction result from the starting time of the current period to the ending time of the uniform linear motion, updating a track prediction output structure array of the uniform linear motion stage, updating the initial position, the initial time, the track prediction residual time and the track serial number of the next period, and then entering the step S116; S116, inquiring turning maneuver time t ti of the unmanned aerial vehicle under the current route turning angle, judging whether the track prediction residual time delta t is less than or equal to the turning maneuver time t ti , if yes, updating a track prediction output structure array and outputting, otherwise, entering a step S117; s117, updating the turning stage track prediction output structure body array, updating the initial position, the initial time, the track prediction residual time, the target point number and the track serial number of the next time period, and returning to the step S114.
5. The unmanned aerial vehicle collision detection method based on the track prediction and speed obstacle method according to claim 1, wherein in the step S1, for a fixed wing unmanned aerial vehicle, the unmanned aerial vehicle is assumed to fly only in a cruising stage without height change and is abstracted into a two-dimensional plane; for a fixed wing unmanned aerial vehicle, predicting the flight trajectory of each unmanned aerial vehicle in the space domain in a prediction period, and outputting a structure array predicted by the flight trajectory of each unmanned aerial vehicle, wherein the structure array comprises: S121, judging whether the current state is turning maneuver, namely judging whether the angular speed of the unmanned aerial vehicle is larger than the set turning judgment angular speed, if so, judging that the unmanned aerial vehicle is turning maneuver and entering the step S122, otherwise, judging that the unmanned aerial vehicle is in uniform linear motion and entering the step S123; S122, acquiring a historical moment when the unmanned aerial vehicle starts to enter a round state, calculating historical time t th of turning maneuver of the unmanned aerial vehicle by calculating a difference value between the current moment and the historical moment, inquiring and acquiring turning maneuver time t t1 of the unmanned aerial vehicle under a turning angle, calculating turning maneuver remaining time t tr = t t1 -t th , calculating a round area range where a target point is located, updating a turning stage track prediction output structure array, updating an initial position, an initial moment, track prediction remaining time, a target point number and a track serial number of the next time period, and then entering a step S123; S123, if the real-time state of the unmanned aerial vehicle is turning maneuver, taking the cruising speed of the unmanned aerial vehicle as the constant-speed linear motion speed, otherwise taking the real-time speed V now of the current telemetry data as the constant-speed linear motion speed, calculating the length t i required by the unmanned aerial vehicle to reach the intersection point (x i1 ,y i1 ) of the navigation section and the next round area on the basis of the constant-speed linear motion speed, judging whether t i is greater than or equal to the track prediction residual time delta t, if yes, entering the step S124, otherwise entering the step S125; S124, the unmanned aerial vehicle does uniform linear motion in the track prediction residual time, the predicted unmanned aerial vehicle flies to the direction of the target point in uniform linear motion from the current position according to the real-time speed/cruising speed of the current telemetry data, and the position coordinate (x e ,y e ) reached after the flight delta t time is updated and output; s125, firstly making uniform linear motion and then turning maneuver on the unmanned aerial vehicle in the track prediction residual time, calculating a track prediction result from the starting time of the current period to the ending time of the uniform linear motion, updating a track prediction output structure array of the uniform linear motion stage, updating the initial position, the initial time, the track prediction residual time and the track serial number of the next period, and then entering into the step S126; S126, inquiring turning maneuver time t ti of the unmanned aerial vehicle under the current route turning angle, judging whether the track prediction residual time delta t is less than or equal to the turning maneuver time t ti , if so, calculating turning circle coordinates, updating a track prediction output structure array and outputting, otherwise, entering into the S127; S127, calculating a circle coordinate of a turn, updating a turn-stage track prediction output structure array, updating the initial position, the initial time, the track prediction residual time, the target point number and the track serial number of the next time period, and returning to the step S124.
6. The unmanned aerial vehicle collision detection method based on the trajectory prediction and the speed obstacle method according to claim 1, wherein the step S2 comprises the steps of: S201, acquiring each unmanned aerial vehicle structure array of track prediction output; s202, judging whether the unmanned aerial vehicle is in a threshold area, if so, entering a step S203, and if not, entering a step S208; s203, judging whether the unmanned aerial vehicle has no height change in the cruising stage, if so, entering a step S204, and if not, entering a step S205; S204, judging whether the heights of the airlines are overlapped, if so, entering a step S206, and if not, entering a step S208; s205, judging whether the shortest distance between the navigation sections of the two unmanned aerial vehicles in the cruising stage is larger than the sum of the envelope radiuses of the two unmanned aerial vehicles, if so, entering a step S208, otherwise, entering a step S206; S206, judging whether the two speeds conflict with each other by combining an improved speed obstacle method of track prediction, if so, entering a step S207, and if not, entering a step S208; s207, judging conflict between the two unmanned aerial vehicles; s208, judging that the two unmanned aerial vehicles do not collide.
7. The unmanned aerial vehicle collision detection method based on the trajectory prediction and the speed obstacle method according to claim 1, wherein in the step S2, the threshold method comprises: Wherein, the Is a threshold radius; the maximum horizontal speed of the first unmanned aerial vehicle; The maximum horizontal speed of the second unmanned aerial vehicle; Predicting time for the track; A first unmanned aerial vehicle safety envelope radius; A second unmanned aerial vehicle safety envelope radius; Is the upper height of the threshold; the maximum rising speed of the first unmanned aerial vehicle is set; The maximum descent speed of the second unmanned aerial vehicle; An upper envelope distance of the first unmanned aerial vehicle; A lower envelope distance of the second unmanned aerial vehicle; is the height under the threshold; The maximum descent speed of the first unmanned aerial vehicle; the maximum rising speed of the second unmanned aerial vehicle is set; a lower envelope distance for the first drone; the upper envelope distance of the second unmanned aerial vehicle; Is a threshold height; The unmanned aerial vehicle safety envelope is defined as an unmanned aerial vehicle safety range area, is set to be a cylinder, and if the distance between the first unmanned aerial vehicle and the second unmanned aerial vehicle is smaller than a threshold radius The height difference is smaller than And judging that the two unmanned aerial vehicles possibly have conflict conditions, and further utilizing the route height overlapping judgment, otherwise, judging that the two unmanned aerial vehicles do not have conflict conditions.
8. The unmanned aerial vehicle collision detection method based on the trajectory prediction and the speed obstacle method according to claim 1, wherein in the step S2, the route altitude overlap determination includes: Let unmanned aerial vehicle telemetry data altitude error be The unmanned aerial vehicle route height is Envelope height distance on unmanned aerial vehicle Lower envelope height distance The range of the unmanned aerial vehicle is as follows Hypothesis is that The range of the unmanned aerial vehicle is that Conflicting object height ranges ; If it is If the heights are overlapped, the possible conflict situation exists, the conflict is judged by using the improved speed obstacle method, otherwise, the heights are not overlapped, and the conflict situation does not exist.
9. The unmanned aerial vehicle collision detection method based on the track prediction and the speed obstacle method according to claim 1, wherein in the step S2, the collision determination by using the improved speed obstacle method combined with the structure array comprises the following steps: Constructing a first unmanned aerial vehicle time axis, a second unmanned aerial vehicle time axis and a track prediction state diagram of a corresponding period based on the structural body array; Based on an improved speed obstacle method, traversing each time period in a track prediction state diagram to carry out conflict judgment, and judging that two unmanned aerial vehicles conflict if the judgment result of a certain time period is conflict, and judging that two unmanned aerial vehicles do not conflict if the judgment result of all time periods is non-conflict until the judgment of the last time period is completed; The conflict judgment comprises a line segment-to-line segment time period conflict comparison, a line segment-to-point/circle time period conflict comparison and a point/circle-to-point/circle time period conflict comparison.
10. The unmanned aerial vehicle collision detection method based on the track prediction and the speed obstacle method according to claim 9, wherein in the step S2, the collision comparison between the line segment and the line segment period comprises the steps of adding a time variable into a speed obstacle model, converting a ray into the line segment, judging whether a line segment which is passed by the line segment and the line segment period of the relative speed intersects with a superimposed safety envelope, judging that two unmanned aerial vehicles collide with the line segment period if the line segment is intersected with the superimposed safety envelope, and otherwise judging that the two unmanned aerial vehicles do not collide with the line segment period, and specifically comprising the following steps: Constructing initial position coordinates of a first unmanned aerial vehicle and a second unmanned aerial vehicle, and constructing a speed obstacle circle by taking the initial position coordinates of the second unmanned aerial vehicle as a circle center and taking the first unmanned aerial vehicle safety envelope radius and the second unmanned aerial vehicle safety envelope radius as radii; Taking the initial position coordinate of the first unmanned aerial vehicle as a starting point, taking the relative speed direction as a direction vector, and constructing a line segment with the length of a line segment of relative speed v Phase (C) and a line segment period; judging whether the constructed line segment is intersected with the speed obstacle circle, if so, judging that the two unmanned aerial vehicles collide with the line segment period, otherwise, judging that the two unmanned aerial vehicles do not collide with the line segment period, and continuing to judge the next period; in step S2, the comparing the line segment conflict with the point/circle time segment includes: Constructing a speed obstacle model comprising a speed obstacle circle, wherein in the speed obstacle model, the relative speed is the speed of a first unmanned aerial vehicle, the starting point of the first unmanned aerial vehicle is the position coordinate of the first unmanned aerial vehicle at the starting moment of a line segment and a point/circle time period calculated after a track prediction module is called, the circle center of the speed obstacle circle is the circle center/point coordinate of a circular area predicted by the track, and the radius of the speed obstacle circle = the first unmanned aerial vehicle safety envelope radius + the second unmanned aerial vehicle safety envelope radius + the fixed wing track predicted circular area radius/the multi-rotor track predicted turning judgment distance; Constructing a line segment of a relative speed v Phase (C) and a line segment of a point/circle period in a speed obstacle model, and judging whether the constructed line segment is intersected with the speed obstacle circle or not; in step S2, the comparing the point/circle to the point/circle time period conflict includes: constructing a speed obstacle model comprising a safe circular area, wherein the speed obstacle model is as follows: taking the circle center/point coordinates of the first unmanned aerial vehicle, which are predicted and output by the track of the point/circle and the track of the point/circle period, as a circle center, fixing the radius of the wing track predicted circular area/the multi-rotor track predicted turning judgment distance+the radius of the first unmanned aerial vehicle safety envelope as a radius, and constructing a first unmanned aerial vehicle safety circular area; taking the circle center/point coordinates of the second unmanned aerial vehicle predicted and output by the point/circle and the point/circle time period track as the circle center, fixing the radius of the wing track predicted circular area/the multi-rotor track predicted turning judgment distance+the radius of the second unmanned aerial vehicle safety envelope as the radius, and constructing a second unmanned aerial vehicle safety circular area; and judging whether the two unmanned aerial vehicle safety round areas overlap in a two-dimensional space, if so, judging that the two unmanned aerial vehicles collide with the point/circle in the time period, otherwise, judging that the two unmanned aerial vehicles do not collide with the point/circle in the time period, and continuing to judge the next time period.

Description

Unmanned aerial vehicle conflict detection method based on track prediction and speed obstacle method Technical Field The invention relates to the technical field of unmanned aerial vehicle conflict detection, in particular to an unmanned aerial vehicle conflict detection method based on a track prediction and speed obstacle method. Background The speed obstacle method is used as a core geometric algorithm in the unmanned aerial vehicle anti-collision field, and a core mechanism of the speed obstacle method is established on the basis of analysis of kinematic relations among multiple intelligent agents. According to the method, the speed vector difference value between the aircrafts is calculated, the space constraint of the safety envelopes of the two aircrafts is fused, and the speed obstacle space is constructed. When the relative motion speed vector of the two unmanned aerial vehicles enters the speed obstacle space, the potential collision risk between the aircrafts is judged. The traditional speed obstacle method does not consider the flight plan of the unmanned aerial vehicle, namely the speed obstacle method considers that two aircrafts are always in the current speed direction, and the current speed is kept to fly at a constant speed in a straight line. Because the set route is usually formed by splicing a plurality of straight lines instead of one straight line, the unmanned aerial vehicle turns after reaching the target point, and does not ideally keep the same speed direction to do uniform linear motion all the time. Therefore, there is a need for an unmanned aerial vehicle collision detection method to improve the conventional speed barrier method. Disclosure of Invention In order to overcome the defects in the prior art, the invention discloses an unmanned aerial vehicle collision detection method based on a track prediction and speed obstacle method. According to the invention, the track prediction is carried out by acquiring information such as the flight plan, the maneuvering performance and the real-time flight data of the unmanned aerial vehicle, the track prediction result related to the flight plan is used as the input of the improved speed obstacle method, and the limitation of the traditional speed obstacle method in the constant speed direction can be improved by constructing the speed obstacle model in a time-sharing manner. The method can reduce the false alarm rate and the false alarm rate of unmanned aerial vehicle conflict judged by the traditional speed obstacle method. In order to achieve the above purpose, the present invention adopts the technical scheme that: An unmanned aerial vehicle conflict detection method based on track prediction and a speed obstacle method comprises the following steps: 1. Flight trajectory prediction S1, acquiring input data required by track prediction, inputting the input data into a track prediction module to predict the flight track of each unmanned aerial vehicle in a prediction period, and outputting a structure array predicted by the flight track of each unmanned aerial vehicle; 1. Input data Preferably, in step S1, the input data includes unmanned aerial vehicle fixed parameters, unmanned aerial vehicle flight plan and unmanned aerial vehicle real-time telemetry data, wherein: the unmanned aerial vehicle fixed parameters include: for a fixed wing unmanned aerial vehicle, the fixed parameters of the unmanned aerial vehicle comprise an unmanned aerial vehicle model, cruising speed, acceleration, deceleration, maximum speed, minimum speed, turning radius and turning maneuvering time under different turning angles; for a multi-rotor unmanned aerial vehicle, the fixed parameters of the unmanned aerial vehicle comprise an unmanned aerial vehicle model, cruising speed, acceleration, deceleration, maximum speed, hovering maneuver time and turning maneuver time under different turning angles; The unmanned aerial vehicle flight plan is represented by waypoint information of a route and comprises a waypoint number, a longitude, a latitude and a height, and when the unmanned aerial vehicle is a fixed-wing unmanned aerial vehicle, the unmanned aerial vehicle flight plan also comprises a waypoint radius; The unmanned aerial vehicle real-time telemetry data comprises an unmanned aerial vehicle serial number, a real-time position and a speed vector, and when the unmanned aerial vehicle is a fixed wing unmanned aerial vehicle, the unmanned aerial vehicle real-time telemetry data also comprises an angular speed. 2. Structure array Preferably, in the step S1, the structural body array comprises an unmanned aerial vehicle serial number, a track serial number, a predicted track type and a structural body, wherein the predicted track type comprises a line segment, a circle and a point, and the structural body comprises a line segment structural body, a circle structural body and a point structural body; The line segment structure body represents that the unmanne