A high-speed unmanned aerial vehicle tracking method and system based on Kalman filtering

Abstract

This invention provides a high-speed UAV tracking method and system based on Kalman filtering, belonging to the field of target tracking technology. The method includes: acquiring asynchronous data streams of the high-speed UAV to be tracked; performing spatiotemporal registration and target association processing on the asynchronous data streams to obtain the historical position sequence of the high-speed UAV to be tracked; constructing a Kalman filter based on a current statistical model; inputting the historical position sequence into the Kalman filter to output the predicted future position; comparing the predicted future position with the current position of the high-speed UAV to be tracked to obtain the position error; adjusting the position error using a proportional-integral-derivative (PI-DE) control algorithm to obtain the turntable control command for the high-speed UAV to be tracked; and driving the high-speed UAV to be tracked to travel according to the predicted future position according to the turntable control command, thereby achieving tracking of the high-speed UAV to be tracked. This invention can reduce tracking loss and improve the stability and real-time performance of tracking.

Description

Technical Field

[0001] This invention relates to the field of target tracking technology, and in particular to a high-speed UAV tracking method and system based on Kalman filtering. Background Technology

[0002] In sensitive areas such as airports, nuclear power plants, military bases, and large event venues, how to effectively detect, identify, and stably track unauthorized drones, and then take timely interference measures to curb the frequent occurrence of drone black flights and privacy violations, has become a core technical challenge that urgently needs to be solved in the field of low-altitude security.

[0003] Existing drone tracking solutions primarily capture drone images, extract brightness information, optimize image sequences, estimate target movement direction and flight speed to generate motion trajectories, and combine flight anomaly detection and shape adjustment strategies to reverse-optimize image capture parameters and algorithm sensitivity. This addresses the challenges of sudden target maneuvers and, to some extent, solves the problems of difficult drone identification under complex lighting conditions and easy tracking loss of high-speed targets. It also possesses a certain degree of versatility and tracking stability.

[0004] However, existing technologies rely on a single vision sensor, have weak anti-interference capabilities, cannot acquire true three-dimensional information of the target, resulting in poor tracking accuracy, and lack the ability to predict the future motion state of the target. They also exhibit significant lag in tracking high-speed UAVs, making it difficult to adapt to their sudden maneuvering characteristics and prone to tracking loss. Consequently, they cannot meet the high-precision tracking requirements of low-altitude security in sensitive areas. Summary of the Invention

[0005] In view of the shortcomings of the prior art, the purpose of this invention is to provide a high-speed UAV tracking method based on Kalman filtering, which can solve the technical problems of existing technologies that rely on a single vision sensor, have weak anti-interference ability, cannot obtain the true three-dimensional information of the target, resulting in poor tracking accuracy; and lack the ability to predict the future motion state of the target, resulting in significant lag in tracking high-speed UAVs, making it difficult to adapt to their sudden maneuvering characteristics, and easily leading to tracking loss, thus failing to meet the high-precision tracking requirements of low-altitude security in sensitive areas.

[0006] A first aspect of this invention proposes a high-speed UAV tracking method based on Kalman filtering, comprising:

[0007] S1: Collect asynchronous data streams from the high-speed drone to be tracked;

[0008] S2: Perform spatiotemporal registration and target association processing on the asynchronous data stream to obtain the historical position sequence of the high-speed UAV to be tracked;

[0009] S3: Construct a Kalman filter based on the current statistical model;

[0010] S4: Input the historical location sequence into the Kalman filter and output the future predicted location;

[0011] S5: Compare the predicted future position with the current position of the high-speed drone to be tracked to obtain the position error;

[0012] S6: The position error is adjusted by the proportional-integral-derivative control algorithm to obtain the turntable control command for the high-speed UAV to be tracked;

[0013] S7: Drive the high-speed UAV to be tracked to travel to the predicted future position according to the turntable control command, so as to achieve tracking of the high-speed UAV to be tracked.

[0014] A second aspect of this invention provides a high-speed unmanned aerial vehicle (UAV) tracking system based on Kalman filtering, comprising: a processor and a memory;

[0015] The memory stores programs or instructions that can run on the processor, which, when executed by the processor, implement the steps of the high-speed UAV tracking method based on Kalman filtering as described in the first aspect.

[0016] A third aspect of the present invention provides a readable storage medium on which a program or instructions are stored, which, when executed by a processor, implement the steps of the high-speed UAV tracking method based on Kalman filtering as described in the first aspect.

[0017] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following:

