An unmanned ship RTK positioning drift compensation method and system based on laser radar assistance

Abstract

This invention discloses a method and system for RTK positioning drift compensation for unmanned surface vessels (USVs) based on lidar-assisted radar. The system comprises four layers: perception, processing, execution, and communication. The method involves: parallel data acquisition via dual-antenna RTK and lidar; extraction of RTK's HDOP, solution state, CNR, and coordinate jump variables, followed by weighted smoothing to obtain a comprehensive score Q; locking valid RTK coordinates as initial values ​​when Q is below a threshold; lidar point cloud data undergoing intensity filtering and clustering, followed by inter-frame ICP matching to calculate displacement increments and accumulating them to the initial value; dynamic weighted fusion of the two data streams based on the Q value, followed by smoothing and filtering to output positioning coordinates for navigation command injection, supporting resume transmission after network interruption. This invention directly quantifies signal quality based on the internal physical parameters of the RTK, eliminating the need for occlusion analysis, effectively compensating for positioning drift caused by multipath reflections on the water surface and satellite signal obstruction, ensuring continuous high-precision navigation for USVs.

Description

Technical Field

[0001] This invention belongs to the field of water quality testing technology, specifically relating to a method and system for RTK positioning drift compensation of unmanned surface vessels based on lidar-assisted radar. Background Technology

[0002] Unmanned surface vessels (USVs) rely on high-precision positioning systems for accurate navigation and pinpointing in applications such as waterway patrol, water quality monitoring, and underwater topographic mapping. Currently, Real-time Dynamic Differential (RTK) positioning technology, with its theoretical centimeter-level positioning accuracy, has become the mainstream solution for precise positioning of USVs. However, the aquatic environment has its unique characteristics: water surfaces exhibit a strong specular reflection effect on satellite signals. After reflection, the satellite signal is superimposed on the direct signal, resulting in multipath errors. This interference mechanism is fundamentally different in physical mechanism from positioning failures caused by buildings and trees obstructing satellite signals in terrestrial environments—multipath interference persists even in open water without any obstructions and changes in real-time with water surface fluctuations.

[0003] Currently, there are several main solutions for unmanned surface vessel (USV) positioning technology: (1) Single RTK positioning scheme. An RTK module is installed on the unmanned vessel, and the GPS positioning result is corrected by receiving differential signals from the base station. Theoretically, the positioning accuracy can reach the centimeter level. This scheme relies entirely on the quality of satellite signals and does not design any compensation mechanism for multipath reflection interference on the water surface. Under water surface operation conditions, the positioning error caused by multipath effect can deteriorate from the theoretical centimeter level to the decimeter level or even the meter level, which seriously affects the positioning accuracy of the unmanned vessel.

[0004] (2) RTK+IMU Integrated Navigation Scheme. This scheme integrates an inertial measurement unit (IMU) into the RTK system, utilizing the IMU's short-term, high-frequency attitude data to supplement the RTK output. This scheme is primarily designed for land-based mobile platforms. In water-surface undulation environments, the IMU is significantly affected by low-frequency hull movement and experiences drift errors that accumulate over time, leading to a continuous decrease in accuracy after prolonged water surface operations. Furthermore, unmanned surface vessels cannot be equipped with wheeled odometers, making it difficult to directly adapt the underlying compensation architecture, which relies on IMU / odometer as a safety net, for land-based integrated navigation schemes.

[0005] (3) Differential GPS (DGPS) positioning scheme. Differential correction data is broadcast by ground reference stations to perform differential correction on the shipborne GPS receiver. The positioning accuracy is about 0.5~2m, which is lower than that of RTK. It is mostly used in water surface operation scenarios where the positioning accuracy requirement is not high. It cannot meet the high-precision operation requirements such as precise point sampling and centimeter-level track tracking.

[0006] (4) RTK and LiDAR Fusion Navigation Scheme Based on Land Scenes. Existing RTK and LiDAR fusion navigation schemes are mainly designed for land robots. They indirectly infer the reliability of RTK signals by analyzing the occlusion areas of obstacles such as buildings and vegetation in the laser point cloud, and combine laser SLAM mapping, IMU, and wheeled odometry to achieve multi-sensor fusion navigation. This type of scheme relies on occlusion area detection as the core basis for RTK quality assessment. In open, unobstructed water environments, even if the RTK signal has drifted severely due to multipath reflections on the water surface, the occlusion detection method cannot identify it, leading to assessment failure. At the same time, wheeled odometry cannot be used on surface unmanned vessels, and water surface fluctuations also seriously affect IMU accuracy, making it impossible to directly apply the above-mentioned land fusion schemes to surface unmanned vessel platforms.

