Perceptual data processing and tracking method suitable for low-altitude economic unmanned aerial vehicle

Abstract

The invention discloses a perception data processing and tracking method suitable for a low-altitude economic unmanned aerial vehicle, and relates to the technical field of unmanned aerial vehicles, and the method comprises the following steps: receiving measurement data from various sensors and local state data; establishing a track for each detection target of each sensor and linking tracks of the same target from different sensors; managing tracks in the track database and finding out an optimal sensor track; according to the method, data of ADS-B, ATAR, Camera and GBSS multi-source sensors are integrated, coordinate conversion and a track association algorithm are combined, accurate recognition and tracking of cooperative and non-cooperative intrusion targets are achieved, compared with a single sensor or a simple weighted fusion method, the method has the advantages that the method is simple, and the method is easy to implement. According to the method, the missing detection rate and the false detection rate are effectively reduced, and the accuracy and the environmental adaptability of sensing data in a complex low-altitude environment are remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles (UAVs), and in particular relates to a perception data processing and tracking method applicable to low-altitude economical UAVs. Background Art

[0002] With the continuous development of the low-altitude economy, the number of drones in the low-altitude airspace will gradually increase, and air traffic safety will also face huge challenges. Especially in the case of beyond-visual-range flight, the air environment is highly unknown and the real-time data transmission link is unstable. Drones need to complete flight missions and respond to emergencies autonomously. Even when flying within visual range, to prevent human misoperation and reduce operational errors, drones in the low-altitude economy need better autonomous flight capabilities. Therefore, autonomous flight is of great help to improve the safety and flight efficiency of drones in the low-altitude economy.

[0003] In order to realize autonomous flight of drones, the perception and avoidance function needs to be implemented on drones. It can be seen from the perception and avoidance function of drones currently on the market that the research on perception and avoidance systems in the field of drones has not yet been perfected. The perception and avoidance function that can ensure the safety of drones is usually used as an additional selling point rather than a necessary function. Moreover, its implementation conditions are very harsh and the functions that can be implemented are still very limited. The autonomy of drones still requires more intelligent software and hardware to achieve. Autonomous flight is the ability to fly with autonomous control. Autonomous flight relies on drones to perceive their flight environment through sensors and select appropriate flight strategies and flight routes based on the perception results. The accuracy, stability and timeliness of the perception results are particularly important for autonomous drones. Therefore, autonomous flight under multi-sensor conditions requires an intelligent and reasonable perception data processing and tracking method to meet the functional requirements of the perception and avoidance system, improve the accuracy, timeliness and stability of the perception results, and thus ensure the flight safety and efficiency of drones.

[0004] Currently, there are basically no drones on the market that can achieve fully autonomous flight. Many drones claiming to have perception and obstacle avoidance capabilities have very limited functions and are very demanding on the real environment. Many of them directly rely on the perception results of a single sensor, or simply take a weighted average of the perception results of two sensors. They do not conduct in-depth tracking and state prediction for each target, nor do they perform reasonable data fusion and optimal sensor selection. The stability and accuracy of the perception results are worrying, which affects the drone's ability to change its flight strategy and flight route in a timely manner. When the number of drones in the air increases, flight safety and efficiency cannot be guaranteed. Summary of the Invention

[0005] To solve the above technical problems, the present invention is achieved through the following technical solutions:

[0006] The present invention is a method for processing and tracking perception data of low-altitude economical UAVs, comprising the following steps:

[0007] Step S1: The sensor data processing subsystem receives measurement data from various sensors and local status data;

[0008] Step S2: Create a trajectory for each detected target of each sensor and link the trajectories of the same target from different sensors;

[0009] Step S3: manage the trajectories in the trajectory database and find the best sensor trajectory;

[0010] Step S4: Extrapolate the predicted position and speed to a common time point and send it to the automatic avoidance system.

[0011] Furthermore, the step S1 includes the following steps:

[0012] Step S11: The perception data processing subsystem receives measurement data from the four sensors, ADS-B, ATAR, Camera, and GBSS, and receives the local state data from the UAV based on the multi-source sensor data fusion framework. The ADS-B, Camera, and GBSS measurement data and the local state data are received through the UART serial port, and the ATAR measurement data is received through the CAN port.

[0013] The local status data refers to the status information from the UAV, including: local longitude, local latitude, local flight altitude, local pitch angle, local roll angle, local yaw angle, local flight speed and local flight direction;

[0014] Step S12, the ADS-B sensor and the GBSS sensor are responsible for monitoring cooperative intrusion targets in air traffic and can provide the following target information: ICAO_address, target longitude, target latitude, target altitude, target heading, data valid flag, target horizontal speed, target vertical speed, and altitude type;

[0015] In step S13, the ATAR sensor and the Camera sensor are responsible for monitoring cooperative and non-cooperative intrusion targets in air traffic and can provide the following target information:

[0016] Tracking number, range, range rate, azimuth, elevation, target detection time, range error standard deviation, range rate standard deviation, azimuth error standard deviation, and elevation error standard deviation.

[0017] Furthermore, step S2 includes the following steps:

[0018] Step S21, trajectory establishment and linking: Based on the multi-source sensor data fusion framework, a complete target trajectory should include the following target attributes: position, velocity, state error covariance, trajectory update time and trajectory tracking number;

