A long-distance small target ranging method and system based on master-slave guidance and composite angle intersection

Abstract

This invention provides a method and system for ranging small targets at long distances based on master-slave guidance and composite angular intersection. A relative spatial coordinate system is constructed using a fixed wide-angle camera as the origin, and a master-slave architecture with one fixed and one moving camera is employed for target guidance and tracking. Based on this, distortion correction and zoom axis drift compensation are performed on the heterogeneous cameras. Temporal and spatial forced alignment is achieved by combining hardware triggering and Kalman filtering. The Denavit-Hartenberg (DH) kinematic model is used to calculate the dynamic projection baseline between the master and slave cameras in real time. Finally, a dimension-reduced intersection strategy is used to achieve high-precision ranging of the target. This invention requires only one initial relative calibration and can adapt to the continuous dynamic movement of telephoto zoom and gimbal, achieving high-precision relative three-dimensional positioning of small, distant targets without relying on external absolute positioning equipment.

Description

Technical Field

[0001] This invention relates to the field of three-dimensional positioning technology for long-distance targets, and in particular to a method and system for ranging small targets at long distances based on master-slave guidance and compound angle intersection. Background Technology

[0002] In existing technologies, 3D localization of distant targets often employs a binocular stereo vision ranging architecture. This method typically relies on two cameras with fixed relative positions, using stereo correction and a semi-global block matching (SGBM) algorithm to obtain a disparity map for depth calculation. However, for large-scale, long-distance spatial monitoring, existing technologies suffer from the following significant problems and limitations:

[0003] 1) Traditional feature matching algorithms fail in scenarios involving small, distant targets: In long-range monitoring (e.g., over 200 meters), small infrared or visible light targets often occupy only a few pixels and lack surface texture gradients, causing traditional image feature matching-based SGBM algorithms to completely fail. If a long-focus binocular architecture is simply used, the extremely narrow field of view makes it very easy for the target to miss.

[0004] 2) Heterogeneous cameras have large differences in optical parameters, making high-precision coordination difficult: In order to balance large field-of-view search and long-distance detail observation, the system often needs to use heterogeneous cameras with a combination of wide-angle and telephoto lenses. However, there are huge differences in the optical characteristics of the two. When zooming at the telephoto end, the optical principal point will drift irregularly, causing the system to frequently generate errors and making it difficult to maintain the calibration state.

[0005] 3) Dynamic baselines are difficult to calculate in real time, introducing significant geometric errors: When a dual-gimbal or master-slave gimbal architecture is introduced, the continuous movement and eccentricity of the gimbals cause the spatial position of the camera's optical center to change continuously, resulting in dynamic changes in the binocular baseline vector. Existing methods typically assume a fixed baseline, which cannot adapt to such continuous motion scenarios, thus introducing huge intersection ranging errors.

[0006] 4) Hardware and data are out of sync in time and space, affecting rendezvous accuracy: The servo feedback of the gimbal's mechanical angle has a communication lag, making it impossible to accurately align with the moment of image exposure. This time deviation between the angle observation value and the actual optical axis pointing will directly lead to inaccurate spatial calculations during dynamic target tracking. Summary of the Invention

[0007] In view of this, the purpose of the present invention is to provide a method and system for ranging small targets at long distances based on master-slave guidance and compound angular intersection. It only requires one initial relative calibration and can adapt to the continuous dynamic movement of telephoto zoom and gimbal. It can achieve high-precision relative three-dimensional positioning of small targets at long distances without relying on external absolute positioning equipment.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: a long-range small target ranging method based on master-slave guidance and compound angle intersection, comprising the following steps:

[0009] Step S1: Use the first image acquisition device established at the origin of the relative coordinate system to detect the airspace, perform distortion correction on the image and extract the target sub-pixel centroid, back-calculate the target observation yaw angle, and combine the prior distance to calculate the guide angle, send a rotation command to the second image acquisition device to guide its rotation to ensure that the target falls into the telephoto field of view.

[0010] Step S2: After the second image acquisition device captures the target, the target pixel residual is calculated by the visual algorithm, and the second image acquisition device is driven to rotate to center the target and form a visual closed loop; when the second image acquisition device zooms, the optical principal point offset is compensated in real time and the camera intrinsic parameters are updated to complete the sub-pixel visual miss amount calculation of the target.

[0011] Step S3: Using the origin of the relative spatial coordinate system as a reference, obtain the initial physical baseline vector of the base of the second image acquisition device; establish the DH coordinate system, and calculate the transient relative coordinates of the second image acquisition device based on the mechanical yaw angle, pitch angle and camera eccentricity of the second image acquisition device, and then extract the dynamic projection baseline and baseline tilt angle of the horizontal plane to complete the dynamic baseline compensation.

[0012] Step S4: Timestamp align the heterogeneous data of the first image acquisition device and the second image acquisition device; fuse the transient mechanical yaw angle and pitch angle of the second image acquisition device with the corresponding sub-pixel residual angle of the image to obtain high-precision composite yaw angle and high-precision composite pitch angle.

[0013] Step S5: Substitute the dynamic projection baseline and the high-precision composite yaw angle into the two-dimensional plane sine intersection equation to solve for the two-dimensional horizontal relative distance of the target; finally, based on the horizontal relative distance and the high-precision composite pitch angle, perform spatial vertical projection extension to calculate the relative altitude of the target and complete the high-precision three-dimensional positioning of the target.

[0014] In a preferred embodiment, in step S1, the first image acquisition device extracts the target sub-pixel centroid and inversely calculates the observed yaw angle. Specifically, it includes:

[0015] The original wide-angle image from the first image acquisition device is acquired. Using a pre-calibrated camera intrinsic parameter matrix and distortion coefficients, the original wide-angle image undergoes distortion correction processing to eliminate radial and tangential distortion. On the ideal imaging plane after distortion correction, a sub-pixel feature extraction algorithm is used to obtain the stable centroid coordinates (x0, y0) of the target. Based on the distortion-corrected centroid coordinates and the intrinsic parameters of the detection device, the initial observation yaw angle of the target relative to the baseline direction on the horizontal projection plane is calculated. The solution formula is:

