A method for following a moving target

Abstract

The application discloses a moving target following method and relates to the technical field of unmanned aerial vehicle autonomous control, has good safety, completely avoids dangerous diving and emergency stop actions caused by height misjudgment of the unmanned aerial vehicle when flying over buildings and the like, and the flight track is stable. Meanwhile, the method has good following continuity, significantly reduces target loss probability through short-time inertia prediction and intelligent recapture, and realizes seamless connection after occlusion. In addition, the method realizes the balance between precision and robustness, keeps accurate distance following capability in flat terrain, and automatically degenerates into stable visual following when the terrain changes suddenly.

Description

Technical Field

[0001] This invention relates to the field of autonomous control technology for unmanned aerial vehicles (UAVs), and in particular to a method for tracking moving targets based on a micro-optical pod. Background Technology

[0002] The core of the motion target tracking technology based on photoelectric pods is a closed-loop system of perception, evaluation, and control. The most important aspect is how to effectively evaluate and obtain the accurate coordinate position of the target through the perceived data, thereby achieving precise tracking control.

[0003] Based on the configuration of the onboard optoelectronic pod, existing technologies can be mainly divided into the following two categories: 1. Precise positioning and following based on laser ranging When the electro-optical pod integrates a high-precision laser rangefinder (LRF), the system can acquire the target's slant range R. Combining the pod's frame angle, the UAV's attitude, and RTK positioning, the system calculates the target's absolute latitude, longitude, and altitude in real time through a coordinate system transformation chain (pixel system → pod system → aircraft system → navigation system → geodetic system). The flight control system uses these coordinates and the target's velocity vector to calculate the desired position using the line-of-sight (LOS) method or a nonlinear guidance law, achieving precise point-to-point or distance-to-distance following.

[0004] The limitations of this approach are that laser rangefinders are expensive and heavy, making them unsuitable for integration into increasingly common micro-pods, thus restricting the application of small drones.

[0005] 2. Vision-based laser-free tracking For micro-pods, which are characterized by miniaturization, low cost, and no laser, it is necessary to recover depth information through other information sources or directly avoid dependence on depth information. Existing laser-free solutions mainly rely on the following two methods: (1) Fixed-height plane assumption method Assuming the target is attached to a horizontal surface, the drone's altitude H above the ground is used, combined with the pod's pitch angle θ, to estimate the target's horizontal distance through trigonometric relationships, thereby controlling the drone to maintain a set distance.

[0006] The core flaw of this method is that when the terrain changes abruptly (such as flying over buildings, overpasses, or potholes), the relative height H changes abruptly, but the algorithm mistakenly interprets this as a sudden change in target distance. In scenarios where the drone flies over buildings, the system misjudges this as a "target instantly approaching," triggering the drone to brake suddenly, pitch up, and retreat; after flying over a rooftop, H suddenly increases again, and the system misjudges this as a "target instantly moving away," causing the drone to swoop down to chase. This drastic misjudgment not only leads to severe oscillations in the following trajectory and target loss, but also poses a serious safety hazard.

[0007] (2) Motion model filtering method (pure visual bias servo) This method abandons absolute distance measurement and uses only the pixel deviation of the target from the image center output by the pod as the error. A PI controller outputs the yaw / pitch rate to rotate the UAV, eliminating the deviation and keeping the target centered in the frame. Speed ​​control typically relies on the target bounding box area change rate or a fixed cruise speed.

[0008] The drawbacks of this method are: it cannot directly obtain the target distance and physical coordinates, and cannot achieve precise distance tracking; the speed control is coarse, and it is prone to oscillations during acceleration / deceleration. More importantly, when the target is briefly obscured (such as being obscured by the edge of a building while flying over it), the system lacks open-loop prediction capability after the visual feedback is lost, and the drone is prone to stalling or drifting blindly; when re-acquiring after the obstruction ends, it is very easy to mistakenly lock onto a similar target appearing in the frame.

[0009] In summary, existing micro pod following solutions present a contradiction between "terrain robustness" and "precise distance control": the constant altitude plane assumption method cannot adapt to sudden terrain changes and may cause dangerous maneuvers; the motion model filtering method has strong terrain adaptability but cannot control distance, and is easy to lose track after obstruction.

[0010] Therefore, how to provide a following method that can integrate the advantages of both, intelligently switch, and is safe and reliable is a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0011] In view of the above problems, the present invention provides a moving target tracking method to overcome or at least partially solve the above problems. It addresses the issues of high safety risks, poor tracking stability, and low re-acquisition success rate in existing laser-free rangefinder-based micro-pod tracking schemes in complex terrain and occluded scenarios, which rely solely on fixed-altitude plane ranging or pure visual servoing. This method utilizes an optoelectronic pod (especially a laser-free rangefinder-based micro-pod) to achieve stable tracking of moving targets, and can cope with complex environments such as sudden terrain changes and target occlusion.