[0019] ADS-B and GBSS provide complete status information about targets in the coordinate system, and through coordinate transformation, the perception data processing subsystem uses tracking information from them without tracking;

[0020] ATAR and Camera require a perception data processing subsystem to track each target;

[0021] When the perception data processing subsystem receives the ATAR or camera sensor measurement value, the system associates the measurement result with the existing track according to the track tracking number, and finds the track record with the same tracking number as the measurement value. If the association is successful, the track and measurement value are sent to the track establishment module or the extended Kalman filter module according to whether the track has been established. If the association fails, the measurement value will be sent to the track initialization module to start track recording;

[0022] Step S22, track initialization: Create a new track, update the track number to the tracking number provided by the sensor, set the update time to the sensor target detection time, update the track position, update the track velocity, initially set the target velocity vector to [0, 0, 0]′, and update the state error covariance of the track; the sensor provides the target relative distance, azimuth, and pitch angle in the aircraft coordinate system. The actual position of the target needs to be calculated in combination with the aircraft state data. The calculation method is as follows:

[0023] Convert sensor measurements to the Northeastern Sky coordinate system:

[0024]

[0025] Where e is the easting coordinate of the target in the northeast celestial coordinate system, n is the northing coordinate of the target in the northeast celestial coordinate system, u is the celestial coordinate of the target in the northeast celestial coordinate system, p is the distance measured by the sensor, β is the azimuth measured by the sensor, and ε is the pitch angle measured by the sensor. The measurement value is converted to the matrix M(α,γ,η) of the ENU coordinate system. The formula of M(α,γ,η) is as follows:

[0026]

[0027] Where m 00 =cosηcosγ+sinγsinαsinη;

[0028] m 01 =cosαsinη;

[0029] m 02 =sinγcosη-cosγsinαsinη;

[0030] m 10 =-cosγsinη+sinγsinαcosη;

[0031] m 11 =cosαcosη;

[0032] m 12 =-sinγsinη-cosγsinαcosη;

[0033] m 20 = -sinγcosα;

[0034] m 21 = sinα;

[0035] m 22 =cosγcosα;

[0036] Where α is the own-plane pitch angle, γ is the own-plane roll angle, and η is the own-plane yaw angle. The target's position vector in the ENU coordinate system is set to [e, n, u]′. The longitude, latitude, and altitude of the own-plane can be obtained from the own-plane state data, and the position information of the own-plane can be converted to the Earth-centered Earth-fixed coordinate system, as follows: Figure 3 As shown;

[0037] The position of this aircraft in the ECEF coordinate system is as follows:

[0038]

[0039] φ s is the local latitude, λ s is the longitude of the aircraft, h s is the height of the machine, α is the semi-major axis of the Earth = 6.378137×10 6 m, ε is the first eccentricity of the Earth = 0.081819190842622;

[0040] In the ECEF coordinate system, the position of the target relative to the own aircraft is calculated as follows:

[0041]

[0042] Where, X t-s , Y t-s , Z t-s is the coordinate of the target relative to the aircraft in the ECEF coordinate system, T(φ s ,λ s ) is the conversion matrix from ENU to ECEF;

[0043] Transformation matrix T(φ s ,λ s )for:

[0044]

[0045] The target location in ECEF coordinates is as follows:

[0046]

[0047] The target's ECEF coordinate position is converted to the latitude, longitude and altitude coordinate system as follows:

[0048]

[0049] E 2 =a 2 -b 2 ;

[0050] F=54b 2 Z t 2 ;

[0051]

[0052]

[0053] Where a is the semi-major axis of the Earth = 6.378137×10 6 m, b is the semi-minor axis of the Earth = 6.3567523142×10 6 m, φ t is the target latitude, λ t is the target longitude, h t is the target earth height, r ecef is the horizontal projection distance of the target in the ECEF coordinate system;

[0054] Step S23, trajectory establishment, is as follows: the update time is set to the sensor target detection time, the updated trajectory position, the updated trajectory speed, and the updated trajectory state error covariance; the sensor does not directly provide the target's actual speed in the ENU coordinate system, so it needs to be calculated based on the difference between the two position updates and the update time;

[0055] Assume that the coordinates of the trajectory in the ENU coordinate system at the time of the last position update are [e o ,n o ,u o ], the currently detected target position is converted to the ENU coordinate system coordinates [e, n, u], the current target detection time and the last target detection time difference is: Δt = current measurement time - last measurement time, the target velocity vector is updated as follows:

[0056]

[0057] Where, is the eastward speed of the target, is the northbound velocity of the target, is the upward speed of the target;

[0058] Step S24, the extended Kalman filter algorithm performs the following: trajectory state prediction, state error covariance prediction, predicted measurement value calculation, filter gain calculation, state update, state error covariance update, and time update;

[0059] Step S25: Linking between sensors:

[0060] The sensors are linked and the trajectory link parameters are calculated as follows:

[0061] Assume [e s , n s ,u s ]′ is the trajectory position detected by the sensor and converted to the ENU coordinate system, [e f , n f ,u f ]′ is the predicted position of the center trajectory in the ENU coordinate system; let [p s , β s , ε s ]′ and [p f , β f , ε f ]′ are the trajectory position detected by the sensor and the predicted position of the center trajectory in the local coordinate system:

[0062] Δx=e f -e s ;

[0063] Δy=n f -n s ;

[0064] Horizontal position difference;

[0065] Δz=|u f -u s |: vertical position difference;