[0016]

[0017] Where x0 is the x-axis coordinate of the target centroid after distortion correction, u1 is the x-coordinate of the principal point of the probe camera, and f x1 To detect the focal length of the camera in the x-direction.

[0018] In a preferred embodiment, step S1, the prior distance estimation and the field-of-view encompassing blind guidance strategy specifically includes:

[0019] Set the system's prior depth Lp and maximum predicted depth deviation ΔL tol Based on the prior depth, the relative yaw angle of the detection equipment to the observation, and the initial relative physical baseline, the guidance command angle θ of the tracking gimbal is calculated. cmd The dynamic parallax tolerance limit is calculated based on the maximum estimated depth deviation and converted into a target field-of-view control command for the tracking telephoto camera. This ensures that even if the target's actual depth deviates from the prior depth during guidance, the target will still fall completely within the dynamic field of view of the tracking camera. The adaptive formulas for the guidance command angle and field of view angle are as follows:

[0020]

[0021]

[0022] in, To detect the camera's relative observation yaw angle, D0 is the initial relative physical baseline, and L... p For the set prior depth, FOV2 represents the maximum estimated depth deviation, φ represents the target field of view of the tracking telephoto camera, and φ represents the safety margin reserved for the system's mechanical control.

[0023] In a preferred embodiment, step S3, the dynamic projection baseline and tilt angle calculation based on the DH link matrix, specifically includes:

[0024] Using the tracking gimbal base as the base coordinate system, a DH link model is constructed, defining three link coordinate systems corresponding to gimbal rotation axis I (horizontal rotation), rotation axis II (tilt rotation), and the camera coordinate system. The DH link transformation matrix formula is as follows:

[0025]

[0026] When expanded, it appears as follows:

[0027]

[0028] in Let represent the homogeneous transformation matrix of the i-th link coordinate system relative to the (i-1)-th link coordinate system, where Rot represents the rotation transformation operator and Trans represents the translation transformation operator. This represents the X-axis of the coordinate system of the (i-1)th link. Represents the Z-axis of the coordinate system of the i-th link; Let be the rotation angle about the x-axis. Let x be the translation along the x-axis. Let be the rotation angle about the z-axis. The translation is along the z-axis; the homogeneous transformation matrix of the camera relative to the base coordinate system is obtained by multiplying the transformation matrices of each link. Extract the homogeneous transformation matrix The translation components are used to obtain the local three-dimensional offset (ΔXDH, ΔYDH, ΔZDH) of the tracking device's optical center relative to its own base.

[0029] Let the coordinates of the first image acquisition device be (0,0,0), and pre-calibrate the initial physical baseline of the base of the second image acquisition device relative to the origin as D0; through spatial translation transformation, calculate the transient relative projection coordinates (Xr,Yr) of the optical center of the tracking camera in the relative spatial coordinate system, and its spatial transformation equation is:

[0030] Xr=D0+ΔXDH

[0031] Yr=ΔYDH

[0032] Furthermore, the dynamic projection baseline Dh and baseline inclination angle γ of the horizontal plane in the relative spatial coordinate system are calculated, and the calculation formula is updated as follows:

[0033] Dh=Xr2+Yr2

[0034] γ=arctan2(Yr,Xr).

[0035] In a preferred embodiment, step S2, the intrinsic parameter update, optical axis principal point offset lookup table (LUT) compensation, and sub-pixel visual off-target calculation during the zoom process, specifically includes:

[0036] Beforehand, establish the relationship between zoom ratio r and optical principal point offset through camera calibration. The correspondence table of intrinsic parameter matrix K; when the tracking camera zoom is detected, the current zoom ratio r is obtained, the corresponding principal point offset is queried from the LUT, and the principal point coordinates after compensation are ( 2+Δ , 2+Δ Simultaneously, based on the relationship between zoom magnification and field of view, the focal length parameter in the intrinsic parameter matrix is ​​updated in real time. The relationship between field of view and focal length is as follows:

[0037]

[0038]

[0039] in, , The horizontal and vertical field of view before zooming. , These are the horizontal and vertical field of view angles after zooming, where W and H are the width and height of the phase plane, respectively. , Let u2 and v2 be the updated focal lengths in the x and y directions, respectively, and u2 and v2 be the principal point coordinates of the tracking camera before zooming. The updated intrinsic parameter matrix K is:

[0040] .

[0041] In a preferred embodiment, step S2, the sub-pixel visual off-target calculation, specifically includes:

[0042] Based on image processing algorithms, high-precision relative pixel positions of the target on the phase plane are extracted to obtain the sub-pixel centroid coordinates (u). target ,v target ), calculate the horizontal subpixel residual Ru and vertical subpixel residual Rv of the target relative to the current dynamic principal point:

[0043]

[0044]

[0045] Subsequently, by combining the updated focal length parameters fx and fy, the subpixel residuals in the image domain are transformed into horizontal subpixel compensation angles Δθres and vertical subpixel compensation angles Δϕres in the spatial angle domain:

[0046]

[0047]

[0048] The Δθres and Δϕres are used as high-precision visual feedback quantities to be fused and compensated with the transient mechanical yaw and pitch angles of the tracking gimbal, so as to support subsequent high-precision composite angular rendezvous and high-precision target calculation.

[0049] In a preferred embodiment, step S4, hardware-level synchronization of heterogeneous data timestamps and alignment with Kalman filter interpolation, specifically includes:

[0050] A hardware-triggered synchronization method is adopted, using a synchronization trigger signal to control two devices to acquire images simultaneously, ensuring consistent timestamps in the original data. When timestamp discrepancies occur, Kalman filtering is used for interpolation alignment. The Kalman filter state equation and observation equation are as follows:

[0051]

[0052]

[0053] in, This is a state vector, containing timestamp offset and data change rate; Let A be the state vector at time k-1; let A be the state transition matrix; and let B be the control matrix. To control the quantity, For process noise, Let H be the observation vector and H be the observation matrix. To observe noise, timestamp deviation is estimated using Kalman filtering, and the deviation data is interpolated to correct the time alignment of heterogeneous data.

[0054] In a preferred embodiment, step S4 involves fusing the tracking gimbal mechanical angle and the pixel residual angle to obtain a high-precision composite yaw angle and a high-precision composite pitch angle, specifically including:

[0055] Obtain the transient mechanical yaw angle θ2_mech and transient mechanical pitch angle ϕ2_mech of the second imaging device after timestamp alignment; perform algebraic fusion of the transient mechanical angles with the horizontal subpixel compensation angle Δθres and vertical subpixel compensation angle Δϕres calculated in step S2 to calculate the high-precision composite yaw angle θ2_comp and high-precision composite pitch angle ϕ2_comp of the second imaging device, respectively. The compensation equation is:

[0056]

[0057]

[0058] Simultaneously, the high-precision observation yaw angle θ of the first imaging device after distortion correction and sub-pixel extraction is obtained. 1_comp ; the above θ 1_comp With θ 2_compThe system is uniformly converted to the triangle interior angle system based on the dynamic projection baseline Dh.

[0059] In a preferred embodiment, step S5 involves substituting the dynamic projection baseline and the high-precision composite yaw angle into the two-dimensional plane sine intersection equation, and calculating the target's relative altitude based on the horizontal relative distance and the high-precision composite pitch angle. Specifically, this includes:

[0060] Using the dynamic projection baseline Dh obtained in step S3 and the high-precision composite yaw angle, the two-dimensional horizontal relative distance Lh of the target in the horizontal plane relative to the optical center of the first imaging device is calculated. The sinusoidal intersection solution equation is:

[0061]

[0062] Where, θ 2_comp θ represents the high-precision composite yaw angle of the second imaging device. 1_comp L represents the high-precision observed yaw angle of the first imaging device after distortion correction and sub-pixel extraction. h >0 indicates that the observed rays converge and intersect in front of the detection system, the distance data is valid and output; L h A value less than 0 indicates that the observed ray is diverging in space. The system identifies the current ranging result as a false alarm and executes an interception, triggering the wide-area early warning module to reacquire the target.

[0063] Subsequently, the transient optical center elevation information H of the tracking device in the relative spatial coordinate system is acquired. d Combined with the calculated two-dimensional horizontal relative distance L of the target h and the high-precision composite pitch angle ϕ 2_comp The trigonometric leveling is performed based on the spatial vertical projection extension relationship; the relative altitude H of the target is... target The solution model is as follows:

[0064]

[0065] Among them, the high-precision composite pitch angle ϕ 2_comp The transient mechanical pitch angle and the vertical subpixel compensation angle of the second tracking device are algebraic sums; the transient optical center elevation information H d The relative three-dimensional coordinates are extracted from the forward kinematics output of the DH model in step S3 to dynamically compensate for the mechanical eccentricity error introduced by the pitch motion of the gimbal on its optical center elevation reference.

[0066] This invention also provides a long-range small target relative ranging system based on master-slave guidance and compound angle intersection, which, when executed, is a long-range small target ranging method based on master-slave guidance and compound angle intersection, comprising:

[0067] Master-slave camera unit: includes a fixed wide-angle detection camera and a telephoto tracking gimbal camera; wherein, the optical center of the wide-angle detection camera is used as the origin of the relative spatial coordinate system, and is used for wide-area detection and extraction of the target sub-pixel centroid; the telephoto tracking gimbal camera is used to receive guidance, track the target and perform zoom operation;

[0068] Wide-area detection and guidance module: Connects to the master and slave camera units, used to receive the centroid data of the wide-angle detection camera, back-calculate the initial observation yaw angle of the target in the relative coordinate system, combine with the prior distance to calculate the guidance angle, and send rotation commands to control the rotation of the telephoto tracking gimbal to ensure that the target falls into the telephoto field of view;

[0069] Visual loop closure and zoom update module: Connects to the telephoto tracking gimbal camera, used to calculate the target pixel residual, drive the gimbal to rotate so that the target is centered to form a visual loop; at the same time, during zooming, it compensates for the optical principal point offset by looking up the table through LUT and updates the camera intrinsic parameters in real time.

[0070] DH Dynamic Baseline Compensation Module: Connects to the telephoto tracking gimbal camera and is used to establish the DH kinematic linkage model. Based on the gimbal mechanical angle and camera eccentricity, it calculates the transient three-dimensional relative coordinates of the optical center of the telephoto tracking gimbal camera relative to the origin of the wide-angle detection camera and extracts the dynamic projection baseline and baseline tilt angle of the horizontal plane.

[0071] Time alignment module: Connects the master and slave camera units and is used to align the timestamps of heterogeneous data from the two image acquisition devices. It uses a combination of hardware-triggered synchronization and Kalman filter interpolation to eliminate time deviations in mechanical feedback and image exposure, ensuring data time consistency.

[0072] Precision rendezvous and ranging module: Connects the time alignment module and the DH dynamic baseline compensation module. It is used to fuse the tracking gimbal mechanical angle and pixel-level compensation angle to obtain high-precision composite yaw angle and pitch angle. Substitute them into the plane sine rendezvous equation to solve the two-dimensional horizontal relative distance of the target to the wide-angle detection camera, and further calculate the relative height of the target based on the relative elevation difference.

[0073] Compared with the prior art, the present invention has the following beneficial effects:

[0074] 1. Differentiated compensation for heterogeneous cameras improves angle measurement accuracy: To address distortion in wide-angle lenses and zoom axis drift in telephoto lenses, inverse transformation for distortion correction and pixel-level physical parameter compensation (LUT) are employed respectively, ensuring sub-pixel accuracy in calculating the target azimuth and pitch angles in the relative coordinate system.

[0075] 2. Spatiotemporal forced alignment ensures data synchronization: Hardware global triggering combined with Kalman filtering forward inference eliminates the time asynchrony between image exposure and gimbal angle feedback, providing an accurate time reference for dynamic target tracking.

[0076] 3. Real-time dynamic baseline calculation using DH kinematic model: A kinematic linkage model between master and slave devices is established using Denavit-Hartenberg parameters. The three-dimensional offset of the tracking camera's optical center relative to the origin of the probe camera is obtained in real time through forward kinematics. The horizontal projection baseline and baseline tilt angle are dynamically calculated, thus solving the geometric error caused by the gimbal movement.

[0077] 4. Dimensional reduction intersection strategy balances real-time performance and robustness: The 3D intersection problem is decomposed into horizontal plane sinusoidal intersection and vertical plane projection solution based on relative coordinate system. The computation is small and no image texture features are required. It has strong robustness in extreme scenarios with distant and weak targets.

[0078] 5. Open-loop and closed-loop coordinated control balances search efficiency and tracking accuracy: Open-loop scanning is guided by prior distance and parallax tolerance to ensure that the target falls into the telephoto field of view quickly; closed-loop tracking is based on real-time calculated sub-pixel miss distance for precise pointing, realizing the unification of wide-area surveillance and high-precision ranging. Attached Figure Description

[0079] Figure 1 This is a schematic diagram of the system architecture according to an embodiment of the present invention;

[0080] Figure 2 This is a schematic diagram illustrating the definition of the DH coordinate system for the gimbal in an embodiment of the present invention;

[0081] Figure 3 This is a schematic diagram of the geometric relationship of the intersection of horizontal projection planes in an embodiment of the present invention;

[0082] Figure 4 This is a flowchart illustrating the overall method of an embodiment of the present invention. Detailed Implementation

[0083] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0084] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of this application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains.

[0085] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations according to this application; as used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise; furthermore, it should be understood that when the terms “comprising” and / or “including” are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0086] A ranging method for small targets at long range based on master-slave guidance and compound angle intersection, referencing Figure 1-4 This includes the following steps:

[0087] Step S1: Use the first image acquisition device—a fixed wide-angle camera established at the origin of the relative coordinate system—to detect the airspace, correct the distortion of the image and extract the target sub-pixel centroid, infer the target observation yaw angle, and combine the prior distance to calculate the guide angle. Send a rotation command to the second image acquisition device—a telephoto tracking gimbal camera—to guide its rotation and ensure that the target falls into the telephoto field of view.

[0088] Step S2: After the tracking gimbal camera captures the target, the target pixel residual is calculated through a visual algorithm, and the gimbal is driven to rotate to center the target and form a visual closed loop. When the tracking camera zooms, the optical principal point offset is compensated in real time and the camera intrinsic parameters are updated to complete the sub-pixel visual miss distance calculation of the target.

[0089] Step S3: Using the origin of the relative spatial coordinate system as a reference, obtain the initial physical baseline vector of the base of the second image acquisition device; establish the DH coordinate system, calculate the transient relative coordinates of the tracking gimbal based on the mechanical yaw angle, pitch angle and camera eccentricity of the tracking device, and then extract the dynamic projection baseline and baseline tilt angle of the horizontal plane to complete the dynamic baseline compensation.

[0090] Step S4: Timestamp align the heterogeneous data from the wide-angle detection camera and the tracking gimbal camera; fuse the transient mechanical yaw angle and pitch angle of the tracking device with the corresponding sub-pixel residual angle of the image to obtain high-precision composite yaw angle and high-precision composite pitch angle.

[0091] Step S5: Substitute the dynamic projection baseline and the high-precision composite yaw angle into the two-dimensional plane sine intersection equation to solve for the two-dimensional horizontal relative distance of the target; finally, based on the horizontal relative distance and the high-precision composite pitch angle, perform spatial vertical projection extension to calculate the relative altitude of the target and complete the high-precision three-dimensional positioning of the target.

[0092] In step S1, the first image acquisition device extracts the target sub-pixel centroid and inversely calculates the observed yaw angle θ. 1_raw Specifically, it includes:

[0093] The original wide-angle image from the first image acquisition device is acquired. Using a pre-calibrated camera intrinsic parameter matrix and distortion coefficients, the original wide-angle image undergoes distortion correction processing to eliminate radial and tangential distortion. On the ideal imaging plane after distortion correction, a sub-pixel feature extraction algorithm is used to obtain the stable centroid coordinates (x0, y0) of the target. Based on the distortion-corrected centroid coordinates and the intrinsic parameters of the detection device, the initial observation yaw angle of the target relative to the baseline direction on the horizontal projection plane is calculated. The solution formula is:

[0094]

[0095] Where x0 is the x-axis coordinate of the target centroid after distortion correction, u1 is the x-coordinate of the principal point of the probe camera, and f x1 To detect the focal length of the camera in the x-direction.

[0096] In step S1, the "prior distance prediction + field of view inclusive blind guidance strategy" specifically includes:

[0097] Set the system's prior depth Lp and maximum predicted depth deviation ΔL tol Based on the prior depth, the relative yaw angle of the detection equipment to the observation, and the initial relative physical baseline, the guidance command angle θ of the tracking gimbal is calculated. cmd The dynamic parallax tolerance limit is calculated based on the maximum estimated depth deviation and converted into a target field-of-view control command for the tracking telephoto camera. This ensures that even if the target's actual depth deviates from the prior depth during guidance, the target will still fall completely within the dynamic field of view of the tracking camera. The adaptive formulas for the guidance command angle and field of view angle are as follows:

[0098]

[0099]

[0100] in, To detect the camera's relative observation yaw angle, D0 is the initial relative physical baseline, and L... p For the set prior depth, For the maximum estimated depth deviation, FOV2 is the target field of view of the tracking telephoto camera, and φ is the safety margin reserved for the mechanical control of the system.

[0101] Step S3, the dynamic projection baseline and tilt angle calculation based on the DH link matrix, specifically includes:

[0102] Using the tracking gimbal base as the base coordinate system, a DH link model is constructed, defining three link coordinate systems corresponding to gimbal rotation axis I (horizontal rotation), rotation axis II (tilt rotation), and the camera coordinate system, respectively. The DH link transformation matrix formula is as follows:

[0103]

[0104] When expanded, it appears as follows:

[0105]

[0106] in, Let represent the homogeneous transformation matrix of the i-th link coordinate system relative to the (i-1)-th link coordinate system, where Rot represents the rotation transformation operator and Trans represents the translation transformation operator. Let X represent the x-axis of the coordinate system of the (i-1)th link. Represents the Z-axis of the coordinate system of the i-th link; Let be the rotation angle about the x-axis. Let x be the translation along the x-axis. It is the rotation angle (yaw / pitch angle) about the z-axis. The translation along the z-axis (camera eccentricity) is given; the homogeneous transformation matrix of the camera relative to the base coordinate system is obtained by multiplying the transformation matrices of each link. Extract the homogeneous transformation matrix The translation components are used to obtain the local three-dimensional offset (ΔXDH, ΔYDH, ΔZDH) of the tracking device's optical center relative to its own base.

[0107] Let the coordinates of the first image acquisition device (relative to the origin of the coordinate system) be (0,0,0). The initial physical baseline of the second image acquisition device's base relative to the origin is pre-calibrated as D0 (assuming the two device bases are installed collinearly along the X-axis on a horizontal plane). Through spatial translation transformation, the transient relative projection coordinates (Xr, Yr) of the tracking camera's optical center in the relative spatial coordinate system are calculated. The spatial transformation equation is:

[0108] Xr=D0+ΔXDH

[0109] Yr=ΔYDH

[0110] Furthermore, the dynamic projection baseline Dh and baseline inclination angle γ of the horizontal plane in the relative spatial coordinate system are calculated, and the calculation formula is updated as follows:

[0111] Dh=Xr2+Yr2

[0112] γ=arctan2(Yr,Xr)

[0113] In step S2, the intrinsic parameter update, optical axis principal point offset lookup table (LUT) compensation, and sub-pixel visual off-target calculation during the zoom process specifically include:

[0114] Beforehand, establish the relationship between zoom ratio r and optical principal point offset through camera calibration. The mapping table (LUT) for the intrinsic parameter matrix K; when the tracking camera zoom is detected, the current zoom ratio r is obtained, and the corresponding principal point offset is retrieved from the LUT. The compensated principal point coordinates are ( 0+Δ , 0+Δ Simultaneously, based on the relationship between zoom magnification and field of view, the focal length parameter in the intrinsic parameter matrix is ​​updated in real time. The relationship between field of view and focal length is as follows:

[0115]

[0116]

[0117] in, , The horizontal and vertical field of view are before zooming, and W and H are the width and height of the phase plane, respectively. , Here, u1 and v2 are the updated focal lengths in the x and y directions, respectively, and the principal point coordinates of the tracking camera before zooming are u2 and v2. The updated intrinsic parameter matrix is:

[0118]

[0119] In step S2, the sub-pixel visual off-target calculation specifically includes:

[0120] Based on image processing algorithms, the high-precision relative pixel position of the target on the phase plane is extracted, and the sub-pixel centroid coordinates (utarget, vtarget) of the target are obtained. The horizontal sub-pixel residual Ru and the vertical sub-pixel residual Rv of the target relative to the current dynamic principal point are calculated.

[0121]

[0122]

[0123] Subsequently, combined with the updated focal length parameter f x2 with f y2 The subpixel residuals in the image domain are transformed into horizontal subpixel compensation angles Δθres and vertical subpixel compensation angles Δϕres in the spatial angle domain:

[0124]

[0125]

[0126] The Δθres and Δϕres are used as high-precision visual feedback quantities to be fused and compensated with the transient mechanical yaw and pitch angles of the tracking gimbal, so as to support subsequent high-precision composite angular rendezvous and high-precision target calculation.

[0127] In step S4, the hardware-level synchronization and Kalman filter interpolation alignment of heterogeneous data timestamps specifically include:

[0128] A hardware-triggered synchronization method is adopted, using a synchronization trigger signal to control two devices to acquire images simultaneously, ensuring consistent timestamps in the original data. When timestamp discrepancies occur, Kalman filtering is used for interpolation alignment. The Kalman filter state equation and observation equation are as follows:

[0129]

[0130]

[0131] in, It is a state vector (including timestamp deviation and data change rate). Let A be the state vector at time k-1; let A be the state transition matrix; and let B be the control matrix. To control the quantity, For process noise, Let H be the observation vector (actual timestamp deviation), and let H be the observation matrix. To observe noise, timestamp deviation is estimated using Kalman filtering, and the deviation data is interpolated to correct the time alignment of heterogeneous data.

[0132] In step S4, the tracking gimbal mechanical angle and pixel residual angle are fused to obtain high-precision composite yaw angle and high-precision composite pitch angle, specifically including:

[0133] Obtain the transient mechanical yaw angle θ2_mech and transient mechanical pitch angle ϕ2_mech of the second imaging device after timestamp alignment. Algebraically fuse these transient mechanical angles with the horizontal subpixel compensation angles Δθres and Δϕres calculated in step S2 to calculate the high-precision composite yaw angle θ2_comp and high-precision composite pitch angle ϕ2_comp of the second imaging device. The compensation equations are:

[0134]

[0135]

[0136] Simultaneously, the high-precision observed yaw angle θ1_comp of the first imaging device after distortion correction and sub-pixel extraction is obtained. The aforementioned θ1_comp and θ2_comp are then uniformly converted to a triangle interior angle system based on the dynamic projection baseline Dh.

[0137] In step S5, the dynamic projection baseline and the high-precision composite yaw angle are substituted into the two-dimensional plane sine intersection equation, and the relative altitude of the target is calculated based on the horizontal relative distance and the high-precision composite pitch angle. Specifically, this includes:

[0138] Using the dynamic projection baseline Dh obtained in step S3 and the high-precision composite yaw angle, the two-dimensional horizontal relative distance Lh of the target in the horizontal plane relative to the optical center of the first imaging device is calculated. The sinusoidal intersection solution equation is:

[0139]

[0140] Among them, L h >0 indicates that the observed rays converge and intersect in front of the detection system, the distance data is valid and output; L h A value less than 0 indicates that the observed ray is diverging in space. The system identifies the current ranging result as a false alarm and executes an interception, triggering the wide-area early warning module to reacquire the target.

[0141] Subsequently, the transient optical center elevation information H of the tracking device in the relative spatial coordinate system is acquired. d Combined with the calculated two-dimensional horizontal relative distance L of the target h and the high-precision composite pitch angle ϕ 2_comp The trigonometric leveling is performed based on the spatial vertical projection extension relationship; the relative altitude H of the target is... target The solution model is as follows:

[0142]

[0143] Among them, the high-precision composite pitch angle ϕ 2_comp The transient mechanical pitch angle and the vertical subpixel compensation angle of the second tracking device are algebraic sums; the transient optical center elevation information H d The relative three-dimensional coordinates are extracted from the forward kinematics output of the DH model in step S3 to dynamically compensate for the mechanical eccentricity error introduced by the pitch motion of the gimbal on its optical center elevation reference.

[0144] A relative ranging system for long-range small targets based on master-slave guidance and composite angle intersection includes:

[0145] Master-slave camera unit: It adopts a "one fixed and one moving" master-slave architecture, including a fixed wide-angle detection camera and a telephoto tracking gimbal camera; the optical center of the wide-angle detection camera is used as the origin of the relative spatial coordinate system to detect wide area and extract the sub-pixel centroid of the target; the telephoto tracking gimbal camera is used to receive guidance, track the target and perform zoom operation.

[0146] Wide-area detection and guidance module: Connects to the master and slave camera units, used to receive the centroid data of the wide-angle detection camera, back-calculate the initial observation yaw angle of the target in the relative coordinate system, combine with the prior distance to calculate the guidance angle, and send rotation commands to control the rotation of the telephoto tracking gimbal to ensure that the target falls into the telephoto field of view;

[0147] Visual loop closure and zoom update module: Connects to the telephoto tracking gimbal camera, used to calculate the target pixel residual, drive the gimbal to rotate so that the target is centered to form a visual loop; at the same time, during zooming, it compensates for the optical principal point offset by looking up the table through LUT and updates the camera intrinsic parameters in real time.

[0148] DH Dynamic Baseline Compensation Module: Connects to the telephoto tracking gimbal camera and is used to establish the DH kinematic linkage model. Based on the gimbal mechanical angle and camera eccentricity, it calculates the transient three-dimensional relative coordinates of the optical center of the telephoto tracking gimbal camera relative to the origin of the wide-angle detection camera and extracts the dynamic projection baseline and baseline tilt angle of the horizontal plane.

[0149] Time alignment module: Connects the master and slave camera units and is used to align the timestamps of heterogeneous data from the two image acquisition devices. It uses a combination of hardware-triggered synchronization and Kalman filter interpolation to eliminate time deviations in mechanical feedback and image exposure, ensuring data time consistency.

[0150] Precision rendezvous and ranging module: Connects the time alignment module and the DH dynamic baseline compensation module. It is used to fuse the tracking gimbal mechanical angle and pixel-level compensation angle to obtain high-precision composite yaw angle and pitch angle. Substitute them into the plane sine rendezvous equation to solve the two-dimensional horizontal relative distance of the target to the wide-angle detection camera, and further calculate the relative height of the target based on the relative elevation difference.

Claims

1. A method for ranging small targets at long distances based on master-slave guidance and composite angle intersection, characterized in that, Includes the following steps: Step S1: Use the first image acquisition device established at the origin of the relative coordinate system to detect the airspace, perform distortion correction on the image and extract the target sub-pixel centroid, back-calculate the target observation yaw angle, and combine the prior distance to calculate the guide angle, send a rotation command to the second image acquisition device to guide its rotation to ensure that the target falls into the telephoto field of view. Step S2: After the second image acquisition device captures the target, the target pixel residual is calculated through a visual algorithm, and the second image acquisition device is driven to rotate to center the target and form a visual closed loop. When the second image acquisition device zooms, it compensates for the optical principal point offset in real time and updates the camera intrinsic parameters to complete the sub-pixel visual miss distance calculation of the target. Step S3: Using the origin of the relative spatial coordinate system as a reference, obtain the initial physical baseline vector of the base of the second image acquisition device; establish the DH coordinate system, and calculate the transient relative coordinates of the second image acquisition device based on the mechanical yaw angle, pitch angle and camera eccentricity of the second image acquisition device, and then extract the dynamic projection baseline and baseline tilt angle of the horizontal plane to complete the dynamic baseline compensation. Step S4: Timestamp alignment of heterogeneous data from the first image acquisition device and the second image acquisition device; The transient mechanical yaw angle and pitch angle of the second image acquisition device are fused with the corresponding sub-pixel residual angle of the image to obtain a high-precision composite yaw angle and a high-precision composite pitch angle. Step S5: Substitute the dynamic projection baseline and the high-precision composite yaw angle into the two-dimensional plane sine intersection equation to solve for the two-dimensional horizontal relative distance of the target; finally, based on the horizontal relative distance and the high-precision composite pitch angle, perform spatial vertical projection extension to calculate the relative altitude of the target and complete the high-precision three-dimensional positioning of the target.

2. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S1, the first image acquisition device extracts the target sub-pixel centroid and inversely calculates the observed yaw angle. Specifically, it includes: The original wide-angle image from the first image acquisition device is acquired. Using a pre-calibrated camera intrinsic parameter matrix and distortion coefficients, the original wide-angle image undergoes distortion correction processing to eliminate radial and tangential distortion. On the ideal imaging plane after distortion correction, a sub-pixel feature extraction algorithm is used to obtain the stable centroid coordinates (x0, y0) of the target. Based on the distortion-corrected centroid coordinates and the intrinsic parameters of the detection device, the initial observation yaw angle of the target relative to the baseline direction on the horizontal projection plane is calculated. The solution formula is: Where x0 is the x-axis coordinate of the target centroid after distortion correction, u1 is the x-coordinate of the principal point of the probe camera, and f x1 To detect the focal length of the camera in the x-direction.

3. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S1, the prior distance prediction and the field-of-view inclusive blind guidance strategy specifically include: Set the system's prior depth Lp and maximum predicted depth deviation ΔL tol Based on the prior depth, the relative yaw angle of the detection equipment to the observation, and the initial relative physical baseline, the guidance command angle θ of the tracking gimbal is calculated. cmd The dynamic parallax tolerance limit is calculated based on the maximum estimated depth deviation and converted into a target field-of-view control command for the tracking telephoto camera. This ensures that even if the target's actual depth deviates from the prior depth during guidance, the target will still fall completely within the dynamic field of view of the tracking camera. The adaptive formulas for the guidance command angle and field of view angle are as follows: , ,in, To detect the camera's relative observation yaw angle, D0 is the initial relative physical baseline, and L... p For the set prior depth, FOV2 represents the maximum estimated depth deviation, φ represents the target field of view of the tracking telephoto camera, and φ represents the safety margin reserved for the system's mechanical control.

4. The long-range small target ranging method based on master-slave guidance and composite angle intersection as described in claim 1, characterized in that, Step S3, the dynamic projection baseline and tilt angle calculation based on the DH link matrix, specifically includes: Using the tracking gimbal base as the base coordinate system, a DH link model is constructed, defining three link coordinate systems corresponding to gimbal rotation axis I (horizontal rotation), rotation axis II (tilt rotation), and the camera coordinate system. The DH link transformation matrix formula is as follows: When expanded, it becomes: ,in Let represent the homogeneous transformation matrix of the i-th link coordinate system relative to the (i-1)-th link coordinate system, where Rot represents the rotation transformation operator and Trans represents the translation transformation operator. This represents the X-axis of the coordinate system of the (i-1)th link. Represents the Z-axis of the coordinate system of the i-th link; Let be the rotation angle about the x-axis. Let x be the translation along the x-axis. Let be the rotation angle about the z-axis. The translation is along the z-axis; the homogeneous transformation matrix of the camera relative to the base coordinate system is obtained by multiplying the transformation matrices of each link. Extract the homogeneous transformation matrix The translation components are used to obtain the local three-dimensional offset (ΔXDH, ΔYDH, ΔZDH) of the tracking device's optical center relative to its own base. Let the coordinates of the first image acquisition device be (0,0,0), and pre-calibrate the initial physical baseline of the base of the second image acquisition device relative to the origin as D0; through spatial translation transformation, calculate the transient relative projection coordinates (Xr,Yr) of the optical center of the tracking camera in the relative spatial coordinate system, and its spatial transformation equation is: Xr=D0+ΔXDH Yr=ΔYDH Furthermore, the dynamic projection baseline Dh and baseline inclination angle γ of the horizontal plane in the relative spatial coordinate system are calculated, and the calculation formula is updated as follows: Dh=Xr2+Yr2 γ=arctan2(Yr,Xr).

5. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S2, the intrinsic parameter update, optical axis principal point offset lookup table (LUT) compensation, and sub-pixel visual off-target calculation during the zoom process specifically include: Beforehand, establish the relationship between zoom ratio r and optical principal point offset through camera calibration. The correspondence table of intrinsic parameter matrix K; when the tracking camera zoom is detected, the current zoom ratio r is obtained, the corresponding principal point offset is queried from the LUT, and the principal point coordinates after compensation are ( 2+Δ , 2+Δ Simultaneously, based on the relationship between zoom magnification and field of view, the focal length parameter in the intrinsic parameter matrix is ​​updated in real time. The relationship between field of view and focal length is as follows: , ,in, , The horizontal and vertical field of view before zooming. , These are the horizontal and vertical field of view angles after zooming, where W and H are the width and height of the phase plane, respectively. , Let u2 and v2 be the updated focal lengths in the x and y directions, respectively, and u2 and v2 be the principal point coordinates of the tracking camera before zooming. The updated intrinsic parameter matrix K is: 。 6. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S2, the sub-pixel visual off-target calculation specifically includes: Based on image processing algorithms, high-precision relative pixel positions of the target on the phase plane are extracted to obtain the sub-pixel centroid coordinates (u). target ,v target ), calculate the horizontal subpixel residual Ru and vertical subpixel residual Rv of the target relative to the current dynamic principal point: , Subsequently, by combining the updated focal length parameters fx and fy, the subpixel residuals in the image domain are transformed into horizontal subpixel compensation angles Δθres and vertical subpixel compensation angles Δϕres in the spatial angle domain: , The Δθres and Δϕres are used as high-precision visual feedback quantities to be fused and compensated with the transient mechanical yaw angle and pitch angle of the tracking gimbal, so as to support subsequent high-precision composite angular intersection and high-precision target calculation.

7. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S4, the hardware-level synchronization and Kalman filter interpolation alignment of heterogeneous data timestamps specifically include: A hardware-triggered synchronization method is adopted, using a synchronization trigger signal to control two devices to acquire images simultaneously, ensuring consistent timestamps in the original data. When timestamp discrepancies occur, Kalman filtering is used for interpolation alignment. The Kalman filter state equation and observation equation are as follows: , ,in, This is a state vector, containing timestamp offset and data change rate; Let A be the state vector at time k-1; let A be the state transition matrix; and let B be the control matrix. To control the quantity, For process noise, Let H be the observation vector and H be the observation matrix. To detect noise, timestamp bias is estimated using Kalman filtering, and the biased data is then interpolated to achieve time alignment of heterogeneous data.

8. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection as described in claim 1, characterized in that, In step S4, the tracking gimbal mechanical angle and pixel residual angle are fused to obtain high-precision composite yaw angle and high-precision composite pitch angle, specifically including: Obtain the transient mechanical yaw angle θ2_mech and transient mechanical pitch angle ϕ2_mech of the second imaging device after timestamp alignment; perform algebraic fusion of the transient mechanical angles with the horizontal subpixel compensation angle Δθres and vertical subpixel compensation angle Δϕres calculated in step S2 to calculate the high-precision composite yaw angle θ2_comp and high-precision composite pitch angle ϕ2_comp of the second imaging device, respectively. The compensation equation is: , Simultaneously, the high-precision observed yaw angle θ of the first imaging device after distortion correction and sub-pixel extraction is obtained. 1_comp ; the above θ 1_comp With θ 2_comp The system is uniformly converted to the triangle interior angle system based on the dynamic projection baseline Dh.

9. The method for ranging small targets at long distances based on master-slave guidance and compound angle intersection according to claim 1, characterized in that, In step S5, the dynamic projection baseline and the high-precision composite yaw angle are substituted into the two-dimensional plane sine intersection equation, and the relative altitude of the target is calculated based on the horizontal relative distance and the high-precision composite pitch angle. Specifically, this includes: Using the dynamic projection baseline Dh obtained in step S3 and the high-precision composite yaw angle, the two-dimensional horizontal relative distance Lh of the target in the horizontal plane relative to the optical center of the first imaging device is calculated. The sinusoidal intersection solution equation is: , where θ 2_comp θ represents the high-precision composite yaw angle of the second imaging device. 1_comp L represents the high-precision observed yaw angle of the first imaging device after distortion correction and sub-pixel extraction. h >0 indicates that the observed rays converge and intersect in front of the detection system, the distance data is valid and output; L h A value less than 0 indicates that the observed ray is diverging in space. The system identifies the current ranging result as a false alarm and executes an interception, triggering the wide-area early warning module to reacquire the target. Subsequently, the transient optical center elevation information H of the tracking device in the relative spatial coordinate system is acquired. d Combined with the calculated two-dimensional horizontal relative distance L of the target h and the high-precision composite pitch angle ϕ 2_comp The trigonometric leveling is performed based on the spatial vertical projection extension relationship; the relative altitude H of the target is... target The solution model is as follows: The high-precision composite pitch angle ϕ 2_comp The transient mechanical pitch angle and the vertical subpixel compensation angle of the second tracking device are algebraic sums; the transient optical center elevation information H d The relative three-dimensional coordinates are extracted from the forward kinematics output of the DH model in step S3 to dynamically compensate for the mechanical eccentricity error introduced by the pitch motion of the gimbal on its optical center elevation reference.

10. A relative ranging system for long-range small targets based on master-slave guidance and composite angle intersection, characterized in that, The method for ranging long-range small targets based on master-slave guidance and compound angle intersection as described in any one of claims 1 to 9, when executed, includes: Master-slave camera unit: includes a fixed wide-angle detection camera and a telephoto tracking gimbal camera; wherein, the optical center of the wide-angle detection camera is used as the origin of the relative spatial coordinate system, and is used for wide-area detection and extraction of the target sub-pixel centroid; the telephoto tracking gimbal camera is used to receive guidance, track the target and perform zoom operation; Wide-area detection and guidance module: Connects to the master and slave camera units, used to receive the centroid data of the wide-angle detection camera, back-calculate the initial observation yaw angle of the target in the relative coordinate system, combine with the prior distance to calculate the guidance angle, and send rotation commands to control the rotation of the telephoto tracking gimbal to ensure that the target falls into the telephoto field of view; Visual loop closure and zoom update module: Connects to the telephoto tracking gimbal camera, used to calculate the target pixel residual, drive the gimbal to rotate so that the target is centered to form a visual loop; at the same time, during zooming, it compensates for the optical principal point offset by looking up the table through LUT and updates the camera intrinsic parameters in real time. DH Dynamic Baseline Compensation Module: Connects to the telephoto tracking gimbal camera and is used to establish the DH kinematic linkage model. Based on the gimbal mechanical angle and camera eccentricity, it calculates the transient three-dimensional relative coordinates of the optical center of the telephoto tracking gimbal camera relative to the origin of the wide-angle detection camera and extracts the dynamic projection baseline and baseline tilt angle of the horizontal plane. Time alignment module: Connects the master and slave camera units and is used to align the timestamps of heterogeneous data from the two image acquisition devices. It uses a combination of hardware-triggered synchronization and Kalman filter interpolation to eliminate time deviations in mechanical feedback and image exposure, ensuring data time consistency. Precision rendezvous and ranging module: Connects the time alignment module and the DH dynamic baseline compensation module. It is used to fuse the tracking gimbal mechanical angle and pixel-level compensation angle to obtain high-precision composite yaw angle and pitch angle. Substitute them into the plane sine rendezvous equation to solve the two-dimensional horizontal relative distance of the target to the wide-angle detection camera, and further calculate the relative height of the target based on the relative elevation difference.

Images