An unmanned ship target tracking and ranging control system

Abstract

The application discloses an unmanned ship target tracking and distance measuring control system, relates to the field of unmanned ship target tracking and distance measuring control, and collects photoelectric images, millimeter wave radars and inertial measurement unit data, constructs a posture compensation model to correct image sequences, and extracts initial pixel positions of targets; multi-source fusion is carried out in combination with radar distance measurement, a target three-dimensional relative position estimation model is established, extended Kalman filtering is adopted to realize continuous estimation and correction of target trajectories, target states are distinguished by analyzing trajectory and distance change trends, distance measurement frequency and weight factors are dynamically adjusted in a dynamic state, high-precision target distance estimation values are obtained, future position ranges of targets are further predicted, and the future position ranges are transmitted to a remote control center; the system has high-precision positioning, high-robust tracking and real-time prediction capabilities, and is suitable for water surface target sensing and response control tasks in complex environments.

Description

Technical Field

[0001] This invention relates to the field of unmanned surface vessel (USV) target tracking and ranging control, and specifically to an unmanned surface vessel target tracking and ranging control system. Background Technology

[0002] With the development of artificial intelligence and intelligent control systems, unmanned surface vessels (USVs) are increasingly being used in tasks such as surface patrol, target reconnaissance, and surface monitoring. In practical applications, it is often necessary to continuously track and accurately measure the distance to dynamic targets (such as floating objects, surface vehicles, and unauthorized intrusion targets) in order to subsequently perform identification, reporting, or interception operations. Traditional image recognition and radar ranging systems often experience recognition drift or ranging errors due to environmental interference (such as changes in wave height, water surface reflection, and target obstruction). This is especially true in environments with dense obstructions and significant hydrological disturbances, such as estuaries, harbors, and complex watersheds, where stable and high-precision target tracking and distance determination are often difficult to achieve.

[0003] Furthermore, most current unmanned surface vessel (USV) ranging systems rely on a single radar or vision device for target identification and ranging. However, during actual navigation, significant changes in the USV's attitude (such as yaw, pitch, and roll) and strong water current disturbances lead to data asynchrony between devices, affecting target trajectory judgment and ranging accuracy. Inaccurate ranging or discontinuous tracking can cause errors in the strike system or mission execution, resulting in irreversible mission failure or misoperation, which is particularly fatal in high-risk scenarios such as remote operations and unmanned collaborative warfare. Therefore, there is an urgent need for an USV control method that can adapt to complex water disturbance conditions and has high-precision target tracking and real-time ranging capabilities to improve system stability and execution reliability. Summary of the Invention

[0004] The purpose of this invention is to provide an unmanned surface vessel target tracking and ranging control system to address the shortcomings of the prior art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: an unmanned surface vessel target tracking and ranging control system, comprising: The data acquisition module acquires multi-sensor target data sequences carried by the unmanned vessel within a set time period, including photoelectric image sequences G, millimeter-wave radar distance data, and inertial measurement unit (IMU) attitude data. The target localization module performs temporal correction on the image sequence G based on IMU attitude data to obtain a stable image sequence G′, and extracts the initial pixel position L0 of the target in the image space based on G′. The multi-source information fusion module combines the initial pixel position L0 with radar distance data R to construct a three-dimensional relative position estimation model M0 for the target. The trajectory estimation and filtering module, based on the three-dimensional relative position estimation model M0, uses the extended Kalman filter algorithm to predict and correct the position of the target in consecutive frames, and obtains the target's continuous estimated trajectory T. The target state discrimination module determines whether the target is stationary or dynamic based on the rate of change of the target position in each frame of the continuously estimated target trajectory T, combined with the rate of change of the radar range data R, and performs real-time ranging in the dynamic state. The ranging control module dynamically adjusts the ranging frequency and sensor weighting factor based on the continuous relative position change of the target during real-time ranging, and generates a target distance estimate D. The trajectory prediction module predicts the possible location range Zτ of the target τ seconds later based on the target distance estimate D and the target continuous estimated trajectory T, and sends Zτ to the remote control center.

[0006] Preferably, the target localization module performs temporal correction on the image sequence using an attitude compensation rotation matrix, wherein the rotation matrix is ​​a three-dimensional combined rotation matrix constructed based on Euler angles, and is rotated and superimposed in the order of heading angle, pitch angle and roll angle.