[0018] In this embodiment of the invention, the position error is adjusted by a proportional-integral-derivative (PID) control algorithm, which can accurately obtain the true three-dimensional information of the target and help improve tracking accuracy. A Kalman filter based on the current statistical model is constructed, which can adapt to the sudden maneuvering characteristics of high-speed UAVs. The motion state of high-speed UAVs is predicted in real time by this Kalman filter. Then, the position error is calculated, the error is adjusted by the PID control algorithm, and turntable control commands are generated. This can reduce the occurrence of tracking loss, improve the stability and real-time performance of tracking, and ultimately help to better meet the high-precision tracking needs of low-altitude security in sensitive areas. Attached Figure Description

[0019] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts. Obviously, the drawings described below are merely some embodiments of the present invention, and those skilled in the art can obtain other drawings based on these drawings without any creative effort.

[0020] Figure 1 This is a flowchart illustrating a high-speed UAV tracking method based on Kalman filtering provided in an embodiment of the present invention.

[0021] Figure 2 This is a schematic diagram of a high-speed UAV tracking system based on Kalman filtering provided in an embodiment of the present invention. Detailed Implementation

[0022] To enable those skilled in the art to better understand the technical solutions in the embodiments of the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. It should be understood that these descriptions are merely exemplary and are not intended to limit the scope of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0023] The high-speed UAV tracking method based on Kalman filtering provided by the present invention will be described in detail below with reference to the accompanying drawings, through specific embodiments and application scenarios.

[0024] Reference manual attached Figure 1 The diagram illustrates a flowchart of a high-speed UAV tracking method based on Kalman filtering provided in an embodiment of the present invention.

[0025] This invention provides a high-speed UAV tracking method based on Kalman filtering, which may include the following steps:

[0026] S1: Collect asynchronous data streams from the high-speed drone to be tracked.

[0027] It should be noted that, based on a unified high-precision clock source, the visible light camera, infrared sensor, and laser rangefinder are synchronously driven to collaboratively collect data from the target UAV. Specifically, the visible light camera identifies and detects the UAV and outputs its coordinate information in a two-dimensional image; the infrared sensor outputs infrared image information to assist in identification; the laser rangefinder outputs the precise slant distance between the target and the system; and the turntable's built-in encoder provides real-time feedback on the azimuth and pitch angles.

[0028] Specifically, a unified central clock is established, which sends a unified synchronization trigger signal to all sensors. On the rising edge of each synchronization clock pulse, all sensors simultaneously acquire data. The system employs a master-slave clock synchronization scheme. An image processing board or host computer is designated as the master clock node, maintaining a global time reference through a high-precision clock source. The image acquisition board synchronizes with the master clock node via Ethernet supporting the PTP precision clock synchronization protocol, achieving microsecond-level time synchronization. For nodes such as the STM32 turntable controller that lack PTP functionality, the master node periodically sends precise global time information to them via serial communication, thereby achieving time unification across the entire system.

[0029] It should be noted that a unified hardware timestamp is injected into all physical sources of data generation, forming a time-stamped asynchronous data stream.

[0030] In this embodiment of the invention, injecting a unified hardware timestamp at the physical source of high-speed UAV data generation and forming an asynchronous data stream provides a precise and unified time reference for multi-source heterogeneous data such as images, turntable angles, and laser ranging. This avoids time offset errors caused by software processing and transmission from the source, providing a reliable temporal basis for subsequent spatiotemporal registration and target association, ensuring accurate alignment and fusion of multi-source data. It also supports the backtracking positioning mechanism to obtain the precise turntable angle at the time of target image acquisition through timestamp matching or interpolation, effectively compensating for the delay introduced by image processing and transmission links. At the same time, it provides high-quality observation data with time alignment for Kalman filtering, improving the accuracy of target state estimation and motion prediction. This is the core foundation for achieving high-precision, low-latency tracking of high-speed UAVs.

[0031] S2: Perform spatiotemporal registration and target association processing on the asynchronous data stream to obtain the historical position sequence of the high-speed UAV to be tracked.

[0032] It should be noted that, based on a unified time reference, the target pixel coordinates, turntable angles, and laser ranging information extracted from the image are aligned. Subsequently, through coordinate transformation, the three-dimensional position of the target in a unified world coordinate system is calculated. To address the inherent delays introduced by image processing and transmission, a backtracking positioning mechanism is adopted. This involves using the image acquisition timestamp to query the precise angle of the turntable at that moment, compensating for the delay in the observation values, and finally outputting the true spatial coordinate sequence of the target at a precise historical moment.

