Collision detection method based on multi-time scale trajectory prediction

Abstract

The invention discloses a collision detection method based on multi-time scale trajectory prediction, belongs to aircraft navigation, guidance and control, and is used for solving the technical problems of poor real-time performance and insufficient prediction precision of an existing flight trajectory prediction method. Firstly, an aircraft six-degree-of-freedom kinetic model and a control system model are established, and a future flight path is predicted through integration according to a current flight state and a control instruction; secondly, constructing an aircraft spherical collision surrounding body, and comparing a predicted trajectory with a digital map to carry out collision detection and avoidance; and finally, dynamically adjusting the trajectory prediction period and the prediction length according to the obstacle distance. According to the method, the calculation efficiency is improved through dynamic adjustment on the basis of ensuring the flight path prediction precision, and meanwhile, the method has good engineering practicability.

Description

1. Technical field

[0001] The present invention relates to an aircraft terrain collision detection method, in particular to a collision detection method based on multi-time scale trajectory prediction, and belongs to the field of aircraft navigation, guidance and control. 2. Background Technology

[0002] Collision detection is not only a crucial technical tool for ensuring aircraft flight safety and improving mission efficiency, but also holds significant application value in aircraft path planning and obstacle avoidance system design. Collision detection consists of two steps: trajectory prediction and collision detection. First, trajectory prediction primarily utilizes methods based on dynamic modeling, which considers the motion patterns of objects. However, in dynamic environments, computational speed decreases significantly as model complexity increases, making it difficult to meet real-time requirements. Furthermore, deep learning-based methods utilize data-driven approaches to improve detection accuracy, but they rely heavily on large, high-quality datasets and training techniques, lack physical interpretability, and are difficult to adapt to complex and changing scenarios. Second, collision detection primarily utilizes methods based on geometric modeling. While computationally efficient, these methods struggle to effectively handle interactions between high-speed objects in complex dynamic scenarios, and their ability to detect dynamic obstacles is limited. Sensor-based detection methods, while capable of real-time environmental awareness, are heavily dependent on sensor accuracy and environmental conditions. For example, in poor weather or lighting conditions, detection accuracy can significantly decline.

[0003] The paper "Terrain Matching-Based Forward Warning Method for Helicopter Low-Altitude Flight" (Zhang Shuoyan and Lu Yang, Journal of Beijing University of Aeronautics and Astronautics, Vol. 2, Issue 45, 2019) proposes using an aircraft flight dynamics model to predict escape trajectories and, based on this, generate warning boundaries for collision detection. In practical engineering applications, real-time trajectory prediction requires high hardware resources. This method, however, uses a fixed, long-term trajectory prediction, which significantly impacts the real-time performance and accuracy of collision detection.

[0004] This paper designs a collision detection method based on multi-timescale trajectory prediction for automatic ground collision avoidance systems during near-ground flight. This method enables rapid and effective detection of terrain obstacle threats, ensuring near-ground flight safety. To address the significant computational time consumption associated with trajectory prediction, this paper dynamically adjusts the trajectory prediction period and length to improve computational efficiency. This method is concise and readily applicable to engineering applications. 3. Summary of the invention

[0005] The present invention solves the technical problem by adopting a technical solution: a collision detection method based on multi-time-scale trajectory prediction, which is characterized by comprising the following steps:

[0006] Step 1: Establish the aircraft's six-degree-of-freedom kinematic and dynamic model

[0007]

[0008] Where x, y, and z are the positions of the aircraft below the ground; V is the aircraft airspeed; α and β are the airflow angles, namely the angle of attack and the sideslip angle, respectively; γ, χ, and μ are the track inclination angle, track azimuth angle, and track roll angle, respectively; T is the engine thrust; L, D, and Y are the aerodynamic lift, drag, and side force, respectively. θ, ψ are the aircraft's roll angle, pitch angle, and yaw angle, respectively; p, q, and r are the roll rate, pitch rate, and yaw rate, respectively; I xx , I yy , I zz , I xz is the aircraft's moment of inertia; L M 、M M 、N M They are rolling moment, pitching moment and yaw aerodynamic moment respectively.

[0009] Select the state quantity as X = [x, y, z, V, α, β, φ, θ, ψ, p, q, r] T , the input is U=[Tδ a δ e δ r ] T , where δ a , δ e , δ r is the deflection of the aircraft actuator (aileron, elevator, rudder), and equation (1) can be written as

[0010]

[0011] The aircraft control system model is established as

[0012]

[0013] Where V cmd 、φ cmd ,θ cmd , β cmd They are speed command, roll angle command, pitch angle command, and sideslip angle command respectively; K p1 , K p2 , K d2 , K p3 , K i3 , K d3 , K p4 , K i4 , K d4 are the autopilot PID control parameters.