[0007] Preferably, the target localization module extracts target candidate regions based on the corrected image sequence using a feature response consistency algorithm, and selects the final target region based on the principle of minimizing the Euclidean distance between multiple frames, with the center pixel coordinates defined as the initial pixel position L0.

[0008] Preferably, the multi-source information fusion module back-projects the initial pixel position L0 into a unit line-of-sight direction vector using camera intrinsic parameters, and calculates the three-dimensional spatial position by combining the distance measured by the millimeter-wave radar in the current frame.

[0009] Preferably, the trajectory estimation and filtering module adopts the extended Kalman filter algorithm, whose state vector includes six components: three-dimensional position and velocity. The state transition model is based on the assumption of uniform motion, the observation model only collects position data, and the covariance matrix is ​​initialized with a 6×6 diagonal matrix.

[0010] Preferably, the target state discrimination module calculates the rate of the target's continuously estimated trajectory and simultaneously calculates the rate of change of the radar distance within the corresponding time period. If the moving average of the two is lower than a set threshold, the target is determined to be in a stationary state; otherwise, it is in a dynamic state.

[0011] Preferably, the ranging control module compares the standard deviation of the velocity fluctuation of the target's continuous relative position with a set threshold and dynamically adjusts the radar ranging frequency. The ranging frequency is 2 Hz when the fluctuation is small and is increased to 10 Hz when the fluctuation is large.

[0012] Preferably, the ranging control module calculates the image ranging confidence level and the radar ranging confidence level respectively, and calculates the fusion weight based on the confidence level normalization, and outputs the target distance estimate D of the current frame using a weighted fusion method.

[0013] Preferably, the trajectory prediction module uses a uniformly accelerated motion model to predict the target's position within the next τ seconds based on the position and velocity state of the current frame, and outputs the predicted position sequence Tτ.

[0014] Preferably, the trajectory prediction module performs covariance modeling on the predicted position sequence Tτ, and constructs a three-dimensional confidence ellipsoid region Zτ using the predicted mean and covariance as the spatial range in which the target may appear τ seconds later.

[0015] The technical effects and advantages provided by the present invention in the above technical solution are as follows: 1. This invention constructs a full-process target tracking and ranging control system, including data acquisition, image correction, multi-source fusion, trajectory estimation, state discrimination, ranging control, and trajectory prediction, achieving stable identification and high-precision positioning of small targets by unmanned surface vessels (USVs) in dynamically disturbed environments. Compared to traditional single-image or radar solutions, this invention introduces an attitude compensation and correction mechanism and a three-dimensional orientation-range joint modeling method, significantly improving the spatial accuracy and temporal consistency of target positioning, solving the tracking discontinuity problem caused by water surface turbulence and angle shift, and effectively enhancing the system's practicality in complex environments.

[0016] 2. This invention achieves dynamic estimation of the continuous trajectory of a target through extended Kalman filtering, and combines trajectory rate and radar change rate as dual indicators to determine the target's motion state. Based on this, it dynamically adjusts the ranging frequency and fusion weights to achieve adaptive control of the ranging strategy. This method improves the stability and accuracy of ranging results while ensuring response speed. Combined with future position prediction and spatial confidence interval modeling, it can provide reliable strike position references for remote control centers, significantly improving the efficiency and accuracy of intelligent task execution. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0018] Figure 1 This is a flowchart of the system modules of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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 some embodiments of the present invention, 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.

[0020] For examples, please refer to Figure 1 As shown in this embodiment, an unmanned surface vessel target tracking and ranging control system includes: The data acquisition module acquires multi-sensor target data sequences carried by the unmanned vessel within a set time period, including photoelectric image sequences G, millimeter-wave radar distance data, and inertial measurement unit (IMU) attitude data.

[0021] In this invention, the data acquisition module is used to jointly control and synchronously sample multiple sensors mounted on the unmanned surface vessel within a set time period to obtain raw observation data of the target object, thereby supporting subsequent image processing, fusion modeling, and trajectory prediction operations. Specifically, the data acquisition module includes: By installing photoelectric cameras on the front or top of the unmanned vessel, image data of the observed area is continuously acquired, forming a photoelectric image sequence G. The image sampling frequency can be set according to the mission scenario, typically ranging from 10 to 60 frames per second, to ensure continuous visibility of the target in the image.

[0022] The system acquires relative distance information of targets within a certain range ahead of the unmanned surface vessel, forming millimeter-wave radar distance data. By transmitting and receiving millimeter-wave signals in real time, the reflection time difference is calculated to obtain high-precision target distance.