[0033] In one possible implementation, S2 specifically includes:

[0034] S201: Calibrate the camera and determine its intrinsic and extrinsic parameter matrices.

[0035] It should be noted that a sensor coordinate system is defined, with the origin located at the emission point of the laser rangefinder. The laser rangefinder directly provides the radial distance between the target and the emission point of the laser rangefinder. The visible light and infrared cameras provide two-dimensional coordinate information in the image pixel coordinate system. Each pixel corresponds to a ray in the sensor coordinate system. Through camera calibration, the intrinsic and extrinsic parameter matrices of the camera are obtained.

[0036] The intrinsic parameter matrix projects points from the camera coordinate system to the pixel coordinate system.

[0037] The extrinsic parameter matrix describes how to transform from the sensor coordinate system to the camera coordinate system.

[0038] S202: Based on the intrinsic and extrinsic parameter matrices, the two-dimensional coordinate information of the asynchronous data stream in the camera coordinate system is back-projected into the sensor coordinate system to obtain the first unit direction vector of the camera in the sensor coordinate system.

[0039] Specifically, the unit direction vector of the camera in the sensor coordinate system is:

[0040] ;

[0041] in, V img This represents the unit direction vector of the camera in the sensor coordinate system. R Represents the rotation matrix. T Indicates transpose. V cam This represents the unit direction vector in the camera coordinate system. x n This represents the normalized horizontal coordinate. y n This represents the normalized vertical coordinate. u Represents the horizontal coordinates in the pixel coordinate system. v Represents the vertical coordinates in the pixel coordinate system. k n This represents the normalization scaling factor. K Represents the camera intrinsic parameter matrix. x c This represents the horizontal axis coordinate in the camera coordinate system. y c Represents the vertical axis coordinates in the camera coordinate system. z c Represents the depth axis coordinates in the camera coordinate system. Represents the camera extrinsic parameter matrix. x This represents the horizontal axis coordinate in the sensor coordinate system. y Represents the vertical axis coordinates in the sensor coordinate system. zThis represents the height axis coordinate in the sensor coordinate system, and 1 indicates a fixed component of the homogeneous coordinate system.

[0042] S203: Construct the second unit direction vector of the laser beam in the sensor coordinate system.

[0043] Specifically, the unit direction vector of the laser beam in the sensor coordinate system is:

[0044] ;

[0045] in, V l This represents the unit direction vector of the laser beam in the sensor coordinate system. α l Indicates the azimuth angle of the turntable. β Indicates the pitch angle of the turntable. sin Represents the sine function. cos This represents the cosine function.

[0046] S204: Combine the first unit direction vector, the second unit direction vector, and the laser range value to calculate the three-dimensional coordinates of the high-speed UAV to be tracked in the sensor coordinate system.

[0047] Specifically, the three-dimensional coordinates are:

[0048] ;

[0049] in, P Represents three-dimensional coordinates. R l This indicates the laser ranging value.

[0050] It should be noted that the three-dimensional coordinates are expanded as follows:

[0051] ;

[0052] in, X l Representing three-dimensional coordinates X Axis coordinates Y l Representing three-dimensional coordinates Y Axis coordinates Z l Representing three-dimensional coordinates Z Axis coordinates.

[0053] S205: By using a backtracking positioning mechanism, delay compensation is performed on the three-dimensional coordinates to obtain the historical position sequence.

[0054] The compensated three-dimensional coordinates are as follows:

[0055] ;

[0056] in, P ( t )express t The three-dimensional coordinates at time [time]. R t express t The laser rangefinder value at time [time]. V l ( t )express t The unit direction vector of the laser beam at any given time, sin Represents the sine function. cos Represents the cosine function. T This indicates transpose.

[0057] It should be noted that the miss distance calculated at the current moment is used to interpolate or query the corresponding turntable azimuth and elevation angles from the cached turntable position queue based on the image acquisition timestamp, and then superimposed to calculate the absolute angular coordinates of the target at a precise past moment, thereby compensating for the delay introduced by image processing and transmission links.

[0058] In one possible implementation, S205 specifically includes:

[0059] S2051: Query the turntable angle corresponding to the timestamp of the image data in the asynchronous data stream.