[0066] Δp=|p f -p s |: distance difference;

[0067] Δβ=|β f -β s |: bearing difference;

[0068] Δε=|ε f-ε s |: elevation angle difference;

[0069] Step S26: The sensor linking process is as follows:

[0070] Input sensor track A i , loop through the available center trajectories F j Until A is found i and F j The best connection between them is as follows:

[0071] Calculate A i and F j The connection parameters Δs, Δz, Δp, Δβ, Δε between them. For the trajectories from ADS-B or GBSS, check whether the ICAO_address of the two trajectories are consistent. If they are consistent, the two trajectories are considered to represent the same target, save rank = 1, and save F saved =F j , save the link parameters; if they are inconsistent, check whether Δp < threshold 1 and Δβ < threshold 2 and Δz < threshold 3 and Δs < threshold 4 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory;

[0072] For target tracks from ATAR and Camera, check A i and F j Has it been linked before? If so, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 1 and save F saved =F j , save the link parameters. If not satisfied, check whether they are close in the horizontal or vertical direction, that is, satisfying Δβ<threshold 1 and Δβ<threshold 2 or satisfying Δp<threshold 1 and Δε<threshold 5. If satisfied, save rank=2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory, if A i and F j If there is no link before, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory;

[0073] If there is no available center trajectory, or no match is found after traversing all center trajectories, the loop is exited;

[0074] After executing the above logic, A i and F saved As input goes to the following process:

[0075] If F saved Valid, and A i If it has been linked before, check whether the center trajectory of the previous link is consistent with F saved Same, if the same, then F saved and A i The link between them is determined. If they are different, check F saved Is it linked to any other target tracks of the same sensor? If F saved is not linked to any other sensor trace of the same sensor, then F saved and A i The link between them is finally determined. If F saved Linked to another target track of the same sensor, then check A i Is it more suitable for F saved If yes, then F saved and A i The link between them is determined; for the previous link to F saved The target trajectory starts a new center trajectory;

[0076] If F saved Invalid, and A i If it has been linked before, then A i The link between A and its center track will be broken; if A i If it is not previously linked to any center track, create a new center track F k , and link A i and F k .

[0077] Furthermore, in step S3, the trajectory management includes the following steps:

[0078] Step S31: If any two existing center trajectories have a matching ICAO address, or the center trajectories are close to each other, then the center trajectory with the larger position uncertainty is deleted;

[0079] Step S32: deleting sensor tracks that have not been updated for more than a specified time;

[0080] Step S33: Remove the center track that has no sensor track link for more than a specified time.

[0081] Furthermore, in step S34, the optimal sensor selection step is as follows:

[0082] After linking, the center track selects the best sensor in the horizontal and vertical directions based on the horizontal position uncertainty propagated by the sensor track at the time of linking and the vertical position and velocity estimated based on the vertical rate uncertainty, and selects the most accurate position and velocity data in the horizontal and vertical directions;

[0083] Algorithm flow for updating center trajectory data with optimal sensor trajectory data.

[0084] Furthermore, in step S4:

[0085] The status of the center track is sent to the active avoidance system at a fixed frequency. The status data of the center track is not always updated at the time when the data is sent. There is a time difference:

[0086] Δt = current sending time - center trajectory update time;

[0087] Assume that the speed of the last updated target center trajectory is The position moved within Δt time is as follows:

[0088]

[0089] Through coordinate conversion, the target position under the coordinate of the current data sending time ECEF can be obtained as [X x , Y x , Z x ], convert to the longitude and latitude coordinate system to obtain the longitude, latitude and altitude of the target at the current time;

[0090] After the center trajectory status is updated to the current time point, it can be sent to the active avoidance system.

[0091] The present invention has the following beneficial effects:

[0092] 1. The present invention integrates data from multiple sensors, including ADS-B, air-to-air radar, cameras, and ground-based surveillance systems (GBSS), and combines coordinate transformation with trajectory association algorithms to achieve accurate identification and tracking of cooperative and non-cooperative intruder targets. Compared with single-sensor or simple weighted fusion methods, this method effectively reduces missed detection and false detection rates, and significantly improves the accuracy and environmental adaptability of perception data in complex low-altitude environments.

[0093] 2. Based on extended Kalman filtering, trajectory initialization, and inter-sensor linking technologies, the present invention dynamically establishes, updates, and manages a target trajectory database, screens the optimal sensor data source in real time, automatically eliminates redundant or low-confidence trajectories through horizontal and vertical uncertainty assessment, optimizes system resource allocation, ensures the real-time and stability of target status updates, and thus enhances the UAV's ability to quickly respond to dynamic environments.

[0094] 3. The present invention adopts a linear extrapolation algorithm to predict the position and speed of the center trajectory to a unified time point. Combined with the efficient conversion of ECEF and longitude and latitude coordinate systems, it solves the problem of asynchrony between sensor data updates and system transmission time. This technology can effectively compensate for data transmission delays and provide the drone obstacle avoidance system (active avoidance system) with high-efficiency and high-precision target status information, significantly improving the reliability and safety of autonomous flight path planning.

[0095] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0097] Figure 1 This is a flow chart of a method for processing and tracking perception data applicable to low-altitude economical UAVs according to the present invention;

[0098] Figure 2 This is a sensor trajectory tracking architecture diagram of a perception data processing and tracking method applicable to low-altitude economical UAVs of the present invention;