[0007] (5) Traditional data transmission radio + remote positioning solution. The operator manually controls the unmanned vessel through the remote control and data transmission radio. It relies on the operator's subjective judgment of the position and has no autonomous and accurate positioning capability. The communication distance is usually limited to within 1~3km, which cannot meet the needs of autonomous inspection in large water areas.

[0008] In summary, existing unmanned surface vessel (USV) positioning technologies have the following main shortcomings: First, single-RTK positioning schemes cannot cope with signal drift caused by multipath reflections on the water surface and lack compensation methods for the unique interference mechanisms of the water surface. Second, RTK is prone to fixed solution loss in scenarios such as under bridges or under building obstruction. Existing schemes lack effective direct signal quality assessment methods, and indirect assessment methods relying on obstruction analysis are completely ineffective in open water environments. Third, the cumulative error of RTK+IMU schemes is difficult to suppress in water environments, and land-based compensation architectures are unsuitable for water platforms due to the lack of hardware foundations such as wheeled odometers. Fourth, the weight allocation mechanism of existing fusion schemes is based on the level of obstruction area, which is not adapted to the dynamic real-time changes in the intensity of multipath interference on the water surface with the waves. Fifth, information on fixed features such as shorelines, bridge piers, and dams around the water area is not fully utilized, and laser SLAM mapping cannot function properly on the water surface due to insufficient effective feature points. Sixth, the communication range of traditional data transmission radios is limited, making it difficult to support the needs of wide-area water operations. Summary of the Invention

[0009] To solve the above-mentioned technical problems, the present invention adopts the following technical solution: A lidar-assisted unmanned surface vessel RTK positioning drift compensation system includes: Perception layer: Composed of a dual-antenna RTK module and a lidar module, used to acquire information about the surrounding environment; Processing layer: Composed of the main control module, used to provide computing power; Execution layer: Composed of the hull control module, used to control the hull state and position; Communication layer: Composed of communication module and cloud platform module, used for communication between the hull and the main control module.

[0010] Furthermore, the main control module is a Raspberry Pi 4B, and the dual-antenna RTK module, lidar module, hull control module, and communication module are respectively connected to the main control module.

[0011] Furthermore, this includes the following steps: S1): Parallel data acquisition via dual-channel: The dual-antenna RTK module and the lidar module begin acquiring data information separately; S2): Real-time RTK signal quality assessment: Directly read the physical parameters inside the dual-antenna RTK module, give the RTK health status a comprehensive score, and record the comprehensive score as Q, which ranges from 0 to 1. When Q ≥ 0.6, it is judged as normal mode; when Q < 0.6, proceed to step 3. S3): LiDAR-assisted positioning: Calculating the ship's position using radar data; S4): Dual-source adaptive weighted fusion: dynamically assigning fusion weights based on the Q-value; S5): Navigation command injection: The hull control module drives the hull to sail along the preset route based on the coordinates fused in step S4; S6): Remote monitoring and resume download after network outage.

[0012] Furthermore, in step 1, the dual-antenna RTK module outputs 10 frames of data per second. Each frame of data includes coordinate values, HDOP values, solution status, CNR, and coordinate jump variables from the previous frame, where the coordinate values ​​are denoted as x_rtk and y_rtk. The lidar module outputs 10 frames of point cloud data per second, and each frame of point cloud data contains the distance, angle and intensity values ​​of objects within a 360° range.

[0013] Furthermore, in step 2, the specific steps for the comprehensive score Q are as follows: S21): Extract the HDOP value, solution state, CNR, and coordinate jump variables from the previous frame, and normalize the mapping; specifically as follows: When the HDOP value is ≤1.0, you get 1 point; when the HDOP value is ≥3.0, you get 0 points; and the remaining values ​​decrease linearly. 1 point is awarded when the solution is a fixed solution, 0.4 points are awarded when the solution is a floating-point solution, and 0 points are awarded when the solution is a single-point solution. A score of 1.0 is awarded for a coordinate jump of ≤0.1m, and a score of 0 is awarded for a jump of >0.3m; the remaining values ​​decrease linearly. CNR is normalized by dividing the measured value by 50, and the score range is 0 to 1. S22): Weighted summation; the formula is... Q_weight = 0.3 × HDOP score + 0.3 × solution state score + 0.2 × coordinate jump variable score + 0.2 × CNR score; S23): Smoothing to prevent false judgments: Take the Q value of the most recent 5 frames and perform a moving average to obtain Q; S24): Decision: If Q ≥ 0.6, it is determined as "normal mode"; if Q < 0.6, it is immediately determined as "drift / failure mode", and the last valid RTK coordinate is locked as the initial value for subsequent calculations, denoted as (x0, y0).