[0012] This invention provides the following solution: A method for tracking a moving target, comprising: The UAV is equipped with a laser-free micro-optical pod, an onboard altitude sensor, and a positioning module. The method includes: The system acquires the ground clearance measured by the airborne altitude sensor, the pod pitch angle output by the micro optoelectronic pod, the image miss distance, and the target confidence level. The default operating mode is mode one, which is based on the fixed altitude plane assumption method. It calculates the target horizontal distance using the ground height and the pod pitch angle, and generates a fixed distance follow command based on the target horizontal distance. Mode one is the distance loop mode. The system monitors the rate of change of altitude H and the amplitude of jitter in the pod's pitch angle in real time. When the rate of change of altitude exceeds a first preset threshold or the amplitude of jitter exceeds a second preset threshold, a terrain change flag is generated, switching from mode one to mode two, disabling the distance loop, and activating the motion model prediction hold function. Mode two is a visual deviation loop mode, which drives the UAV's heading and pitch solely based on the image miss distance to keep the target centered in the image. In the second mode, when the target confidence is lower than the third preset threshold, it is determined that the target is temporarily occluded, and the target motion speed estimated by the extended Kalman filter before occlusion is used to maintain the open-loop output of the UAV's speed and heading. When the height change rate recovers to within the first preset threshold, the jitter amplitude recovers to within the second preset threshold, and the target confidence is greater than or equal to the fourth preset threshold and continues for a first preset time, the observation noise covariance of the extended Kalman filter is dynamically adjusted to smoothly switch back to mode one from mode two.

[0013] Preferably: the height change rate exceeds a first preset threshold as follows: After the ground clearance is processed by median filtering, the difference between the previous and next frames is calculated, and a trigger is activated when the absolute value of the difference is greater than or equal to 3.0 meters. The jitter amplitude exceeds the second preset threshold as follows: The absolute value of the angular velocity of the pod's pitch angle is greater than or equal to 10° / second and is triggered for 5 consecutive frames; The target confidence level being lower than the third preset threshold is: The target confidence level C is less than 0.4 and is triggered for 5 consecutive frames.

[0014] Preferably: by dynamically adjusting the observation noise covariance of the extended Kalman filter. To achieve a smooth switchback, the specific steps include: In Mode 1, the extended Kalman filter uses the nominal value of the observation noise variance. The nominal value is 2.0 m², which is based on the integrated observation of the plane distance at a fixed altitude. At the instant of switching from mode one to mode two, the observation noise covariance is... Setting it to a maximum value makes the Kalman gain of the extended Kalman filter approach zero, which is equivalent to silencing the distance observation; When switching back from mode two to mode one, the observation noise covariance is... The maximum value is smoothly reduced exponentially to the nominal value, causing the influence of the distance observation to gradually increase.

[0015] Preferably, the state vector of the extended Kalman filter is 12-dimensional, defined as:

[0016] in: The relative position of the target in the NED coordinate system; The velocity of the target in the NED coordinate system; The acceleration of the target in the NED coordinate system; Measurement offset for the pod frame angle; This is the altitude offset for the altitude-fixed radar.

[0017] Preferably, the extended Kalman filter supports three types of observations and dynamically switches between them based on data validity: Observation type one is distance observation based on the constant-elevation plane assumption, which is activated when the terrain abrupt change flag is False. The observation equation is:

[0018] Observation type two is an angle observation based on the pod's line of sight, which is always enabled. The observation equation is:

[0019] Observation type three is pixel observation based on image miss rate, which is activated when the target recognition confidence is higher than a threshold. The observation equation is:

[0020] in, Represents the camera intrinsic parameter matrix. This represents the transformation matrix from the pod to the camera. , and This indicates observation noise.

[0021] Preferably, the duration of the open-loop output is 0.5 to 1.5 seconds; when the target confidence level is lower than the third preset threshold for a duration exceeding the second preset time, the system switches from mode two to target loss mode LOST, controls the drone to hover and slowly climb, and simultaneously controls the pod to enter spiral search mode. The climb rate of the hovering climb is 1 meter / second, and the upper limit of the climb is 30 meters; the second preset time is 2.0 seconds.

[0022] Preferably, it also includes target re-identification: When the target is recaptured in the spiral search mode, the original target is confirmed by the target re-identification Re-ID feature matching based on deep learning. When the matching degree is greater than 0.7 and stable for 3 consecutive frames, the LOST mode is switched to the second mode, and the extended Kalman filter is reinitialized using the new observation.

[0023] Preferably, it also includes target velocity dead zone handling: When the target velocity estimation modulus output by the extended Kalman filter is less than 0.5 m / s, the expected following azimuth angle is frozen, and the last valid value is retained. The UAV only performs translation correction at the current position, and the pod maintains the current locked state. When the target velocity estimation modulus is greater than or equal to 0.5 m / s and lasts for more than 1 second, the dead zone is exited and normal feedforward following is resumed.

[0024] Preferably, the formula for calculating the target horizontal distance D is:

[0025] in, This represents the angle of depression of the line-of-sight vector in the NED frame; The method for generating the fixed-distance following instruction is as follows: Let the current position of the target be The target velocity vector is The expected relative distance is The desired location of the drone is:

[0026] The positional error is:

[0027] The flight controller converts position errors into speed commands:

[0028] in, Represents geodetic coordinates. For proportional gain, This is a feedforward term.

[0029] Preferably, the method is implemented through a node-based software architecture, including: The pod drive node is used to subscribe to the miss distance, frame angle, target category, and confidence level output by the micro optoelectronic pod. A height preprocessing node is used to perform median filtering on the ground clearance to suppress clutter; The target state estimator node is used to perform the extended Kalman filter filtering and the switching logic between mode one and mode two; A guidance control node is used to generate speed commands based on the output of the target state estimator node.

[0030] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects: The moving target tracking method provided in this invention offers excellent safety, completely avoiding dangerous dives and sudden stops caused by altitude misjudgments when drones fly over buildings, resulting in a stable flight trajectory. It also exhibits good tracking continuity, significantly reducing the probability of target loss through short-term inertial prediction and intelligent reacquisition, achieving seamless transitions after occlusion. Furthermore, it achieves a balance between accuracy and robustness, maintaining precise distance tracking on flat terrain and automatically degrading to stable visual tracking during sudden terrain changes.

[0031] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0032] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0033] Figure 1 This is a system data flow and module interaction diagram provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the coordinate transformation chain provided in an embodiment of the present invention; Figure 3 This is a node data flow diagram provided in an embodiment of the present invention; Figure 4 This is a block diagram of a follower control system with feedforward compensation provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of an intelligent switching state machine provided in an embodiment of the present invention. Detailed Implementation

[0034] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0035] This invention provides a moving target tracking method applied to a drone, wherein the drone is equipped with a laser-free rangefinder-based miniature optoelectronic pod, an onboard altitude sensor, and a positioning module. The method may include: The system acquires the ground clearance measured by the airborne altitude sensor, the pod pitch angle output by the micro optoelectronic pod, the image miss distance, and the target confidence level. The default operating mode is mode one, based on the fixed-altitude plane assumption method. It calculates the target horizontal distance using the ground clearance and the pod pitch angle, and generates a fixed-distance follow command based on the target horizontal distance. Mode one is a distance loop mode. Specifically, in this embodiment of the invention, the formula for calculating the target horizontal distance D is:

[0036] in, This represents the angle of depression of the line-of-sight vector in the NED frame; The method for generating the fixed-distance following instruction is as follows: Let the current position of the target be The target velocity vector is The expected relative distance is The desired location of the drone is:

[0037] The positional error is:

[0038] The flight controller converts position errors into speed commands:

[0039] in, Represents geodetic coordinates. For proportional gain, This is a feedforward term.

[0040] The system monitors the rate of change of altitude H and the amplitude of jitter in the pod's pitch angle in real time. When the rate of change of altitude exceeds a first preset threshold or the amplitude of jitter exceeds a second preset threshold, a terrain change flag is generated, switching from mode one to mode two, disabling the distance loop, and activating the motion model prediction hold function. Mode two is a visual deviation loop mode, which drives the UAV's heading and pitch solely based on the image miss distance to keep the target centered in the image. In the second mode, when the target confidence is lower than the third preset threshold, it is determined that the target is temporarily occluded, and the target motion speed estimated by the extended Kalman filter before occlusion is used to maintain the open-loop output of the UAV's speed and heading. In specific implementation, embodiments of the present invention can provide that the height change rate exceeds a first preset threshold as follows: After the ground clearance is processed by median filtering, the difference between the previous and next frames is calculated, and a trigger is activated when the absolute value of the difference is greater than or equal to 3.0 meters. The jitter amplitude exceeds the second preset threshold as follows: The absolute value of the angular velocity of the pod's pitch angle is greater than or equal to 10° / second and is triggered for 5 consecutive frames; The target confidence level being lower than the third preset threshold is: The target confidence level C is less than 0.4 and is triggered for 5 consecutive frames.

[0041] When the height change rate recovers to within the first preset threshold, the jitter amplitude recovers to within the second preset threshold, and the target confidence is greater than or equal to the fourth preset threshold and continues for a first preset time, the observation noise covariance of the extended Kalman filter is dynamically adjusted to smoothly switch back to mode one from mode two.

[0042] By dynamically adjusting the observation noise covariance of the extended Kalman filter To achieve a smooth switchback, the specific steps include: In Mode 1, the extended Kalman filter uses the nominal value of the observation noise variance. The nominal value is 2.0 m², which is based on the integrated observation of the plane distance at a fixed altitude. At the instant of switching from mode one to mode two, the observation noise covariance is... Setting it to a maximum value makes the Kalman gain of the extended Kalman filter approach zero, which is equivalent to silencing the distance observation; When switching back from mode two to mode one, the observation noise covariance is... The maximum value is smoothly reduced exponentially to the nominal value, causing the influence of the distance observation to gradually increase.