[0099] Figure 3 The present invention is a method for processing and tracking perception data of a low-altitude economic UAV, which converts the position information of the UAV into an Earth-centered Earth-fixed coordinate system;

[0100] Figure 4 This is a flow chart of sensor-to-sensor links for a method of processing and tracking perception data applicable to low-altitude economical UAVs of the present invention;

[0101] Figure 5 This is a flow chart of the algorithm for updating the center trajectory data using the optimal sensor trajectory data of the present invention. DETAILED DESCRIPTION

[0102] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0103] See also Figure 1-Figure 5As shown, the present invention is a perception data processing and tracking method applicable to low-altitude economical UAVs, comprising the following steps:

[0104] Step S1: The sensor data processing subsystem receives measurement data from various sensors and local status data;

[0105] Step S2: Create a trajectory for each detected target of each sensor and link the trajectories of the same target from different sensors;

[0106] Step S3: manage the trajectories in the trajectory database and find the best sensor trajectory;

[0107] Step S4: Extrapolate the predicted position and speed to a common time point and send it to the automatic avoidance system.

[0108] Step S1 includes the following steps:

[0109] Step S11: The perception data processing subsystem receives measurement data from the four sensors, ADS-B, ATAR, Camera, and GBSS, and receives the local state data from the UAV based on the multi-source sensor data fusion framework. The ADS-B, Camera, and GBSS measurement data and the local state data are received through the UART serial port, and the ATAR measurement data is received through the CAN port.

[0110] The local status data refers to the status information from the UAV, including: local longitude, local latitude, local flight altitude, local pitch angle, local roll angle, local yaw angle, local flight speed and local flight direction;

[0111] Step S12, the ADS-B sensor and the GBSS sensor are responsible for monitoring cooperative intrusion targets in air traffic and can provide the following target information: ICAO_address, target longitude, target latitude, target altitude, target heading, data valid flag, target horizontal speed, target vertical speed, and altitude type;

[0112] In step S13, the ATAR sensor and the Camera sensor are responsible for monitoring cooperative and non-cooperative intrusion targets in air traffic and can provide the following target information:

[0113] Tracking number, range, range rate, azimuth, elevation, target detection time, range error standard deviation, range rate standard deviation, azimuth error standard deviation, and elevation error standard deviation.

[0114] Step S2 includes the following steps:

[0115] Step S21, trajectory establishment and linking: Based on the multi-source sensor data fusion framework, a complete target trajectory should include the following target attributes: position, velocity, state error covariance, trajectory update time and trajectory tracking number;

[0116] ADS-B and GBSS provide complete status information about targets in the coordinate system, and through coordinate transformation, the perception data processing subsystem uses tracking information from them without tracking;

[0117] ATAR and Camera require a perception data processing subsystem to track each target;

[0118] When the perception data processing subsystem receives the ATAR or camera sensor measurement value, the system associates the measurement result with the existing track according to the track tracking number, and finds the track record with the same tracking number as the measurement value. If the association is successful, the track and measurement value are sent to the track establishment module or the extended Kalman filter module according to whether the track has been established. If the association fails, the measurement value will be sent to the track initialization module to start track recording;

[0119] Step S22, track initialization: Create a new track, update the track number to the tracking number provided by the sensor, set the update time to the sensor target detection time, update the track position, update the track velocity, initially set the target velocity vector to [0, 0, 0]′, and update the state error covariance of the track; the sensor provides the target relative distance, azimuth, and pitch angle in the aircraft coordinate system. The actual position of the target needs to be calculated in combination with the aircraft state data. The calculation method is as follows:

[0120] Convert sensor measurements to the Northeastern Sky coordinate system:

[0121]

[0122] Where e is the easting coordinate of the target in the northeast celestial coordinate system, n is the northing coordinate of the target in the northeast celestial coordinate system, u is the celestial coordinate of the target in the northeast celestial coordinate system, p is the distance measured by the sensor, β is the azimuth measured by the sensor, and ε is the pitch angle measured by the sensor. The measurement value is converted to the matrix M(α,γ,η) of the ENU coordinate system. The formula of M(α,γ,η) is as follows:

[0123]

[0124] Where m 00 =cosηcosγ+sinγsinαsinη;

[0125] m 01 =cosαsinη;

[0126] m 02 =sinγcosη-cosγsinαsinη;

[0127] m 10 =-cosγsinη+sinγsinαcosη;

[0128] m 11 =cosαcosη;

[0129] m 12 =-sinγsinη-cosγsinαcosη;

[0130] m 20 = -sinγcosα;

[0131] m 21 = sinα;

[0132] m 22 =cosγcosα;

[0133] Where α is the own-plane pitch angle, γ is the own-plane roll angle, and η is the own-plane yaw angle. The target's position vector in the ENU coordinate system is set to [e, n, u]′. The longitude, latitude, and altitude of the own-plane can be obtained from the own-plane state data, and the position information of the own-plane can be converted to the Earth-centered Earth-fixed coordinate system, as follows: Figure 3 As shown;

[0134] The position of this aircraft in the ECEF coordinate system is as follows:

[0135]

[0136] φ s is the local latitude, λ s is the longitude of the aircraft, h s is the height of the machine, α is the semi-major axis of the Earth = 6.378137×10 6 m, ε is the first eccentricity of the Earth = 0.081819190842622;

