Tug fusion enhanced perception method based on target tracking and obstacle recognition

Abstract

The invention relates to the technical field of perception fusion, and discloses a tug fusion enhanced perception method based on target tracking and obstacle recognition, which comprises the following steps: processing an AIS, an optical fiber compass, a Beidou RTK, a laser radar, a millimeter wave radar and a panoramic camera on a tug to establish a ship coordinate system; and collecting message data of the AIS and combining with the MMSI information to screen the target ship to obtain the initial position of the target ship. And predicting the state sequence of the target ship to obtain the position, speed and heading information of the target ship. And carrying out target identification on the image, outputting identification results of semantic levels of the ship, the buoy and the quay wall, and carrying out space projection alignment with radar data. And fusing the processed results to obtain a fusion result, and performing early warning according to the fusion result. According to the method, the target ship relative pose resolving precision and the obstacle recognition reliability are improved, the sensing link time delay is shortened, and the driver blind area risk is reduced.

Description

Technical Field

[0001] This invention relates to the field of perception fusion technology, and more specifically, to a tugboat fusion-enhanced perception method based on target tracking and obstacle recognition. Background Technology

[0002] Tugboats are core auxiliary equipment in port scheduling and the berthing and unberthing of large vessels. The safety and coordination of tugboat operations directly affect port transportation efficiency and navigation safety. With the continuous expansion of port scale and the development trend of intelligent shipping, tugboats need to complete precise berthing and coordinated operation in complex port environments, thus placing higher demands on their environmental perception capabilities. Currently, tugboat perception methods mainly rely on AIS, radar, and manual observation. These methods suffer from lag in long-range perception and insufficient accuracy in short-range obstacle recognition, making it difficult to meet the high-reliability autonomous perception requirements of future intelligent ports.

[0003] For example, the publicly available patent CN114995419A, "Intelligent Multi-Tugboat Collaborative Control System," proposes a collaborative control scheme for multiple tugboats. This scheme achieves collaborative operation through communication and scheduling between tugboats, which is significant for path planning and control during berthing operations. However, at the perception level, this scheme still relies on conventional single-sensor input and lacks a fusion-enhanced perception mechanism targeting both the target vessel and surrounding obstacles. Especially in complex port environments, AIS messages suffer from low update frequency and susceptibility to signal interference, making it difficult to support high-precision target tracking independently. LiDAR and millimeter-wave radar are limited by distance thresholds and environmental interference, making it difficult to simultaneously achieve continuous perception at both long and short ranges. While panoramic cameras can provide semantic recognition information, their output lacks a spatial alignment mechanism with radar data, making it difficult to directly use for multi-source fusion.

[0004] Therefore, it is necessary to design a tugboat fusion augmented perception method based on target tracking and obstacle recognition to solve the problems existing in the current technology. Summary of the Invention

[0005] In view of this, the present invention proposes a fusion-enhanced perception method for tugboats based on target tracking and obstacle recognition, aiming to solve the current problems of lacking time alignment and spatial registration of multi-source heterogeneous sensors, and lacking a fusion-enhanced perception method for continuous target ship tracking and obstacle recognition.

[0006] In one aspect, the present invention proposes a tugboat fusion-enhanced perception method based on target tracking and obstacle recognition, comprising: Initialize and synchronize the AIS (Automatic Identification System), fiber optic compass, BeiDou RTK (Real-time kinematic) system, lidar, millimeter-wave radar, and panoramic camera on the tugboat to establish the ship's coordinate system; The message data of the AIS is collected and parsed, outlier removal and time-series filtering are performed, and the target ship is screened by combining the MMSI (Maritime Mobile Service Identity) information of the target ship. The initial position of the target ship in the coordinate system of the ship is obtained through geometric correction. Based on Kalman filtering, target tracking and position prediction are performed on the state sequence of the target ship to obtain the target ship's position, speed, and heading information; When the distance between the center point of the target ship and the ship is greater than the distance threshold, the relative distance and relative bearing are calculated based on the AIS, fiber optic compass and Beidou RTK; when the target ship enters the distance threshold range, the lidar is called to obtain the boundary point cloud of the target ship, and the lateral distance, longitudinal distance and center point outline are calculated. At the same time, the distance and relative speed of surrounding obstacles are detected based on the millimeter-wave radar. The panoramic camera performs target recognition on the images it captures, outputs semantic-level recognition results for ships, buoys and shorelines, and aligns them with radar data through spatial projection. The processed AIS data, fiber optic compass data, BeiDou RTK data, lidar data, millimeter-wave radar data, and panoramic camera recognition results are fused to obtain a fusion result, and an early warning is issued based on the fusion result.

[0007] Furthermore, initialization and time synchronization, and the establishment of the ship's coordinate system, include: Using the unified time output by the Beidou RTK as a reference, clock synchronization is performed on AIS, fiber optic compass, lidar, millimeter-wave radar and panoramic camera, time deviation of each sensor is recorded and millisecond-level synchronization window is set, sampling timestamps are interpolated and delay compensation is performed to obtain aligned data frames. Based on the fiber optic compass, bow zero-point calibration is performed to establish a ship coordinate system with the Beidou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis. Read the installation offset parameters of the AIS, fiber optic compass, Beidou RTK, lidar, millimeter-wave radar and panoramic camera, and solve the rigid body transformation matrix of each sensor to the ship's coordinate system. The lidar and panoramic camera complete the external parameter calibration by jointly calibrating the target, and the millimeter-wave radar completes the external parameter calibration by aligning the beam pointing with the reflecting target. The point cloud features of the lidar, the echo of the millimeter-wave radar, and the recognition results of the panoramic camera are projected onto the ship's coordinate system. The heading and position calculated by the fiber optic compass and BeiDou RTK are compared. If the projection error and heading error exceed the error threshold, the corresponding camera extrinsic parameters and time deviation are corrected.

[0008] Furthermore, analysis, outlier removal, and temporal filtering are performed. Target ships are screened using the target ship's MMSI information. When obtaining the initial position of the target ship in the ship's coordinate system through geometric correction, the process includes: The AIS message is unpacked, and MMSI information, latitude and longitude, speed, heading, length, beam, and installation distance parameters from the AIS antenna to the bow, stern, port and starboard sides are extracted to form a state sequence. The state sequence is subjected to threshold rules and sliding window filtering to remove data frames that exceed latitude and longitude limits, time interval limits, sudden speed changes, and heading jumps. The target ship is determined by performing a unique match based on the MMSI information. The latitude and longitude of the target ship are converted from geographical coordinates to coordinates in the ship's own coordinate system. Lever arm compensation is performed using the target ship's AIS antenna installation distance parameters and the ship's Beidou RTK antenna installation offset parameters to estimate the coordinates of the target ship's geometric center in the ship's own coordinate system, thereby obtaining the initial position.

[0009] Furthermore, when performing target tracking and position prediction on the target ship's state sequence based on Kalman filtering to obtain the target ship's position, speed, and heading information, the process includes: A target ship state vector is established in the ship's coordinate system. A uniform turning model (CT) is used as the process model. The state transition is set according to the time interval between adjacent data frames. Kalman filtering is used for prediction and update. The prior position of the target ship is output. The initial position of the target ship is combined with the speed and heading in the AIS message data to form AIS measurements; when the lidar obtains the boundary point cloud of the target ship, the relative position of the center point is extracted as lidar measurements; when the millimeter-wave radar detects the target ship, the radial distance and relative velocity are geometrically converted to obtain the planar position and velocity components as millimeter-wave radar measurements. Consistency checks are performed on measurements from different sensors, and measurements that fail the consistency check are removed. The AIS measurements, lidar measurements, and millimeter-wave radar measurements are weighted based on time freshness to obtain fused measurements. The posterior position of the target ship is obtained in the ship's coordinate system based on the fused measurements. The prior position is compared with the posterior position. When the positions are consistent, the position, speed and heading information of the target ship are obtained. When the positions are inconsistent, the position, speed and heading information of the target ship are obtained based on the posterior position, and the process noise is corrected based on error analysis.

[0010] Furthermore, when calculating relative distance and relative azimuth based on the AIS, fiber optic compass, and BeiDou RTK, the following steps are included: In the ship's coordinate system, the straight-line distance between the geometric center of the target ship and the reference point of the ship is calculated as the relative distance. Based on the heading angle provided by the fiber optic compass, the angle between the target ship and the heading of the ship is determined as the relative bearing. The relative distance and the relative bearing are output as the solution results under long-distance working conditions.

[0011] Furthermore, when using lidar to acquire the boundary point cloud of the target ship, calculating the lateral distance, longitudinal distance, and center point bounding box, and simultaneously detecting the distance and relative velocity of surrounding obstacles based on the millimeter-wave radar, the process includes: Based on the posterior position of the target ship, a region clipping window is set in the coordinate system of the ship. The lidar point cloud is preprocessed, motion compensated, and surface noise is removed. Clustering and segmentation are performed within the region clipping window to obtain the target ship point cloud cluster. Boundary extraction and bounding box fitting are performed on the target ship point cloud cluster to determine the center point and external dimensions of the target ship. The forward and lateral components from the reference point of the ship to the center point are calculated using the bow axis and port axis of the ship's coordinate system, respectively, to obtain the longitudinal and transverse distances, and the bounding box of the center point is obtained at the same time. The echoes of the millimeter-wave radar are detected and clustered to obtain the distance and relative speed of the echo targets; the echoes that fall into the outer frame of the center point and its neighborhood are associated as the target ship measurement, and the remaining unassociated echoes are associated as the surrounding obstacles measurement to obtain the distance and relative speed of the nearest obstacle. Within the aforementioned distance threshold range, the lateral distance, longitudinal distance, and center point outline obtained by the lidar are used as the main measurements, while the distance and relative velocity of the millimeter-wave radar are used for verification and supplementation to obtain a measurement set for close-range working conditions.

[0012] Furthermore, when performing target recognition on the images acquired by the panoramic camera, outputting semantic-level recognition results for ships, buoys, and shorelines, and aligning them with radar data through spatial projection, the process includes: The panoramic camera images are preprocessed, including distortion removal and brightness normalization, and the bounding boxes or pixel masks of ships, buoys and shore walls and their category labels are output based on the target recognition model. The center of the bounding box or the contour points of the pixel mask are converted into line-of-sight rays in the camera coordinate system, and the corresponding set of directions is generated. The lidar point cloud is projected onto the image plane. Based on the projection overlap and spatiotemporal consistency, the recognition results of the panoramic camera are cross-modal correlated with the lidar point cloud clusters to distinguish the point cloud clusters belonging to the target ship from the point cloud clusters belonging to the surrounding obstacles. The near-field water surface reference plane is fitted using the associated lidar point cloud, and the intersection of the line of sight ray and the reference plane is calculated to obtain the spatial position of the panoramic camera recognition result in the ship's coordinate system. Output the category, spatial position, and relative orientation aligned with the ship's coordinate system as a visual measurement.

[0013] Furthermore, before fusing the processed AIS data, fiber optic compass data, BeiDou RTK data, lidar data, millimeter-wave radar data, and panoramic camera recognition results, the following steps are taken: aligning all data under a unified timestamp to establish an asynchronous multi-rate data queue; associating measurements based on consistency checks and spatiotemporal proximity constraints to eliminate inconsistent and duplicate measurements; and aggregating relative distances and relative azimuths, as well as lateral and longitudinal distances, according to long-distance and short-distance operating conditions to obtain a fused measurement set for fusion.