[0043] The state vector of the extended Kalman filter is 12-dimensional and is defined as follows:

[0044] in: The relative position of the target in the NED coordinate system; The velocity of the target in the NED coordinate system; The acceleration of the target in the NED coordinate system; Measurement offset for the pod frame angle; This is the altitude offset for the altitude-fixed radar.

[0045] The extended Kalman filter supports three types of observations and dynamically switches between them based on data validity: Observation type one is distance observation based on the constant-elevation plane assumption, which is activated when the terrain abrupt change flag is False. The observation equation is:

[0046] Observation type two is an angle observation based on the pod's line of sight, which is always enabled. The observation equation is:

[0047] Observation type three is pixel observation based on image miss rate, which is activated when the target recognition confidence is higher than a threshold. The observation equation is:

[0048] in, Represents the camera intrinsic parameter matrix. This represents the transformation matrix from the pod to the camera. , and This indicates observation noise.

[0049] The duration of the open-loop output is 0.5 to 1.5 seconds; when the target confidence level is lower than the third preset threshold for a duration exceeding the second preset time, the system switches from mode two to target loss mode LOST, controls the drone to hover and slowly climb, and simultaneously controls the pod to enter spiral search mode. The climb rate of the hovering climb is 1 meter / second, and the upper limit of the climb is 30 meters; the second preset time is 2.0 seconds.

[0050] It also includes target re-identification: When the target is recaptured in the spiral search mode, the original target is confirmed by the target re-identification Re-ID feature matching based on deep learning. When the matching degree is greater than 0.7 and stable for 3 consecutive frames, the LOST mode is switched to the second mode, and the extended Kalman filter is reinitialized using the new observation.

[0051] It also includes target speed dead zone handling: When the target velocity estimation modulus output by the extended Kalman filter is less than 0.5 m / s, the expected following azimuth angle is frozen, and the last valid value is retained. The UAV only performs translation correction at the current position, and the pod maintains the current locked state. When the target velocity estimation modulus is greater than or equal to 0.5 m / s and lasts for more than 1 second, the dead zone is exited and normal feedforward following is resumed.

[0052] The method is implemented through a node-based software architecture, including: The pod drive node is used to subscribe to the miss distance, frame angle, target category, and confidence level output by the micro optoelectronic pod. A height preprocessing node is used to perform median filtering on the ground clearance to suppress clutter; The target state estimator node is used to perform the extended Kalman filter filtering and the switching logic between mode one and mode two; A guidance control node is used to generate speed commands based on the output of the target state estimator node.

[0053] The moving target following method provided in this embodiment of the invention adopts a dual-mode parallel architecture: Mode 1 (Range Loop Mode): This mode is the default mode and uses the fixed-altitude plane assumption method. It utilizes the low-cost fixed-altitude radar altitude H and the pod pitch angle θ to calculate the target's horizontal distance D in real time, thus achieving precise range tracking.

[0054] Mode 2 (Visual Deviation Loop Mode): Composed of motion model filtering method, it drives the UAV's heading and pitch only based on the pod's miss distance, keeping the target in the center of the image, without relying on distance data.

[0055] Intelligent switching: Real-time monitoring of the altitude change rate of the altitude hold radar and the pitch angle jitter amplitude of the pod. When a certain indicator exceeds a preset safety threshold (determined to be flying over an obstacle or a sudden change in terrain), it immediately and seamlessly switches to mode two, disables the distance loop, and activates the motion model prediction and hold function to ensure that the UAV smoothly passes through abnormal areas. After the altitude data returns to stability and visual re-acquisition is successful, it smoothly switches back to mode one.

[0056] Introducing a "motion prediction and feedforward mechanism based on Kalman filtering": In Mode 2, when the target is briefly obscured, the target's velocity and heading, estimated by the Kalman filter before obscuration, are used to maintain the UAV's open-loop output of velocity and heading angle (inertial hold for 0.5~1.5 seconds). Meanwhile, during normal following, the filtered target velocity vector is directly injected into the flight control speed command as a feedforward, enabling the UAV to respond to target maneuvers in advance and significantly reducing tracking lag.

[0057] An enhanced "target re-identification and spiral search" strategy is adopted: To address recapture after occlusion ends, a deep learning-based target re-identification (Re-ID) feature matching method is introduced to prevent incorrect locking of similar targets. If prolonged occlusion (>1.5 seconds) causes prediction failure, the UAV automatically enters a protection mechanism: it climbs in place to expand its field of view, and the pod initiates a spiral scan until the original target is recaptured.

[0058] Employ engineered security protection logic: In constant-altitude plane ranging, median filtering is used to suppress radar clutter; a target velocity dead zone is set, and when the target is stationary or at extremely low speed, the expected following azimuth is frozen to avoid the UAV's expected point drifting due to positioning noise.