[0014] Furthermore, step 3 specifically includes the following steps: S31): Intensity filtering: Set a threshold I_th = 200. All points with intensity below this value are removed, and fixed ground feature point clouds with high intensity are retained. S32): Euclidean clustering: Set a cluster radius of 0.1m, remove scattered noisy clusters with fewer than 10 points, and retain only the continuous and stable shoreline contour point cloud set P_k; S33): Inter-frame ICP matching: Iteratively match the point cloud P_k of the current frame with the P_{k-1} of the previous frame; specific parameters are: maximum 50 iterations, convergence threshold 0.001m. S34): Calculate the displacement increment: After successful ICP matching, extract the rigid body transformation matrix between the two frames, calculate how many meters the ship has moved in the local coordinate system, and record it as Δx and Δy. If the ICP matching convergence error exceeds 0.05m, discard it directly, and record the displacement increment as (0,0) to prevent sudden jumps. S35): Coordinate accumulation calculation: The tiny displacements calculated in each frame are continuously accumulated and superimposed on the initial coordinate values ​​locked in step 24. The formula is as follows: x_est = x0 + ΣΔx; y_est = y0 + ΣΔy.

[0015] Furthermore, step 4 specifically includes the following steps: S41): Calculate the RTK weights W_rtk: W_rtk = Q 2 / (Q) 2 + (1-Q) 2 ); S42): Calculate the laser weight W_lidar: W_lidar = 1 - W_rtk; S43): Calculate the initial fusion value x_raw: x_raw = W_rtk × x_rtk + W_lidar × x_est; S44): Smooth transition of coordinate data: x_fused(n) = 0.5 × x_raw(n) + 0.5 × x_fused(n-1); y_fused(n) = 0.5 × y_raw(n) + 0.5 × y_fused(n-1).

[0016] Beneficial effects: 1. To address RTK drift caused by multipath reflections on the water surface, a direct signal quality assessment mechanism is established to proactively ensure accuracy: This invention directly measures four parameters in real time from the dual-antenna RTK module output: carrier-to-noise ratio, HDOP value, fixed / floating / single-point solution status, and coordinate jump variables. The resulting comprehensive signal quality score Q is calculated, enabling accurate identification of positioning drift caused by multipath interference in open, unobstructed water environments. Under normal RTK signal conditions, the fused output positioning accuracy is comparable to that of the fixed solution. Under RTK drift conditions, the positioning error after lidar-assisted compensation can be controlled within 0.1m, significantly improving positioning accuracy stability compared to the decimeter-to-meter level error during drift in a single RTK solution.

[0017] 2. Continuous Positioning Independent of Satellite Signals: This invention uses the point cloud features of fixed features around water bodies as a reference, which is completely independent of satellite signals. In scenarios where satellite signals are obstructed, such as under bridges or by buildings, IMU solutions gradually fail due to the linear increase in accumulated drift error over time (typical IMU zero-bias stability is about 0.5° / h, and the error accumulates significantly after long-term operation). However, the displacement estimation error of the inter-frame ICP matching of this invention does not accumulate over time and is only related to the matching accuracy of a single frame. In environments with sufficient fixed features around water bodies, the displacement estimation error of ICP matching can be controlled within 0.05m, providing stable position compensation continuously during the loss of RTK fixed solutions, and the positioning continuity is not affected by fluctuations in satellite signal quality.

[0018] 3. Dual-source adaptive weighting achieves accurate dynamic weight response: The weights are dynamically calculated based entirely on the measured quality parameters of the RTK signal itself, without relying on point cloud density or occlusion region analysis. Even in open, unobstructed water environments, it can still accurately respond to signal quality degradation caused by multipath interference, eliminating the systematic weight bias of existing solutions in water scenarios.