[0014] Furthermore, the fused measurement set is solved using weighted least squares in the ship's coordinate system to obtain the fusion result and uncertainty; wherein, the fusion weight is determined based on the measurement covariance, time freshness, message quality, point cloud density and identification confidence; the fusion result includes the target ship's lateral distance, longitudinal distance, center point distance and relative bearing relative to the ship.

[0015] Furthermore, when issuing an early warning based on the fusion results, the following steps are included: calculating the collision time margin and minimum safety interval based on the relative distance, relative orientation, lateral distance and longitudinal distance, and the distance and relative speed of the nearest obstacle obtained by the millimeter-wave radar; and generating an early warning level based on the safety interval threshold.

[0016] Compared with existing technologies, the advantages of this invention are as follows: A unified ship coordinate system is established through time synchronization and extrinsic parameter calibration, ensuring that data from various sources are consistent in time and space and can be directly superimposed; at a distance, the initial position of the target ship is obtained by parsing, anomaly removal, and temporal filtering of AIS messages, combined with MMSI screening and geometric correction, and Kalman filtering is used to continuously track and predict the position of the target ship, suppressing trajectory jitter caused by AIS delay and jumps; at a close distance, sensors are adaptively scheduled based on distance thresholds, and the relative pose is refined by using lidar to output lateral distance, longitudinal distance, and center point outline, while millimeter-wave radar is used to obtain surrounding obstacles. The distance and relative velocity of objects form a near-field perception that is more sensitive to small moving bodies and non-cooperative targets; the panoramic camera provides semantic-level recognition of ships, buoys and shorelines under a unified coordinate system, and aligns with the spatial projection of radar data to enhance target association and category determination; finally, the results are fused to output a highly reliable fusion result including relative distance, relative bearing, lateral distance, longitudinal distance and center point outline, and a graded warning is generated based on this result. This solves the problem of continuous observability and semantic interpretability from long distance to near distance, improves the accuracy of target ship relative pose calculation and obstacle recognition reliability, shortens the perception link latency, and reduces the risk of blind spots for the driver. Attached Figure Description

[0017] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 A flowchart of a tugboat fusion enhanced perception method based on target tracking and obstacle recognition provided in an embodiment of the present invention. Detailed Implementation

[0018] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the disclosure to those skilled in the art. It should be noted that, unless otherwise specified, embodiments and features in the embodiments of the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0019] For this, please refer to Figure 1 As shown, this application proposes a tugboat fusion-enhanced perception method based on target tracking and obstacle recognition, including: S100: Initializes and synchronizes the AIS, fiber optic compass, Beidou RTK, lidar, millimeter-wave radar, and panoramic camera on the tugboat, and establishes the ship's coordinate system.

[0020] S200: Collects AIS message data and performs parsing, outlier removal and timing filtering. Combines the target vessel's MMSI information to screen the target vessel and obtains the target vessel's initial position in the ship's coordinate system through geometric correction.

[0021] S300: Based on Kalman filtering, it performs target tracking and position prediction on the target ship's state sequence to obtain the target ship's position, speed, and heading information. When the distance between the center point of the target ship and the ship itself is greater than a distance threshold, it calculates the relative distance and relative bearing based on AIS, fiber optic compass, and BeiDou RTK.

[0022] S400: When the target ship enters the distance threshold range, the lidar is invoked to obtain the boundary point cloud of the target ship, and the lateral distance, longitudinal distance and center point outline are calculated. At the same time, the distance and relative speed of surrounding obstacles are detected based on millimeter-wave radar.

[0023] S500: Performs target recognition on images acquired by panoramic cameras, outputs semantic-level recognition results for ships, buoys, and shorelines, and aligns them with radar data through spatial projection.

[0024] S600: It fuses processed AIS data, fiber optic compass data, BeiDou RTK data, lidar data, millimeter-wave radar data, and panoramic camera recognition results to obtain a fusion result, and issues early warnings based on the fusion result.

[0025] Specifically, initialization and time synchronization refer to the unified clock calibration and data alignment of various sensors on the tugboat. This can be achieved by using the time output from BeiDou RTK as a reference, eliminating time deviations between sensors through interpolation and delay compensation, and establishing a unified ship-based coordinate system to provide a spatiotemporal consistency foundation for multi-source data fusion. Target vessel screening involves identifying specific vessels by parsing the MMSI information in AIS messages. This can be achieved by using threshold rules and sliding window filtering to remove abnormal data, combined with geometric correction to compensate for installation offset errors, to accurately estimate the initial position of the target vessel in the ship-based coordinate system. Kalman filtering uses a uniform turning model (CT) to predict the target vessel's motion state. This can be achieved by fusing asynchronous measurements from AIS, LiDAR, and millimeter-wave radar, and weighted updating of the target vessel's position, speed, and heading information to improve tracking accuracy. Distance thresholds are the critical values ​​used to divide the long-range and short-range sensing ranges. Different distance intervals can be set according to the port operation scenario. For long-range sensing, AIS and BeiDou RTK are used to calculate relative parameters; for short-range sensing, LiDAR and millimeter-wave radar are switched to obtain high-precision obstacle information, achieving continuous sensing coverage. Data fusion refers to integrating heterogeneous information from multiple sensors to generate a unified perception result. Specifically, it can use asynchronous multi-rate data queues to align timestamps, and based on weighted least squares fusion measurement sets, output the target ship's lateral distance, longitudinal distance, and collision risk indicators to support early warning decisions.

[0026] This application utilizes a multi-sensor spatiotemporal synchronization and cross-modal data fusion mechanism, combining long-range AIS tracking with short-range lidar and millimeter-wave radar obstacle detection, and introduces panoramic camera semantic recognition and spatial alignment to form an enhanced perception system covering the entire range. This effectively solves the problems of perception lag and insufficient accuracy of tugboats in complex port environments, and improves operational safety and collaborative control reliability.

[0027] The working process and principle of this application are as follows: First, the multi-source sensors on the tugboat are initialized and time-synchronized to establish a unified ship coordinate system. This step uses the unified time reference provided by BeiDou RTK to synchronize the clocks of each sensor and interpolate the sampling timestamps to ensure data frame alignment. Simultaneously, zero-point calibration is performed based on a fiber optic compass to establish a ship coordinate system with the BeiDou RTK antenna projection point as the origin. The rigid body transformation matrix from each sensor to the ship coordinate system is then solved through extrinsic parameter calibration. AIS message data is collected and processed. Key information is extracted from the AIS messages, and outliers are removed using threshold rules and sliding window filtering. The target ship is determined based on MMSI information. Then, the target ship's latitude and longitude are converted to ship coordinates, and lever arm compensation is performed to obtain the initial position of the target ship in the ship coordinate system.

[0028] Target ship tracking and prediction are performed based on Kalman filtering. A target ship state vector is established in the ship's coordinate system, and a uniform turning model (CT) is used as the process model. The prior position of the target ship is obtained through prediction updates. Consistency checks and weighted fusion are performed by combining measurements from AIS, lidar, and millimeter-wave radar to obtain the posterior position. The prior and posterior positions are compared to output the target ship's position, speed, and heading information.

[0029] When the distance between the target vessel and the ship exceeds a threshold, the relative distance and bearing are calculated based on AIS, fiber optic compass, and BeiDou RTK. When the target vessel enters the threshold range, the lidar is used to acquire boundary point clouds, and the lateral distance, longitudinal distance, and center point outline are calculated. Simultaneously, millimeter-wave radar is used to detect the distance and relative speed of surrounding obstacles.

[0030] Finally, the processed multi-source data is fused. First, the data is aligned under a unified timestamp to establish an asynchronous multi-rate data queue. Inconsistent and duplicate measurements are eliminated based on consistency checks and spatiotemporal proximity constraints. Relative distance, bearing, lateral distance, and longitudinal distance information are aggregated according to near and far distance conditions. The fused results are solved using weighted least squares in the ship's coordinate system to obtain the target ship's relative position information. Based on the fused results, the collision time margin and minimum safe interval are calculated, and a warning level is generated.

[0031] It achieves spatiotemporal alignment and fusion of multi-source heterogeneous sensor data to enhance perception, solving problems such as inconsistent spatiotemporal references and measurement error coupling in traditional systems, and improving the accuracy and reliability of target tracking and obstacle recognition.

[0032] As a preferred embodiment, the solution of this application is specifically implemented as follows: An AIS receiver, fiber optic compass, BeiDou RTK positioning module, LiDAR, millimeter-wave radar, and panoramic camera were mounted on the tugboat. First, sensor initialization was performed, using the UTC time output by BeiDou RTK as a reference to synchronize the clocks of the other sensors. The time deviation of each sensor relative to UTC was recorded, and a synchronization window of 10 milliseconds was set. Linear interpolation and delay compensation were performed on the sampling timestamps to obtain aligned data frames.

[0033] Based on the fiber optic compass, bow zero-point calibration was performed to establish a ship-wide coordinate system with the BeiDou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis. The installation offset parameters of each sensor were read, and the rigid body transformation matrix of the ship-wide coordinate system was solved. The lidar and panoramic camera underwent extrinsic parameter calibration via a joint calibration target, while the millimeter-wave radar underwent extrinsic parameter calibration by aligning its beam pointing with the reflecting target.

[0034] Collect AIS message data and extract information such as MMSI, latitude and longitude, speed, and heading. Apply threshold rules and a 30-second sliding window filter to the state sequence to remove abnormal data frames with out-of-bounds latitude and longitude, time intervals exceeding 3 minutes, speed change rates exceeding 5 knots per second, and heading change rates exceeding 30 degrees per second. Data frames with timestamp intervals exceeding 10 seconds but not exceeding 3 minutes can be excluded from current position initialization. Determine the target ship based on the MMSI information.

[0035] The target ship's latitude and longitude are converted to the ship's own coordinate system. The lever arm is compensated using the AIS antenna installation distance parameter and the Beidou RTK antenna offset. The geometric center coordinates of the target ship are estimated as the initial position.

[0036] In this embodiment, to ensure a clear engineering implementation path for the target ship tracking steps, a six-dimensional state description of the target ship is established in the ship's coordinate system. This six-dimensional state description includes, in sequence, the lateral component of the target ship's geometric center position, the longitudinal component of its position, the lateral component of its velocity, the longitudinal component of its velocity, the heading angle, and the turning angular velocity in the ship's coordinate system. Preferably, a linearized discrete uniform turning state space model is used as the process model for target ship state prediction. When the time interval between two adjacent frames is known, the lateral component of the position is determined by the previous lateral component of the position and the previous lateral component of the velocity, combined with the time interval; the longitudinal component of the position is determined by the previous longitudinal component of the position and the previous longitudinal component of the velocity, combined with the time interval; the lateral and longitudinal components of the velocity remain continuous within a single prediction period; the heading angle is determined by the previous heading angle and the previous turning angular velocity, combined with the time interval; the turning angular velocity remains continuous within a single prediction period. Based on the above state recursive relationship, the prediction steps are executed to obtain the prior state estimation result of the target ship and the corresponding prior state covariance result.