[0059] The following section uses a typical application in a following system as an example to provide a detailed description of the moving target following method provided in this embodiment of the invention.

[0060] The typical hardware components of the follower system provided by this invention include: 1. Quadcopter drone platform: maximum takeoff weight 2kg, flight time 55 minutes; 2. Miniature Optical Pod: A two-axis lightweight gimbal, weighing ≤150g, integrating a 10x optical zoom visible light camera, without a laser rangefinder. The pod has a built-in IMU for gimbal stabilization control, and outputs interactive data such as miss distance and frame angle via serial port. 3. Airborne altitude sensor: Low-cost single-point laser rangefinder, ranging range 0.2 - 40m, accuracy ±5cm, update rate 50Hz, installed directly below the drone; 4. Flight control and computing platform: mainstream open-source flight controller + airborne computer (1.4GHz, 8GB RAM). 5. Positioning module: Beidou / GPS positioning (UM982 module + RTK), horizontal positioning accuracy 2cm + 1ppm.

[0061] The following system consists of three closed loops: a perception layer, an estimation layer, and a control layer, such as... Figure 1 As shown. The UAV flight control system adjusts its attitude and position based on data from the micro-pod, maintaining the desired relative spatial relationship (distance and heading) with the target. The core data flow of the system consists of the following three segments: 1. Miniature pod → Flight control Image off-target amount ( , (pixels) Pod frame angle ( ) Target category and confidence level 2. Flight control / airborne sensors → Target state estimation model Drone latitude / longitude / altitude (GPS / RTK) UAV three-axis attitude angles ( , , ) ground height (Radar / Ultrasonic) UAV velocity vector

[0062] 3. Target State Estimation Model → Guidance Law Target relative position vector (NED coordinate system) Absolute coordinates of the target (latitude, longitude, and altitude) target speed and direction The main improvement of this invention lies in the estimation layer. By constructing a dual-mode parallel architecture, an intelligent switching state machine model is designed, enabling the following system to have both terrain robustness and precise distance control.

[0063] First, define the coordinate system, which involves the following coordinate systems: Like a plane coordinate system ( ): The pixel position of the target in the image, with the origin at the center of the image, which is the miss distance; Pod base coordinate system: The line-of-sight vector direction is defined by the pod's azimuth / pitch angle; UAV body coordinate system: considering pod installation offset and vibration reduction; NED (Northeast) coordinate system: the standard reference system for navigation calculation; Geodetic coordinate system: longitude, latitude, altitude (LLH).

[0064] definition This is the line-of-sight unit vector, originating from the optical center of the pod and pointing directly from the pod to the locked moving target. The subscripts are... It is an abbreviation for line of sight, referring to the vector direction from the optical center of the pod towards the target. A diagram illustrating the transformation chain between coordinate systems is shown below. Figure 2 As shown: 1. The unit vector of the line of sight in different coordinate systems The formula for representing is shown below: Line-of-sight unit vector in the pod coordinate system (set as azimuth-pitch type): (1) in: This is the azimuth angle (0° is the direction of the nose). This is the pitch angle (negative for downward).

[0065] 2. Transform to the body coordinate system (considering the pod installation offset matrix) ): (2) 3. Transform to NED coordinate system (using UAV attitude rotation matrix) ): (3) This variable is used to calculate the final line-of-sight direction of the real geographic coordinates, taking into account the tilt of the drone in the air.

[0066] The software system of this invention adopts a node-based design, where each topic node can freely publish and subscribe to topic information. The topic functional modules are loosely coupled, facilitating debugging and replacement. The main nodes used include: Node 1: pod driver; Node 2: altitude filter; Node 3: target estimator; Node 4: guidance controller. Topics and data flows between nodes are as follows: Figure 3 As shown: Detailed implementation of target state estimation: Mode 1: Fixed-height plane assumption method.

[0067] Assuming the target is attached to a horizontal surface, the drone's altitude is used to determine the target's position. and pod pitch angle Inversely calculate the horizontal distance. Assume the terrain height directly below the drone is... The relative distance can be obtained directly from real-time measurement by airborne altitude-keeping radar (such as millimeter-wave radar or ultrasonic radar), or roughly calculated by barometer.

[0068] The intersection of the target's line of sight and the ground satisfies: (4) in: It is the angle of depression of the line-of-sight vector in the NED system (positive downwards). This represents the horizontal distance.

[0069] Then the horizontal distance: (5) Slope distance: (6) Error analysis: Height error caused by ground undulation This will be directly scaled up to the horizontal distance error:

[0070] When the pitch angle is small (for long-distance following). The difference is significant; even a tiny error in altitude can lead to a huge error in ranging.

[0071] Guidance law design—distance following: Let the current position of the target be The target velocity vector is (Obtained by coordinate difference filtering) The desired relative distance can be calculated using formula (5).

