A method for guiding and visually fusing a drone

Abstract

The present application relates to the technical field of guidance and visual fusion, and more particularly to a kind of unmanned aerial vehicle tracking method of guidance and visual fusion, comprising the following steps: target search and detection based on external guidance, receive the target rough three-dimensional coordinates provided by external detection equipment, based on the rough three-dimensional coordinates and unmanned aerial vehicle own position, generate preset tracking waypoint, and control unmanned aerial vehicle to fly to the airspace near target, while controlling airborne gimbal to pre-aim target;Three-dimensional accurate position information resolution based on binocular vision, when the distance between unmanned aerial vehicle and target enters preset threshold, start airborne binocular vision system, obtain target parallax by stereo matching algorithm, and resolve the accurate three-dimensional relative position vector of target relative to unmanned aerial vehicle based on triangulation principle;The present application proposes a kind of unmanned aerial vehicle tracking method of guidance and visual fusion, realizes real-time, accurate and smooth autonomous tracking to dynamic target in the air.

Description

Technical Field

[0001] This invention relates to the field of guidance and vision fusion technology, and more specifically to a method for tracking unmanned aerial vehicles (UAVs) that combines guidance and vision fusion. Background Technology

[0002] With the rapid development of drone technology and its widespread application in security patrols, airspace management, and logistics transportation, the monitoring, identification, and tracking of aerial targets such as "non-cooperative" drones have become key technological challenges in maintaining airspace security and preventing illegal intrusion. Current anti-drone systems mainly employ a "radar detection-electro-optical tracking" model, which uses ground-based radar or TDOA (Time Difference of Arrival) positioning systems to provide a preliminary, rough trajectory of the target, then guides the drone or ground-based electro-optical equipment for optical locking and tracking.

[0003] However, existing technologies have significant shortcomings in achieving fully autonomous, high-precision, and stable tracking: First, traditional methods suffer from poor tracking continuity when the target is performing high-speed, high-maneuverability evasive maneuvers, due to poor coordination between the target identification and tracking modules; second, monocular vision-based ranging methods struggle to provide accurate three-dimensional relative position perception, resulting in insufficient tracking accuracy; third, conventional PID control algorithms have inherent lag, making it difficult to meet the real-time tracking requirements of high-speed maneuvering targets; furthermore, the visual features of weakly textured targets are not obvious, increasing the difficulty of identification and tracking; finally, existing high-precision tracking solutions typically require the addition of expensive sensors such as lidar, significantly increasing system cost and complexity.

[0004] Therefore, there is an urgent need in the field for a system and method that can be integrated into an unmanned aerial vehicle platform to overcome the above challenges in a completely autonomous and self-sufficient manner, provide real-time and accurate absolute distance measurement for high-speed dynamic weakly textured aerial targets, and ultimately achieve stable and efficient three-dimensional tracking. Summary of the Invention

[0005] In view of this, the present invention provides a drone tracking method that integrates guidance and vision, aiming to solve the above-mentioned technical problems.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] Includes the following steps:

[0008] S1: Based on externally guided target search and detection, receive the rough three-dimensional coordinates of the target provided by the external detection device, generate a preset tracking waypoint based on the rough three-dimensional coordinates and the UAV's own position, and control the UAV to fly to the airspace near the target, while controlling the airborne gimbal to pre-aime at the target.

[0009] S2: Based on binocular vision, the system calculates the precise three-dimensional position information. When the distance between the UAV and the target enters the preset threshold, the airborne binocular vision system is activated. The target disparity is obtained through a stereo matching algorithm, and the precise three-dimensional relative position vector of the target relative to the UAV is calculated based on the principle of triangulation.

[0010] S3: Flight control based on target motion prediction; Based on the precise three-dimensional relative position vector, estimate and smooth the target's motion velocity vector, determine the target's motion state based on the velocity vector, and adaptively generate the target's predicted motion direction; Based on the predicted motion direction, preset tracking distance, and tracking altitude, dynamically generate the UAV's desired tracking position; Finally, based on the error between the desired tracking position and the UAV's current position, calculate the UAV's speed control command and the gimbal's angle control command to achieve autonomous target tracking.