[0023] The system acquires attitude information of the unmanned surface vessel (USV) during its motion, including pitch, roll, and yaw angles, and generates attitude data for the inertial measurement unit (IMU). The IMU can be constructed using a combination of a three-axis gyroscope and a three-axis accelerometer, outputting attitude changes in real time as the basic data support for image stabilization and spatial coordinate transformation.

[0024] The data acquisition module of this invention ensures temporal consistency and spatial correlation in the subsequent data fusion process by synchronously acquiring and aligning multi-source heterogeneous data of images, radar and attitude data, which significantly improves the accuracy of target tracking and ranging. It is suitable for complex unmanned surface vessel application scenarios with high dynamics, severe water surface disturbances and many obstructions.

[0025] The target localization module performs temporal correction on the image sequence G based on IMU attitude data to obtain a stable image sequence G′, and extracts the initial pixel position L0 of the target in the image space based on G′.

[0026] In this invention, the target localization module is used to perform temporal correction on the image sequence based on the attitude data of the inertial measurement unit and to extract the initial pixel position of the target in the image space. This process includes the following steps: First, the inertial measurement unit (IMU) acquires the three-axis attitude information of each frame of the unmanned surface vessel (USV) during image acquisition, including pitch, roll, and yaw angles. Let θ be the pitch angle, φ be the roll angle, and ψ be the yaw angle corresponding to a certain image frame. Based on the Euler angle rotation principle, an image attitude change compensation model is constructed. This model uses the aforementioned three-axis angles as input parameters and calculates the rotation transformation relationship of the camera coordinate system relative to the initial image frame through a three-dimensional rotation matrix mapping. The rotation matrix is ​​combined in Euler angle order, in the following form: First, rotate around the X1 axis (roll) to obtain the first rotation matrix; Then rotate around the Y-axis (pitch) to obtain the second rotation matrix; Finally, rotate around the Z-axis (heading) to obtain the third rotation matrix; The three components are combined through sequential matrix multiplication to construct a complete attitude compensation rotation matrix. This rotation matrix describes the offset angle of the image coordinate system caused by changes in the attitude of the unmanned surface vessel.

[0027] Using a constructed pose change compensation rotation matrix, the pixel coordinates of each frame in the image sequence are spatially remapped. This process specifically includes the following steps: The two-dimensional image coordinates of each pixel in the image frame are back-projected onto the unit sphere to obtain its three-dimensional unit vector in the camera coordinate system. The vector is transformed using a rotation matrix to eliminate image shift caused by changes in the attitude of the unmanned vessel; The transformed 3D vector is reprojected back onto the image plane to obtain the new position of the corrected pixel. An interpolation algorithm is used to reconstruct the pixel values ​​of the remapped image to obtain a corrected and stable image sequence.

[0028] The above methods can significantly suppress image jitter and viewpoint shift caused by ship attitude fluctuations during navigation, thereby improving the accuracy of subsequent target detection.

[0029] In stable image sequences, a target region extraction algorithm based on feature response consistency is employed. This algorithm uses the brightness gradient distribution, edge texture direction, and color histogram in consecutive frames as basic features, combined with temporal consistency constraints, to locate the target. The specific process is as follows: Perform multi-scale sliding window scanning on each frame of the image and extract the local image feature vectors within each scanning window; The Euclidean distance between similar feature regions in the current frame and the previous frame is calculated using a feature matching method. Set the feature similarity threshold to TT (e.g., 0.75). If the feature distance between the current window and the corresponding region of the previous frame is less than TT, they are determined to be the same target candidate region. Regions that meet the conditions are used as the target candidate region set for the current frame.

[0030] This method exhibits high matching stability when the target size does not change drastically, making it suitable for continuous detection of small targets on the water surface.

[0031] After determining the set of candidate target regions, based on the image spatial coordinate system, the region with the smallest offset from the previous frame is selected as the final target location. The center pixel coordinates of this target region are denoted as L0, defined as an ordered pair of horizontal and vertical pixel indices in the image. This coordinate will serve as the initial pixel position output of the target in image space, providing the basic input for subsequent 3D position estimation and multi-source data fusion.

[0032] Through the above steps, the present invention can achieve image sequence stability processing under dynamic attitude change conditions of unmanned vessels, and accurately extract the target position based on the stable image, significantly improving the positioning accuracy in the initial stage of target tracking.

[0033] The multi-source information fusion module combines the initial pixel position L0 with radar distance data R to construct a three-dimensional relative position estimation model M0 for the target.