[0060] The timestamp is a precise and unified time identifier assigned to multi-source data at the physical source of high-speed UAV data collected by sensors such as cameras, laser rangefinders, and turntables. Unlike software timestamps, it is injected during the data generation stage, establishing a unique and synchronous time reference for asynchronous data streams such as pixel coordinates, laser range values, and turntable angles output by various sensors. This becomes the core timing basis for spatiotemporal registration of multi-source data and target association. At the same time, this hardware timestamp can accurately mark the actual acquisition time of various types of data, providing a reliable time reference for the backtracking positioning mechanism. It facilitates querying the precise turntable angle at the corresponding time, effectively compensating for the inherent delays in image processing and transmission links, ensuring the timing accuracy of multi-source data fusion and target 3D position calculation, and laying a good time synchronization foundation for subsequent accurate acquisition of target historical position sequences and construction of Kalman filters.

[0061] S2052: Determine if a turntable angle sample exists that matches the timestamp. If so, use the turntable angle sample as the target turntable angle. Otherwise, filter a preset number of adjacent time sample points and calculate the target turntable angle using linear interpolation.

[0062] The specific judgment conditions are as follows:

[0063] ;

[0064] in, t j Indicates the cached timestamp. t Indicates the target timestamp. e This represents the threshold.

[0065] The interpolation formula is as follows:

[0066] ;

[0067] in, α t express t The azimuth angle of the turntable at any given moment. α p express t p The azimuth angle of the turntable at any given moment. α n express t n The azimuth angle of the turntable at any given moment. t p Indicates the timestamp of the preceding sample. t n Indicates the timestamp of subsequent samples. β t express t The tilt angle of the turntable at any moment. β p express t p The tilt angle of the turntable at any moment. β n express t n The tilt angle of the turntable at any given moment.

[0068] Those skilled in the art can set the preset quantity and threshold size according to actual needs, and the present invention does not limit this.

[0069] S2053: Based on the target turntable angle, correct the spatial pointing relationship corresponding to the three-dimensional coordinates to complete the delay compensation.

[0070] S2054: The three-dimensional coordinates after delay compensation are used as the spatial position of the high-speed UAV to be tracked at a historical moment, forming a historical position sequence.

[0071] In this embodiment of the invention, the intrinsic and extrinsic parameter matrices are obtained through camera calibration, enabling precise back-projection from pixel coordinates to the sensor coordinate system. Combined with laser ranging and turntable angle, three-dimensional coordinate calculation is completed. Simultaneously, relying on the backtracking positioning mechanism with a unified timestamp, the precise turntable angle at the time of image acquisition is obtained through timestamp matching or linear interpolation, effectively compensating for the delay introduced by image processing and transmission links. This achieves spatiotemporal registration and target association of multi-source asynchronous data, and finally outputs the true spatial position sequence of the target at a precise historical moment. This not only eliminates the observation errors caused by timing deviations and system delays, but also provides clean, lag-free, high-quality observation data for subsequent Kalman filtering, which is a key link in improving the tracking accuracy and reliability of high-speed UAVs.

[0072] S3: Construct a Kalman filter based on the current statistical model.

[0073] Among them, the current statistical model is an adaptive motion model designed specifically for maneuvering targets (such as high-speed UAVs). Its core assumption is that the target acceleration is a time-dependent stochastic process with a non-zero mean. It abandons the ideal assumption of "constant acceleration" in the traditional uniform acceleration model. It can dynamically adjust the mean acceleration according to the real-time motion state of the target. At the same time, it quantifies the noise variance of the acceleration process through a dedicated formula, accurately characterizing the uncertainty of random maneuvers such as acceleration, deceleration, and sharp turns of UAVs. As the core motion model of the Kalman filter, it provides a transfer basis that fits the actual maneuvering characteristics for state prediction. It effectively solves the problem that traditional models cannot adapt to the complex random maneuvers of high-speed UAVs and is a key motion modeling foundation for achieving high-precision tracking of maneuvering targets.

[0074] The Kalman filter is an optimal recursive estimation algorithm for linear systems. Through a closed-loop process of "prediction and update," it fuses the system motion model and observation data to achieve accurate estimation of the target state. Based on the current statistical model, it first predicts the UAV's motion state (position, velocity, acceleration) at the next moment based on the target's historical position sequence. Then, it corrects the prediction result using the target's actual spatial position observations after backtracking and delay compensation. Simultaneously, it quantifies the UAV's maneuver uncertainty through the process noise variance matrix and characterizes the observation error through the measurement noise variance matrix, dynamically balancing the weights of model prediction and actual observation, and finally outputs the optimal target state estimate.