[0019] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, the preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the overall system architecture of the present invention; Figure 2 This is a schematic diagram of the software processing flow of the present invention; Explanation of reference numerals in the attached figures: 1. Dual-antenna RTK module; 2. LiDAR module; 3. Main control module; 4. Hull control module; 5. Communication module; 6. Cloud platform. Detailed Implementation

[0021] The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby providing a clearer and more explicit definition of the scope of protection of the present invention. Specific Implementation Example 1: This embodiment uses a certain type of unmanned surface vessel (hull length 2.4m, width 1.2m, displacement approximately 85kg) as the test platform and conducts field tests on an inland river channel (water surface width approximately 60m, both banks are concrete revetments, and the shoreline is continuous) to provide a detailed description of the lidar-assisted unmanned surface vessel RTK positioning drift compensation method and system of the present invention.

[0023] I. System Hardware Configuration like Figure 1 As shown, the RTK positioning drift compensation system based on lidar-assisted positioning carried by the unmanned vessel in this embodiment includes a perception layer, a processing layer, an execution layer, and a communication layer.

[0024] The perception layer consists of a dual-antenna RTK module 1 and a lidar module 2. The dual-antenna RTK module 1 uses the Hexin Xingtong UM982 module, supporting joint positioning with GPS / BDS / GLONASS / Galileo systems. The dual antennas are spaced 0.6m apart, with a data output frequency of 10Hz and a nominal positioning accuracy of ±0.8cm +1ppm horizontally. The lidar module 2 uses a Leishen Intelligent C16-151B 16-line mechanical lidar with a 360° horizontal field of view, a 30° vertical field of view (-15° to +15°), a ranging range of 0.5m to 150m, an accuracy of ±2cm, a data output frequency of 10Hz, and outputs approximately 320,000 points per second, including distance, angle, and intensity information. The dual-antenna RTK module 1 and lidar module 2 are connected to the main control module 3 via USB serial port and Ethernet interface, respectively. Time synchronization is achieved through PPS (pulse-per-second) signals, ensuring a unified time reference for the two sensor data streams.

[0025] The processing layer consists of main control module 3. Main control module 3 uses a Raspberry Pi 4B development board, equipped with a Broadcom BCM2711 quad-core Cortex-A72 processor (1.5GHz), 4GB LPDDR4 memory, and runs the Ubuntu 20.04LTS operating system. It deploys the ROS Noetic robot operating system framework to receive and process perception layer data and generate control commands.

[0026] The execution layer consists of the hull control module 4. The hull control module 4 includes an STM32F407 microcontroller, two brushless DC thrusters (rated power 1.2kW, maximum thrust 12kg), and a rudder. It receives heading and speed commands from the main control module 3 via PWM signals to control the ship's heading and speed. The hull control module 4 communicates with the main control module 3 via a UART serial port (baud rate 115200bps).

[0027] The communication layer consists of communication module 5 and cloud platform 6. Communication module 5 uses a 4G full-network compatible wireless data transmission terminal (Quectel EC200U module) to establish a bidirectional communication link with cloud platform 6 via the MQTT protocol, enabling the uploading of ship positioning data and status information, as well as the issuance of remote control commands. Cloud platform 6 is deployed on Alibaba Cloud ECS server, running Node-RED data stream processing service for real-time monitoring of ship position and status, and has local data caching and resume transmission capabilities in case of network interruption.

[0028] II. Software Processing Flow like Figure 2 As shown, the software processing flow of this embodiment includes the following steps: S1) Parallel acquisition of dual-channel data After the unmanned vessel is launched, the main control module 3 starts the dual-antenna RTK module 1 and the lidar module 2, and the two modules collect data in parallel at a frequency of 10Hz.

[0029] The dual-antenna RTK module 1 outputs the following data per frame: coordinate values ​​(denoted as x_rtk, y_rtk, in meters), HDOP value (horizontal precision factor), solution status (fixed solution / floating-point solution / single-point solution / no solution), CNR values ​​(carrier-to-noise ratio, in dB·Hz) for each satellite, and the coordinate jump variable (in meters) between the current frame and the previous frame. In this embodiment, when the RTK signal is good in open water, the HDOP value is stable between 0.8 and 1.2, the solution status is fixed, and the CNR values ​​for each satellite are in the range of 35 to 48 dB·Hz.