[0034] The multi-source information fusion module in this invention is used to establish a three-dimensional initial position estimation model of the target in the relative coordinate system of the unmanned vessel based on the pixel position of the target in the image and the distance information returned by the radar. This module integrates key steps such as camera modeling, attitude calculation, sensor time synchronization, and spatial consistency judgment, and mainly includes the following operation flow: The two-dimensional pixel coordinates L0=(u,v) of the target in the image are transformed into a unit line-of-sight vector Q, which serves as the directional basis for constructing the three-dimensional spatial position. fx and fy are the focal lengths (in pixels) in the horizontal and vertical directions of the image, respectively; cx and cy are the coordinates of the principal point of the image (i.e., the image center); normalizing the target pixel position (u, v) yields the normalized image coordinates: , Extend the normalized coordinates to unit vector form: Vector Q represents the spatial direction from the camera's optical center toward the target.

[0035] If the unmanned surface vessel (USV) is not in a horizontal position, it is necessary to use external attitude data (pitch angle θ, roll angle φ, heading angle ψ) to perform attitude compensation on the direction vector Q, transforming it into a direction vector Q′ in the USV's body coordinate system. The transformation formula is based on the combination of Euler angle three-dimensional rotation matrices, in the order Z (heading) - Y (pitch) - X1 (roll): Where Rx, Ry, and Rz are rotation matrices for the corresponding axes.

[0036] By fusing the target's line-of-sight direction Q′ with the target distance given by the millimeter-wave radar, the relative position of the target in three-dimensional space is determined.

[0037] The distance data returned by the radar module represents the spatial distance (in meters) of the target in the direction of the radar beam. This value is the measured distance and needs to be collected at the moment of synchronization with the image frame.

[0038] Multiplying the attitude-compensated unit vector Q′ by the distance R yields the spatial position E of the target relative to the camera coordinate system. Vector E is a three-dimensional real vector representing the relative position coordinates of the target in the camera coordinate system of the unmanned vessel.

[0039] To improve the reliability and accuracy of the fused data, vector E is subjected to dual time and space constraints for filtering: The image frame timestamp is set to timg, and the millimeter-wave radar return data timestamp is set to tradar. The time synchronization threshold Δt is set to 100 milliseconds. If: If the value is greater than 0.1, it is considered out of sync, and the current data pair is discarded.

[0040] Project the spatial position E back onto the image plane, and use the current frame pose and intrinsic parameter matrix K to convert E into pixel coordinates (u′, v′). Calculate the Euclidean distance between E and the original pixel position L0 = (u, v): Convert this distance into the actual spatial error ΔE within the field of view, and set a threshold of 0.5 meters. If ΔE exceeds this value, it indicates that the line of sight is inconsistent with the distance measurement value, and the data point is discarded.

[0041] The data points selected and retained above are used to build a three-dimensional spatial estimation model M0 for the target, which is constructed as follows: The estimated model M0 is a structured data object containing the following fields: Spatial position vector: E= ; Timestamp: t, representing the unified time corresponding to the observed data; Observation confidence value: C, ranging from [0,1], used for subsequent filtering weighting; Data source identifiers include image number, radar frame number, etc. The confidence value C is calculated as follows: Let the target edge intensity in the image region be Simg, and the radar reflection echo intensity be Sradar. After normalization, the confidence value C is obtained using the following weighted average formula: If C is lower than the preset lower limit (e.g., 0.3), the estimated point will not be used for trajectory initialization.

[0042] The output of the estimated model M0 is used as the initial observation input for the extended Kalman filter algorithm to initialize the target state vector, including the initial position and observation confidence.

[0043] The above-mentioned multi-source information fusion process spatially reprojects and jointly filters the image pixel positions and millimeter-wave radar ranging results, ensuring that the constructed target three-dimensional relative position estimation model has rigorous consistency in the three dimensions of time, space and credibility, which greatly improves the perception accuracy and tracking stability of unmanned vessels for small targets in complex and disturbed water environments.

[0044] The trajectory estimation and filtering module, based on the three-dimensional relative position estimation model M0, uses the extended Kalman filter algorithm to predict and correct the position of the target in consecutive frames, thereby obtaining the target's continuous estimated trajectory T.