[0072] Desired drone location: (7) If the target speed is very low, you can use the target heading or the historical trajectory direction directly.

[0073] Position error ( (Geometric coordinates provided by BeiDou for the drone) (8) The flight controller converts this error into the desired speed command: (9) Among them: feedforward term This is crucial, enabling drones to move at the target speed in advance, avoiding lag.

[0074] Key points for project implementation: Altitude-fixed radar requires median filtering to suppress clutter.

[0075] When detected When the mutation rate exceeds the threshold (e.g., when flying over an obstacle), the ranging result is immediately marked as unreliable, and the system switches to a backup strategy.

[0076] Mode 2: Motion model filtering method (pure vision servoing).

[0077] This method no longer measures absolute distance; instead, it utilizes image planar errors to drive the drone's movement, keeping the target centered in the frame. Definition The center of the image.

[0078] Control input (miss distance): (10) Speed ​​command generation (simplified PI control law): (11) (12) For forward and backward speed (throttle), there is no direct distance feedback, so one of the following two strategies is used: 1. Fuzzy control of area change rate: If the area A of the target box continuously increases, then move backward; if it decreases, then move forward.

[0079] 2. Fixed cruise speed + area limit: Maintain a baseline forward speed, and only perform acceleration or deceleration corrections when the target frame is too large or too small.

[0080] Motion model enhancement (occlusion prediction): When the target is briefly lost, the filter maintains the inertial output of the UAV's heading angular rate and velocity based on the target image motion state of the previous N frames. The duration of the current state is predicted. Based on project testing experience, a timeout of 0.5 to 1.5 seconds is acceptable; if the timeout exceeds this range, the system will enter recapture mode.

[0081] Velocity feedforward and prediction compensation: Target velocity after EKF filtering Directly inject speed command feedforward channel into flight control, such as Figure 4 As shown, This is the feedforward gain (usually set to 1.0; if the target is highly maneuverable, it can be increased appropriately, but overshoot should be avoided).

[0082] Extended Kalman Filter (EKF) Design State vector definition (12-dimensional):

[0083] in: The target's relative position in the NED coordinate system; : The velocity of the target in the NED coordinate system; : The acceleration of the target in the NED coordinate system; Measurement offset of the pod frame angle; Altitude offset of the altitude-fixed radar (used to compensate for terrain errors).

[0084] Process model (using uniform acceleration CA model): (13) in, It means, until All observations at any given time, for The optimal posterior correction estimate of the state at time step; It means, using The optimal estimate of time is Time prediction.

[0085] State transition matrix as follows:

[0086] in, It is a 3×3 identity matrix. This refers to the model's execution cycle.

[0087] The EKF of this invention supports the following three types of observations, and dynamically switches between them based on data validity. Among them, , and It is observation noise Observation Type 1: Distance observation based on the constant elevation plane assumption. Enabled when the terrain change alarm flag is False. The observation equation is: (14) Among them, actual observed values It was calculated using the aforementioned fixed-height plane assumption method.

[0088] Observation Type 2: Angle observation based on the pod's line of sight. Always enabled (as long as the pod locks onto the target). The observation equation is: (15) The observations are from the frame angles issued by the pod. .

[0089] Observation Type 3: Pixel observation based on image miss rate. Enabled when target recognition confidence is above a threshold. The observation equation is: (16) in, For the camera intrinsic parameter matrix, This is the transformation matrix from the pod to the camera.

[0090] Intelligent switching design: Define the input signal and triggering condition: Terrain change flag :high After filtering, the difference between the preceding and following frames is calculated. Triggered at time m, indicating that the drone is flying over an obstacle or pothole, and the constant altitude plane assumption method fails.

[0091] pod pitch and vibration indicator Pitch angle The angular velocity or amplitude fluctuates abnormally when Five consecutive frames triggering this indicates that the pod is experiencing search-related swaying due to obstruction or target loss, making the line of sight unreliable.

[0092] Target confidence flag Confidence level of the target detection / tracking algorithm for the pod Decrease, when If triggered for 5 consecutive frames, it indicates that the target may be occluded or its features may be degraded, and visual lock-on is about to be lost.

[0093] State machine transition model: To avoid frequent model transitions, a state machine with hysteresis and acknowledgment time is used, defining three core states (chart): NOMINAL (normal mode, i.e., mode one), DEGRADED (degraded mode, i.e., mode two), and LOST (target lost mode). A schematic diagram of the intelligent switching state machine is shown below. Figure 5 As shown.

[0094] The state transition rules are detailed below: 1. From NOMINAL → DEGRADED condition: or

[0095] action: a) Immediately switch the follow mode to the visual deviation loop.

[0096] b) Disable fixed-height plane distance observation in EKF (remove its observation noise covariance) Set to the maximum value to achieve a soft disconnect.

[0097] c) Activate the motion model prediction and hold function and record the current target velocity estimate. and course.

[0098] d) The pod's operating mode remains unchanged; continue attempting to lock on.