[0030] The lidar module 2 outputs point cloud data within a 360° horizontal field of view per frame, including the distance (in meters), horizontal angle (in degrees), and intensity value (dimensionless, range 0-255) for each point. In this embodiment, the lidar is installed at the top center of the hull, 0.8 meters above the water surface, with the scanning plane parallel to the horizontal plane. In river operation scenarios, the lidar can clearly detect the point cloud echoes of the concrete revetments on both banks. The intensity value of the shoreline point cloud is typically between 180 and 230, significantly higher than that of the water surface point cloud (intensity value is typically below 50).

[0031] S2) Real-time assessment of RTK signal quality The main control module 3 performs a comprehensive score on each frame of RTK data, denoted as Q, ranging from 0 to 1. The specific steps are as follows: S21) Parameter extraction and normalization mapping: Four parameters are extracted from the RTK data frame: HDOP value, solution state, coordinate jump variable, and CNR value, and then normalized and scored.

[0032] HDOP score: 1.0 points are awarded when the HDOP value is ≤1.0; 0 points are awarded when the HDOP value is ≥3.0; intermediate values ​​are calculated in a linear decreasing order. For example, in this embodiment, the HDOP value of a certain frame of RTK data is 1.5, then the HDOP score = 1.0 - (1.5-1.0) / (3.0-1.0) = 1.0 - 0.25 = 0.75 points.

[0033] Solution status scoring: Fixed solution gets 1.0 point, floating-point solution gets 0.4 points, single-point solution gets 0 points, and no solution gets 0 points. In this embodiment, when the unmanned vessel is operating in open water, the solution status is a fixed solution; when the unmanned vessel travels under a bridge, the satellite signal is partially blocked, and the solution status switches to a floating-point solution.

[0034] Coordinate jump variable scoring: 1.0 point is awarded when the coordinate jump variable is ≤0.1m; 0 points are awarded when the coordinate jump variable is >0.3m; the intermediate values ​​decrease linearly. For example, if the coordinate jump variable of a certain frame of RTK data is 0.05m from the previous frame, then this indicator scores 1.0 points; if the coordinate jump variable is 0.2m, then the score is 1.0 - (0.2-0.1) / (0.3-0.1) = 1.0 - 0.5 = 0.5 points.

[0035] CNR score: The average CNR value of all visible satellites in the current frame is taken and normalized by dividing the measured value by 50. The score ranges from 0 to 1. For example, if the average CNR value of satellites in a certain frame is 42 dB·Hz, then the CNR score = 42 / 50 = 0.84 points.

[0036] S22) Weighted summation: The initial score Q_weight is calculated using the following weights: Q_weight = 0.3 × HDOP score + 0.3 × solution state score + 0.2 × coordinate jump variable score + 0.2 × CNR score; For example, if a frame of data has the following scores: HDOP score 0.75, solution state score 1.0 (fixed solution), coordinate jump variable score 1.0, and CNR score 0.84, then Q_weight = 0.3×0.75 + 0.3×1.0 + 0.2×1.0 + 0.2×0.84 = 0.225 + 0.3 + 0.2 + 0.168 = 0.893 points.

[0037] S23) Smoothing to prevent misjudgment: The overall score Q is obtained by taking the moving average of the Q-weights of the most recent 5 frames. For example, if the Q-weights of the 5 consecutive frames are 0.893, 0.910, 0.885, 0.902, and 0.896, then Q = (0.893+0.910+0.885+0.902+0.896) / 5 = 0.897 points.

[0038] S24) Pattern Decision: If Q ≥ 0.6, it is determined to be "normal mode" and the system directly uses the RTK positioning result; if Q < 0.6, it is determined to be "drift / failure mode" and the system locks the last valid RTK coordinate as the initial value for subsequent calculations, denoted as (x0, y0).

[0039] In this embodiment, when the unmanned surface vessel (USV) is navigating normally in open water, the Q value is stable between 0.85 and 0.95, and the system is in normal mode. When the USV enters a range of approximately 5 meters below the bridge, the satellite signal is blocked by the bridge structure, the HDOP value rises to 2.8, the solution state changes from a fixed solution to a floating-point solution, the Q value drops to 0.45, and the system automatically switches to drift / failure mode. It also locks the last valid RTK coordinates (e.g., x0=342156.782, y0=3478923.451, CGCS2000 coordinate system, central meridian 120°E) before entering the bridge area as the initial value for subsequent lidar-assisted positioning.