[0037] In this embodiment, the process noise covariance matrix is ​​initialized using a diagonal matrix. The position disturbance variances corresponding to the lateral and longitudinal position components have the same initial value, as do the velocity disturbance variances corresponding to the lateral and longitudinal velocity components. The heading angle corresponds to the heading disturbance variance, and the steering angular velocity corresponds to the steering disturbance variance. Preferably, the standard deviation of the position disturbance is 1 to 3 meters, the standard deviation of the velocity disturbance is 0.2 to 1 meter per second, the standard deviation of the heading disturbance is 2 to 6 degrees, and the standard deviation of the steering angular velocity disturbance is 0.2 to 1 degree per second. During matrix operations, all angular quantities are uniformly converted to radians. This initialization method ensures that the disturbance ranges corresponding to different state components have clear engineering implications.

[0038] The speed and heading from the AIS message are combined with the initial position to form the AIS measurement. When the lidar obtains the target ship's boundary point cloud, the relative position of the target ship's geometric center is extracted as the lidar measurement. When the millimeter-wave radar detects the target ship, the planar position and velocity components are calculated using radial distance, azimuth angle, and relative velocity as the millimeter-wave radar measurement. Consistency checks are performed on the measurements from different sensors, preferably using Mahalanobis distance. If the Mahalanobis distance between the measurement and the predicted state is not greater than a preset chi-square threshold, the measurement is considered to have passed the consistency check; otherwise, it is considered an abnormal measurement and is discarded.

[0039] After obtaining measurements that pass the consistency test, an innovation quantity is constructed based on the difference between the current fused measurement and the prior prediction result, and the theoretical innovation covariance is determined based on the prior state covariance and the measurement noise covariance. Simultaneously, an empirical innovation covariance is constructed using a sliding update method: 80% of the empirical innovation covariance from the previous time step is retained, and 20% of the outer product term corresponding to the current innovation quantity is included, thus obtaining the empirical innovation covariance at the current time step. Then, the ratio of the trace of the empirical innovation covariance to the trace of the theoretical innovation covariance is calculated to obtain a scaling factor reflecting the degree of model perturbation matching. When the scaling factor is greater than 1.2, it indicates that the current process model underestimates the process perturbation, and the diagonal terms of the process noise covariance matrix should be increased; when the scaling factor is less than 0.8, it indicates that the current process model overestimates the process perturbation, and the diagonal terms of the process noise covariance matrix should be decreased; when the scaling factor is between 0.8 and 1.2, the process noise covariance matrix remains unchanged. During the correction, the candidate process noise covariance matrix is ​​first obtained based on the above judgment results, and then smoothly updated according to the principle that the current process noise covariance matrix accounts for 80% and the candidate process noise covariance matrix accounts for 20%. To avoid drastic fluctuations in the process noise covariance matrix caused by a single abnormal measurement, upper and lower limits are set for each diagonal term.

[0040] Kalman update is performed based on the fused measurements that have passed consistency checks and undergone weighted processing to obtain the posterior state estimation and posterior state covariance of the target ship. The prior and posterior positions are compared. If the Euclidean distance between them is not greater than a preset position consistency threshold, the current position prediction is determined to be consistent with the update result, and the target ship's position, speed, and heading information corresponding to the posterior state are output. If the Euclidean distance is greater than the preset position consistency threshold, the posterior state is still used as the current output result, and the above-mentioned process noise adaptive correction is triggered simultaneously. Preferably, the position consistency threshold is between 0.5 meters and 1 meter.

[0041] When the distance between the target ship and the center point of the ship is greater than 200 meters, the relative distance and bearing are calculated based on AIS, fiber optic compass, and BeiDou RTK. The straight-line distance between the geometric center of the target ship and the reference point of the ship is calculated as the relative distance, and the angle between the target ship and the bow of the ship is determined based on the heading angle of the fiber optic compass as the relative bearing.

[0042] When the target ship enters a 200-meter range, the lidar is used to acquire the boundary point cloud. A region clipping window is set based on the target ship's posterior position, and the point cloud is preprocessed, motion compensated, and surface noise removed. Euclidean clustering is then performed within the clipping window to obtain the target ship's point cloud cluster.

[0043] Boundary extraction and bounding box fitting are performed on the target ship point cloud cluster to determine the center point and external dimensions. Boundary extraction can employ α-shape, convex hull, or other boundary extraction methods suitable for sparse point clouds, while bounding box fitting can use the minimum bounding rectangle method. The forward and lateral components from the ship's reference point to the center point are calculated using the bow axis and port axis of the ship's coordinate system, respectively, to obtain the longitudinal and transverse distances, and simultaneously, the bounding box of the center point is obtained.

[0044] Millimeter-wave radar echoes are detected and clustered to obtain the distance and relative velocity of echo targets. Detection and clustering can employ constant false alarm rate (CFAR) detection, density clustering, or other echo separation methods. Echoes falling within the center point's bounding box and its preset neighborhood are used as target ship measurements, while the rest are used as surrounding obstacle measurements to obtain the distance and relative velocity of the nearest obstacle. The neighborhood range can be set according to the bounding box dimensions, radar resolution, and on-site statistical errors.

[0045] All data are aligned under a unified timestamp, and an asynchronous multi-rate data queue is established. Using the current fusion time as a benchmark, AIS measurements, LiDAR measurements, millimeter-wave radar measurements, and visual measurements are included in the same fusion window. For measurements exceeding the synchronization window but not exceeding the corresponding time limit, alignment is achieved through timestamp interpolation and delay compensation. Measurements exceeding the corresponding time limit are not used as the current main fusion measurement; only auxiliary verification information is retained. Preferably, measurements are correlated based on Mahalanobis distance consistency checks and 3-meter spatiotemporal proximity constraints, eliminating inconsistent and duplicate measurements. Then, relative distance, relative azimuth, lateral distance, and longitudinal distance information are aggregated according to long-distance and short-distance conditions respectively, forming a fused measurement set.

[0046] In this embodiment, a basic measurement noise covariance is pre-set for each type of measurement, and a comprehensive correction weight is determined based on this covariance and the freshness of the measurement. The higher the measurement quality and the fresher the time, the greater the weight of that type of measurement in the weighted least squares solution. The lower the measurement quality and the more outdated the time, the smaller the weight of that type of measurement in the weighted least squares solution.

[0047] Time freshness reflects the impact of the time difference between the measurement timestamp and the current fusion time on the measurement reliability. Preferably, the freshness decay constant is 10 seconds for AIS measurements, 1 second for LiDAR measurements, 0.5 seconds for millimeter-wave radar measurements, and 0.5 seconds for visual measurements. Time freshness is calculated according to the negative exponential relationship between the current measurement time difference and the corresponding freshness decay constant; the larger the time difference, the lower the time freshness. When the time difference of a certain type of measurement exceeds three times its corresponding freshness decay constant, the measurement is deemed to have insufficient timeliness and will not participate in the current main fusion solution, but will only be used as an auxiliary verification measurement.

[0048] For AIS measurements, message quality is determined jointly based on field completeness, verification pass rate, and timing continuity. Field completeness is the ratio of the number of valid fields to the required number of fields in the current AIS message; verification pass rate is the ratio of the number of verified frames in the most recent five consecutive AIS messages to 5; timing continuity is determined by the deviation of the time interval between the current AIS message and the previous valid AIS message from the expected AIS update cycle. The closer the two are, the higher the timing continuity; the greater the deviation, the lower the timing continuity. When the deviation exceeds the expected update cycle, the timing continuity is counted as 0. Preferably, message quality is calculated by weighting the above three items in a 5:3:2 ratio, i.e., field completeness accounts for 50%, verification pass rate accounts for 30%, and timing continuity accounts for 20%.

[0049] For lidar measurements, point cloud density is determined based on the effective point density of the target point cloud cluster on its horizontal projected area. Specifically, the point cloud density is obtained by dividing the effective point count after outlier removal by the projected area of ​​the target point cloud outline on the horizontal plane. When the point cloud density is not less than 6 points per square meter, the lidar measurement quality is considered sufficient, and its density correction coefficient is set to the maximum value of 1. When the point cloud density is between 2 and 6 points per square meter, the density correction coefficient is determined using a linear mapping method from low to high, i.e., the closer the point cloud density is to 6 points per square meter, the closer the correction coefficient is to 1. When the point cloud density is less than 2 points per square meter, the lidar measurement density is considered insufficient and is not used as the main fusion measurement.

[0050] For visual measurements, the recognition confidence score is determined based on the category confidence score output by the target recognition model. When the category confidence score is not less than 0.85, the visual measurement is considered to have high confidence, and its confidence correction coefficient is set to the maximum value of 1. When the category confidence score is between 0.60 and 0.85, the confidence correction coefficient is determined according to a linear mapping method from low to high, that is, the closer the category confidence score is to 0.85, the closer the correction coefficient is to 1. When the category confidence score is less than 0.60, the visual measurement is considered to have insufficient confidence and is not included in the main fusion measurement. If the reprojection error of the visual measurement exceeds a preset threshold after spatial projection alignment, the projection error attenuation coefficient is further multiplied by the above confidence correction coefficient to reduce the impact of large-scale spatial alignment errors on the measurement.

[0051] For millimeter-wave radar measurements, the measurement quality is determined jointly by the normalized echo signal-to-noise ratio (SNR) and the Doppler consistency value, both of which are normalized to the range of 0 to 1. Preferably, the millimeter-wave radar measurement quality is calculated by weighting the normalized echo SNR and the Doppler consistency value at a ratio of 6:4, i.e., the normalized echo SNR accounts for 60% and the Doppler consistency value accounts for 40%. When multipath reflection, echo discrepancies, or unstable velocity calculations exist, the millimeter-wave radar measurement quality decreases accordingly.

[0052] In this embodiment, the comprehensive correction weight for each type of measurement is determined by both time freshness and the corresponding quality coefficient. For AIS measurements, the comprehensive correction weight is determined by both AIS time freshness and AIS message quality; for LiDAR measurements, it is determined by both LiDAR time freshness and point cloud density correction coefficient; for millimeter-wave radar measurements, it is determined by both millimeter-wave radar time freshness and millimeter-wave radar quality; and for visual measurements, it is determined by both visual time freshness and recognition confidence correction coefficient. If a certain type of measurement does not involve a particular quality factor, that quality factor is defaulted to 1. Ultimately, the actual weight of each type of measurement in the fusion is determined by both its comprehensive correction weight and the base measurement noise. The smaller the base measurement noise and the larger the comprehensive correction weight, the greater the contribution of that type of measurement to the solution.

[0053] The fused measurement set is solved using weighted least squares in the ship's coordinate system. During the solution process, the difference between the predicted and actual measured values ​​for each measurement is used as the residual. The residuals are weighted according to the determined weights to ensure that high-quality, timely measurements have a greater impact on the results, while low-quality, infrequent measurements have a smaller impact. The target ship's lateral distance, longitudinal distance, center point distance, and relative bearing are solved iteratively. The solution is considered convergent when the changes in lateral distance and longitudinal distance obtained from two consecutive iterations are both less than 0.05 meters. If the convergence condition is met before reaching the preset maximum of 5 iterations, the iteration is stopped early. If the convergence condition is not met after reaching the preset maximum of 5 iterations, the result of the 5th iteration is output as the current fused result, along with the corresponding uncertainty.