[0014] Step 2: During the flight, pre Perform a trajectory prediction at every moment, T pre The prediction method is to first give the initial state value X0 of the navigation system at the current moment and the control instruction (V cmd ,φ cmd ,θ cmd ,β cmd ), define the predicted trajectory time length as L pre , use the numerical integration method to deduce the future L pre The trajectory in time. Discretize the continuous trajectory and record the trajectory at the nth moment as x n =x(nh),y n =y(nh),z n =z(nh), where h is the sampling and integration time. The predicted trajectory can be expressed as {(x0,y0,z0),(x1,y1,z1),…,(x n ,y n ,z n )},(n=L pre / h-1). The numerical integration method is selected as the fourth-order Runge-Kutta integration, and the system input {U0, U1,…, U n}, and use the following formula to realize trajectory recursion

[0015]

[0016] in, is the Runge-Kutta integral term, which is calculated as follows

[0017]

[0018] Step 3: Construct the aircraft collision boundary volume by predicting the trajectory and compare it with the digital elevation model to determine whether a collision has occurred. The aircraft collision boundary volume is assumed to be a spherical bounding volume centered on the center of mass of the aircraft, with a bounding volume radius of R p , set the alarm condition: in the area existence|z i -h T |≤R p Among them, h T is the height of the obstacle terrain. If the warning condition is met, further collision detection is performed, and the conditions are If this condition is met, the aircraft is considered to be on a collision course with the obstacle.

[0019] Step 4: Since the forecast period T preThis directly impacts the prediction error ε. Specifically, the smaller the prediction period, the higher the prediction accuracy. The specific relationship between the prediction period and prediction error is typically related to the aircraft type, maneuver intensity, and prediction method. Furthermore, based on collision detection requirements, the closer the object is to the obstacle, the higher the prediction accuracy required. Therefore, the prediction period is dynamically adjusted based on the distance to the obstacle. A large prediction period is used before a collision is detected, and a small prediction period is used after a collision is detected, as shown in the following formula.

[0020]

[0021] Where, T pre0 To set the large-scale prediction period before collision; t col To predict the collision moment; T pre1 To set the small-scale prediction period after the collision.

[0022] Step 5: Considering computational efficiency, dynamically adjust the prediction length L pre Affected by the integration method and computer performance, the calculation time T is usually equal to the prediction period T pre , prediction length L pre is positively correlated, that is

[0023] T∝(T pre ×L pre )(7)

[0024] Considering the tolerable calculation time, the prediction length adjustment scheme is designed as

[0025] L pre =min{L col ,L0} (8)

[0026] Where L0 is the maximum prediction length under the set tolerable calculation time; L col Displays the collision time to obstacles for the predicted trajectory.

[0027] The beneficial effects of the present invention are as follows: the method establishes a six-degree-of-freedom dynamic model and a control system model for an aircraft, and realizes trajectory prediction by giving an initial state and control instructions; the collision threat is judged based on the relationship between the aircraft collision bounding volume and a digital map to ensure flight safety; and a dynamic adjustment mechanism for the prediction period and prediction length is designed, taking into account the time consumption and accuracy of trajectory prediction.

[0028] (1) The aircraft is simplified into a spherical collision envelope and compared with the terrain, effectively utilizing the predicted trajectory information to ensure the collision threat detection capability in complex environments, while also having strong engineering practicality.

[0029] (2) Considering the poor real-time performance of trajectory prediction at a fixed time scale, the prediction length under large and small scale prediction cycles and tolerance time is designed according to the distance between the aircraft and the obstacle, so as to achieve efficient and accurate trajectory prediction. 4. Description of the accompanying drawings

[0030] Figure 1 A flowchart of a collision detection method based on multi-time-scale trajectory prediction provided according to a specific implementation example of the present invention is shown. The present invention discloses a collision detection method based on multi-time-scale trajectory prediction, which is used to solve the technical problems of poor real-time performance and insufficient prediction accuracy of existing flight trajectory prediction methods. The method first establishes a six-degree-of-freedom dynamic model and a control system model of the aircraft, and uses a numerical integration method to recursively predict the flight trajectory in the future time based on the current flight status and control instructions. Subsequently, a spherical collision bounding volume is constructed with the center of mass of the aircraft as the center, and the predicted trajectory is compared with a digital map for collision detection and avoidance. Finally, considering the computational efficiency of trajectory prediction, a large-scale prediction cycle is used when no collision is detected; when a collision is detected, a small-scale prediction cycle is switched, and the length of the prediction cycle is set to be dynamically adjusted within a tolerable computing time. While ensuring the accuracy of flight trajectory prediction, this method improves computational efficiency through staged processing and dynamic adjustment, and at the same time has good engineering practicality. 5. Specific implementation methods

[0031] Reference Figure 1 The specific steps of the collision detection method based on multi-time scale trajectory prediction of the present invention are as follows:

[0032] Step 1: Establish the aircraft's six-degree-of-freedom kinematics and dynamics model

[0033]

[0034] Where x, y, and z are the positions of the aircraft below the ground; V is the aircraft airspeed; α and β are the airflow angles, namely the angle of attack and the sideslip angle, respectively; γ, χ, and μ are the track inclination angle, track azimuth angle, and track roll angle, respectively; T is the engine thrust; L, D, and Y are the aerodynamic lift, drag, and side force, respectively. θ, ψ are the aircraft's roll angle, pitch angle, and yaw angle, respectively; p, q, and r are the roll rate, pitch rate, and yaw rate, respectively; I xx , I yy , I zz , I xz is the aircraft's moment of inertia; L M 、M M 、N M They are rolling moment, pitching moment and yaw aerodynamic moment respectively. Select F16 fighter as the object and set the model parameters to Ixx =9496slug / ft 2 , I yy =55814slug / ft 2 , I zz =63100slug / ft 2 , I xz =982 slug / ft 2 ,m=20500lb.

[0035] Select the state quantity as X = [x, y, z, V, α, β, φ, θ, ψ, p, q, r] T , the input is U=[Tδ a δ e δ r ] T , where δ a , δ e , δ r is the deflection of the aircraft actuator (aileron, elevator, rudder), and equation (9) can be written as

[0036]

[0037] The aircraft control system model is established as

[0038]

[0039] Where V cmd 、φ cmd ,θ cmd , β cmd They are speed command, roll angle command, pitch angle command, and sideslip angle command. p1 =0.638, K p2 =1.5, K d2 =1.6, K p3 =5.5, K i3 =3, K d3 =1.5, K p4 =4, K i4 =0.1, K d4 =0.02 is the autopilot PID control parameter.

[0040] Step 2: During the flight, pre Perform a trajectory prediction at every moment, T pre The prediction method is to first give the initial state value X0 = [0, 0, 10000ft, 450ft / s, 0.084rad, 0rad, 0rad, 0rad, 0.084rad, 0rad, 0rad / s, 0rad / s, 0rad / s] according to the current navigation system. T, and the control instructions received by the control system (V cmd =300ft / s,φ cmd =60°,θ cmd =20°,β cmd =0°), and define the predicted trajectory time length as L pre , use the numerical integration method to deduce the future L pre The trajectory in time. Discretize the continuous trajectory and record the trajectory at the nth moment as x n =x(nh),y n =y(nh),z n = z(nh), where h = 0.01s is the sampling and integration time. The predicted trajectory can be expressed as {(x0, y0, z0), (x1, y1, z1), …, (x n ,y n ,z n )},(n=L pre / h-1). The numerical integration method is selected as the fourth-order Runge-Kutta integration, and the system input {U0, U1,…, U n}, and use the following formula to realize trajectory recursion

[0041]

[0042] in, is the Runge-Kutta integral term, which is calculated as follows

[0043]

[0044] Step 3: Construct the aircraft collision boundary volume by predicting the trajectory and compare it with the digital elevation model to determine whether a collision has occurred. The aircraft collision boundary volume is assumed to be a spherical bounding volume centered on the center of mass of the aircraft, with a bounding volume radius of R p =200ft, set alarm condition: in area existence|z i -h T |≤R p Among them, h T is the height of the obstacle terrain. If the warning condition is met, further collision detection is performed, and the conditions are If this condition is met, the aircraft is considered to be on a collision course with the obstacle.

[0045] Step 4: Since the forecast period T preThis directly impacts the prediction error ε. Specifically, the smaller the prediction period, the higher the prediction accuracy. The specific relationship between the prediction period and prediction error is typically related to the aircraft type, maneuver intensity, and prediction method. Furthermore, based on collision detection requirements, the closer the object is to the obstacle, the higher the prediction accuracy required. Therefore, the prediction period is dynamically adjusted based on the distance to the obstacle. A large prediction period is used before a collision is detected, and a small prediction period is used after a collision is detected, as shown in the following formula.

[0046]

[0047] Where, T pre0 = 1s is the large-scale prediction period before collision; t col To predict the collision moment; T pre1 = 0.1s is the small-scale prediction period after collision.

[0048] Step 5: Considering computational efficiency, dynamically adjust the prediction length L pre Affected by the integration method and computer performance, the calculation time T is usually equal to the prediction period T pre , prediction length L pre is positively correlated, that is

[0049] T∝(T pre ×L pre )(14)

[0050] Considering the tolerable calculation time, the prediction length adjustment scheme is designed as

[0051] L pre =min{L col ,L0} (15)

[0052] Where L0=10s is the maximum prediction length under the set tolerable calculation time; L col Displays the time to collision with obstacles for the predicted trajectory.