[0075] It should be noted that a model constitutes a "current statistical model" only when the statistical characteristics of acceleration (mean, variance) are modeled as a "current adaptive" stochastic process and used to dynamically adjust the process noise covariance.

[0076] In one possible implementation, S3 specifically includes:

[0077] S301: Define the state vector based on the historical position sequence.

[0078] Specifically, the state vector is:

[0079] ;

[0080] in, X ( k )express k The state vector at time t, x x Represents the high-speed UAV in a two-dimensional coordinate system X Axis coordinates y y Represents the high-speed UAV in a two-dimensional coordinate system Y Axis coordinates v x Represents the high-speed UAV in a two-dimensional coordinate system X Axis velocity components, v y Represents the high-speed UAV in a two-dimensional coordinate system Y Axis velocity components, a x Represents the high-speed UAV in a two-dimensional coordinate system X Axial acceleration components, a y Represents the high-speed UAV in a two-dimensional coordinate system Y Axial acceleration components, T This indicates transpose.

[0081] S302: Based on the state vector, construct the state equation and observation equation to obtain the Kalman filter.

[0082] The state equations are as follows:

[0083] ;

[0084] in, X ( k +1 indicates k The state vector at time +1 F ( k )express k The state transition matrix at time t, U ( k )express k The control input matrix at each time step, express k The mean acceleration control vector at time t. W ( k )express k The process noise vector at time step, Indicates the sampling time interval. α Indicates the frequency of maneuvers.

[0085] The observation equation is as follows:

[0086] ;

[0087] in, Z ( k )express k Sensor measurement vector at time [time]. H ( k )express k The observation matrix at time, V ( k )express k The observation noise vector at time step, g Represents gravitational acceleration. i Indicates the pitch angle of the drone. f Indicates the roll angle of the drone. tan This represents the tangent function.

[0088] In this embodiment of the invention, a current statistical model Kalman filter adapted to the maneuvering characteristics of high-speed UAVs is constructed based on the target's historical position sequence. By defining a six-dimensional state vector containing position, velocity, and acceleration, and combining precise state equations and observation equations, system modeling is completed. This not only abandons the ideal assumptions of traditional uniform acceleration models based on the current statistical model, but also dynamically adapts to random maneuvers such as UAV acceleration, deceleration, and sharp turns, and accurately quantifies maneuvering uncertainties. Furthermore, through the closed-loop process of "prediction and update" of Kalman filtering, the motion model and the observation data after delay compensation are integrated. By dynamically balancing the weights of prediction and observation using process noise and measurement noise matrices, the optimal target motion state estimate is finally output, significantly improving the adaptability and accuracy of tracking high-speed maneuvering UAVs.

[0089] S4: Input the historical location sequence into the Kalman filter and output the future predicted location.

[0090] In one possible implementation, S4 specifically includes:

[0091] S401: Calculate the state vector of the high-speed UAV to be tracked at the next moment using a Kalman filter.

[0092] Specifically, the state vector is:

[0093] ;

[0094] in, X ( k )express k The state vector at time t, F Represents the state transition matrix. X ( k -1) indicates k The state vector at time -1U ( k )express k The control input matrix at each time step, This represents the mean acceleration control vector.

[0095] S402: Calculate the prediction error covariance matrix based on the current acceleration value of the high-speed UAV to be tracked.

[0096] Specifically, the prediction error covariance matrix is ​​as follows:

[0097] ;

[0098] in, Indicates in k At time -1, k The prediction error covariance matrix at time 1. express k The prediction error covariance matrix at time -1 T Indicates transpose. Q Represents the process noise variance matrix. α Indicates the frequency of maneuvering. s This represents the standard deviation of noise during the acceleration process. Indicates the sampling time interval.

[0099] The standard deviation of the acceleration process is calculated and adjusted using an adaptive method. When the acceleration is positive, the variance is:

[0100] ;

[0101] in, s 2 This represents the noise variance during the acceleration process. a M This indicates the maximum acceleration. It represents the average acceleration.

[0102] When the acceleration is negative, the variance is:

[0103] ;

[0104] in, This represents the minimum acceleration.

[0105] In one possible implementation, after S402 and before S403, the following is also included:

[0106] Update the prediction error covariance matrix.

[0107] The specific formula for updating the prediction error covariance matrix is ​​as follows:

[0108] ;

[0109] in, express k The prediction error covariance matrix at time 1. I Represents the identity matrix. express k The Kalman gain matrix at time 10:00. H Represents the observation matrix. Indicates in k At time -1, k The prediction error covariance matrix at time 1.

[0110] S403: Calculate the Kalman gain based on the prediction error covariance matrix.

[0111] The Kalman gain is a core adjustment parameter in the Kalman filter update process, balancing the weights of predicted and observed states. Its value is determined by the predicted state covariance, the observation matrix, and the measurement noise variance. The Kalman gain dynamically adjusts based on the UAV's maneuver uncertainty (quantified by the process noise variance matrix) and the accuracy of the observation data (characterized by the measurement noise variance matrix). When the UAV maneuvers are turbulent and prediction uncertainty is high, the Kalman gain increases, placing greater trust in the target's true observations after backtracking and delay compensation, quickly correcting prediction biases. When the UAV's movement is stable and prediction accuracy is high, the Kalman gain decreases, relying more on the state prediction results based on the current statistical model, reducing observation noise interference. Ultimately, the optimal fusion of predicted and observed values ​​is achieved through the Kalman gain, outputting an accurate target motion state estimate, providing a reliable basis for subsequent turntable tracking control.

[0112] Specifically, the Kalman gain is:

[0113] ;

[0114] in, K ( k )express k Kalman gain at time step H Represents the observation matrix. This represents the observation noise covariance matrix.

[0115] S404: Calculate the optimal estimate based on the Kalman gain and the state vector.

[0116] Specifically, the optimal estimate is:

[0117] ;

[0118] in, express k The optimal estimate of the time. Indicates in kAt time -1, k The optimal estimate of the time. Z ( k )express k Sensor measurement vector at time [time]. express k The optimal estimate at time -1.

[0119] S405: Update the state vector based on the optimal estimate to obtain the future predicted position.

[0120] In this embodiment of the invention, the motion state prediction of a high-speed UAV is completed by a Kalman filter based on the current statistical model. The uncertainty of the UAV's random maneuvers is accurately quantified by the adaptively adjusted acceleration process noise variance. Combined with the closed-loop process of "state prediction - covariance update - Kalman gain calculation - optimal estimation", the weights of the model prediction and the observation data after delay compensation are dynamically balanced. This not only quickly outputs the accurate predicted position of the UAV in the future, providing an effective lead for turntable tracking control, but also adaptively adjusts the filter gain according to the intensity of the UAV's maneuvers. It reduces observation noise interference during stable flight and quickly corrects prediction deviations during violent maneuvers, greatly improving the accuracy and robustness of the motion state prediction of high-speed UAVs. This lays the core foundation for achieving high-precision, real-time target tracking.

[0121] S5: Compare the predicted future position with the current position of the high-speed drone to be tracked to obtain the position error.

[0122] In one possible implementation, S5 specifically includes:

[0123] S501: Calculate the predicted azimuth and predicted elevation angles based on the predicted future location.

[0124] Specifically, the predicted azimuth angle is as follows:

[0125] ;

[0126] in, Az pred Indicates the predicted azimuth angle. atan Represents the arctangent function. x p This indicates the predicted future location of the target in the Cartesian coordinate system. X Axial components, y p This indicates the predicted future location of the target in the Cartesian coordinate system. Y Axial components.

[0127] Specifically, the predicted pitch angle is:

[0128] ;

[0129] in, El pred Indicates the predicted pitch angle. arcsin Represents the arcsine function. z p This indicates the predicted future location of the target in the Cartesian coordinate system. Z Axial components, R p This indicates the predicted spatial slant distance.

[0130] S502: The position error is obtained based on the predicted azimuth and predicted elevation angles.

[0131] Specifically, the positional error is as follows:

[0132] ;

[0133] in, e Indicates positional error. P pred This represents the expected value of the turntable. P curr Indicates the current actual position of the turntable. T Indicates transpose. Az curr Indicates the actual azimuth angle. El curr This indicates the actual pitch angle.

[0134] In this embodiment of the invention, by converting the predicted future position of the target into the predicted azimuth and pitch angles required for turntable tracking, and then accurately comparing them with the current actual azimuth and pitch angles of the turntable, the abstract Cartesian coordinate system position deviation is transformed into an angle-type position error that fits the turntable control requirements. This not only clearly quantifies the degree of deviation between the predicted future position of the target and the current actual position of the UAV, providing accurate and clear error input for the subsequent proportional-integral-derivative control algorithm, but also builds a key bridge between target motion prediction and real-time turntable control, ensuring that the turntable can subsequently adjust the angle to compensate for the deviation, thus providing a core error judgment basis for achieving accurate and real-time tracking of high-speed UAVs.

[0135] S6: The position error is adjusted by the proportional-integral-derivative control algorithm to obtain the turntable control command for the high-speed UAV to be tracked.

[0136] The proportional-integral-derivative (PID) control algorithm takes the deviation between the desired and actual states of the system as input. It processes the deviation through three stages—proportional, integral, and derivative—in multiple dimensions and then weights and fuses them to output the control quantity. The PID uses the error between the target's predicted future position (predicted by Kalman filtering) and the turntable's actual position as the deviation input. The proportional term outputs a correction control quantity in real time based on the deviation magnitude for rapid response. The integral term accumulates historical deviations to eliminate steady-state errors and ensure tracking accuracy. The derivative term predicts and suppresses overshoot based on the deviation trend, improving tracking stability. These three components work together to dynamically adjust the turntable control commands, driving the turntable to precisely align with the target UAV, achieving high-precision and stable tracking of high-speed maneuvering targets.

[0137] In one possible implementation, S6 specifically includes:

[0138] S601: Calculate the proportional, integral, and differential terms of the position error.

[0139] The proportional term determines the instantaneous response speed.

[0140] The integral term eliminates steady-state error.

[0141] Among them, the differential term suppresses overshoot and oscillation.

[0142] S602: Add the proportional term, integral term, and derivative term to obtain the turntable control output.

[0143] ;

[0144] in, u PID This indicates the output quantity of the turntable control. K p Represents the proportional gain coefficient. e Indicates positional error. K i Represents the integral gain coefficient. K d This represents the differential gain coefficient.

[0145] S603: Sends the turntable control output to the turntable servo driver to obtain the turntable control command.

[0146] In this embodiment of the invention, a proportional-integral-derivative (PID) control algorithm is used to precisely adjust the position error. The proportional term outputs a correction control quantity in real time according to the error magnitude to achieve rapid response. The integral term accumulates historical deviations to completely eliminate steady-state errors and ensure tracking accuracy. The derivative term predicts the trend of deviation changes to effectively suppress overshoot and oscillation to improve control stability. The three terms are weighted and fused to obtain the turntable control output quantity and convert it into control commands that can be executed by the turntable servo driver. This not only efficiently compensates for the deviation between the target's predicted future position and the turntable's current actual position, but also achieves the speed, accuracy, and stability of turntable control. It establishes a closed-loop link between error adjustment and turntable movement, ensuring that the turntable can be aligned with the high-speed maneuvering UAV in real time and accurately. This provides directly executable core control support for ultimately achieving high-precision and stable target tracking.

[0147] S7: Drive the high-speed UAV to be tracked to travel to the predicted future position according to the turntable control command, so as to achieve tracking of the high-speed UAV to be tracked.

[0148] Reference manual attached Figure 2 The diagram shows a schematic representation of a high-speed UAV tracking system based on Kalman filtering, provided by an embodiment of the present invention.

[0149] This invention provides a high-speed UAV tracking system 20 based on Kalman filtering, comprising: a processor 201 and a memory 202;

[0150] The memory 202 stores programs or instructions that can run on the processor 201. When the program or instructions are executed by the processor 201, they implement the steps of the high-speed UAV tracking method based on Kalman filtering described above and achieve the same technical effect. To avoid repetition, the present invention will not elaborate further.

[0151] It should be understood that the processor 201 in this embodiment of the invention may be a central processing unit (CPU), or it may be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor.

[0152] It should also be understood that the memory 202 in the embodiments of the present invention can be volatile memory or non-volatile memory, or may include both volatile and non-volatile memory. The non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of random access memory are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct memory bus RAM (DR RAM).

[0153] The above embodiments can be implemented, in whole or in part, by software, hardware (such as circuits), firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, as a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, all or part of the processes or functions described in the embodiments of the present invention are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more sets of available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium. A semiconductor medium can be a solid-state drive.

[0154] It should be understood that, in various embodiments of the present invention, the order of the above-mentioned process numbers does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0155] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0156] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the devices, apparatuses, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0157] In the several embodiments provided by this invention, it should be understood that the disclosed devices, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0158] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0159] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0160] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, essentially, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0161] This invention provides a readable storage medium comprising: storing a program or instructions on the readable storage medium, wherein when the program or instructions are executed by a processor, the program or instructions implement the steps of the above-described high-speed UAV tracking method based on Kalman filtering and achieve the same technical effect. To avoid repetition, this invention will not elaborate further.