[0011] Furthermore, the specific method for generating the preset tracking waypoint in S1 is as follows: calculate the relative vector between the current position of the UAV and the approximate position of the target on the horizontal plane; based on the relative vector and the preset expected tracking distance, generate the preset tracking waypoint behind the target, and the calculation formula is as follows:

[0012]

[0013] in, For the target's rough coordinates, A horizontal relative vector, Let L be its modulus, and L be the preset desired tracking distance.

[0014] Furthermore, in S1, during flight control, a proportional control law is used to generate a speed command. The speed command consists of a position error term and a feedforward term, and is subjected to amplitude limiting processing to ensure that it does not exceed the maximum safe speed of the UAV.

[0015] Furthermore, in step S1, when the system determines that the horizontal distance between the UAV and the target has entered a preset threshold, it activates the target detector based on a deep learning model and uses Kalman filtering to smooth the position of the detection box between consecutive frames in order to provide stable pixel-level coordinate input.

[0016] Furthermore, in step S2, a semi-global matching algorithm is used to perform stereo matching to calculate the disparity map, and a robust statistic is taken as the representative disparity d of the target for the disparity value within the target detection box.

[0017] Furthermore, in step S2, the target depth value Z is calculated using the following formula:

[0018]

[0019] Where B is the baseline length of the stereo camera, and f is the focal length of the camera. It is the calculated disparity value.

[0020] Furthermore, in step S3, the specific method for estimating and smoothing the target's velocity vector is as follows: calculate the instantaneous velocity by the difference between the target's position at the current time and the previous time; and use an exponential moving average filtering algorithm to smooth the instantaneous velocity to obtain a smoothed velocity vector.

[0021] Furthermore, in step S3, the specific method for adaptively generating the predicted motion direction of the target is as follows: the magnitude of the smoothed target horizontal velocity vector is compared with a preset velocity threshold; if the magnitude exceeds the threshold, the target is determined to be in an active motion state, and the current velocity direction is directly used as the predicted motion direction; if the magnitude is lower than the threshold, the target is determined to be in a hovering or low-speed drifting state, and the previously valid motion direction vector is maintained.

[0022] Furthermore, in step S3, the desired tracking position of the UAV is dynamically generated. The specific method is as follows:

[0023]

[0024] in, The current position of the target. The preset tracking distance, The preset tracking height, The position above the target.

[0025] Compared with the prior art, the present invention has the following positive effects:

[0026] By reusing the UAV's existing binocular obstacle avoidance camera and combining it with a semi-global matching algorithm and triangulation principles, high-precision depth perception and 3D positioning of the target are achieved, solving the problem of inaccurate monocular vision ranging and improving distance measurement accuracy to the centimeter level. Through a dynamic expected position generation algorithm, optimal tracking points are constructed behind and above the target's direction of motion, ensuring both optimal viewing angle and maintaining a safe distance. This allows the system to adapt to various target maneuvering modes, including high-speed flight, hovering, and low-speed drift. All perception, computation, and control are performed independently on the UAV, without relying on any ground-based auxiliary equipment or external communication links, truly achieving end-to-end autonomous tracking capability. Attached Figure Description

[0027] Figure 1 The present invention provides a flowchart of a drone tracking algorithm based on guidance and vision fusion.

[0028] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort. Detailed Implementation

[0029] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] This invention aims to solve the problems in existing UAV autonomous tracking technology, such as poor connection between the target recognition module and the tracking module, inaccurate three-dimensional relative position perception, and lag in flight control commands.

[0031] This invention proposes a drone tracking method based on guidance and vision fusion, combined with a control algorithm based on target motion state prediction, referencing... Figure 1 This enables real-time, accurate, and smooth autonomous tracking of dynamic aerial targets.