[0045] In a preferred embodiment of the present invention, the extended Kalman filter algorithm is used to dynamically estimate and correct the spatial position of the target in consecutive frames. This algorithm achieves a stable output of the target trajectory by introducing the state difference between prediction and observation, and is particularly suitable for real-time tracking of the target state in a nonlinear state space. This module includes the following steps: The target's state vector is denoted as X, containing six state variables including position and velocity, and is defined as follows: Where x, y, z are the spatial coordinates of the target in the camera coordinate system, vx, vy, vz are the velocity components in each direction, and T1 is the vector transpose.

[0046] The initial position (x0, y0, z0) is provided by the previous three-dimensional relative position estimation model M0; the initial velocity is set to 0, indicating that the target is initially stationary unless there is a separate velocity estimation input.

[0047] The covariance matrix P is a 6×6 diagonal matrix representing the initial uncertainty of each state variable. The diagonal elements related to position are set to 0.5 squared, and those related to velocity are set to 1.0 squared. The matrix P is shown below: P=diag(0.25,0.25,0.25,1.0,1.0,1.0); this setting allows the filter to have higher sensitivity to velocity changes in the initial stage.

[0048] Assuming the target moves at a constant velocity for a short period of time, the state transition function adopts a linear motion model: ; This represents the target state vector at time k-1. The state transition matrix F represents the prior estimate vector of the state at time k; the state transition matrix F is defined as: Where Δt is the image frame sampling period, in seconds, and is usually taken as 0.033 (about 30 frames per second).

[0049] The observation vector Z represents the target's three-dimensional position coordinates, provided by the spatial position output from the fusion of image and radar data. The observation matrix H is: This indicates that only the position component is observed.

[0050] At the beginning of each frame image processing cycle, prior prediction is performed using the updated state X and covariance matrix P from the previous frame through the state transition model F: Predicted status: In the formula, It represents the prior estimate vector of the state at time k (predicting the current state based on the state at the previous time). This represents the posterior state vector output by the filter at the previous time step (k-1); Predicting covariance: In the formula, This is a priori estimate of the uncertainty (covariance matrix) of the current state estimation. The covariance matrix of the previous state estimate (posterior). Let F be the transpose of F. The process noise covariance matrix QM represents the uncertainty of the motion process. It is a 6×6 diagonal matrix, and the velocity-related elements are usually set to be large, such as: QM=diag(0.01,0.01,0.01,0.5,0.5,0.5); The predicted state vector is the estimated position and velocity of the target at the current moment, which is used to draw the trajectory prediction line.

[0051] After receiving the image radar fusion observation value Z of the current frame, the filter correction step is performed: Calculate the residual: Where Y is the observation residual, which is the deviation between the actual observation and the predicted observation; Z is the actual observation (position after image + radar ranging fusion); and H is the observation matrix, which maps the state space to the observation space (usually extracting the position component).

[0052] Calculate the Kalman gain: In the formula, RH is the observation noise covariance matrix, set as a diagonal matrix, and the position observation error is set to the square of 0.2 meters: RH=diag(0.04,0.04,0.04); S is the covariance matrix of the observation residuals (used to judge the confidence level of the prediction error). K is the Kalman gain matrix, which determines the respective weights of the predicted and observed values ​​during the update. The larger K is, the higher the confidence in the observation; the smaller K is, the more confidence in the model prediction.

[0053] Update the state vector and covariance matrix: The updated state estimate is obtained, which is the target position and velocity estimate for the current frame. This represents the updated state vector, i.e., the estimated optimal state of the target at the current moment. I represents the covariance matrix of the current state estimate (after uncertainty update), where I is the identity matrix (with the same dimension as P).

[0054] Take the position part (x, y, z) in the state vector as the target estimated position in the current frame, and construct the target continuous estimated trajectory T by connecting the positions of consecutive frames.

[0055] Through the above steps, this invention performs prediction and correction processes within each frame image period, achieving smooth, continuous, and high-precision trajectory estimation of the target under complex perturbation conditions. The extended Kalman filter algorithm fully integrates the target's historical state with current observations, enabling real-time correction of the dynamic target's position and providing reliable data support for subsequent target state discrimination and dynamic ranging.

[0056] The target state discrimination module determines whether the target is stationary or dynamic based on the rate of change of the target position in each frame of the continuously estimated target trajectory T, combined with the rate of change of radar range data R, and performs real-time ranging in the dynamic state.