[0054] The collision time margin and minimum safety interval are calculated based on the fusion results. A warning level is triggered when the collision time margin is less than 30 seconds or the minimum safety interval is less than 10 meters. An emergency warning level is triggered when the collision time margin is less than 15 seconds or the minimum safety interval is less than 5 meters. In some implementations, a transitional warning level can also be set between the warning level and the emergency level warning.

[0055] Through the above scheme, this application achieves spatiotemporal alignment and fusion enhancement of multi-source heterogeneous sensor data on tugboats. By establishing a unified ship-based coordinate system and a millisecond-level time synchronization mechanism, dynamic deviations are eliminated, improving the accuracy of data fusion. A multi-modal sensor collaborative mechanism based on distance thresholds achieves continuity in long-range and short-range perception, solving the problems of AIS lag and radar interference. The cross-modal projection association method enables spatial alignment between visual recognition results and radar data, enhancing the fusion of semantic information and spatial data. Asynchronous multi-rate data queues and weighted fusion strategies improve the stability and reliability of the fusion results.

[0056] This improves the tugboat's perception accuracy of target vessels and surrounding obstacles. At long range, by fusing AIS and radar data, the target vessel position prediction error can be reduced from a large meter-level range to a much smaller range. At close range, combining lidar point cloud and visual recognition results can reduce the target vessel outline fitting error. For surrounding obstacles, fusing millimeter-wave radar and visual recognition results can reduce the false alarm rate and improve detection reliability.

[0057] This method achieves continuous and stable perception from a distance of 200 meters to a distance of 5 meters, providing a high-precision, low-latency environmental model for the tugboat control system. Through multi-source data fusion and error correction, collision risk assessment becomes more accurate, providing a reliable basis for collision avoidance decisions. Simultaneously, the combination of semantic information and spatial data enhances the system's understanding of complex port environments, which is beneficial for improving the accuracy of collaborative control and emergency response capabilities.

[0058] This application further proposes using the unified time output by BeiDou RTK as a benchmark to synchronize the clocks of AIS, fiber optic compass, lidar, millimeter-wave radar, and panoramic camera. It records the time deviation of each sensor and sets a millisecond-level synchronization window, interpolating and compensating for delays in the sampling timestamps to obtain aligned data frames. Based on the fiber optic compass, bow zero-point calibration is performed, establishing a ship-wide coordinate system with the BeiDou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis. The installation offset parameters of AIS, fiber optic compass, BeiDou RTK, lidar, millimeter-wave radar, and panoramic camera are read, and the rigid body transformation matrix of each sensor to the ship-wide coordinate system is solved. The lidar and panoramic camera complete extrinsic parameter calibration through a joint calibration target, while the millimeter-wave radar completes extrinsic parameter calibration by aligning the beam pointing with the reflecting target. The point cloud features from the lidar, the echo from the millimeter-wave radar, and the recognition results from the panoramic camera are projected onto the ship's coordinate system. The heading and position calculated by the fiber optic compass and BeiDou RTK are compared. If the projection error and heading error exceed the error threshold, the corresponding camera extrinsic parameters and time deviations are corrected. In some implementations, BeiDou RTK can employ a dual-antenna configuration to output position and heading information, which, together with the heading information from the fiber optic compass, can be used for error verification.

[0059] Clock synchronization is achieved by unifying the clocks of all sensors using the time reference provided by BeiDou RTK, and interpolation and delay compensation are employed to eliminate millisecond-level time deviations. The ship's coordinate system uses the BeiDou RTK antenna projection point as the origin, and the bow axis is calibrated using a fiber optic compass to establish a spatial reference. The rigid body transformation matrix is ​​calculated based on the sensor installation offset parameters. The lidar and panoramic camera acquire extrinsic parameters through calibration targets, and the millimeter-wave radar completes extrinsic parameter calibration by aligning with a reflected target. Projection error verification is achieved by comparing the fiber optic compass data with the BeiDou RTK data; if the error exceeds the limit, the extrinsic parameters or time deviation are corrected.

[0060] Specifically, using the time signal output by BeiDou RTK as a reference, clock synchronization is performed on sensors such as AIS and fiber optic compasses. Interpolation algorithms are used to align the timestamps of data from each sensor to a unified time axis, eliminating millisecond-level deviations caused by different clock sources. Zero-point calibration is performed based on the heading angle output by the fiber optic compass, establishing a coordinate system with the BeiDou RTK antenna position as the origin to ensure spatial reference consistency. Installation offset parameters of the lidar and panoramic camera are obtained through joint calibration targets, and rigid body transformation matrices are calculated to convert sensor data to the ship's coordinate system. The millimeter-wave radar completes extrinsic parameter calibration by analyzing the spatial relationship between the beam pointing and the reflected target. After projecting the lidar point cloud, millimeter-wave radar echo, and camera recognition results onto the ship's coordinate system, the heading angle and position coordinates calculated by the fiber optic compass and BeiDou RTK are compared. If the projection error exceeds a preset threshold, the camera's extrinsic parameters or time compensation parameters are adjusted in reverse. For example, when the deviation between the lidar point cloud projection position and the BeiDou RTK calculated position exceeds 0.5 meters, the lidar's extrinsic parameter matrix is ​​recalibrated. This enables the unification of spatiotemporal references across multiple sensors, improving the accuracy of subsequent data fusion.

[0061] As a preferred embodiment, the solution of this application is specifically implemented as follows: During initialization and time synchronization, and when establishing the ship's coordinate system, the following steps are included: Using the unified time output by BeiDou RTK as a reference, clock synchronization is performed on AIS, fiber optic compass, lidar, millimeter-wave radar and panoramic camera, the time deviation of each sensor is recorded and a millisecond-level synchronization window is set, and the sampling timestamp is interpolated and delayed to obtain aligned data frames.

[0062] Specifically, the process begins by reading the UTC time output from the BeiDou RTK receiver as the reference time. Then, the timestamps from the AIS, fiber optic compass, lidar, millimeter-wave radar, and panoramic camera are read. For each sensor, the time deviation from the reference time is calculated and recorded. Next, a 10-millisecond synchronization window is set, and data falling within this window is linearly interpolated to align with the reference time. For sensors with lower sampling frequencies, such as AIS, forward padding is used for delay compensation. Finally, aligned data frames from all sensors at the same time point are obtained.

[0063] Based on the fiber optic compass, the bow zero point is calibrated, and a ship coordinate system is established with the Beidou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis.

[0064] First, the heading angle output from the fiber optic compass is read and used as the positive x-axis of the ship's coordinate system. Then, the installation position of the BeiDou RTK antenna on the ship is determined, and its vertical projection point is used as the origin of the coordinate system. A right-handed coordinate system is established with the heading as the positive x-axis and the direction perpendicular to the x-axis towards port as the positive y-axis. In this way, a local coordinate system with the ship as the reference is established.

[0065] The installation offset parameters of AIS, fiber optic compass, Beidou RTK, lidar, millimeter-wave radar and panoramic camera are read, and the rigid body transformation matrix of each sensor to the ship's coordinate system is solved. The lidar and panoramic camera complete the external parameter calibration by jointly calibrating the target, and the millimeter-wave radar completes the external parameter calibration by aligning the beam pointing with the reflecting target.

[0066] For example, for lidar and panoramic cameras, a calibration target with a checkerboard pattern is used to simultaneously acquire lidar point clouds and camera images. By identifying the positions of the checkerboard corners in the images and point clouds, the relative pose between the camera and lidar is calculated. For millimeter-wave radar, a high-intensity metal reflector is placed at a known location. By adjusting the radar beam direction to align with the reflector, the radar's installation position and attitude relative to the ship's hull are determined. Finally, the installation positions and attitudes of all sensors are uniformly transformed into the ship's coordinate system with the BeiDou RTK antenna as the origin.

[0067] The point cloud features of the lidar, the echo of the millimeter-wave radar, and the recognition results of the panoramic camera are projected onto the ship's coordinate system. The heading and position calculated by the fiber optic compass and Beidou RTK are compared. If the projection error and heading error exceed the error threshold, the corresponding camera extrinsic parameters and time deviation are corrected.

[0068] Specifically, the lidar point cloud, millimeter-wave radar echo, and panoramic camera recognition results are first projected onto the ship's coordinate system using the rigid body transformation matrix obtained earlier. Then, some feature points, such as the dock edge and buoys, are selected, and their positions in different sensor data are compared. Simultaneously, the heading given by the fiber optic compass is compared with the heading calculated by BeiDou RTK. If the projected position error exceeds a preset threshold (e.g., 0.5 meters), or the heading error exceeds a preset threshold (e.g., 2 degrees), it is considered that there may be calibration or time synchronization errors. In this case, the camera's extrinsic parameter matrix is ​​fine-tuned or the time deviation is adjusted to achieve better spatial and temporal consistency among the sensor data.

[0069] Through the above technical solutions, this application achieves precise time synchronization and spatial alignment of multi-sensor data. This improves the accuracy and reliability of subsequent fusion processing. Simultaneously, by establishing a unified shipboard coordinate system, the coordinate transformation process for different sensor data is simplified, which is beneficial for improving computational efficiency. Furthermore, by verifying and correcting extrinsic parameters in real time, the system's adaptability to installation errors and environmental changes is enhanced, ensuring long-term operational stability.

[0070] This application further proposes unpacking AIS messages to extract MMSI information, latitude and longitude, speed, heading, length, beam, and installation distance parameters from the AIS antenna to the bow, stern, port, and starboard sides, forming a state sequence. Threshold rules and sliding window filtering are applied to the state sequence to remove data frames with out-of-bounds latitude and longitude, excessive time intervals, sudden speed changes, and heading jumps. A unique match is performed based on the MMSI information to determine the target ship. The target ship's latitude and longitude are converted from geographic coordinates to the ship's own coordinate system. Lever compensation is then performed using the target ship's AIS antenna installation distance parameters and the ship's BeiDou RTK antenna installation offset parameters to estimate the target ship's geometric center coordinates in the ship's own coordinate system, obtaining the initial position.

[0071] The installation distance parameters extracted during AIS message unpacking include the longitudinal distance from the bow to the AIS antenna and the lateral distance from the port side to the AIS antenna, used for subsequent geometric compensation. Sliding window filtering preferably uses a window of several consecutive frames or a corresponding preset duration, based on the AIS message update frequency, to remove data showing sudden changes in speed and heading. Lever arm compensation is achieved through coordinate transformation, converting the geographic coordinates of the target ship's AIS antenna to its geometric center coordinates, while simultaneously compensating for the offset between the ship's BeiDou RTK antenna and the ship's reference point.

[0072] Specifically, after unpacking the AIS message, a state sequence containing MMSI identifiers is generated, and invalid data is filtered out using threshold rules. For example, when the latitude and longitude exceed the port's geographic fence range, it is considered an out-of-bounds violation. When the timestamp interval exceeds a preset freshness threshold, the corresponding data frame is not used as the basis for initializing the current position. When the timestamp interval exceeds a preset invalidation threshold, the corresponding data frame is directly discarded. Sliding window filtering further eliminates instantaneous noise from speed and heading, preserving a continuous and stable motion state. Unique matching based on MMSI ensures data association for the same vessel, avoiding target confusion. During geographic coordinate transformation, Gaussian projection is used to convert the latitude and longitude of the target ship's AIS antenna into planar coordinates, and then ship hull parameter compensation is superimposed. The deviation between the ship's Beidou RTK antenna and the ship's reference point is corrected through coordinate rotation and translation compensation, and the final output is the initial position of the target ship that can be used for Kalman filter initialization.