[0099] 2. Maintain at DEGRADED Condition: Triggering condition not lifted or However, the timeout period for data loss has not yet expired.

[0100] Behavior: a) If the target confidence level EKF is updated using line-of-sight observations, and the UAV is driven by the miss distance.

[0101] b) If the target confidence level (Brief obstruction): The EKF only performs the prediction step, while the flight control maintains the prediction. Maintain inertia and course for a maximum of = 1.5 seconds.

[0102] 3. From DEGRADED → NOMINAL (Restore) condition: and and target confidence And it remained stable for more than 2.0 seconds.

[0103] action: a) Gradually restore fixed-height horizontal distance observation (will) (Smoothly reduce from the maximum value to the nominal value to achieve soft access).

[0104] b) Switch back to the distance loop in follow mode and re-establish precise distance following.

[0105] c) Eliminate prediction preservation labels.

[0106] 4. From DEGRADED → LOST Condition: Target confidence level Duration exceeding Seconds (including predicted hold time).

[0107] action: a) Switch the flight control command to hover and slowly climb (climb rate 1m / s, up to the maximum of 30m).

[0108] b) The pod enters the spiral search mode (uniform azimuth rotation, sine sweep of pitch).

[0109] c) EKF stops updating, but retains the last valid state.

[0110] 5. From LOST to DEGRADED (recapture) Conditions: The pod re-acquires the target during the search, and the target is confirmed to be the original target based on Re-ID feature matching (matching degree > 0.7, stable for 3 consecutive frames).

[0111] action: a) Immediately switch to DEGRADED state (visual bias loop) and reinitialize EKF using the new observations.

[0112] b) The drone descends to its original following altitude and resumes following, waiting to be further restored to NOMINAL.

[0113] Soft mode switching Achieving "soft switching" through dynamic adjustment of EKF observation noise variance: Traditional hard-switching direct switching control laws are prone to causing abrupt changes in control variables. This invention utilizes the adjustable characteristics of the observation noise covariance of a Kalman filter to achieve a smooth transition at the mathematical level.

[0114] a) In NOMINAL state: Observation noise variance at constant elevation plane distance Set to the nominal value (e.g.) The filter fully trusts this distance information and uses it to accurately correct the target's horizontal position.

[0115] b) Triggering the switch instant: Instead of directly deleting the observation equation in the code, but instead... Instantaneous increase. From the Kalman gain formula. It can be seen that, lead to The observation's contribution to the state update tends to zero, which is equivalent to "silence".

[0116] c) During recovery: The nominal value is smoothly reduced from the maximum value in an exponential decay manner, so that the influence of distance observation gradually increases and the step control command is avoided.

[0117] This mechanism ensures that the same filter instance is always in operation during mode switching, while the weights of the information source change dynamically, achieving truly seamless switching.

[0118] Target speed dead zone handling: When the target is stationary or moving at extremely low speed, the calculated velocity vector direction will randomly change. In this case, the following processing procedure is adopted: 1. EKF output target velocity estimation modulus .

[0119] 2. If Enter dead zone protection: a) Freeze expectation follows azimuth angle Keep the last valid value b) The drone only performs translational corrections at its current position (PID position loop). c) The pod remains locked. 3. If If it continues for more than 1 second, it exits the dead zone and resumes normal feedforward following.

[0120] In summary, the moving target tracking method provided by this invention offers excellent safety, completely avoiding dangerous dives and sudden stops caused by altitude misjudgments when drones fly over buildings, resulting in a stable flight trajectory. It also exhibits good tracking continuity, significantly reducing the probability of target loss through short-term inertial prediction and intelligent reacquisition, achieving seamless transitions after occlusion. Furthermore, it achieves a balance between accuracy and robustness, maintaining precise distance tracking on flat terrain and automatically degrading to stable visual tracking during sudden terrain changes.

[0121] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0122] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that the present invention can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of the present invention.

[0123] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. 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 modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0124] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A method for following a moving target, characterized in that, Applied to unmanned aerial vehicles (UAVs), wherein the UAV is equipped with a laser-free micro-optical pod, an onboard altitude sensor, and a positioning module, the method includes: The system acquires the ground clearance measured by the airborne altitude sensor, the pod pitch angle output by the micro optoelectronic pod, the image miss distance, and the target confidence level. The default operating mode is mode one, which is based on the fixed altitude plane assumption method. It calculates the target horizontal distance using the ground height and the pod pitch angle, and generates a fixed distance follow command based on the target horizontal distance. Mode one is the distance loop mode. The system monitors the rate of change of altitude H and the amplitude of jitter in the pod's pitch angle in real time. When the rate of change of altitude exceeds a first preset threshold or the amplitude of jitter exceeds a second preset threshold, a terrain change flag is generated, switching from mode one to mode two, disabling the distance loop, and activating the motion model prediction hold function. Mode two is a visual deviation loop mode, which drives the UAV's heading and pitch solely based on the image miss distance to keep the target centered in the image. In the second mode, when the target confidence is lower than the third preset threshold, it is determined that the target is temporarily occluded, and the target motion speed estimated by the extended Kalman filter before occlusion is used to maintain the open-loop output of the UAV's speed and heading. When the height change rate recovers to within the first preset threshold, the jitter amplitude recovers to within the second preset threshold, and the target confidence is greater than or equal to the fourth preset threshold and continues for a first preset time, the observation noise covariance of the extended Kalman filter is dynamically adjusted to smoothly switch back to mode one from mode two.