[0057] In this invention, the target state discrimination and real-time ranging process is based on the dynamic changing trend of the target's continuously estimated trajectory and millimeter-wave radar ranging data. A joint rate determination method is used to determine whether the target is stationary or dynamic. When the target is in a dynamic state, a high-precision ranging process is activated to improve the efficiency of ranging resource utilization and enhance tracking accuracy. This process mainly includes the following steps: By analyzing the position changes in the continuously estimated trajectory of the target, the rate of position change between adjacent frames is extracted to form a trajectory rate sequence, which serves as the primary indicator for judging the target's motion state.

[0058] Let the estimated spatial positions of the target in consecutive N frames of images be: SP1=(x1,y1,z1), SP2=(x2,y2,z2), …, SPN=(xN,yN,zN); the velocity vk between every two adjacent frames is defined as the three-dimensional Euclidean distance divided by the time interval Δt between frames. For k = 1 to N−1, calculate in sequence to form a trajectory speed sequence: VT={v1,v2,…,vN−1}; the frame interval Δt is determined by the image processing frame rate and is usually 0.033 seconds. The obtained speed unit is meters per second (m / s) and is used for subsequent comparison of the stationary state threshold.

[0059] Process the millimeter-wave radar distance sequence corresponding to the above trajectory frame time, extract the radar distance change rate, and use it as the second index for judging the target motion state.

[0060] Obtain the millimeter-wave radar distance sequence for the corresponding time period: R1={r1,r2,…,rk,…,rN}; each rk is the target distance measured by the radar at the k-th frame time, with the unit of meter.

[0061] The calculation formula for the distance change rate dk between two adjacent frames is: ; for k = 1 to N−1, form a radar speed sequence: VR={d1,d2,…,dN−1}.

[0062] Compare the trajectory speed sequence and the radar speed sequence with the set stationary threshold respectively, and jointly judge whether the target is in a stationary state or a dynamic state.

[0063] Let the trajectory speed threshold TA be 0.05 meters per second and the radar speed threshold TB be 0.1 meters per second. The threshold TA is used to control the image trajectory judgment error, and TB is used to alleviate the small jitter of radar ranging.

[0064] Calculate the moving average of the trajectory speed sequence and the moving average of the radar speed sequence , and the expression is: ; If the following two conditions are met: <TA and <TB; then it is determined that the target is in a stationary state; otherwise, it is determined that the target is in a dynamic state. The determination result is used as the trigger condition for the subsequent ranging process.

[0065] If the target is determined to be in a dynamic state, activate the real-time ranging process, dynamically adjust the ranging frequency and fusion parameters, and achieve high-precision ranging.

[0066] In the stationary state, the ranging frequency f defaults to 1 hertz; in the dynamic state, the ranging frequency f is increased to 10 hertz to ensure rapid acquisition of the target distance change.

[0067] The image and radar ranging weight coefficients are dynamically adjusted based on the target's speed. Let the image fusion weight be W1 and the radar fusion weight be W2, satisfying W1 + W2 = 1. If the target's speed exceeds 1.0 m / s, then W1 = 0.3 and W2 = 0.7 are set to enhance the radar's dominance during high-speed movement.

[0068] The weighted fusion ranging result is used as the target high-precision distance estimate in the current frame, and then used as the input for the next step of trajectory prediction and position update.

[0069] This invention, by jointly analyzing trajectory rate and radar change rate, effectively determines the target's motion state while ensuring continuous target identification, and adaptively adjusts the ranging strategy. Compared to traditional methods using fixed ranging frequencies and threshold judgments, it possesses stronger dynamic response capabilities and ranging accuracy, making it more suitable for precise tracking and strike command support of moving targets in complex and disturbed environments.

[0070] The ranging control module dynamically adjusts the ranging frequency and sensor weighting factor based on the continuous relative position change of the target during real-time ranging, and generates a target distance estimate D.

[0071] In this invention, the ranging control module is used to dynamically adjust the ranging frequency and perform weighted fusion of multi-source ranging results after the target is determined to be in a dynamic state, based on the target's position changes in consecutive frames, to generate a high-precision target distance estimate D, thereby enhancing the real-time performance and accuracy of ranging. This module includes the following steps: Determining whether the target's motion state is stable provides a basis for subsequent ranging frequency adjustments.

[0072] Let the sequence of target positions in the camera coordinate system in M ​​consecutive frames be: SP1=(x1,y1,z1),SP2=(x2,y2,z2),…,SPM=(xM,yM,zM); Calculate the spatial velocity vk between adjacent frames based on the inter-frame time interval Δt; thus obtaining the rate sequence. .