[0162] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the embodiments of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the protection scope of the present invention.

Claims

1. A high-speed UAV tracking method based on Kalman filtering, characterized in that, include: S1: Collect asynchronous data streams from the high-speed drone to be tracked; S2: Perform spatiotemporal registration and target association processing on the asynchronous data stream to obtain the historical position sequence of the high-speed UAV to be tracked; S2 specifically includes: S201: Calibrate the camera and determine its intrinsic and extrinsic parameter matrices; S202: Based on the intrinsic parameter matrix and the extrinsic parameter matrix, the two-dimensional coordinate information of the asynchronous data stream in the camera coordinate system is back-projected into the sensor coordinate system to obtain the first unit direction vector of the camera in the sensor coordinate system; S203: Construct the second unit direction vector of the laser beam in the sensor coordinate system; S204: Combine the first unit direction vector, the second unit direction vector, and the laser range value to calculate the three-dimensional coordinates of the high-speed UAV to be tracked in the sensor coordinate system; S205: By using a backtracking positioning mechanism, delay compensation is performed on the three-dimensional coordinates to obtain the historical position sequence; S3: Construct a Kalman filter based on the current statistical model; S4: Input the historical location sequence into the Kalman filter and output the future predicted location; S5: Compare the predicted future position with the current position of the high-speed UAV to be tracked to obtain the position error; S6: The position error is adjusted by a proportional-integral-derivative control algorithm to obtain the turntable control command for the high-speed UAV to be tracked; S7: According to the turntable control command, drive the high-speed UAV to be tracked to travel according to the future predicted position, so as to achieve tracking of the high-speed UAV to be tracked.