2. The method according to claim 1, characterized in that, The height change rate exceeds the first preset threshold as follows: After the ground clearance is processed by median filtering, the difference between the previous and next frames is calculated, and a trigger is activated when the absolute value of the difference is greater than or equal to 3.0 meters. The jitter amplitude exceeds the second preset threshold as follows: The absolute value of the angular velocity of the pod's pitch angle is greater than or equal to 10° / second and is triggered for 5 consecutive frames; The target confidence level being lower than the third preset threshold is: The target confidence level C is less than 0.4 and is triggered for 5 consecutive frames.

3. The method according to claim 1, characterized in that, By dynamically adjusting the observation noise covariance of the extended Kalman filter To achieve a smooth switchback, the following are included: In Mode 1, the extended Kalman filter uses the nominal value of the observation noise variance. The nominal value is 2.0 m², which is based on the integrated observation of the plane distance at a fixed altitude. At the instant of switching from mode one to mode two, the observation noise covariance is... Setting it to a maximum value makes the Kalman gain of the extended Kalman filter approach zero, which is equivalent to silencing the distance observation; When switching back from mode two to mode one, the observation noise covariance is... The maximum value is smoothly reduced exponentially to the nominal value, causing the influence of the distance observation to gradually increase.

4. The method according to claim 3, characterized in that, The state vector of the extended Kalman filter is 12-dimensional and is defined as follows: in: The relative position of the target in the NED coordinate system; The velocity of the target in the NED coordinate system; The acceleration of the target in the NED coordinate system; Measurement offset for the pod frame angle; This is the altitude offset for the altitude-fixed radar.

5. The method according to claim 4, characterized in that, The extended Kalman filter supports three types of observations and dynamically switches between them based on data validity: Observation type one is distance observation based on the constant-elevation plane assumption, which is activated when the terrain abrupt change flag is False. The observation equation is: Observation type two is an angle observation based on the pod's line of sight, which is always enabled. The observation equation is: Observation type three is pixel observation based on image miss rate, which is activated when the target recognition confidence is higher than a threshold. The observation equation is: in, Represents the camera intrinsic parameter matrix. This represents the transformation matrix from the pod to the camera. , and This indicates observation noise.

6. The method according to claim 1, characterized in that, The duration of the open-loop output is 0.5 to 1.5 seconds; when the target confidence level is lower than the third preset threshold for a duration exceeding the second preset time, the system switches from mode two to target loss mode LOST, controls the drone to hover and slowly climb, and simultaneously controls the pod to enter spiral search mode. The climb rate of the hovering climb is 1 meter / second, and the upper limit of the climb is 30 meters; the second preset time is 2.0 seconds.

7. The method according to claim 6, characterized in that, It also includes target re-identification: When the target is recaptured in the spiral search mode, the original target is confirmed by the target re-identification Re-ID feature matching based on deep learning. When the matching degree is greater than 0.7 and stable for 3 consecutive frames, the LOST mode is switched to the second mode, and the extended Kalman filter is reinitialized using the new observation.

8. The method according to claim 1, characterized in that, It also includes target speed dead zone handling: When the target velocity estimation modulus output by the extended Kalman filter is less than 0.5 m / s, the expected following azimuth angle is frozen, and the last valid value is retained. The UAV only performs translation correction at the current position, and the pod maintains the current locked state. When the target velocity estimation modulus is greater than or equal to 0.5 m / s and lasts for more than 1 second, the dead zone is exited and normal feedforward following is resumed.

9. The method according to claim 1, characterized in that, The formula for calculating the horizontal distance D of the target is: in, This represents the angle of depression of the line-of-sight vector in the NED frame; The method for generating the fixed-distance following instruction is as follows: Let the target's current position be The target velocity vector is The expected relative distance is The desired location of the drone is: The positional error is: The flight controller converts position errors into speed commands: in, Represents geodetic coordinates. For proportional gain, This is a feedforward term.

10. The method according to claim 1, characterized in that, The method is implemented through a node-based software architecture, including: The pod drive node is used to subscribe to the miss distance, frame angle, target category, and confidence level output by the micro optoelectronic pod. A height preprocessing node is used to perform median filtering on the ground clearance to suppress clutter; The target state estimator node is used to perform the extended Kalman filter filtering and the switching logic between mode one and mode two; A guidance control node is used to generate speed commands based on the output of the target state estimator node.

Images