[0073] Calculate the standard deviation σv of this rate sequence to measure the degree of fluctuation in the target's motion state.

[0074] The direction of adjustment of the ranging frequency is determined by the fluctuation degree σv of the target's motion state, so as to realize the adaptive allocation of ranging resources.

[0075] The velocity fluctuation threshold Tv is set to 0.3 meters per second. This threshold represents the tolerance range for changes in the target's motion rate and is used to distinguish between stable movement and drastic changes.

[0076] If σv>Tv, it indicates that the target's motion state is unstable, and the ranging frequency f is set to 10 Hz; If σv≤Tv, it indicates that the target motion is stable and the ranging frequency f decreases to 2 Hz.

[0077] Frequency f directly affects the trigger control of radar and image ranging, enabling real-time response to changes in target status.

[0078] The fusion weights of the two ranging results are dynamically adjusted based on the current observation results of the image and radar.

[0079] The ranging confidence level Cimg is comprehensively evaluated by considering the sharpness of the target edge, texture continuity, and feature response intensity in the image. The mean normalization of the edge gradient magnitude can be mapped to the [0,1] interval as the scoring criterion.

[0080] The confidence level Cradar is calculated using millimeter-wave radar echo intensity, target size stability, and consistency of multiple ranging measurements. For example: Where SQ is the normalized value of the echo signal intensity, σr is the standard deviation of the ranging over the last 3 frames, and α and β are weighting coefficients (e.g., 0.6 and 0.4).

[0081] Let Wimg = Cimg / (Cimg + Cradar) and Wradar = 1 − Wimg to obtain the weighting factor between the image and the radar ranging.

[0082] By fusing the observations from the two ranging sources, the final target distance estimate D is output.

[0083] Let the ranging result of the current frame image be Dimg, and the radar ranging result be Dradar.

[0084] The formula for calculating the target distance estimate D is as follows: The fused target distance estimate D serves as the final distance estimate for the current frame, exhibiting both stability and response speed.

[0085] This invention achieves adaptive adjustment of ranging frequency and dynamic weighted fusion of multi-source data through continuous analysis of the target's motion state. Compared with fixed-frequency, fixed-ratio fusion strategies, it has stronger environmental adaptability and ranging accuracy, and is particularly suitable for real-time tracking and ranging tasks of moving targets in complex and disturbed environments.

[0086] The trajectory prediction module predicts the possible location range Zτ of the target τ seconds later based on the target distance estimate D and the target continuous estimated trajectory T, and sends Zτ to the remote control center.

[0087] In this invention, the trajectory prediction module is used to model and predict the target's position change within the next τ seconds based on the target's historical trajectory data and current distance estimation results. The predicted position range Zτ is then transmitted as a control command parameter to a remote control center to support subsequent mission decisions such as strikes or evasion. The specific implementation steps of this module are as follows: Motion information of the target in the most recent consecutive frames is extracted to construct a basic model of the target's motion state at the current moment.

[0088] Let the estimated positions of the target in M ​​consecutive frames be: SP1=(x1,y1,z1), SP2=(x2,y2,z2),…,SPM=(xM,yM,zM); calculate the velocity sequence V based on the inter-frame position difference and time interval Δt; based on the established motion state model, use a uniformly accelerated linear motion model to predict the target position change trend within the next τ seconds. Assume the prediction time window τ is 2 seconds, the prediction interval is Δτ=0.1 seconds, and a total of 20 predicted position points are generated.

[0089] For any future time t, the predicted target position SPt is given by the following formula: In the formula, At the current speed, For acceleration, calculate the target's future position prediction sequence for each t=0.1, 0.2, ...,2.0: Tτ={SP0.1,SP0.2,...,SP2.0}; if the difference between the predicted position point and the current frame distance estimate in Euclidean distance exceeds the set tolerance δ (e.g., 0.5 meters), then use the most recent measured distance value to correct the radial component of the corresponding prediction point to improve prediction stability.

[0090] To characterize the uncertainty of the predicted target location, covariance modeling is performed on the predicted sequence to generate confidence intervals, thus forming the final location range Zτ.

[0091] For each dimension component (x, y, z) in the predicted sequence Tτ, calculate the mean. With covariance matrix Σ: Based on the mean μ and covariance Σ, a three-dimensional spatial confidence ellipsoid is constructed to represent the spatial range Zτ in which the target may appear with a 90% confidence probability after τ seconds.