[0040] S3) LiDAR-assisted positioning When the system enters drift / failure mode, the lidar-assisted positioning module is activated to calculate the ship's position using radar data. The specific steps are as follows: S31) Intensity filtering: An intensity threshold I_th = 200 is set, and intensity filtering is applied to the current frame's LiDAR point cloud. Points with intensity values ​​below 200 are all removed, retaining only the point cloud of fixed features with high intensity. In this embodiment, the point cloud intensity values ​​of the concrete revetments on both banks are between 195 and 225, and are retained after intensity filtering; the point cloud of the water surface (intensity value usually below 50) and the point cloud of floating objects (intensity value usually below 100) are effectively removed.

[0041] S32) Euclidean clustering: The filtered point cloud is subjected to Euclidean clustering with a cluster radius of 0.1m. Scattered, noisy clusters with fewer than 10 points are removed, leaving only a continuous and stable set of shoreline contour point clouds, P_k. In this embodiment, after clustering, two large point cloud clusters are formed on both banks of the revetment, each containing 200-500 points, forming a clear shoreline contour.

[0042] S33) Inter-frame ICP matching: The point cloud P_k of the current frame is iteratively matched with the point cloud P_{k-1} of the previous frame using Intermediate Closest Point (ICP) matching. Specific parameters are set as follows: maximum number of iterations 50, convergence threshold 0.001m. In this embodiment, when the unmanned vessel travels along the river at a speed of 1.5 m / s, the overlap rate of the point clouds between two adjacent frames (0.1 s interval) is approximately 85%, and ICP matching typically converges after 15-25 iterations.

[0043] S34) Solve for the displacement increment: After a successful ICP match, the rigid body transformation matrix between the two frames is extracted, and the displacement increments Δx and Δy (in meters) of the hull in the local coordinate system are calculated. If the ICP matching convergence error exceeds 0.05 meters, the match is deemed unreliable, and the displacement increment is recorded as (0,0) to prevent sudden jumps.

[0044] In this embodiment, during the RTK failure period under the bridge, the ICP matching convergence error statistics for 50 consecutive frames (5 seconds) are as follows: the maximum error is 0.038m, the minimum error is 0.012m, and the average error is 0.021m, all of which are less than the 0.05m threshold, and all displacement increments are valid.

[0045] S35) Coordinate accumulation calculation: The tiny displacements calculated for each frame are continuously accumulated and superimposed onto the initial coordinates (x0, y0) locked in step S24: x_est = x0 + ΣΔx y_est = y0 + ΣΔy In this embodiment, during the 5-second period when the unmanned vessel travels under the bridge (a total of 50 frames), the cumulative displacement ΣΔx = 7.482m and ΣΔy = 1.235m. Therefore, the position calculated by the lidar is x_est = 342156.782 + 7.482 = 342164.264m and y_est = 3478923.451 + 1.235 = 3478924.686m.

[0046] S4) Dual-source adaptive weighted fusion The main control module 3 dynamically calculates the fusion weights of RTK and LiDAR based on the RTK quality score Q value of the current frame, and performs data fusion. The specific steps are as follows: S41) Calculate RTK weights: W_rtk = Q 2 / (Q) 2 + (1-Q) 2 ) S42) Calculate the laser weight: W_lidar = 1 - W_rtk S43) Calculate the initial fusion value: x_raw = W_rtk × x_rtk + W_lidar × x_est y_raw = W_rtk × y_rtk + W_lidar × y_est S44) Smooth transition of coordinate data: To prevent inter-frame jumps in the fused coordinates, a first-order low-pass filter is used for smoothing. x_fused(n) = 0.5 × x_raw(n) + 0.5 × x_fused(n-1) y_fused(n) = 0.5 × y_raw(n) + 0.5 × y_fused(n-1) In this embodiment, as the unmanned vessel exits from under the bridge and the RTK signal recovers, the Q value gradually increases from 0.45 to 0.85. During this process, the weights dynamically change as follows: when Q=0.45, W_rtk = 0.45. 2 / (0.45 2 +0.55 2When Q=0.65, W_rtk = 0.4225 / (0.4225+0.1225) = 0.775, W_lidar = 0.225; when Q=0.85, W_rtk = 0.7225 / (0.7225+0.0225) = 0.970, W_lidar = 0.030, the system mainly uses RTK. Smoothing filtering effectively eliminates coordinate jumps during mode switching, with coordinate changes between adjacent frames during the transition period all less than 0.02m.

[0047] S5) Navigation command injection The main control module 3 compares the fused coordinates (x_fused, y_fused) with the preset route, calculates the heading deviation and lateral deviation, generates PID control commands (heading angle command and thruster speed command), and sends them to the hull control module 4 via the UART serial port. The hull control module 4 drives the thrusters and rudder, enabling the unmanned vessel to navigate along the planned route.