[0073] As a preferred embodiment, the solution of this application is specifically implemented as follows: The AIS message is unpacked, and MMSI information, latitude and longitude, speed, heading, length, beam, and installation distance parameters from the AIS antenna to the bow, stern, port, and starboard sides are extracted to form a state sequence. Further, threshold rules and sliding window filtering are applied to the state sequence to remove data frames with out-of-bounds latitude and longitude, excessive time intervals, sudden speed changes, and heading jumps. Preferably, the time freshness threshold can be set to 10 seconds, the failure threshold to 3 minutes, the speed change threshold to 5 knots per second, and the heading jump threshold to 30 degrees per second. Based on this, a unique match is performed using the MMSI information to determine the target ship.

[0074] For example, a sliding window matching the AIS update frequency can be used to smooth the state sequence, preferably a 15-30 second sliding window to suppress outliers. For a given target ship, its latitude and longitude are converted from geographic coordinates to ship-specific coordinates. Furthermore, lever arm compensation is performed using the target ship's AIS antenna installation distance parameters and the ship's BeiDou RTK antenna installation offset parameters to estimate the target ship's geometric center coordinates in the ship's own coordinate system, thus obtaining the initial position.

[0075] Through the above technical solutions, this application achieves effective parsing and processing of AIS message data, improving the accuracy of target vessel position estimation. Threshold rules and sliding window filtering effectively eliminate outlier data, enhancing the stability of the state sequence. Unique matching using MMSI information ensures correct target vessel identification. Coordinate transformation and lever arm compensation improve the accuracy of the initial target vessel position estimation. This method effectively enhances the tugboat's perception of target vessels, providing reliable data support for precise berthing and coordinated operation in complex port environments.

[0076] This application further proposes establishing a target ship state vector in the ship's coordinate system, using a uniform turning model (CT) as the process model, setting state transitions based on the time interval between adjacent data frames, and using Kalman filtering for prediction updates to output the target ship's prior position. The initial position of the target ship is combined with the speed and heading from the AIS message data to form an AIS measurement. When the lidar obtains the target ship's boundary point cloud, the relative position of the center point is extracted as the lidar measurement. When the millimeter-wave radar detects the target ship, the planar position and velocity components are obtained through geometric conversion using radial distance and relative velocity as the millimeter-wave radar measurement. Consistency checks are performed on measurements from different sensors, and measurements that fail the consistency check are removed. A weighted average of the AIS, lidar, and millimeter-wave radar measurements is obtained based on time freshness to obtain a fused measurement. The posterior position of the target ship is then obtained in the ship's coordinate system based on the fused measurement. The prior position is compared with the posterior position. When the positions are consistent, the target ship's position, speed, and heading information are obtained. When the positions are inconsistent, the target ship's position, speed, and heading information are obtained based on the posterior position, and process noise is corrected based on error analysis.

[0077] The uniform turning model (CT) is used to describe the target ship's motion. The state vector preferably includes two-dimensional position, two-dimensional velocity components, heading angle, and turning angular velocity. The time interval between adjacent data frames is set as the time parameter for state transition, and the process noise covariance matrix is ​​initialized based on the target ship's motion characteristics. AIS-measured speed and heading data are directly obtained through message parsing; lidar measurements are generated through point cloud clustering and geometric center calculation; and millimeter-wave radar measurements are obtained through polar coordinate to planar coordinate conversion. Consistency testing uses a Mahalanobis distance threshold to determine the degree of matching between measured and predicted states, and the time freshness weight is dynamically adjusted based on the update time difference of sensor measurements. Error analysis uses the residual covariance matrix to evaluate the matching degree between process noise and measurement noise, and the corrected process noise is used in the next round of Kalman filtering iterations.

[0078] Specifically, the target ship's state vector can be modeled as a six-dimensional vector in the ship's coordinate system, including two-dimensional position, two-dimensional velocity components, heading angle, and turning angular velocity. The state transition matrix of the uniform turning model (CT) is calculated based on the time interval between adjacent data frames, and the Kalman filter prediction step outputs the target ship's prior position. The speed and heading data measured by AIS are combined with the initial position to form a complete measurement vector. The lidar extracts the target ship's geometric center coordinates from the point cloud boundary as a position measurement, and the millimeter-wave radar calculates the planar position and velocity components using radial distance and Doppler velocity. After the measurements from different sensors are verified by Mahalanobis distance, they are weighted according to the update time difference, weighted and fused, and then input into the Kalman filter update step to obtain the posterior position. When the deviation between the prior and posterior positions exceeds a preset threshold, the residual covariance matrix is ​​used to adjust the process noise parameters, thereby optimizing the prediction accuracy of the next round of filtering. Through the dynamic correction of process noise and the fusion mechanism of multi-source measurements, the target ship's position, speed, and heading information remain stable under complex operating conditions. As a preferred embodiment, the solution of this application is specifically implemented as follows: The target ship's state vector is established in the local coordinate system. The uniform turning model (CT) is used as the process model. The state transition is set according to the time interval between adjacent data frames. Kalman filtering is used for prediction and update to output the prior position of the target ship.

[0079] The initial position of the target ship is combined with the speed and heading data from the AIS message to form AIS measurements. When the lidar obtains the target ship's boundary point cloud, the relative position of the center point is extracted as the lidar measurement. When the millimeter-wave radar detects the target ship, the radial distance and relative velocity are geometrically converted to obtain the planar position and velocity components as the millimeter-wave radar measurement.

[0080] Consistency checks are performed on measurements from different sensors, and measurements that fail the consistency check are removed. AIS measurements, lidar measurements, and millimeter-wave radar measurements are weighted based on time freshness to obtain fused measurements. The posterior position of the target ship is obtained in the ship's coordinate system based on the fused measurements.

[0081] The prior position is compared with the posterior position. When the distance difference between the prior and posterior positions is not greater than a preset position consistency threshold, the target ship's position, speed, and heading information are obtained. When the distance difference is greater than the preset position consistency threshold, the target ship's position, speed, and heading information are obtained based on the posterior position, and process noise is corrected based on error analysis.

[0082] Through the above technical solutions, this application achieves accurate tracking and position prediction of target vessels. Multi-source sensor data fusion improves the accuracy and reliability of target vessel position, speed, and heading information. Kalman filtering for state estimation effectively handles measurement noise and system errors, improving tracking accuracy. A measurement fusion strategy based on consistency checks and time freshness effectively reduces the impact of abnormal data and improves the stability of the fusion results. Comparison of prior and posterior positions and dynamic correction of process noise further enhance the adaptability and robustness of the tracking algorithm. This solution provides reliable target vessel state information for precise berthing and coordinated operation of tugboats in complex port environments, effectively improving the tugboat's environmental awareness and operational safety.

[0083] In some of the solutions mentioned above in this application, during the process of calculating relative distance and relative azimuth based on AIS, fiber optic compass and Beidou RTK, the time synchronization error and coordinate transformation deviation of the sensor data under long-distance conditions result in insufficient accuracy in calculating the relative position between the geometric center of the target ship and the reference point of the ship, which cannot meet the stability requirements of long-distance tracking.

[0084] This application further proposes to calculate the straight-line distance between the geometric center of the target ship and the reference point of the ship in the ship's coordinate system as the relative distance, and to determine the angle between the target ship and the bow of the ship based on the heading angle provided by the fiber optic compass as the relative bearing, and output the relative distance and relative bearing as the solution results for long-distance working conditions.

[0085] The ship's coordinate system uses the BeiDou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis. Zero-point calibration is performed using a fiber optic compass to ensure the coordinate system's orientation aligns with the ship's actual course. The target ship's geometric center coordinates are converted to the ship's coordinate system using latitude and longitude parsed from AIS messages, and antenna installation position deviations are corrected using a lever arm. Relative distance calculations employ the Euclidean distance formula, directly reflecting the straight-line spatial distance between the two ships' center points. Relative bearing calculations use the ship's bow as a reference, employing vector projection or trigonometric functions to solve for the target ship's center point's azimuth angle in the ship's coordinate system, eliminating conversion errors between the geographic coordinate system and the local coordinate system.

[0086] Specifically, in long-distance operation, when the distance between the target ship and the center point of the main ship exceeds a set threshold, the AIS message parsing obtains the target ship's latitude and longitude. Combined with the real-time position of the main ship provided by BeiDou RTK and the heading angle provided by the fiber optic compass, the target ship's geographical coordinates are converted into local coordinates in the main ship's coordinate system. A lever-arm compensation algorithm corrects the installation offset between the AIS antenna and the geometric center of the target ship, obtaining accurate coordinates of the target ship's center point. Subsequently, based on the coordinate difference between the main ship's reference point and the target ship's center point, the distance components in the x and y axes are calculated, and the straight-line distance is obtained using the Pythagorean theorem as the relative distance. Simultaneously, based on the target ship's center point's coordinate position in the main ship's coordinate system and the main ship's bow angle, the arctangent function is used to calculate the angle between the target ship and the main ship's bow, outputting the relative azimuth angle. This process, through a unified coordinate system and real-time heading calibration, avoids the cumulative error of coordinate transformation during multi-sensor data fusion, improving the accuracy of relative position and azimuth calculations in long-distance operation.

[0087] Through the above technical solution, this application achieves accurate perception of the target vessel's position at long distances. By employing multi-source data fusion from AIS, fiber optic compass, and BeiDou RTK, the shortcomings of low update frequency and susceptibility to interference associated with single AIS data are overcome, improving the accuracy and reliability of long-range target tracking. Simultaneously, by transforming geographic coordinates to the vessel's coordinate system, subsequent processing is simplified, providing a unified coordinate reference for close-range perception. Furthermore, the calculation of relative distance and relative bearing provides an intuitive reference for tugboat control decisions, contributing to improved safety and efficiency in tugboat operations.

[0088] This application further proposes to utilize lidar to acquire the boundary point cloud of the target ship, calculate the lateral distance, longitudinal distance, and center point bounding box, and simultaneously detect the distance and relative velocity of surrounding obstacles based on millimeter-wave radar. This includes: setting a region clipping window within the ship's coordinate system based on the target ship's posterior position; preprocessing the lidar point cloud, performing motion compensation and surface noise removal; and clustering within the region clipping window to obtain the target ship point cloud cluster. Boundary extraction and bounding box fitting are performed on the target ship point cloud cluster to determine the target ship's center point and external dimensions. The forward and lateral components from the ship's reference point to the center point are calculated using the bow and port axes of the ship's coordinate system, respectively, to obtain the longitudinal and lateral distances, and simultaneously obtain the center point bounding box. The millimeter-wave radar echoes are detected and clustered to obtain the distance and relative velocity of the echo targets. Echoes falling within the center point bounding box and its neighborhood are associated with the target ship measurement, while unassociated echoes are associated with surrounding obstacles to obtain the distance and relative velocity of the nearest obstacle. Within the distance threshold range, the lateral distance, longitudinal distance, and center point outline obtained by lidar are the main measurements, and the distance and relative velocity of millimeter-wave radar are used for verification and supplementation to obtain a measurement set for close-range working conditions.