[0053] Parts of the present invention that are not described in detail belong to common knowledge among those skilled in the art.

Claims

1. A collision detection method based on multi-time scale trajectory prediction, characterized in that: The collision detection method includes: 1) establishing a six-degree-of-freedom kinematic and dynamic model of the aircraft, and an aircraft control system model; 2) By giving the initial state value and control instructions, trajectory prediction is achieved based on numerical integration; 3) Determine whether a collision has occurred based on the comparison of the aircraft collision boundary volume with the digital elevation model; 4) Dynamically adjust the prediction period based on the distance to the obstacle, which is divided into two scales: large and small; 5) Considering the tolerable computing time, dynamically adjust the prediction length.

2. The collision detection method according to claim 1, wherein: Establish the six-degree-of-freedom kinematic and dynamic model of the aircraft Where x, y, and z are the positions of the aircraft below the ground; V is the aircraft airspeed; α and β are the airflow angles, namely the angle of attack and the sideslip angle, respectively; γ, χ, and μ are the track inclination angle, track azimuth angle, and track roll angle, respectively; T is the engine thrust; L, D, and Y are the aerodynamic lift, drag, and side force, respectively. θ, ψ are the aircraft's roll angle, pitch angle, and yaw angle, respectively; p, q, and r are the roll rate, pitch rate, and yaw rate, respectively; I xx , I yy , I zz , I xz is the aircraft's moment of inertia; L M 、M M 、N M They are rolling moment, pitching moment and yaw aerodynamic moment respectively. Select the state quantity as X = [x, y, z, V, α, β, φ, θ, ψ, p, q, r] T , the input is U=[Tδ a δ e δ r ] T , where δ a , δ e , δ r is the deflection of the aircraft actuator (aileron, elevator, rudder), and equation (1) can be written as The aircraft control system model is established as Where V cmd 、φ cmd ,θ cmd , β cmd They are speed command, roll angle command, pitch angle command, and sideslip angle command respectively; K p1 , K p2 , K d2 , K p3 , K i3 , K d3 , K p4 , K i4 , K d4 are the autopilot PID control parameters.

3. The collision detection method according to claim 1, wherein: During the flight, every T pre Perform a trajectory prediction at every moment, T pre The prediction method is to first give the initial state value X0 of the navigation system at the current moment and the control instruction (V cmd ,φ cmd ,θ cmd ,β cmd ), define the predicted trajectory time length as L pre , use the numerical integration method to deduce the future L pre The trajectory in time. Discretize the continuous trajectory and record the trajectory at the nth moment as x n =x(nh),y n =y(nh),z n =z(nh), where h is the sampling and integration time. The predicted trajectory can be expressed as {(x0,y0,z0),(x1,y1,z1),…,(x n ,y n ,z n )},(n=L pre / h-1). The numerical integration method is selected as the fourth-order Runge-Kutta integration, and the system input {U0, U1,…, U n }, and use the following formula to realize trajectory recursion in, is the Runge-Kutta integral term, which is calculated as follows 4. The collision detection method according to claim 1, wherein: The aircraft collision boundary volume is constructed by predicting the trajectory and compared with the digital elevation model to determine whether a collision has occurred. The aircraft collision boundary volume is assumed to be a spherical bounding volume centered on the center of mass of the aircraft, with a bounding volume radius of R p , set the alarm condition: in the area existence|z i -h T |≤R p Among them, h T is the height of the obstacle terrain. If the warning condition is met, further collision detection is performed, and the conditions are If this condition is met, the aircraft is considered to be on a collision course with the obstacle.

5. The collision detection method according to claim 1, wherein: Since the forecast period T pre This directly impacts the prediction error ε. Specifically, the smaller the prediction period, the higher the prediction accuracy. The specific relationship between the prediction period and prediction error is typically related to the aircraft type, maneuver intensity, and prediction method. Furthermore, based on collision detection requirements, the closer the object is to the obstacle, the higher the prediction accuracy required. Therefore, the prediction period is dynamically adjusted based on the distance to the obstacle. A large prediction period is used before a collision is detected, and a small prediction period is used after a collision is detected, as shown in the following formula. Where, T pre0 To set the large-scale prediction period before collision; t col To predict the collision moment; T pre1 To set the small-scale prediction period after the collision.

6. The atmospheric data analysis method according to claim 1, characterized in that: Considering computational efficiency, the prediction length L is dynamically adjusted pre Affected by the integration method and computer performance, the calculation time T is usually equal to the prediction period T pre , prediction length L pre is positively correlated, that is T∝(T pre ×L pre )(7) Considering the tolerable calculation time, the prediction length adjustment scheme is designed as L pre =min{L col ,L0} (8) Where L0 is the maximum prediction length under the set tolerable calculation time; L col Displays the collision time to obstacles for the predicted trajectory.

Images