[0032] The UAV tracking method based on guidance and vision fusion disclosed in this invention includes the following steps:

[0033] Step 1: Externally Guided Target Search and Detection

[0034] First, the system receives the approximate three-dimensional coordinates of the target from external detection equipment (such as a radar and TDOA fusion system), denoted as... This invention designs a hierarchical autonomous approach strategy to guide the UAV to the vicinity of the target airspace (e.g., within 50 meters) and create optimal conditions for visual detection.

[0035] The details are as follows:

[0036] 1. Navigation and Waypoint Generation:

[0037] The drone is based on its current location Calculate the relative vector on the horizontal plane with respect to the approximate position of the target. Its modulus length This refers to the current horizontal distance. To achieve a stable approach at a safe distance (e.g., 50 meters) behind the target, this invention does not fly directly to the target point, but instead generates a temporary, pre-set tracking waypoint. The formula for calculating this waypoint is:

[0038]

[0039] in, This is the preset desired tracking distance (e.g., 50 meters). Essentially, this formula calculates the distance behind the target along the line connecting the drone and the target. Define a point and guide the drone to fly toward this point, so that it is naturally positioned to the side and rear of the target. This positioning strategy is beneficial for subsequent visual tracking.

[0040] 2. Flight control laws:

[0041] The UAV flight control system uses a proportional control law to track the waypoint. The resulting speed commands... It consists of a position error term and a feedforward term, and can be specifically described as follows:

[0042]

[0043] in, This is an adjustable proportional gain coefficient used to map position errors to a velocity command. The calculated velocity command needs to be limited to ensure it does not exceed the maximum safe speed allowed by the UAV platform.

[0044] 3. Enhanced gimbal pre-aiming and visual inspection:

[0045] During flight, the airborne main gimbal dynamically calculates the desired yaw and pitch angles based on the target's approximate coordinates and the UAV's real-time pose (obtained by fusing GPS and IMU data), ensuring that the target is roughly located in the center of the image's field of view before the UAV reaches the visual recognition range.

[0046] When the system determines that the horizontal distance has entered a preset threshold (e.g., 60 meters, slightly larger than 50 meters to allow for margin), it activates a target detector based on a lightweight deep learning model (e.g., YOLOv7-tiny). To improve detection robustness, this invention utilizes Kalman filtering to smooth the bounding box positions between consecutive frames, significantly suppressing detection result jitter caused by image noise and illumination changes, providing stable and accurate pixel-level coordinate input for subsequent binocular ranging.

[0047] Step 2: Solving the precise 3D position information based on binocular vision

[0048] The system activates a forward-facing binocular obstacle avoidance camera and uses a well-adjusted hybrid stereo vision algorithm to calculate the accurate visual depth information of the target.

[0049] After the visual detection module successfully locks onto the target, the system immediately activates the forward-facing binocular vision system and initiates a high-precision three-dimensional relative position calculation process.

[0050] 1. Stereo matching and disparity calculation:

[0051] A stereo camera, precisely calibrated and epipolar corrected, performs stereo matching on the left and right images. This invention preferably employs a semi-global matching (SGM) algorithm, which efficiently and robustly calculates the disparity map for each pixel by aggregating costs across multiple paths. For the target region within the detection box, a robust statistic (such as the median) of its disparity value is taken as the representative disparity of the target. .

[0052] 2. Depth information calculation:

[0053] Based on the principle of triangulation, the depth value of the target The absolute distance between the drone and the target (i.e., the distance between them) is inversely proportional to the parallax d. The core calculation formula is:

[0054]

[0055] in: It is the baseline length of the binocular camera (i.e., the distance between the optical centers of the two cameras), which is a fixed physical parameter. It is the camera's focal length (in pixels), a constant obtained through camera intrinsic parameter calibration. It is the calculated disparity value (in pixels).

[0056] 3. Three-dimensional coordinate transformation:

[0057] In obtaining depth information and image coordinates Then, using the camera pinhole model, the target is transformed from two-dimensional image coordinates to three-dimensional coordinates in the left camera coordinate system. :