[0137] In the ECEF coordinate system, the position of the target relative to the own aircraft is calculated as follows:

[0138]

[0139] Where, X t-s , Y t-s , Z t-s is the coordinate of the target relative to the aircraft in the ECEF coordinate system, T(φ s ,λ s ) is the conversion matrix from ENU to ECEF;

[0140] Transformation matrix T(φ s ,λ s )for:

[0141]

[0142] The target location in ECEF coordinates is as follows:

[0143]

[0144] The target's ECEF coordinate position is converted to the latitude, longitude and altitude coordinate system as follows:

[0145] E 2 =a 2 -b 2 ;

[0146] F=54b 2 Z t 2 ;

[0147]

[0148]

[0149] Where a is the semi-major axis of the Earth = 6.378137×10 6 m, b is the semi-minor axis of the Earth = 6.3567523142×10 6 m, φ t is the target latitude, λ t is the target longitude, h t is the target geodetic height, recef is the horizontal projection distance of the target in the ECEF coordinate system;

[0150] Step S23, trajectory establishment, is as follows: the update time is set to the sensor target detection time, the updated trajectory position, the updated trajectory speed, and the updated trajectory state error covariance; the sensor does not directly provide the target's actual speed in the ENU coordinate system, so it needs to be calculated based on the difference between the two position updates and the update time;