[0092] Using an isosurface with a Mahalanobis distance of 3 as the ellipsoid boundary (corresponding to approximately 90% confidence level): the generated target location range Zτ is encoded and sent to the remote control center for task instruction generation.

[0093] Through the design of the trajectory prediction module described above, this invention achieves spatial location uncertainty prediction for the short-term future, starting from the target's historical trajectory and current distance, and constructs a confidence region Zτ for response decision-making at the remote control center. Compared to a fixed prediction point strategy, this method introduces motion modeling and covariance modeling mechanisms, possessing stronger adaptability and flexibility.

[0094] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. An unmanned surface vessel target tracking and ranging control system, characterized in that: include: The data acquisition module acquires multi-sensor target data sequences carried by the unmanned vessel within a set time period, including photoelectric image sequences G, millimeter-wave radar distance data, and inertial measurement unit (IMU) attitude data. The target localization module performs temporal correction on the image sequence G based on IMU attitude data to obtain a stable image sequence G′, and extracts the initial pixel position L0 of the target in the image space based on G′. The multi-source information fusion module combines the initial pixel position L0 with radar distance data R to construct a three-dimensional relative position estimation model M0 for the target. The trajectory estimation and filtering module, based on the three-dimensional relative position estimation model M0, uses the extended Kalman filter algorithm to predict and correct the position of the target in consecutive frames, and obtains the target's continuous estimated trajectory T. The target state discrimination module determines whether the target is stationary or dynamic based on the rate of change of the target position in each frame of the continuously estimated target trajectory T, combined with the rate of change of the radar range data R, and performs real-time ranging in the dynamic state. The ranging control module dynamically adjusts the ranging frequency and sensor weighting factor based on the continuous relative position change of the target during real-time ranging, and generates a target distance estimate D. The trajectory prediction module predicts the possible location range Zτ of the target τ seconds later based on the target distance estimate D and the target continuous estimated trajectory T, and sends Zτ to the remote control center.

2. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The target localization module performs temporal correction on the image sequence using an attitude compensation rotation matrix, wherein the rotation matrix is ​​a three-dimensional combined rotation matrix constructed based on Euler angles, and is rotated and superimposed in the order of heading angle, pitch angle and roll angle.

3. The unmanned surface vessel target tracking and ranging control system according to claim 2, characterized in that: The target localization module extracts target candidate regions based on the corrected image sequence using a feature response consistency algorithm, and selects the final target region based on the principle of minimizing the Euclidean distance between multiple frames, with the center pixel coordinates defined as the initial pixel position L0.

4. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The multi-source information fusion module back-projects the initial pixel position L0 into a unit line-of-sight direction vector using camera intrinsic parameters, and calculates the three-dimensional spatial position by combining the distance measured by the millimeter-wave radar in the current frame.

5. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The trajectory estimation and filtering module adopts the extended Kalman filter algorithm. Its state vector includes six components: three-dimensional position and velocity. The state transition model is based on the assumption of uniform motion. The observation model only collects position data. The covariance matrix is ​​initialized with a 6×6 diagonal matrix.

6. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The target state discrimination module calculates the rate of the target's continuously estimated trajectory and simultaneously calculates the rate of change of the radar distance within the corresponding time period. If the moving average of the two is lower than a set threshold, the target is determined to be in a stationary state; otherwise, it is in a dynamic state.

7. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The ranging control module compares the standard deviation of the velocity fluctuation of the target's continuous relative position with a set threshold and dynamically adjusts the radar ranging frequency. When the fluctuation is small, the ranging frequency is 2 Hz, and when the fluctuation is large, it is increased to 10 Hz.

8. The unmanned surface vessel target tracking and ranging control system according to claim 7, characterized in that: The ranging control module calculates the image ranging confidence level and the radar ranging confidence level respectively, and calculates the fusion weight based on the confidence level normalization. It then outputs the target distance estimate D of the current frame using a weighted fusion method.

9. The unmanned surface vessel target tracking and ranging control system according to claim 1, characterized in that: The trajectory prediction module uses a uniformly accelerated motion model to predict the target's position within the next τ seconds based on the position and velocity state of the current frame, and outputs the predicted position sequence Tτ.

10. The unmanned surface vessel target tracking and ranging control system according to claim 9, characterized in that: The trajectory prediction module performs covariance modeling on the predicted position sequence Tτ, and constructs a three-dimensional confidence ellipsoid region Zτ using the predicted mean and covariance as the spatial range in which the target may appear τ seconds later.

Images