[0058]

[0059] in, These are the coordinates of the principal point of the image. It's the camera. and The focal length in the direction of movement, and these parameters together constitute the camera's intrinsic parameter matrix. It is obtained through pre-calibration.

[0060] Subsequently, the extrinsic parameter matrix (rotation matrix) between the pre-calibrated camera and the drone body is used. Translation vector Transform the target point from the camera coordinate system to the UAV body coordinate system:

[0061]

[0062] At this point, the system has obtained the precise three-dimensional relative position vector of the target with respect to the center of the UAV body. This vector is the most direct and critical input for subsequent tracking and control decisions.

[0063] Step 3: Flight control based on target motion prediction

[0064] After acquiring the target's precise 3D position information, the system calculates the target's 3D relative position vector in the UAV's body coordinate system. Subsequently, the predictive control algorithm is activated to calculate the UAV's desired velocity and the gimbal's yaw and pitch directions. The core idea of ​​this algorithm is not only to eliminate the "current" position error but also to predict the target's "future" position and respond in advance, thereby overcoming system latency.

[0065] The specific implementation method is as follows:

[0066] First, the system performs accurate estimation and filtering of the target's velocity state. The system records the current moment. target location and the previous moment Location The instantaneous velocity vector of the target is obtained through differential calculation:

[0067]

[0068] in, This represents the time interval between two samplings. To eliminate sudden speed changes caused by measurement noise, an exponential moving average filtering algorithm is used to smooth the speed:

[0069]

[0070] Among them, the filter coefficients With a value of 0.3, this weighted averaging strategy can both quickly respond to real speed changes and effectively suppress measurement noise interference.

[0071] Based on the smoothed horizontal velocity vector The system uses a set speed threshold The system intelligently determines the target's motion state. When the target's horizontal speed exceeds a threshold, the system determines that the target is in an active motion state and directly uses the current speed direction as the predicted motion direction. When the speed is below the threshold, the system determines that the target is in a hovering or low-speed drifting state. To avoid unnecessary control adjustments due to minor vibrations, the system maintains the use of the previously valid motion direction vector. This state-adaptive mechanism ensures that the UAV can accurately understand the target's macroscopic motion intentions.

[0072] After accurately predicting the target's direction of motion, the algorithm dynamically generates an optimal desired tracking position. This location is not simply aligned with the target's current location, but is based on a preset tracking distance. and tracking height Construct a safe accompanying occupancy point behind and above the target's predicted direction of motion:

[0073]

[0074] in, The current position of the target. The preset tracking distance, The preset tracking height, The position above the target.

[0075] This dynamic positioning strategy ensures that the UAV is always in an advantageous position that allows it to clearly observe the target while maintaining a safe distance, providing the best pose basis for subsequent tracking and control.

[0076] During the control command resolution phase, the system first calculates the three-dimensional error vector between the desired tracking position and the UAV's current position:

[0077]

[0078] The position error is converted into a speed command using a proportional control law:

[0079]

[0080] in For horizontal position error, For height error, and This is an adjustable proportional gain coefficient.

[0081] Meanwhile, to keep the visual sensor consistently and stably aligned with the target, the system calculates the relative position vector from the UAV to the target in real time. Based on this, the required yaw and pitch angles for the gimbal are calculated:

[0082]

[0083] Output angle command and Directly driving the gimbal movement ensures that the target is always located in the center of the image field of view, providing continuous and stable observation conditions for visual tracking.

[0084] Beneficial effects

[0085] Precise three-dimensional relative positioning was achieved: by reusing the original binocular obstacle avoidance camera of the UAV, combined with the semi-global matching algorithm and the triangulation principle, high-precision depth perception and three-dimensional positioning of the target were achieved, solving the problem of inaccurate monocular visual ranging and improving the distance measurement accuracy to the centimeter level.