[0048] In this embodiment, the preset route is along the river centerline (coordinate sequence pre-stored in the main control module) from upstream point A (coordinates x=342100.000, y=3478900.000) to downstream point B (coordinates x=342300.000, y=3479000.000), with a total distance of approximately 223m. During the RTK signal normal range (approximately 85% of the total range), the lateral deviation of the route tracking is ≤0.05m; during the RTK failure range under the bridge (approximately 15% of the total range), the lateral deviation of the route tracking after lidar-assisted positioning is ≤0.12m, meeting the accuracy requirements (lateral deviation ≤0.3m) for unmanned surface vessel water quality sampling point-based operations.

[0049] S6) Remote monitoring and resume download after network outage Communication module 5 transmits information such as the ship's positioning coordinates, Q value, fusion weight, and equipment status to cloud platform 6 via a 4G network at a frequency of 1Hz. Cloud platform 6 displays the unmanned ship's navigation trajectory and various status parameters in real time.

[0050] When the 4G network signal is interrupted (e.g., when passing through a remote river section), the main control module 3 caches the unuploaded data (including timestamps, coordinates, Q values, etc.) in a local SQLite database. After the network is restored, the system automatically re-uploads the cached data to the cloud platform 6, ensuring the integrity of the monitoring data. In this embodiment, during the test, there were two network interruptions lasting approximately 30 seconds each, with 63 cached data entries. After the network was restored, all data were successfully re-uploaded, achieving a 100% data integrity rate.

[0051] III. Experimental Verification and Performance Comparison To verify the technical effect of the present invention, a comparative test was conducted under the same experimental conditions in this embodiment: (1) Normal operating conditions of RTK signal (open water, Q≥0.6): Using the fusion positioning scheme of this invention, with the measurement values ​​of a high-precision total station (Leica TS60, angle measurement accuracy 0.5″, distance measurement accuracy 0.6mm+1ppm) as the true value, 300 data points were continuously tested. The average horizontal error of the fusion positioning was 0.008m (0.8cm), and the maximum error was 0.015m, which is comparable to the accuracy of a single RTK fixed solution (0.007m). The fusion did not introduce significant accuracy loss.

[0052] (2) Deterioration of RTK signal (under bridge, Q<0.6): The single RTK solution suffers from signal obstruction leading to the loss of fixed solutions, resulting in a deterioration in positioning error from the centimeter level to the decimeter level (maximum error 0.85m, average 0.41m). Under the same operating conditions, the fusion solution of this invention, after lidar-assisted compensation, has an average positioning error of 0.052m and a maximum error of 0.089m, improving positioning accuracy by approximately 87% compared to the single RTK solution.

[0053] (3) Long-term drift compensation capability: Under extreme conditions of continuous RTK signal failure (simulated occlusion for 120 seconds), the average displacement estimation error of the inter-frame ICP matching of the lidar of this invention is 0.023m, and the error does not accumulate over time (the error at the 120th second is 0.031m, which is not significantly increased compared with the error at the 10th second of 0.019m), verifying that the lidar fixed ground object compensation method does not have the characteristic of time accumulation error.

[0054] The experimental data above show that the present invention can effectively compensate for RTK positioning drift under complex conditions such as multipath reflection on the water surface and satellite signal blockage, and ensure continuous high-precision positioning and navigation of unmanned ships.

[0055] The above description is merely an embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural transformations made based on the content of the present invention specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A lidar-assisted unmanned surface vessel RTK positioning drift compensation system, characterized in that, include: Perception layer: Composed of a dual-antenna RTK module (1) and a lidar module (2), used to acquire information about the surrounding environment; Processing layer: Composed of main control module (3), used to provide computing power; Execution layer: Composed of the hull control module (4), used to control the hull status and position; Communication layer: It consists of a communication module (5) and a cloud platform (6) module, which are used for communication between the hull and the main control module.

2. The lidar-assisted RTK positioning drift compensation system for unmanned surface vessels according to claim 1, characterized in that: The main control module (3) is a Raspberry Pi 4B. The dual-antenna RTK module (1), the lidar module (2), the hull control module (4), and the communication module (5) are connected to the main control module (3) respectively.