[0151] Assume that the coordinates of the trajectory in the ENU coordinate system at the time of the last position update are [e o ,n o ,u o ], the current detected target position is converted to the ENU coordinate system coordinates [e, n, u[, the current target detection time and the last target detection time difference is: Δt = current measurement time - last measurement time, the target velocity vector is updated as follows:

[0152]

[0153] Where, is the eastward speed of the target, is the northbound velocity of the target, is the upward speed of the target;

[0154] Step S24, the extended Kalman filter algorithm performs the following: trajectory state prediction, state error covariance prediction, predicted measurement value calculation, filter gain calculation, state update, state error covariance update, and time update;

[0155] Step S25: Linking between sensors:

[0156] The sensors are linked and the trajectory link parameters are calculated as follows:

[0157] Assume [e s , n s ,u s ]′ is the trajectory position detected by the sensor and converted to the ENU coordinate system, [e f , n f ,u f ]′ is the predicted position of the center trajectory in the ENU coordinate system; let [ps, βs, εs]′ and [p f , β f , ε f ]′ are the trajectory position detected by the sensor and the predicted position of the center trajectory in the local coordinate system:

[0158] Δx=e f -e s ;

[0159] Δy=n f -n s ;

[0160] Horizontal position difference;

[0161] Δz=|u f -u s |: vertical position difference;

[0162] Δp=|p f -p s |: distance difference;

[0163] Δβ=|β f -β s |: bearing difference;

[0164] Δε=|ε f -ε s |: elevation angle difference;

[0165] Step S26: The sensor linking process is as follows:

[0166] Input sensor track A i , loop through the available center trajectories F j Until A is found i and F j The best connection between them is as follows:

[0167] Calculate A i and F j The connection parameters Δs, Δz, Δp, Δβ, Δε between them. For the trajectories from ADS-B or GBSS, check whether the ICAO_address of the two trajectories are consistent. If they are consistent, the two trajectories are considered to represent the same target, save rank = 1, and save F saved =F j , save the link parameters; if they are inconsistent, check whether Δp < threshold 1 and Δβ < threshold 2 and Δz < threshold 3 and Δs < threshold 4 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory;

[0168] For target tracks from ATAR and Camera, check A i and F j Has it been linked before? If so, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 1 and save F saved =F j , save the link parameters. If not satisfied, check whether they are close in the horizontal or vertical direction, that is, satisfying Δβ<threshold 1 and Δβ<threshold 2 or satisfying Δp<threshold 1 and Δε<threshold 5. If satisfied, save rank=2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory, if A i and F j If there is no link before, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory;

[0169] If there is no available center trajectory, or no match is found after traversing all center trajectories, the loop is exited;

[0170] After executing the above logic, A i and F savedAs input goes to the following process:

[0171] If F saved Valid, and A i If it has been linked before, check whether the center trajectory of the previous link is consistent with F saved Same, if the same, then F saved and A i The link between them is determined. If they are different, check F saved Is it linked to any other target tracks of the same sensor? If F saved is not linked to any other sensor trace of the same sensor, then F saved and A i The link between them is finally determined. If F saved Linked to another target track of the same sensor, then check A i Is it more suitable for F saved If yes, then F saved and A i The link between them is determined; for the previous link to F saved The target trajectory starts a new center trajectory;

[0172] If F saved Invalid, and A i If it has been linked before, then A i The link between A and its center track will be broken; if A i If it is not previously linked to any center track, create a new center track F k , and link A i and F k .

[0173] In step S3, trajectory management includes the following steps:

[0174] Step S31: If any two existing center trajectories have a matching ICAO address, or the center trajectories are close to each other, then the center trajectory with the larger position uncertainty is deleted;

[0175] Step S32: deleting sensor tracks that have not been updated for more than a specified time;

[0176] Step S33: Remove the center track that has no sensor track link for more than a specified time.

[0177] In step S34, the optimal sensor selection steps are as follows:

[0178] After linking, the center track selects the best sensor in the horizontal and vertical directions based on the horizontal position uncertainty propagated by the sensor track at the time of linking and the vertical position and velocity estimated based on the vertical rate uncertainty, and selects the most accurate position and velocity data in the horizontal and vertical directions;

[0179] Algorithm flow for updating center trajectory data with optimal sensor trajectory data.

[0180] In step S4:

[0181] The status of the center track is sent to the active avoidance system at a fixed frequency. The status data of the center track is not always updated at the time when the data is sent. There is a time difference:

[0182] Δt = current sending time - center trajectory update time;

[0183] Assume that the speed of the last updated target center trajectory is The position moved within Δt time is as follows:

[0184]

[0185] Through coordinate conversion, the target position under the coordinate of the current data sending time ECEF can be obtained as [X x , Y x , Z x ], convert to the longitude and latitude coordinate system to obtain the longitude, latitude and altitude of the target at the current time;

[0186] After the center trajectory status is updated to the current time point, it can be sent to the active avoidance system.

[0187] A specific application of this embodiment is:

[0188] 1. System composition:

[0189] Hardware configuration:

[0190] UAV platform: equipped with a quad-rotor drone, integrated with a main control unit (MCU), ADS-B receiver module (UART interface), air-to-air radar (CAN interface), high-definition camera (UART interface), and ground surveillance system (GBSS) receiver module (UART interface);

[0191] Data processing unit: Embedded processor (such as NVIDIA Jetson AGX Xavier), running trajectory management algorithm and extended Kalman filter module;

[0192] Communication module: 4G / 5G link, used to transmit prediction data to the ground active avoidance system (autonomous obstacle avoidance control) system in real time;

[0193] Software Architecture:

[0194] Data receiving layer: multi-threaded processing of asynchronous data streams from different sensors to achieve timestamp alignment and data parsing;

[0195] Trajectory processing layer: includes trajectory initialization, extended Kalman filter, and sensor link logic;

[0196] Trajectory management layer: dynamically maintains the central trajectory database, performs trajectory fusion and redundant elimination;

[0197] Prediction and extrapolation layer: Generates target state data at a unified time point based on a linear extrapolation algorithm;

[0198] 2. Detailed implementation steps

[0199] Step 1: Multi-source sensor data reception and preprocessing

[0200] Data interface configuration:

[0201] ADS-B and GBSS data: received via the UART serial port and parsed into ICAO address, latitude and longitude, altitude, speed and other fields;

[0202] Radar data: received via the CAN bus and parsed into distance, azimuth, elevation angle, and error standard deviation;

[0203] Camera data: Detects targets in real time using image processing algorithms (such as YOLOv5), outputting target pixel coordinates and distance estimates.

[0204] Time synchronization:

[0205] Add local high-precision timestamps (μs level) to each sensor data and use interpolation algorithms to align asynchronous data streams;

[0206] Coordinate transformation:

[0207] Radar and camera data: Convert the measured values ​​(range p, azimuth β, pitch angle ε) in the aircraft's coordinate system to the ENU (East-North-Sky) coordinate system;

[0208]

[0209] Among them, the M matrix is ​​calculated and generated by the aircraft's attitude angle (pitch angle α, yaw angle γ, yaw angle η);

[0210] ENU to ECEF conversion:

[0211] Combined with the local latitude and longitude data, through the transformation matrix T(φ s ,λ s) converts the target position to the Earth-centered Earth-fixed coordinate system and finally converts it to the target latitude and longitude coordinates (φ t ,λ t ,h t ).

[0212] Step 2: Trajectory Association and Extended Kalman Filter

[0213] 1. Trajectory initialization

[0214] For the target detected by the radar or camera for the first time, a new trajectory is created and the velocity vector is initialized to [0 0 0] T , the covariance matrix is ​​set to the square of the sensor measurement error;

[0215] 2. Trajectory Association

[0216] Cooperative Target (ADS-B / GBSS): Directly matches ICAO addresses. If they match, they are associated as the same target.

[0217] Non-cooperative targets (radar / camera): Calculate parameters such as horizontal position difference Δs, vertical difference Δz, and azimuth difference Δβ. If the thresholds are met (e.g., Δs < 20m, Δβ < 5°), the targets are considered to be the same.

[0218] 3. Status Update

[0219] The extended Kalman filter (EKF) is used to predict the target state at the next moment:

[0220]

[0221] Where f is the nonlinear state transfer function, F k is the Jacobian matrix, Q k is the process noise covariance.

[0222] Step 3: Trajectory Fusion and Optimal Sensor Selection

[0223] 1. Redundant track removal

[0224] If the I CAO addresses of the two center trajectories are the same or the horizontal distance is less than 10 m, the trajectory with the smaller covariance is retained;

[0225] 2. Best sensor selection:

[0226] Horizontal direction: select the sensor data with the smallest horizontal position covariance (e.g. ADS-B has better accuracy than radar);

[0227] Vertical direction: Select the sensor data with the smallest vertical velocity covariance (e.g. GBSS altitude data is more stable).

[0228] Throughout this specification, references to terms such as "one embodiment," "example," or "specific example" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, schematic representations of these terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0229] The preferred embodiments of the present invention disclosed above are intended only to help illustrate the present invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the present invention to the specific embodiments described. Obviously, many modifications and variations are possible based on the content of this specification. These embodiments are selected and described in detail in this specification to better explain the principles and practical applications of the present invention, thereby enabling those skilled in the art to better understand and utilize the present invention. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A perception data processing and tracking method for low-altitude economical UAVs, characterized by: The following steps are involved: Step S1: The sensor data processing subsystem receives measurement data from various sensors and local status data; Step S2: Create a trajectory for each detected target of each sensor and link the trajectories of the same target from different sensors; Step S3: manage the trajectories in the trajectory database and find the best sensor trajectory; Step S4: Extrapolate the predicted position and speed to a common time point and send it to the automatic avoidance system.

2. The method for processing and tracking perception data suitable for low-altitude economical UAVs according to claim 1, characterized in that: The step S1 includes the following steps: Step S11: The perception data processing subsystem receives measurement data from the four sensors, ADS-B, ATAR, Camera, and GBSS, and receives the local state data from the UAV based on the multi-source sensor data fusion framework. The ADS-B, Camera, and GBSS measurement data and the local state data are received through the UART serial port, and the ATAR measurement data is received through the CAN port. The local status data refers to the status information from the UAV, including: local longitude, local latitude, local flight altitude, local pitch angle, local roll angle, local yaw angle, local flight speed and local flight direction; Step S12, the ADS-B sensor and the GBSS sensor are responsible for monitoring cooperative intrusion targets in air traffic and can provide the following target information: ICAO_address, target longitude, target latitude, target altitude, target heading, data valid flag, target horizontal speed, target vertical speed, and altitude type; In step S13, the ATAR sensor and the Camera sensor are responsible for monitoring cooperative and non-cooperative intrusion targets in air traffic and can provide the following target information: Tracking number, range, range rate, azimuth, elevation, target detection time, range error standard deviation, range rate standard deviation, azimuth error standard deviation, and elevation error standard deviation.

3. The method for processing and tracking perception data suitable for low-altitude economical UAVs according to claim 1, characterized in that: The step S2 includes the following steps: Step S21, trajectory establishment and linking: Based on the multi-source sensor data fusion framework, a complete target trajectory should include the following target attributes: position, velocity, state error covariance, trajectory update time and trajectory tracking number; ADS-B and GBSS provide complete status information about targets in the coordinate system, and through coordinate transformation, the perception data processing subsystem uses tracking information from them without tracking; ATAR and Camera require a perception data processing subsystem to track each target; When the perception data processing subsystem receives the ATAR or camera sensor measurement value, the system associates the measurement result with the existing track according to the track tracking number, and finds the track record with the same tracking number as the measurement value. If the association is successful, the track and measurement value are sent to the track establishment module or the extended Kalman filter module according to whether the track has been established. If the association fails, the measurement value will be sent to the track initialization module to start track recording; Step S22, track initialization: Create a new track, update the track number to the tracking number provided by the sensor, set the update time to the sensor target detection time, update the track position, update the track velocity, initially set the target velocity vector to [0, 0, 0]′, and update the state error covariance of the track; the sensor provides the target relative distance, azimuth, and pitch angle in the aircraft coordinate system. The actual position of the target needs to be calculated in combination with the aircraft state data. The calculation method is as follows: Convert sensor measurements to the Northeastern Sky coordinate system: Where e is the easting coordinate of the target in the northeast celestial coordinate system, n is the northing coordinate of the target in the northeast celestial coordinate system, u is the celestial coordinate of the target in the northeast celestial coordinate system, p is the distance measured by the sensor, β is the azimuth measured by the sensor, and ε is the pitch angle measured by the sensor. The measurement value is converted to the matrix M(α,γ,η) of the ENU coordinate system. The formula of M(α,γ,η) is as follows: Where m 00 =cosηcosγ+sinγsinαsinη; m 01 =cosαsinη; m 02 =sinγcosη-cosγsinαsinη; m 10 =-cosγsinη+sinγsinαcosη; m 11 =cosαcosη; m 12 =-sinγsinη-cosγsinαcosη; m 20 =-sinγcosα; m 21 =sinα; m 22 =cosγcosα; Where α is the own-aircraft pitch angle, γ is the own-aircraft roll angle, and η is the own-aircraft yaw angle. The target's position vector in the ENU coordinate system is set to [e, n, u]′. The own-aircraft's longitude, latitude, and altitude can be obtained from the own-aircraft status data, and the own-aircraft's position information is converted to the Earth-centered Earth-fixed coordinate system. The position of this aircraft in the ECEF coordinate system is as follows: φ s is the local latitude, λ s is the longitude of the aircraft, h s is the height of the machine, α is the semi-major axis of the Earth = 6.378137×10 6 m, ε is the first eccentricity of the Earth = 0.081819190842622; In the ECEF coordinate system, the position of the target relative to the own aircraft is calculated as follows: Where, X t-s , Y t-s , Z t-s is the coordinate of the target relative to the aircraft in the ECEF coordinate system, T(φ s ,λ s ) is the conversion matrix from ENU to ECEF; Transformation matrix T(φ s ,λ s )for: The target location in ECEF coordinates is as follows: The target's ECEF coordinate position is converted to the latitude, longitude and altitude coordinate system as follows: E 2 =a 2 -b 2 ; F=54b 2 Z t 2 ; Where a is the semi-major axis of the Earth = 6.378137×10 6 m, b is the semi-minor axis of the Earth = 6.3567523142×10 6 m, φ t is the target latitude, λ t is the target longitude, h t is the target earth height, r ecef is the horizontal projection distance of the target in the ECEF coordinate system; Step S23, trajectory establishment, is as follows: the update time is set to the sensor target detection time, the updated trajectory position, the updated trajectory speed, and the updated trajectory state error covariance; the sensor does not directly provide the target's actual speed in the ENU coordinate system, so it needs to be calculated based on the difference between the two position updates and the update time; Assume that the coordinates of the trajectory in the ENU coordinate system at the time of the last position update are [e o ,n o ,u o ], the currently detected target position is converted to the ENU coordinate system coordinates [e, n, u], the current target detection time and the last target detection time difference is: Δt = current measurement time - last measurement time, the target velocity vector is updated as follows: Where, is the eastward speed of the target, is the northbound velocity of the target, is the upward speed of the target; Step S24, the extended Kalman filter algorithm performs the following: trajectory state prediction, state error covariance prediction, predicted measurement value calculation, filter gain calculation, state update, state error covariance update, and time update; Step S25: Linking between sensors: The sensors are linked and the trajectory link parameters are calculated as follows: Assume [e s , n s ,u s ]′ is the trajectory position detected by the sensor and converted to the ENU coordinate system, [e f , n f ,u f ]′ is the predicted position of the center trajectory in the ENU coordinate system; let [p s , β s , ε s ]′ and [p f , β f , ε f ]′ are the trajectory position detected by the sensor and the predicted position of the center trajectory in the local coordinate system: Δx=e f -e s ; Δy=n f -n s ; Horizontal position difference; Δz=|u f -u s |: vertical position difference; Δp=|p f -p s |: distance difference; Δβ = |β f - β s | : azimuth difference; Δε=|ε f -ε s |:Elevation angle difference; Step S26: The sensor linking process is as follows: Input sensor track A i , loop through the available center trajectories F j Until A is found i and F j The best connection between them is as follows: Calculate A i and F j The connection parameters Δs, Δz, Δp, Δβ, Δε between them. For the trajectories from ADS-B or GBSS, check whether the ICAO_address of the two trajectories are consistent. If they are consistent, the two trajectories are considered to represent the same target, save rank = 1, and save F saved =F j , save the link parameters; if they are inconsistent, check whether Δp < threshold 1 and Δβ < threshold 2 and Δz < threshold 3 and Δs < threshold 4 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory; For target tracks from ATAR and Camera, check A i and F j Has it been linked before? If so, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 1 and save F saved =F j , save the link parameters. If not satisfied, check whether they are close in the horizontal or vertical direction, that is, satisfying Δβ<threshold 1 and Δβ<threshold 2 or satisfying Δp<threshold 1 and Δε<threshold 5. If satisfied, save rank=2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory, if A i and F j If there is no link before, check whether Δp < threshold 1 and Δβ < threshold 2 and Δε < threshold 5 are satisfied. If so, save rank = 2 and save F saved =F j , save the link parameters; if not satisfied, continue to find the next center trajectory; If there is no available center trajectory, or no match is found after traversing all center trajectories, the loop is exited; After executing the above logic, A i and F saved As input goes to the following process: If F saved Valid, and A i If it has been linked before, check whether the center trajectory of the previous link is consistent with F saved Same, if the same, then F saved and A i The link between them is determined. If they are different, check F saved Is it linked to any other target tracks of the same sensor? If F saved is not linked to any other sensor trace of the same sensor, then F saved and A i The link between them is finally determined. If F saved Linked to another target track of the same sensor, then check A i Is it more suitable for F saved If yes, then F saved and A i The link between them is determined; for the previous link to F saved The target trajectory starts a new center trajectory; If F saved Invalid, and A i If it has been linked before, then A i The link between A and its center track will be broken; if A i If it is not previously linked to any center track, create a new center track F k , and link A i and F k .

4. The method for processing and tracking perception data suitable for low-altitude economical UAVs according to claim 1, characterized in that: The step S3 includes the following steps: Step S31: track management: Step S31: If any two existing center trajectories have a matching ICAO address, or the center trajectories are close to each other, then the center trajectory with the larger position uncertainty is deleted; Step S32: deleting sensor tracks that have not been updated for more than a specified time; Step S33: Remove the center track that has no sensor track link for more than a specified time.

5. The method for processing and tracking perception data suitable for low-altitude economical UAVs according to claim 4, characterized in that: The step S3 includes the following steps: Step S34: Optimal sensor selection: After linking, the center track selects the best sensor in the horizontal and vertical directions based on the horizontal position uncertainty propagated by the sensor track at the time of linking and the vertical position and velocity estimated based on the vertical rate uncertainty, and selects the most accurate position and velocity data in the horizontal and vertical directions; Algorithm flow for updating center trajectory data with optimal sensor trajectory data.

6. The method for processing and tracking perception data suitable for low-altitude economical UAVs according to claim 1, characterized in that: In the step S4: The status of the center track is sent to the active avoidance system at a fixed frequency. The status data of the center track is not always updated at the time when the data is sent. There is a time difference: Δt = current sending time - center trajectory update time; Assume that the speed of the last updated target center trajectory is The position moved within Δt time is as follows: Through coordinate conversion, the target position under the coordinate of the current data sending time ECEF can be obtained as [X x , Y x , Z x ], convert to the longitude and latitude coordinate system to obtain the longitude, latitude and altitude of the target at the current time; After the center trajectory status is updated to the current time point, it can be sent to the active avoidance system.

Images