2. The high-speed UAV tracking method based on Kalman filtering according to claim 1, characterized in that, S205 specifically includes: S2051: Based on the timestamp of the image data in the asynchronous data stream, query the turntable angle corresponding to the timestamp; S2052: Determine whether there is a turntable angle sample that matches the timestamp; if so, use the turntable angle sample as the target turntable angle; otherwise, filter a preset number of adjacent time sample points and calculate the target turntable angle by linear interpolation. S2053: Based on the target turntable angle, the spatial pointing relationship corresponding to the three-dimensional coordinates is corrected to complete the delay compensation; S2054: The three-dimensional coordinates after delay compensation are used as the spatial position of the high-speed UAV to be tracked at a historical moment, forming the historical position sequence.

3. The high-speed UAV tracking method based on Kalman filtering according to claim 1, characterized in that, S3 specifically includes: S301: Define a state vector based on the historical position sequence; S302: Based on the state vector, construct the state equation and the observation equation to obtain the Kalman filter.

4. The high-speed UAV tracking method based on Kalman filtering according to claim 1, characterized in that, S4 specifically includes: S401: Calculate the state vector of the high-speed UAV to be tracked at the next moment using the Kalman filter; S402: Calculate the prediction error covariance matrix based on the current acceleration value of the high-speed UAV to be tracked; S403: Calculate the Kalman gain based on the prediction error covariance matrix; S404: Calculate the optimal estimate based on the Kalman gain and the state vector; S405: Update the state vector based on the optimal estimate to obtain the predicted future position.