[0089] The region clipping window dynamically adjusts its spatial range based on the target ship's posterior position. The window size can be set according to the target ship's dimensions and tracking uncertainty, preferably expanded to 1.5 to 2 times the ship's length and 2 to 3 times its width. Surface noise removal can employ a height threshold method or a reflection intensity constraint method, preferably considering point clouds below 0.2 meters below the water surface reference plane as candidate invalid data and removing them in conjunction with continuous frame consistency. Boundary extraction can use convex hull, α-shape, or other boundary extraction algorithms suitable for sparse point clouds, and bounding box fitting can use minimum bounding rectangle fitting. Millimeter-wave radar echo correlation can be performed based on a neighborhood distance threshold for the bounding box, preferably set to 1 to 1.5 times the feature side length of the bounding box. These parameters can be calibrated based on port operation data statistics, sensor deployment spacing, and target ship dimensions.

[0090] Specifically, when the target ship enters the distance threshold range, the LiDAR point cloud undergoes motion compensation to eliminate the influence of the ship's displacement, and surface noise is removed, retaining the valid target ship point cloud. A region clipping window narrows the processing range, and clustering eliminates isolated noise points, forming a target ship point cloud cluster. A convex hull algorithm extracts the boundary vertices of the point cloud cluster, and a least-squares method fits a rectangular frame to determine the center point coordinates. Based on the ship's coordinate system, forward and lateral components are calculated, outputting precise longitudinal and lateral distances. Millimeter-wave radar echoes are spatially correlated to distinguish the target ship from surrounding obstacles. Correlated echoes from the target ship are used to verify LiDAR measurements, while uncorrelated echoes are output independently as obstacle information. The two types of sensor data are spatiotemporally aligned and then fused complementaryly. LiDAR provides high-precision position information, while millimeter-wave radar supplements velocity parameters, jointly constructing a complete measurement set for close-range conditions. This scheme effectively improves close-range perception accuracy and reduces the impact of single-sensor errors on obstacle recognition through multimodal data collaborative processing.

[0091] As a preferred embodiment, the solution of this application is specifically implemented as follows: Based on the target ship's posterior position, a region clipping window is set within the ship's coordinate system. The LiDAR point cloud is preprocessed, motion compensated, and surface noise removed. Clustering and segmentation are then performed within the clipping window to obtain the target ship's point cloud clusters. Specifically, firstly, a rectangular region clipping window is set within the ship's coordinate system based on the predicted position of the target ship. The window size is dynamically adjusted according to the target ship's dimensions. Then, the raw point cloud data acquired by the LiDAR is preprocessed, including outlier removal and downsampling. Further, motion compensation is performed on the point cloud based on the ship's motion parameters to eliminate motion distortion during the scanning process. Thus, surface noise such as water reflections is removed using height thresholds and reflection intensity features. Finally, Euclidean clustering is applied within the clipping window to extract the target ship's point cloud clusters.

[0092] Boundary extraction and bounding box fitting are performed on the target ship's point cloud cluster to determine the target ship's center point and overall dimensions. The forward and lateral components from the ship's reference point to the center point are calculated using the bow and port axes of the ship's coordinate system, respectively, yielding the longitudinal and transverse distances, and simultaneously obtaining the center point bounding box. For example, the α-shape algorithm is used to extract the boundary points of the point cloud cluster, and then the minimum bounding rectangle algorithm is used to fit the bounding box, obtaining the target ship's center point coordinates and length, width, and height dimensions. Further, the center point coordinates are decomposed onto the x-axis (bow) and y-axis (port) of the ship's coordinate system to obtain the longitudinal and transverse distances relative to the ship's reference point.

[0093] The echoes from millimeter-wave radar are detected and clustered to obtain the range and relative velocity of the echo targets. Echoes falling within the spatial bounding box of the center point and its neighborhood are associated with the target ship measurement, while other unassociated echoes are used as surrounding obstacle measurements to obtain the range and relative velocity of the nearest obstacle. Specifically, firstly, constant false alarm rate (CFAR) detection is performed on the raw millimeter-wave radar echo data to extract valid target echoes. Then, the DBSCAN algorithm is used to cluster the echo points, obtaining the range and Doppler velocity information of multiple targets. Further, the echo points are projected onto the ship's coordinate system to determine whether they fall within the target ship bounding box and its neighborhood obtained by the lidar. Thus, echoes falling within this range are associated with the target ship measurement, while the remaining echoes are used as surrounding obstacle measurements. Finally, the nearest target is selected from the obstacle measurements, and its range and relative velocity are output.

[0094] Within a range threshold, the lateral distance, longitudinal distance, and center point outline obtained by lidar are the primary measurements, supplemented by distance and relative velocity data from millimeter-wave radar to obtain a measurement set for close-range conditions. For example, when a target ship enters the preset range threshold, the high-precision measurement results of lidar are prioritized. Simultaneously, distance and velocity information from millimeter-wave radar are used for cross-validation; if the difference between the two exceeds a preset threshold, data fusion is performed to improve measurement reliability. Furthermore, millimeter-wave radar is more sensitive to metallic targets and can be used to supplement the detection blind spots of lidar in rainy or foggy weather.

[0095] Through the above technical solutions, this application achieves precise tracking of the target vessel and comprehensive perception of surrounding obstacles. The complementary advantages of lidar and millimeter-wave radar improve the accuracy and reliability of short-range measurements. By setting distance thresholds and dynamically clipping windows, the computational load of data processing is reduced. Multi-sensor fusion enhances the system's adaptability in complex port environments, providing reliable environmental perception support for the safe operation of tugboats.

[0096] In some of the solutions described above in this application, although a unified ship coordinate system is established through sensor extrinsic parameter calibration and time synchronization, in practical applications, there is still a problem of spatial projection inconsistency between the visual recognition results output by the panoramic camera and the radar data, which makes it difficult to effectively correlate cross-modal data and affects the fusion accuracy.

[0097] This application further proposes a scheme to perform target recognition on images acquired by a panoramic camera, output semantic-level recognition results for ships, buoys, and shorelines, and spatially project and align them with radar data.

[0098] The preprocessing steps eliminate optical distortion and illumination differences through distortion correction and brightness normalization. The target recognition model generates bounding boxes or pixel masks based on deep learning algorithms. The orientation set is constructed using a line-of-sight ray model within the camera. Cross-modal association filters and matches point cloud clusters based on projection overlap and spatiotemporal consistency. Reference plane fitting uses LiDAR point clouds to establish a geometric model of the water surface. Spatial position calculation achieves 3D coordinate transformation by finding the intersection of the line-of-sight ray and the reference plane.

[0099] Specifically, images captured by the panoramic camera, after distortion correction and brightness normalization, are input into the target recognition model, which outputs bounding boxes or pixel masks with category labels. LiDAR point clouds are projected onto the image plane, and visual recognition results are associated with point cloud clusters through overlap and timestamp matching. This spatially aligns the output visual measurement data with the radar data, eliminating cross-modal data bias.

[0100] Understandably, panoramic cameras and LiDAR are typically mounted at a certain height, such as on dock cranes, shore monitoring poles, or ship roofs. For sensors mounted on the shore, the near field refers to the area directly below the sensor, which physically corresponds to the water area adjacent to the shore. Therefore, the main physical object in the near field area directly below the sensor is the water itself. Although the shoreline is also near, it is usually located at the extreme edge of the field of view or outside of it. When using correlated point clouds, due to perspective, the bottom of the image corresponds to the near field, which is usually a large area of ​​water. Therefore, only the LiDAR points in this area are extracted, naturally avoiding distant ships. The angular resolution of a lidar is fixed. The closer the distance, the more laser points per unit area there are. In the near-field water surface, the laser points are very dense, forming hundreds or thousands of points. When using a plane fitting algorithm, the algorithm will look for the plane model with the most supporting points. Because the near-field water surface points are extremely dense, they constitute the largest geometric cluster in the point cloud. In comparison, the small amount of noise mixed in, such as nearby ripples, birds, or floating objects, cannot shake the dominance of the water surface plane on an order of magnitude. This achieves the near-field water surface reference plane in this embodiment, which avoids interference by using geometric perspective and physical distance attenuation, resulting in an efficient and accurate water surface reference plane.

[0101] As a preferred embodiment, the solution of this application is specifically implemented as follows: The panoramic camera images are preprocessed, including distortion correction, brightness normalization, and wave reflection suppression. Further, an existing target recognition model capable of identifying ships, buoys, and shorelines is used to process the preprocessed images, outputting bounding boxes or pixel masks of the ships, buoys, and shorelines, along with their category labels.

[0102] The LiDAR point cloud is first transformed into the camera coordinate system and projected onto the image plane. Cross-modal association is then performed between the visual recognition result and the LiDAR point cloud cluster based on the projection overlap and temporal consistency. Preferably, the association is considered successful when the overlap between the projected area of ​​the point cloud cluster and the bounding box or pixel mask is not less than 30%, and the difference between the image timestamp and the point cloud timestamp is not greater than 100 milliseconds. If the same recognition result corresponds to multiple point cloud clusters, the point cloud cluster with the closest projection centroid to the center of the recognition area and the highest overlap is selected as the associated point cloud cluster.

[0103] After obtaining the associated point cloud clusters, instead of directly fitting the water surface reference plane using the associated point cloud clusters, water surface point cloud segmentation is first performed. Specifically, the point cloud within the range of 5 to 80 meters forward from the ship's coordinate system, ±40 meters laterally, and 0.5 meters above and below the current water level estimate vertically is used as the near-field candidate point set. Points belonging to the target ship, buoys, shore walls, and other obstacles are removed from the near-field candidate point set. The removal method is as follows: the projection area of ​​the associated target's point cloud is extended outward by five pixels on the image plane to form an exclusion area. Point clouds projected into the exclusion area are not considered as water surface candidate points. The local normal vector and local elevation undulation are further calculated for the remaining point cloud. Only points whose normal vector has an angle of no more than 10 degrees with the vertical axis of the ship's coordinate system and whose neighborhood elevation standard deviation is no more than 0.08 meters are retained as water surface candidate points. Then, the random sampling consensus algorithm is used to fit the candidate points on the water surface to the plane to obtain the near-field water surface reference plane. When the average distance from the fitted point to the plane is greater than 0.1 meters or the number of effective in-point points is less than the preset minimum number of points, the fitting result of the current frame is abandoned and the effective water surface reference plane of the previous moment is called as the current reference plane.

[0104] After determining the near-field water surface reference plane, feature pixel point selection for intersection is performed. For targets such as ships and buoys, the geometric center of the bounding box is not used as the intersection point; instead, the lower edge feature point corresponding to the water surface contact position is preferred. When the recognition result is a pixel mask, one or more contour points located within the lowest 15% height band and with the smallest distance from the lower boundary of the associated point cloud cluster projection are selected as lower edge feature points. When the recognition result is only a bounding box, the midpoint of the lower edge of the bounding box is used as the initial point, and the final lower edge feature point is determined by combining the lower boundary of the associated point cloud cluster projection within the preset search windows on its left and right. For targets such as shore walls, the lower edge feature point corresponding to the water surface boundary in the shore wall mask or bounding box is preferred. If the lower edge of the shore wall is occluded in the image, the projection position of the boundary point with the lowest elevation and closest to the water surface reference plane in the associated point cloud cluster is used as a substitute feature point. By using the above method, we can ensure that the pixels participating in the intersection calculation correspond to the part of the target that is in contact with or closest to the water surface, thus avoiding the use of pixels in the middle of the object, which would cause the intersection position to fall behind the target.