[0086] The stability and adaptability of target tracking have been improved: by using a dynamic expected position generation algorithm, the optimal tracking points are constructed behind and above the target's direction of motion, which ensures both the optimal observation angle and a safe distance, enabling the system to adapt to various maneuvering modes of the target, including high-speed flight, hovering and low-speed drift.

[0087] It has achieved a fully autonomous onboard tracking solution: all sensing, computing and control are completed independently on the UAV, without relying on any ground auxiliary equipment or external communication links, truly realizing end-to-end autonomous tracking capability.

[0088] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

[0089] The various embodiments described in this specification are presented in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A drone tracking method that integrates guidance and vision, characterized in that, Includes the following steps: S1: Based on externally guided target search and detection, receive the rough three-dimensional coordinates of the target provided by the external detection device, generate a preset tracking waypoint based on the rough three-dimensional coordinates and the UAV's own position, and control the UAV to fly to the airspace near the target, while controlling the airborne gimbal to pre-aime at the target. S2: Based on binocular vision, the system calculates the precise three-dimensional position information. When the distance between the UAV and the target enters the preset threshold, the airborne binocular vision system is activated. The target disparity is obtained through a stereo matching algorithm, and the precise three-dimensional relative position vector of the target relative to the UAV is calculated based on the principle of triangulation. In step S2, a semi-global matching algorithm is used to perform stereo matching to calculate the disparity map, and a robust statistic is taken as the representative disparity d of the target for the disparity value within the target detection box. In step S2, the target depth value Z is calculated using the following formula: ； Where B is the baseline length of the stereo camera, and f is the focal length of the camera. It is the calculated disparity value; S3: Flight control based on target motion prediction; Based on the precise three-dimensional relative position vector, estimate and smooth the target's motion velocity vector, determine the target's motion state based on the velocity vector, and adaptively generate the target's predicted motion direction; Based on the predicted motion direction, preset tracking distance, and tracking altitude, dynamically generate the UAV's desired tracking position; Finally, based on the error between the desired tracking position and the UAV's current position, calculate the UAV's speed control command and the gimbal's angle control command to achieve autonomous target tracking.

2. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, The specific method for generating the preset tracking waypoint in S1 is as follows: Calculate the relative vector between the current position of the UAV and the approximate position of the target on the horizontal plane; based on the relative vector and the preset expected tracking distance, generate the preset tracking waypoint behind the target, and the calculation formula is as follows: ； in, For the target's rough coordinates, A horizontal relative vector, Let L be its modulus, and L be the preset desired tracking distance.

3. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, In S1, during flight control, a proportional control law is used to generate a speed command. The speed command consists of a position error term and a feedforward term, and is subjected to amplitude limiting processing to ensure that it does not exceed the maximum safe speed of the UAV.

4. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, In step S1, when the system determines that the horizontal distance between the UAV and the target has entered a preset threshold, it starts the target detector based on a deep learning model and uses Kalman filtering to smooth the position of the detection box between consecutive frames in order to provide stable pixel-level coordinate input.

5. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, In step S3, the specific method for estimating and smoothing the target's velocity vector is as follows: calculate the instantaneous velocity by the difference between the target's position at the current time and the previous time; and use an exponential moving average filtering algorithm to smooth the instantaneous velocity to obtain a smoothed velocity vector.

6. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, In step S3, the specific method for adaptively generating the predicted motion direction of the target is as follows: the magnitude of the smoothed target horizontal velocity vector is compared with a preset velocity threshold; if the magnitude exceeds the threshold, the target is determined to be in an active motion state, and the current velocity direction is directly used as the predicted motion direction; if the magnitude is lower than the threshold, the target is determined to be in a hovering or low-speed drifting state, and the previously valid motion direction vector is maintained.

7. The drone tracking method based on guidance and vision fusion according to claim 1, characterized in that, In step S3, the desired tracking position of the UAV is dynamically generated. The specific method is as follows: ； in, The current position of the target. The preset tracking distance, The preset tracking height, The position above the target.

Images