5. The high-speed UAV tracking method based on Kalman filtering according to claim 4, characterized in that, After S402 and before S403, the following is also included: The prediction error covariance matrix is ​​updated.

6. The high-speed UAV tracking method based on Kalman filtering according to claim 1, characterized in that, S5 specifically includes: S501: Calculate the predicted azimuth and predicted elevation angles based on the predicted future position; S502: The position error is obtained based on the predicted azimuth angle and the predicted elevation angle.

7. The high-speed UAV tracking method based on Kalman filtering according to claim 1, characterized in that, S6 specifically includes: S601: Calculate the proportional term, integral term, and differential term of the position error; S602: Add the proportional term, the integral term, and the derivative term to obtain the turntable control output; S603: Send the turntable control output to the turntable servo driver to obtain the turntable control command.

8. A high-speed UAV tracking system based on Kalman filtering, characterized in that, include: Processor and memory; The memory stores programs or instructions that can run on the processor, which, when executed by the processor, implement the steps of the high-speed UAV tracking method based on Kalman filtering as described in any one of claims 1 to 7.

9. A readable storage medium, characterized in that, The readable storage medium stores a program or instructions that, when executed by a processor, implement the steps of the high-speed UAV tracking method based on Kalman filtering as described in any one of claims 1 to 7.

Images