3. A method for RTK positioning drift compensation of an unmanned surface vessel based on lidar-assisted positioning according to any one of claims 1-2, characterized in that: Includes the following steps: S1): Parallel data acquisition via dual-channel data: The dual-antenna RTK module (1) and the lidar module (2) start acquiring data information respectively; S2): Real-time evaluation of RTK signal quality: Directly read the physical parameters inside the dual-antenna RTK module (1) and give a comprehensive score to the health status of the RTK. The comprehensive score is recorded as Q, which ranges from 0 to 1. When Q ≥ 0.6, it is judged as normal mode. When Q < 0.6, step 3 is executed. S3): LiDAR-assisted positioning: Calculating the ship's position using radar data; S4): Dual-source adaptive weighted fusion: dynamically assigning fusion weights based on the Q-value; S5): Navigation command injection: The hull control module drives the hull to sail along the preset route based on the coordinates fused in step S4; S6): Remote monitoring and resume download after network outage.

4. The method for RTK positioning drift compensation of unmanned surface vessels based on lidar-assisted positioning according to claim 3, characterized in that: In step 1, the dual-antenna RTK module (1) outputs 10 frames of data per second. Each frame of data includes coordinate values, HDOP values, solution status, CNR, and coordinate jump variables from the previous frame. The coordinate values ​​are denoted as x_rtk and y_rtk. The lidar module (2) outputs 10 frames of point cloud data per second, and each frame of point cloud data contains the distance, angle and intensity values ​​of objects within a 360° range.

5. The method for RTK positioning drift compensation of unmanned surface vessels based on lidar-assisted positioning according to claim 4, characterized in that: In step 2, the specific steps for the comprehensive scoring Q are as follows: S21): Extract the HDOP value, solution state, CNR, and coordinate jump variables from the previous frame, and normalize the mapping; specifically as follows: When the HDOP value is ≤1.0, you get 1 point; when the HDOP value is ≥3.0, you get 0 points; and the remaining values ​​decrease linearly. 1 point is awarded when the solution is a fixed solution, 0.4 points are awarded when the solution is a floating-point solution, and 0 points are awarded when the solution is a single-point solution. A score of 1.0 is awarded for a coordinate jump of ≤0.1m, and a score of 0 is awarded for a jump of >0.3m; the remaining values ​​decrease linearly. CNR is normalized by dividing the measured value by 50, and the score range is 0 to 1. S22): Weighted summation; S23): Smoothing to prevent false judgments: Take the Q value of the most recent 5 frames and perform a moving average to obtain Q; S24): Decision: If Q ≥ 0.6, it is determined as "normal mode"; if Q < 0.6, it is immediately determined as "drift / failure mode", and the last valid RTK coordinate is locked as the initial value for subsequent calculations, denoted as (x0, y0).

6. The method for RTK positioning drift compensation of unmanned surface vessels based on lidar-assisted positioning according to claim 5, characterized in that: Step 3 specifically includes the following steps: S31): Intensity filtering: Set a threshold I_th = 200. All points with intensity below this value are removed, and fixed ground feature point clouds with high intensity are retained. S32): Euclidean clustering: Set a cluster radius of 0.1m, remove scattered noisy clusters with fewer than 10 points, and retain only the continuous and stable shoreline contour point cloud set P_k; S33): Inter-frame ICP matching: Iteratively match the point cloud P_k of the current frame with the P_{k-1} of the previous frame; specific parameters are: maximum 50 iterations, convergence threshold 0.001m. S34): Calculate the displacement increment: After successful ICP matching, extract the rigid body transformation matrix between the two frames, calculate how many meters the ship has moved in the local coordinate system, and record it as Δx and Δy. If the ICP matching convergence error exceeds 0.05m, discard it directly, and record the displacement increment as (0,0) to prevent sudden jumps. S35): Coordinate accumulation calculation: The tiny displacements calculated in each frame are continuously accumulated and superimposed on the initial coordinate values ​​locked in step 24.

7. The method for RTK positioning drift compensation of unmanned surface vessels based on lidar-assisted positioning according to claim 6, characterized in that: Step 4 specifically includes the following steps: S41): Calculate the RTK weights W_rtk; S42): Calculate the laser weight W_lidar; S43): Calculate the initial fusion value x_raw; S44): Smooth transition of coordinate data.

Images