[0105] The lower edge feature points are converted into a line-of-sight ray in the camera coordinate system. Specifically, the pixel coordinates of the lower edge feature points are converted into a normalized line-of-sight direction using the camera intrinsic parameter matrix. Then, based on the extrinsic parameter matrix between the camera and the ship's coordinate system, the line-of-sight direction is converted into the ship's coordinate system, forming a spatial line-of-sight ray originating from the camera's optical center. Subsequently, the intersection point of the spatial line-of-sight ray and the near-field water surface reference plane is calculated. Only when the ray and the plane have a unique positive intersection point is the intersection point considered as a candidate spatial location. If the ray is parallel to the plane, the intersection point is located behind the camera, or the intersection point exceeds the preset near-field working area, the candidate spatial location is deemed invalid.

[0106] When a target corresponds to multiple lower edge feature points, robust convergence is performed on multiple valid intersection points. The median or weighted average method is preferably used to obtain the visual spatial position of the target in the ship's coordinate system. The weights can be determined based on the distance between the feature point and the lower boundary of the projected point cloud cluster, local image sharpness, and time synchronization error. Further, the visual spatial position and the geometric center position of the associated point cloud cluster in the ship's coordinate system are checked for consistency. When the planar distance between them is no greater than one meter, the visual spatial position is considered valid, and the category, spatial position, and relative orientation are output as visual measurements. When the planar distance is greater than one meter, the visual spatial position is not output; only the category label and image display results are retained as semantic auxiliary information.

[0107] In some embodiments, when a valid near-field water surface reference plane cannot be obtained in the current frame, or when the planar distance between the candidate spatial position obtained from the lower edge feature points and the geometric center of the associated point cloud cluster is greater than a preset consistency threshold, the visual spatial position of the current frame is not output. Only the category label and image display result are retained as semantic auxiliary information, and the water surface point cloud segmentation, near-field water surface reference plane fitting, and lower edge feature point intersection process are re-executed in the next frame. Preferably, the consistency threshold is 1 meter.

[0108] Through the above technical solution, this application does not fit the water surface by arbitrarily using point clouds, nor does it intersect the water surface by arbitrarily using image pixels. Instead, it first segments the water surface point cloud, then selects the lower edge feature points corresponding to the contact position between the target and the water surface to perform intersection calculation, and performs consistency verification through the point cloud position. This achieves feasible spatial alignment between the panoramic camera recognition result and the lidar point cloud.

[0109] Meanwhile, by fitting a water surface reference plane, the two-dimensional image recognition results are converted into three-dimensional spatial positions, enhancing the spatial positioning capability of visual perception. Furthermore, outputting measurement results aligned with the ship's coordinate system helps improve the tugboat's environmental awareness and operational safety.

[0110] In some of the solutions described above in this application, multi-source sensor data suffer from problems such as time asynchrony, inconsistent formats, and redundant or conflicting measurements, which makes data alignment difficult during the fusion process and affects the sensing accuracy and reliability.

[0111] This application further proposes aligning all data under a unified timestamp and establishing an asynchronous multi-rate data queue. Measurements are correlated based on consistency checks and spatiotemporal proximity constraints, eliminating inconsistent and duplicate measurements. Relative distances and relative orientations, as well as lateral and longitudinal distances, are aggregated according to long-distance and short-distance operating conditions to obtain a fused measurement set for fusion.

[0112] The asynchronous multi-rate data queue aligns sensor data from different sampling frequencies through timestamp interpolation, ensuring data processing under a unified time reference. Consistency checks determine whether measurements belong to the same target using statistical distribution or geometric constraints, avoiding erroneous associations. Spatiotemporal proximity constraints combine the target motion model with the sensor measurement range to filter valid association regions. The distinction between long-range and short-range operating conditions is based on the distance threshold between the target ship and the ship itself, aggregating different measurement types to adapt to the fusion algorithm.

[0113] Specifically, under a unified timestamp, interpolation compensation is performed on data from AIS, fiber optic compass, BeiDou RTK, LiDAR, millimeter-wave radar, and panoramic cameras to generate time-aligned data frames and store them in an asynchronous queue. Based on the timestamps and spatial locations of each measurement in the queue, consistency checks are performed using Mahalanobis or Euclidean distance to eliminate abnormal measurements that exceed the error ellipsoid or confidence interval. Through spatiotemporal proximity constraints, measurements in spatially overlapping areas within the same time period are correlated to eliminate redundant measurements. In long-range scenarios, the relative distances and azimuths calculated by AIS and BeiDou RTK are converged. In short-range scenarios, the lateral and longitudinal ranges of LiDAR and the obstacle distances of millimeter-wave radar are converged. Through scenario segmentation and measurement convergence, an input set adapted to the multi-source fusion algorithm is formed, improving the robustness of the fusion results.

[0114] As a preferred embodiment, the solution of this application is implemented as follows: Under a unified time reference, a buffer queue containing asynchronous multi-rate data is established. This queue is arranged in ascending order of timestamps and allows data from different sensors to be stored at the original sampling rate. Timestamp alignment is performed on each data frame in the queue, with the alignment error preferably controlled within 10 milliseconds. Data frames exceeding the synchronization window are marked as delayed and a compensation mechanism is triggered. Based on the target ship's identity and spatial location information, cross-sensor correlation is performed on measurements from AIS, lidar, and millimeter-wave radar. The correlation condition can be set to a spatial location deviation of no more than 3 meters, and is checked in conjunction with velocity component consistency. For millimeter-wave radar obstacle echoes that fail the correlation test, if their spatial overlap rate with the target ship's outline is lower than a preset overlap threshold, they are determined to be independent obstacle measurements. In long-range operation, the relative bearing calculated by AIS is combined with the ship's heading output by the fiber optic compass to generate bearing data in the fused measurement set. In close-range operation, the horizontal and vertical distances extracted by the lidar are bound to the relative velocity verified by the millimeter-wave radar to form a spatial motion state measurement group.

[0115] Through the above technical solution, this application effectively solves the problem of fusion inaccuracies caused by inconsistent time bases in multi-source asynchronous sensor data. By establishing strict spatiotemporal correlation rules, redundant measurements and erroneous correlations are reduced, achieving continuous and stable perception of the target ship's motion status under both long-range and short-range conditions. Furthermore, this solution improves the completeness and reliability of obstacle detection in complex port environments by dynamically adjusting the data aggregation strategy.

[0116] In some of the solutions described above in this application, after the multi-source heterogeneous sensor data is time-stamped and correlated with measurements, there are still problems such as data asynchrony, measurement noise differences, and difficulty in quantifying the confidence of different sensors. This makes it difficult to accurately estimate the uncertainty of the fusion result and affects the reliability of early warning decisions.

[0117] This application further proposes a method to solve the fusion measurement set using weighted least squares in the ship's coordinate system, obtaining the fusion result and uncertainty. The fusion weights are determined based on measurement covariance, time freshness, message quality, point cloud density, and identification confidence. The fusion result includes the target ship's lateral distance, longitudinal distance, center point distance, and relative bearing compared to the ship.

[0118] The weighted least squares solution process uses the measurement error covariance matrix of different sensors as initial weights, combines message quality indicators to assess the credibility of AIS data, applies decay weighting to delayed data using a time freshness function, and adjusts the weight contributions of LiDAR and panoramic camera using point cloud density and recognition confidence, respectively. Measurement covariance is obtained through sensor calibration parameters and historical statistics; time freshness uses an exponential decay model; message quality is calculated based on AIS message parsing completeness, checksum pass rate, and temporal continuity; point cloud density is quantified by the number of effective point clouds per unit area; and recognition confidence is determined by the classification probability output of the target recognition model.

[0119] Specifically, after aligning multi-sensor data under a unified timestamp, the latitude and longitude conversion errors of AIS measurements, the position estimation errors of LiDAR point clouds, the distance measurement errors of millimeter-wave radar, and the projection errors of panoramic cameras are modeled as Gaussian distributions to form the covariance matrix of each measurement. The time freshness function uses the current time as a benchmark and applies time difference weighting to historical measurements to ensure that real-time data has a higher weight. In message quality assessment, if an AIS message has missing fields or verification errors, its weight is reduced. When the point cloud density is too low, the weight of LiDAR measurements is reduced proportionally. When the identification confidence is below a threshold, panoramic camera measurements are partially discarded. The weighted least squares algorithm iteratively optimizes the solution to find the optimal estimate of the target ship's position and outputs the trace of the covariance matrix as uncertainty. Thus, the fusion result comprehensively reflects the statistical characteristics and real-time reliability of multi-sensor measurements in the dimensions of lateral distance, longitudinal distance, center point distance, and relative azimuth, providing a highly reliable input for collision time margin calculation.

[0120] As a preferred embodiment, the specific implementation of this application is as follows: In the ship's coordinate system, a fused measurement set consisting of AIS measurements, lidar measurements, millimeter-wave radar measurements, and visual measurements is input into a weighted least squares solver. The weights of the AIS measurements are determined by the message integrity verification result and the time delay compensation coefficient; the weights of the lidar measurements are calculated based on the density distribution of the point cloud clusters and the boundary fitting residuals; the weights of the millimeter-wave radar measurements are generated through echo signal-to-noise ratio and Doppler consistency evaluation; and the weights of the visual measurements are dynamically adjusted based on target recognition confidence and projection alignment error. The covariance matrix of each measurement is jointly updated through historical statistical errors and real-time state estimation, and the time freshness weight is exponentially decayed based on the sensor sampling period and the data frame alignment time difference. During the weighted least squares solution process, the residual function includes constraint terms in four dimensions: lateral distance, longitudinal distance, center point distance, and relative azimuth. The optimal estimate of the fused position is obtained through iterative optimization, and the uncertainty ellipse parameters of the fusion result are calculated based on the Jacobian matrix and the residual covariance.

[0121] Through the above technical solution, this application effectively solves the problem of insufficient accuracy of multi-source heterogeneous sensor data fusion for tugboats in complex port environments. The dynamic weight allocation mechanism suppresses the interference of low-quality measurements on the fusion results, improves the robustness of target ship position estimation, and provides quantitative basis for collision warning decision-making by outputting uncertainty parameters, thereby enhancing the tugboat's continuous perception capability of dynamic obstacles.

[0122] This application further proposes a method for issuing early warnings based on fusion results, including: calculating collision time margin and minimum safety interval based on relative distance, relative azimuth, lateral and longitudinal distances, and the distance and relative velocity of the nearest obstacle obtained from millimeter-wave radar; and generating an early warning level based on a safety interval threshold.

[0123] The collision time margin is calculated based on the closing velocities of the target vessel and the vessel along the potential collision direction, as well as the distance between their center points, preferably obtained by dividing the center point distance by the closing velocity. When the closing velocity is not greater than 0, the collision time margin can be recorded as greater than a preset upper limit. The minimum safe separation is set as a dynamic threshold based on the target vessel's external dimensions, the vessel's motion state, and the port area conditions. The warning levels may include at least a warning level and an emergency level, and in some embodiments, a transitional level warning may also be set. Preferably, a warning level warning is triggered when the collision time margin is lower than a first time threshold or the minimum safe separation is lower than a first separation threshold. An emergency level warning is triggered when the collision time margin is lower than a second time threshold or the minimum safe separation is lower than a second separation threshold, wherein the second time threshold is lower than the first time threshold and the second separation threshold is lower than the first separation threshold.

[0124] Specifically, the early warning mechanism establishes a target ship motion model by fusing multi-sensor data and calculates the relative trajectories between the target ship and the vessel in real time. The collision time margin is determined by the relative velocity component in the fusion result to determine the closing velocity, which is then checked or corrected using radial velocity data from millimeter-wave radar. The minimum safe interval is established as a dynamic safety boundary based on the vessel's real-time position output by BeiDou RTK, the heading angle of the fiber optic compass, and the outer dimensions of the target ship. During the early warning level generation process, the minimum safe interval can be used as the basic criterion, and the collision time margin can be used as an auxiliary criterion to refine the level. The safe interval threshold can be adaptively adjusted based on the port water depth, channel width, and tugboat maneuverability. When a quay or buoy is detected, the early warning sensitivity can be appropriately increased. By quantifying the collision risk level, the early warning mechanism can provide tiered decision support in complex environments, avoiding frequent false alarms caused by a single threshold.

[0125] As a preferred embodiment, the solution of this application is implemented as follows: After the fusion result is obtained, the collision time margin is calculated based on the center point distance and the closing velocity, and the minimum safe separation is solved based on the lateral distance, longitudinal distance, target ship dimensions, and the motion state of the ship itself. When the collision time margin is less than 30 seconds or the minimum safe separation is less than 10 meters, a warning level is triggered. When the collision time margin is less than 15 seconds or the minimum safe separation is less than 5 meters, an emergency level is triggered. In some embodiments, a transitional warning level can also be set between the warning level and the emergency level. After the warning level is generated, the operator is simultaneously alerted through an audible and visual alarm device and the bridge display screen, and suggestions for deceleration or avoidance are output.

[0126] Through the above technical solution, this application solves the problem of misjudgment and missed judgment caused by the lag or error of data from a single sensor in traditional early warning mechanisms, and realizes dynamic risk assessment based on multi-source data fusion. By combining real-time relative motion parameters and obstacle status, the accuracy and timeliness of early warning are effectively improved, and potential collision risks can be accurately identified and responded to in a graded manner, providing reliable decision support for the safe operation of tugboats in complex port environments.

[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A tugboat fusion-enhanced perception method based on target tracking and obstacle recognition, characterized in that, include: Initialize and synchronize the AIS, fiber optic compass, Beidou RTK, lidar, millimeter-wave radar and panoramic camera on the tugboat, and establish the ship's coordinate system; The message data of the AIS is collected and parsed, outlier removal and time-series filtering are performed, and the target ship is screened by combining the MMSI information of the target ship. The initial position of the target ship in the coordinate system of the ship is obtained by geometric correction. Based on Kalman filtering, target tracking and position prediction are performed on the state sequence of the target ship to obtain the target ship's position, speed, and heading information; When the distance between the center point of the target ship and the ship is greater than the distance threshold, the relative distance and relative bearing are calculated based on the AIS, fiber optic compass and Beidou RTK; when the target ship enters the distance threshold range, the lidar is called to obtain the boundary point cloud of the target ship, and the lateral distance, longitudinal distance and center point outline are calculated. At the same time, the distance and relative speed of surrounding obstacles are detected based on the millimeter-wave radar. The panoramic camera performs target recognition on the images it captures, outputs semantic-level recognition results for ships, buoys and shorelines, and aligns them with radar data through spatial projection. The processed AIS data, fiber optic compass data, BeiDou RTK data, lidar data, millimeter-wave radar data, and panoramic camera recognition results are fused to obtain a fusion result, and an early warning is issued based on the fusion result.

2. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 1, characterized in that, During initialization and time synchronization, and when establishing the ship's coordinate system, the following steps are included: Using the unified time output by the Beidou RTK as a reference, clock synchronization is performed on AIS, fiber optic compass, lidar, millimeter-wave radar and panoramic camera, time deviation of each sensor is recorded and millisecond-level synchronization window is set, sampling timestamps are interpolated and delay compensation is performed to obtain aligned data frames. Based on the fiber optic compass, bow zero-point calibration is performed to establish a ship coordinate system with the Beidou RTK antenna projection point as the origin, the bow as the x-axis, and the port side as the y-axis. Read the installation offset parameters of the AIS, fiber optic compass, Beidou RTK, lidar, millimeter-wave radar and panoramic camera, and solve the rigid body transformation matrix of each sensor to the ship's coordinate system. The lidar and panoramic camera complete the external parameter calibration by jointly calibrating the target, and the millimeter-wave radar completes the external parameter calibration by aligning the beam pointing with the reflecting target. The point cloud features of the lidar, the echo of the millimeter-wave radar, and the recognition results of the panoramic camera are projected onto the ship's coordinate system. The heading and position calculated by the fiber optic compass and BeiDou RTK are compared. If the projection error and heading error exceed the error threshold, the corresponding camera extrinsic parameters and time deviation are corrected.

3. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 1, characterized in that, When performing analysis, outlier removal, and temporal filtering, and filtering the target ship based on its MMSI information, and obtaining the initial position of the target ship in the ship's coordinate system through geometric correction, the process includes: The AIS message is unpacked, and MMSI information, latitude and longitude, speed, heading, length, beam, and installation distance parameters from the AIS antenna to the bow, stern, port and starboard sides are extracted to form a state sequence. The state sequence is subjected to threshold rules and sliding window filtering to remove data frames that exceed latitude and longitude limits, time interval limits, sudden speed changes, and heading jumps. The target ship is determined by performing a unique match based on the MMSI information. The latitude and longitude of the target ship are converted from geographical coordinates to coordinates in the ship's own coordinate system. Lever arm compensation is performed using the target ship's AIS antenna installation distance parameters and the ship's Beidou RTK antenna installation offset parameters to estimate the coordinates of the target ship's geometric center in the ship's own coordinate system, thereby obtaining the initial position.

4. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 1, characterized in that, When performing target tracking and position prediction based on the target ship's state sequence using Kalman filtering to obtain the target ship's position, speed, and heading information, the process includes: A target ship state vector is established in the ship's coordinate system. A uniform turning model (CT) is used as the process model. The state transition is set according to the time interval between adjacent data frames. Kalman filtering is used for prediction and update. The prior position of the target ship is output. The initial position of the target ship is combined with the speed and heading in the AIS message data to form AIS measurements; when the lidar obtains the boundary point cloud of the target ship, the relative position of the center point is extracted as lidar measurements; when the millimeter-wave radar detects the target ship, the radial distance and relative velocity are geometrically converted to obtain the planar position and velocity components as millimeter-wave radar measurements. Consistency checks are performed on measurements from different sensors, and measurements that fail the consistency check are removed. The AIS measurements, lidar measurements, and millimeter-wave radar measurements are weighted based on time freshness to obtain fused measurements. The posterior position of the target ship is obtained in the ship's coordinate system based on the fused measurements. The prior position is compared with the posterior position. When the positions are consistent, the position, speed and heading information of the target ship are obtained. When the positions are inconsistent, the position, speed and heading information of the target ship are obtained based on the posterior position, and the process noise is corrected based on error analysis.

5. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 1, characterized in that, When calculating relative distance and relative azimuth based on the aforementioned AIS, fiber optic compass, and BeiDou RTK, the following is included: In the ship's coordinate system, the straight-line distance between the geometric center of the target ship and the reference point of the ship is calculated as the relative distance. Based on the heading angle provided by the fiber optic compass, the angle between the target ship and the heading of the ship is determined as the relative bearing. The relative distance and the relative bearing are output as the solution results under long-distance working conditions.

6. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 4, characterized in that, When using lidar to acquire the boundary point cloud of the target ship, calculating the lateral distance, longitudinal distance, and center point bounding box, and simultaneously detecting the distance and relative velocity of surrounding obstacles based on the millimeter-wave radar, the process includes: Based on the posterior position of the target ship, a region clipping window is set in the coordinate system of the ship. The lidar point cloud is preprocessed, motion compensated, and surface noise is removed. Clustering and segmentation are performed within the region clipping window to obtain the target ship point cloud cluster. Boundary extraction and bounding box fitting are performed on the target ship point cloud cluster to determine the center point and external dimensions of the target ship. The forward and lateral components from the reference point of the ship to the center point are calculated using the bow axis and port axis of the ship's coordinate system, respectively, to obtain the longitudinal and transverse distances, and the bounding box of the center point is obtained at the same time. The echoes of the millimeter-wave radar are detected and clustered to obtain the distance and relative speed of the echo targets; the echoes that fall into the outer frame of the center point and its neighborhood are associated as the target ship measurement, and the remaining unassociated echoes are associated as the surrounding obstacles measurement to obtain the distance and relative speed of the nearest obstacle. Within the aforementioned distance threshold range, the lateral distance, longitudinal distance, and center point outline obtained by the lidar are used as the main measurements, while the distance and relative velocity of the millimeter-wave radar are used for verification and supplementation to obtain a measurement set for close-range working conditions.

7. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 2, characterized in that, When performing target recognition on the images acquired by the panoramic camera, outputting semantic-level recognition results for ships, buoys, and shorelines, and aligning them with radar data through spatial projection, the process includes: The panoramic camera images are preprocessed, including distortion removal and brightness normalization, and the bounding boxes or pixel masks of ships, buoys and shore walls and their category labels are output based on the target recognition model. The center of the bounding box or the contour points of the pixel mask are converted into line-of-sight rays in the camera coordinate system, and the corresponding set of directions is generated. The lidar point cloud is projected onto the image plane. Based on the projection overlap and spatiotemporal consistency, the recognition results of the panoramic camera are cross-modal correlated with the lidar point cloud clusters to distinguish the point cloud clusters belonging to the target ship from the point cloud clusters belonging to the surrounding obstacles. The near-field water surface reference plane is fitted using the associated lidar point cloud, and the intersection of the line of sight ray and the reference plane is calculated to obtain the spatial position of the panoramic camera recognition result in the ship's coordinate system. Output the category, spatial position, and relative orientation aligned with the ship's coordinate system as a visual measurement.

8. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 1, characterized in that, Before fusing the processed AIS data, fiber optic compass data, BeiDou RTK data, lidar data, millimeter-wave radar data, and panoramic camera recognition results, the following steps are taken: aligning all data under a unified timestamp to establish an asynchronous multi-rate data queue; associating measurements based on consistency checks and spatiotemporal proximity constraints to eliminate inconsistent and duplicate measurements; and aggregating relative distances and relative azimuths, as well as lateral and longitudinal distances, according to long-distance and short-distance operating conditions to obtain a fused measurement set for fusion.

9. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 8, characterized in that, The fusion measurement set is solved using weighted least squares in the ship's coordinate system to obtain the fusion result and uncertainty. The fusion weights are determined based on the measurement covariance, time freshness, message quality, point cloud density, and identification confidence. The fusion result includes the target ship's lateral distance, longitudinal distance, center point distance, and relative bearing relative to the ship.

10. The tugboat fusion enhanced perception method based on target tracking and obstacle recognition according to claim 9, characterized in that, When issuing an early warning based on the fusion results, the process includes: calculating the collision time margin and minimum safety interval based on the relative distance, relative orientation, lateral distance and longitudinal distance, and the distance and relative speed of the nearest obstacle obtained by millimeter-wave radar; and generating an early warning level based on the safety interval threshold.

Images