A USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration

Abstract

This invention discloses a USV berthing and unberthing pose estimation method based on ship-shore LiDAR collaboration, comprising: acquiring shipborne and shore-based point clouds and shore-based fixed reference poses; performing odometer calculations on continuous shipborne point clouds to generate LiDAR odometer factors; constructing a global descriptor for the ship-shore point clouds and performing scene matching to generate candidate ship-shore point cloud pairs; extracting key points and calculating FPFH descriptors, solving for high-precision ship-shore relative poses through coarse / fine registration, obtaining the final ship-shore relative poses through asynchronous reconstruction and generating correlation factors; constructing a collaborative factor graph model containing odometer and ship-shore correlation factors; solving the model and minimizing the overall residuals to obtain a globally consistent optimal pose, thereby achieving berthing and unberthing control. This invention improves ship-shore data transmission efficiency and matching accuracy through global descriptor matching and FPFH feature compression; and eliminates the cumulative error of single-ship odometers by leveraging shore-based global pose constraints, achieving high-precision continuous pose estimation under long-term navigation and ensuring the robustness of berthing and unberthing navigation.

Description

Technical Field

[0001] This invention relates to the field of ship berthing and unberthing technology, and in particular to a method for estimating the berthing and unberthing pose of a USV based on ship-shore LiDAR collaboration. Background Technology

[0002] With the continued growth of global trade and the ever-increasing demands for safety and efficiency in the shipping industry, port automation and intelligence have become a focus of attention for the International Maritime Organization and maritime agencies of various countries. Autonomous berthing and unberthing is the final and most crucial step in achieving fully autonomous navigation, and its technological level directly determines the practical application of autonomous surface vessels. During this low-speed, high-precision berthing and unberthing phase, the ability of a vessel to accurately and in real-time determine its position and attitude is a core prerequisite for ensuring navigational safety and improving port operational efficiency.

[0003] Traditional ship attitude estimation primarily relies on a combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS). However, in complex environments such as ports, GNSS signals are often interrupted or suffer from severe multipath effects due to obstruction by tall quay cranes and buildings, leading to a sharp decline in positioning accuracy or even failure. While marine radar can provide target detection, its limited angular resolution makes it difficult to accurately construct detailed information about the dock's outline. Shipborne sensors (such as LiDAR) can actively emit laser beams and measure their round-trip time to acquire high-precision 3D point cloud data of the environment. Unaffected by lighting conditions, it possesses extremely high ranging accuracy and angular resolution, enabling precise reconstruction of the 3D structure of the surrounding environment, including docks and ships. These characteristics give LiDAR a natural advantage in building accurate environmental models and performing relative attitude measurements, providing an ideal data source for solving high-precision positioning problems in GNSS-denied environments. However, in scenarios requiring extremely high accuracy, such as ship berthing and unberthing, a single shipborne or shore-based LiDAR attitude estimation system has a fatal flaw: its positioning error accumulates over time, which is unacceptable for berthing operations requiring centimeter-level accuracy.

[0004] To fundamentally address the aforementioned problems encountered during ship berthing and unberthing, researchers have explored several technological approaches. These solutions can be broadly categorized into the following two types: The first approach is to enhance the autonomous positioning system of a single vessel. This approach is mainly achieved through two methods: First, by employing higher-precision inertial measurement units (IMUs) and fiber optic gyroscopes to physically reduce the drift rate of the sensors themselves. Second, by introducing a more diverse range of sensors and performing deep fusion, for example, by tightly coupling visual odometry (VO) or LiDAR odometry with GNSS / INS. This multi-sensor fusion strategy can leverage the strengths of different sensors under varying environmental conditions, improving the robustness of the system in the short term. However, the limitation of this approach is that it is essentially still a "closed" monolithic system. No matter how advanced the sensors or how complex the algorithms, the inherent problem of error accumulation is only mitigated, not fundamentally eliminated. Once the absolute positioning reference is lost, drift after long-term operation is still unavoidable, failing to meet the stringent accuracy requirements of berthing operations. The second type of approach is map-based assisted positioning. The core idea of ​​this method is to conduct a detailed survey of the dock environment before operations, using high-precision mapping equipment (such as a terrestrial laser scanner) to construct a 3D point cloud map with centimeter-level accuracy. When the ship berths, onboard sensors scan the surrounding environment in real time and match the data with the stored high-precision map to obtain its precise pose. Theoretically, this method can provide extremely high positioning accuracy. However, its drawbacks are also significant: First, it relies on high predictive mapping costs and timelines, making it unsuitable for frequently changing port operating environments. For example, temporary container stacking or quay wall maintenance can cause the map to become out of sync with reality, affecting the timeliness and accuracy of positioning. Second, this method cannot cope with unforeseen circumstances and lacks dynamic adaptability. Summary of the Invention

[0005] This invention provides a USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration to overcome the above-mentioned technical problems.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows: A method for estimating the berthing and departure pose of a USV based on ship-shore LiDAR cooperation, the specific steps of which include: S1. Collect shipborne point cloud data using shipborne LiDAR, collect shore-based point cloud data using shore-based LiDAR, and obtain the fixed reference pose of shore-based LiDAR in the global coordinate system. S2. Perform lidar odometry calculation on two consecutive frames of shipborne point cloud to obtain the motion increment of the unmanned surface vessel at adjacent times, and generate LiDAR odometry factor based on the motion increment. S3. Construct global descriptors for shipborne point clouds and shore-based point clouds respectively to obtain shipborne global descriptors and shore-based global descriptors; perform scene matching between shipborne observations and shore-based observations based on the shipborne global descriptors and shore-based global descriptors, and generate ship-shore candidate point cloud pairs that can be used for cross-platform registration based on the set decision mechanism when the matching is successful. S4. Based on the ship-shore candidate point cloud pair, extract key points and calculate the FPFH descriptor of the key points. Combine the FPFH descriptor and use coarse registration and fine registration methods to solve for the high-precision relative pose of the ship-borne LiDAR relative to the shore-based LiDAR. Reconstruct the high-precision relative pose based on the asynchronous mechanism to obtain the final ship-shore relative pose. Generate the ship-shore pose association factor based on the final ship-shore relative pose. S5. The pose of the unmanned surface vessel at each moment is taken as the variable node to be optimized, and the fixed reference pose of the shore-based LiDAR in the global coordinate system is taken as the fixed anchor point node. The LiDAR odometry factor and the ship-shore pose correlation factor are introduced as odometry continuity constraints and ship-shore relative observation constraints, thereby constructing a ship-shore cooperative factor graph model. S6. Solve the ship-shore cooperative factor graph model. Under the condition of simultaneously satisfying the odometry continuity constraint and the ship-shore relative observation constraint, minimize the overall residual to obtain the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time. Based on the consistent optimal pose, realize berthing and departure control and navigation. At the same time, receive a new frame of ship-borne point cloud and shore-based point cloud, repeat S2-S5, and realize real-time continuous pose estimation.

[0007] Furthermore, in S3, the specific steps of performing scene matching between shipborne and shore-based observations based on the shipborne global descriptor and the shore-based global descriptor, and generating ship-shore candidate point cloud pairs that can be used for cross-platform registration based on a set decision mechanism when a match is successful, include: S321. Calculate the similarity between the shipborne global descriptor and the shore-based global descriptor: For any global descriptor among the global descriptors and shore-based global descriptors, from 0 to N s 1. Perform column shifting, and calculate the distance to another global descriptor after each column shift. The distance formula is defined as: (1) in, k It is the number of column shift steps. k ∈[0, N s 1]; W r It is a radial distance r Relevant weighting factors; | | represents Manhattan distance; The minimum distance is taken as the final distance between the two global descriptors, and this final distance is used as the scene similarity under optimal rotation alignment, expressed as: (2) S322. Based on the established decision-making mechanism, generate candidate ship-shore point cloud pairs that can be used for cross-platform registration, including: Set a distance threshold T dist ; Determine if it is a Distance SC < T dist If so, then perform a time consistency check; Time consistency checks include: Set a sliding time window and check if the Distance condition is met within that sliding time window. SC < T dist If the percentage of frames exceeds a preset ratio, then the two point clouds of the shipborne and shore-based systems at the current moment are determined as ship-shore candidate point cloud pairs. Further, in S4, key points are extracted based on the ship-shore candidate point cloud pair, and FPFH descriptors of the key points are calculated. Combining the FPFH descriptors with coarse and fine registration methods, the high-precision relative pose of the shipborne LiDAR relative to the shore-based LiDAR is obtained. The high-precision relative pose is then reconstructed based on an asynchronous mechanism to obtain the final ship-shore relative pose. The specific steps include: Several key points were obtained based on the selection of candidate point clouds from the ship and shore. Calculate the FPFH descriptor for each keypoint, including: Based on any key point in the ship-shore candidate point cloud pair Within the preset search radius Perform an inner radius search to obtain its neighborhood point set: ; by Using a radius, downsample the aforementioned neighborhood point set; For each key point Local plane fitting is performed within the downsampled neighborhood point set. By constructing the covariance matrix and performing eigenvalue decomposition, the eigenvalues ​​are obtained. and and key points normal vector ; Linearity index defined based on eigenvalues Select the key points that meet the requirements and eliminate redundant points; Combining key points normal vector Construct a local Darboux coordinate system for the neighborhood point set, and calculate the angular features between the key point and the neighborhood point set. α , , θ ); Based on the aforementioned angular features, key points are statistically formed. SPFH histogram; By performing distance-weighted fusion of SPFH values ​​within the neighborhood of keypoints, the final keypoints are obtained. FPFH descriptor; Based on the FPFH descriptor, fast and high-precision relative pose estimation is achieved through coarse and fine registration, including: The SAC-IA algorithm was used for preliminary calibration of the point cloud to obtain the low-precision pose of the shipborne LiDAR in the shore-based point cloud map coordinate system. , is represented as: (3) in, The shore-based point cloud provided for shore-based LiDAR is also known as the target point cloud; This represents the relative pose of the shipborne LiDAR to the shore-based system at the current moment. The pose of the shore base in the point cloud map coordinate system; High-precision registration was performed using Small-GICP: Will As the initial value for registration, use Pose sharing with shore-based LiDAR To put the shipborne LiDAR at the current point in time k FPFH descriptor acquired at each moment Convert to target point cloud In the coordinate system, the source point cloud is obtained that is consistent with the coordinate system of the shore-based point cloud map. , is represented as: (4) Source cloud Registration to target point cloud This allows for a more accurate high-precision coordinate system transformation from the shipborne LiDAR pose to the shore-based point cloud map. ; Combined with coordinate system transformation The high-precision relative pose of the shipborne LiDAR in the coordinate system of the shore-based point cloud map is obtained at the current moment. : (5) The high-precision relative pose is reconstructed based on an asynchronous mechanism to obtain the final ship-shore relative pose estimate at the current moment, expressed as: (6) in, This indicates the current time of the shipborne LiDAR relative to the shore-based coordinate system. S The pose estimation results, express k -1 time unmanned surface vessel coordinate system S The pose estimation results in the middle, and They represent k -1 time and k At any given moment, the pose of the unmanned surface vessel in its own odometry coordinate system is estimated; ) 1 This represents the inverse of the pose transformation matrix, used to achieve inverse transformations between coordinate systems.

[0008] Furthermore, in S6, the specific steps for solving the ship-shore cooperative factor graph model, while simultaneously satisfying the odometry continuity constraint and the ship-shore relative observation constraint, to minimize the overall residual and obtain the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time point include: The ship-shore cooperative factor graph model is solved using the iSAM2 solver, while simultaneously satisfying trajectory continuity constraints and ship-shore relative observation constraints, in order to minimize the sum of nonlinear least squares errors, i.e., minimize the overall residual, expressed as: (7) in, The error model representing the variable factors, The error model representing the shipborne LiDAR odometry factor has a correlation range of two consecutive state nodes. and ; The error model represents the ship-to-shore LiDAR pose correlation factor, with the correlation range being shore-based. And this ship State nodes; and This indicates the weights used when applying different covariance matrices; Represents the set of all variables. This represents the prior terms obtained through marginalization. (8) in, This represents the relative transformation relationship between two consecutive LiDAR odometry coordinate systems. (9) in, Let be the ship-shore relative pose transformation matrix. This indicates the ship's status in the shore-based point cloud map coordinate system.

[0009] Furthermore, in S3, the process of constructing the shipborne global descriptor and the shore-based global descriptor is the same, including: Divide the 3D point cloud space along the vertical axis into n L A series of equal-height levels are used to transform a 3D point cloud into a 2D maximum height map. The two-dimensional maximum height map is divided according to polar coordinates. N r A radial concentric ring and N s Each angle sector forms N s × N r One grid cell; For any point in the point cloud p i =( x i , y i , z i Convert it to polar coordinates: ( ρ i , i , z i ), where radial distance azimuth ; For each grid cell ( s , r ): Filter out sectors that fall within the corresponding angle. s and radial annular region r For all points within the grid, the maximum Z-coordinate is taken as the value of the corresponding grid cell, defined as follows: (10) Where, Φ s and R r Representing the first s The first angle sector and the first r The boundary range of each radial ring region; If there are no points in the grid cell, the value is assigned to 0; All grid cell values ​​are set according to N s × N rThe dimensional arrangement yields the complete global descriptor.

[0010] Beneficial Effects: This invention constructs global descriptors for both shipborne and shore-based point clouds. Based on these descriptors, it performs scene matching between shipborne and shore-based observations. Upon successful matching, a predetermined decision mechanism generates candidate point clouds for both ship and shore. The shipborne and shore-based ends extract key points from these candidate point cloud pairs and calculate a compact and discriminative FPFH descriptor for each key point. This achieves data compression and rapid matching from the original point cloud to the feature point set, enabling real-time exchange of surrounding 3D environmental information between the ship and shore. This improves data transmission efficiency and pose estimation accuracy. By leveraging the fixed global pose provided by the shore-based end, the accuracy of the ship's pose estimation in the global coordinate system is improved. Furthermore, by constructing and solving a ship-shore collaborative factor graph model, a consistent optimal pose of the unmanned surface vessel in the global coordinate system at each moment is obtained, effectively avoiding the cumulative error and pose drift problems inherent in long-term single-ship pose estimation. Attached Figure Description

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

[0012] Figure 1 This is a flowchart of the USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration in this invention. Figure 2 This is a flowchart illustrating the improved FPFH calculation in an embodiment of the present invention. Figure 3 This is a schematic diagram of the structure of the ship-shore collaboration factor graph model in an embodiment of the present invention; Figure 4 This is a schematic diagram of a shipborne communication framework based on ROS in an embodiment of the present invention; Figure 5 This is a schematic diagram of the official VRX competition environment in an embodiment of the present invention; Figure 6 This is a communication flowchart for ship-shore cooperative pose estimation in an embodiment of the present invention; Figure 7 This is a schematic diagram of the Scan Context descriptor for the point clouds of both sides when the distance between the ship and the shore is 66.73m in an embodiment of the present invention; Figure 8 This is a bar chart showing the statistics of samples and the number of successful recognitions in different distance intervals in this embodiment of the invention, as well as the success rate. Figure 9 This is an example diagram illustrating the correct matching of cloud data between the shipborne terminal and the shore terminal in an embodiment of the present invention. Figure 10 This is a top view of some port scenes in an embodiment of the present invention; Figure 11 This is a comparison diagram of berthing trajectories obtained by continuous pose estimation using various methods in embodiments of the present invention; Figure 12 This is a comparison diagram of the departure trajectories of various methods for continuous pose estimation in embodiments of the present invention. Detailed Implementation

[0013] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0014] This embodiment provides a method for estimating the berthing and departure pose of a USV based on ship-shore LiDAR collaboration, such as... Figure 1 As shown, the specific steps include: S1. Collect shipborne point cloud, i.e., the point cloud of the surrounding environment of the unmanned surface vessel, through shipborne LiDAR; collect shore-based point cloud, i.e., the point cloud of the dock and near-shore area, through shore-based LiDAR; obtain the fixed reference pose of the shore-based LiDAR in the global coordinate system through shore-based sensors (i.e., GNSS base stations). S2. Perform lidar odometry calculation on two consecutive frames of shipborne point cloud to obtain the motion increment of the unmanned surface vessel at adjacent times, and generate LiDAR odometry factor based on the motion increment. Specifically, this embodiment performs lidar odometry calculations based on two consecutive frames of shipborne point clouds, including: The relative pose change of the unmanned surface vessel (USV) at adjacent moments is obtained by point cloud registration. This relative pose change is the motion increment of the current moment relative to the previous moment. Combining the uncertainty estimated in the registration process, a LiDAR odometry factor is constructed. This factor connects the pose nodes of the USV at adjacent moments and serves as a trajectory continuity constraint for subsequent optimization of the ship-shore cooperative factor graph model.

[0015] S3. Construct global descriptors for shipborne point clouds and shore-based point clouds respectively to obtain shipborne global descriptors and shore-based global descriptors; perform scene matching between shipborne observations and shore-based observations based on the shipborne global descriptors and shore-based global descriptors, and generate ship-shore candidate point cloud pairs that can be used for cross-platform registration based on the set decision mechanism when the matching is successful. Specifically, in this embodiment, global scene descriptors are constructed for shipborne and shore-based point clouds respectively and fast matching is performed to determine whether the current shipborne observation and the shore-based observation at a certain moment have overlapping scenes. When the matching is successful, a set of point cloud pairs that can be used for cross-platform registration is generated to provide an initial correspondence for subsequent accurate relative pose calculation.

[0016] Specifically, in the collaborative pose estimation process for autonomous berthing and unberthing between ship and shore, the first and crucial step is to achieve environmental detection and identification within the detection range of the unmanned surface vessel (USV) by the shore-based platform. The core of this identification process lies in confirming whether the environment within the USV's detection range has entered the effective detection range of the shore-based LiDAR, and whether both have observed a common scene. This event triggers all subsequent collaborative operations (such as precise pose estimation, factor graph optimization, etc.). This embodiment uses a ScanContext global descriptor to compress scene information and perform rapid matching. The global descriptor compresses data by highly generalizing the information of the entire point cloud into a vector. In ship-shore collaborative berthing and unberthing scenarios, the ScanContext global descriptor is robust to changes in viewpoint and can process information quickly, making it very suitable for dynamic scenarios where USVs approach the shore-based platform from different directions and distances. It meets the requirements of real-time performance and efficiency, solving the problem of traditional position recognition methods attempting to register between original point clouds to find candidate scan frames for common areas between ship and shore, which consumes a large amount of time and computing resources. In a specific embodiment, S3 involves constructing global descriptors for both the shipborne and shore-based point clouds, resulting in a shipborne global descriptor and a shore-based global descriptor. The specific steps for performing scene matching between shipborne and shore-based observations based on these descriptors, and generating candidate ship-shore point cloud pairs for cross-platform registration based on a pre-defined decision mechanism upon successful matching, include: S31. Construct shipborne and shore-based global descriptors: Point clouds collected by unmanned surface vessels or shore-based LiDAR at a given moment P ={ p 1, p 2,…, p N The construction process for shipborne global descriptors and shore-based global descriptors is the same, including: Divide the 3D point cloud space into n along the vertical axis (Z-axis). L There are several levels of equal height. To simplify and adapt to the characteristics of the water surface environment, n is set. L =1, retaining only the highest point within each vertical cylinder, thus transforming the 3D point cloud into a 2D maximum height map; The two-dimensional maximum height map is divided according to polar coordinates. N r A radial concentric ring andN s Each angle sector forms N s × N r One grid cell; For any point in the point cloud p i =( x i , y i , z i Convert it to polar coordinates: ( ρ i , i , z i ), where radial distance azimuth ; For each grid cell ( s , r ): Filter out sectors that fall within the corresponding angle. s and radial annular region r For all points within the grid, the maximum Z-coordinate is taken as the value of the corresponding grid cell, defined as follows: (1) Where, Φ s and R r Representing the first s The first angle sector and the first r The boundary range of each radial ring region; If there are no points in the grid cell, the value is assigned to 0; All grid cell values ​​are set according to N s × N r The dimensional arrangement yields the complete Scan Context matrix, or Scan Context global descriptor, which projects the 3D point cloud into a bird's-eye view representation, effectively capturing the structural features of the environment. In this way, a complete 3D point cloud is encoded into a compact and highly discriminative 2D matrix that preserves the structural contours and height information of the environment, making it particularly effective for identifying static structures such as docks and breakwaters.

[0017] Specifically, the global descriptor compresses data by highly generalizing the information of the entire point cloud into a single vector. In ship-shore cooperative berthing and unberthing scenarios, Scan Context is robust to changes in viewpoint and can process them quickly, making it ideal for dynamic scenarios where unmanned surface vessels (USVs) approach shore-based platforms from different directions and distances. It solves the problem of automatic pairing between the ship-borne and shore-based ends in ship-shore cooperative pose estimation systems, enabling USVs to monitor in real time and automatically determine whether they are currently within the detection range of the shore-based LiDAR, and then trigger the next stage of ship-shore cooperative relative pose estimation.

[0018] In a specific embodiment, when the shore-based LiDAR receives the shipborne global descriptor broadcast by the unmanned surface vessel... SC USV At that time, the corresponding shore-based global descriptor is generated. SC Shore ; S32. The specific steps of performing scene matching based on the shipborne global descriptor and the shore-based global descriptor, and generating ship-shore candidate point cloud pairs that can be used for cross-platform registration based on the set decision mechanism when the matching is successful include: S321. Calculate the similarity between the shipborne global descriptor and the shore-based global descriptor to evaluate the matching relationship between the two frame point clouds; In practice, even if the unmanned surface vessel (USV) and the shore-based platform are in the same location, the orientation of the USV can be arbitrary, resulting in different outputs. SC USV Compared to SC Shore A column shift will occur. For example, if the unmanned surface vessel rotates 90 degrees, its... SC USV The matrix will then occur accordingly. Ns / 4 column cyclic shift. To address this issue, the process of calculating the similarity between the shipborne global descriptor and the shore-based global descriptor includes a step of finding the optimal rotation alignment, specifically as follows: For any global descriptor (e.g.) SC USV Perform all possible column shifts (from 0 to ... N s 1), and calculate its relationship with another global descriptor (e.g., after performing each column shift) SC Shore The distance to ) is defined by the distance formula: (2) in, k It is the number of column shift steps. k ∈[0, N s 1]; W r It is a radial distance r The relevant weighting factor is usually set to 1 / r This gives higher weight to points closer to the center (which are generally more stable and reliable); | | represents Manhattan distance; calculate N s After calculating the distance between two global descriptors under the column shift condition, the minimum distance is taken as the final distance between the two global descriptors, and this final distance is used as the scene similarity under the optimal rotation alignment, expressed as: (3) Specifically, the minimum distance value Distance SC The smaller the value, the more similar the two scenarios are.

[0019] S322. After obtaining scene similarity, the shore-based platform needs a robust decision-making mechanism to ultimately confirm whether the environment within the unmanned surface vessel's detection range has entered the corresponding detection range, in order to avoid false alarms. Based on the established decision-making mechanism, candidate point cloud pairs of ships and shores that can be used for cross-platform registration are generated, including: Set a distance threshold T dist This threshold can be determined through offline data analysis or on-site calibration; Determine if it is a Distance SC < T dist If yes, it means that at the current moment, the scenario described by the unmanned surface vessel (USV) and the scenario described by the shore-based system are highly matched, that is, the USV is likely to have entered the collaborative work area, and then the time consistency check is performed. Since triggering the collaborative mechanism based on a single successful match is unreliable and could lead to misjudgments due to momentary occlusion or similar environmental structures, this embodiment includes a time consistency check, comprising: Set a sliding time window (e.g., the most recent 2 seconds) and check if the Distance condition is met within that sliding time window. SC < T dist Does the percentage of frames exceed a preset ratio (e.g., 80%)? If so, then the two frames of point clouds at the current moment, one shipborne and one shore-based, are determined to be ship-shore candidate point cloud pairs.

[0020] At this point, the shore-based platform finally confirms that the unmanned surface vessel has entered its detection range and generates a "cooperative trigger" signal, thereby initiating the subsequent relative pose estimation and data exchange process. This two-step verification mechanism greatly improves the accuracy of identification and the robustness of the system.

[0021] S4. Based on the ship-shore candidate point cloud pair, extract key points and calculate the FPFH descriptor of the key points. Combine the FPFH descriptor and use coarse registration and fine registration methods to solve for the high-precision relative pose of the ship-borne LiDAR relative to the shore-based LiDAR. Reconstruct the high-precision relative pose based on the asynchronous mechanism to obtain the final ship-shore relative pose. Generate the ship-shore pose association factor based on the final ship-shore relative pose. In a specific embodiment, in S4, key points are extracted based on the ship-shore candidate point cloud pair, and FPFH descriptors of the key points are calculated. Combining the FPFH descriptors with coarse and fine registration methods, the high-precision relative pose of the shipborne LiDAR relative to the shore-based LiDAR is obtained. The high-precision relative pose is then reconstructed based on an asynchronous mechanism to obtain the final ship-shore relative pose. The specific steps include: In order to transition from macroscopic scene recognition to microscopic geometric alignment, this embodiment selects several key points based on the ship-shore candidate point cloud. Specifically, this embodiment employs voxel downsampling or uniform sampling methods to select key points in the point cloud at fixed intervals, ensuring uniform coverage of the point cloud surface. This method has minimal computational overhead and effectively reduces redundant information in the point cloud; Calculate the FPFH (Fast Point Feature Histogram) descriptor for each extracted keypoint; Specifically, the FPFH is a 33-dimensional vector that encodes the geometric information of the local neighborhood surrounding the keypoint. By analyzing the normal direction relationship between points, a histogram is constructed, which can effectively distinguish different geometric structures such as planes, edges, and corners. FPFH has good robustness to sensor noise and changes in point cloud density, making it very suitable for use in scenarios such as shore-based and unmanned surface vessels where different scanning densities and environments may exist. In addition, compared with traditional FPFH, the improved FPFH used in this embodiment greatly simplifies the calculation process, making feature extraction very fast and meeting the requirements of real-time systems. Through this process, an original point cloud frame containing tens of thousands of points is compressed into a feature point set consisting of hundreds or thousands of keypoints and their corresponding 33-dimensional FPFH descriptors. When candidate point cloud pairs between ship and shore appear, both ends of the ship and shore simultaneously begin calculating the FPFH descriptors of the candidate point cloud pairs. By exchanging these compact FPFH descriptors, accurate correspondences are found without exchanging the entire point cloud.

[0022] Specifically, while direct registration of the original point cloud (such as ICP) offers high accuracy, it is computationally intensive, sensitive to initial pose, and imposes significant data transmission pressure in distributed systems. Therefore, this embodiment employs a registration method based on an improved FPFH local feature descriptor to replace traditional point cloud matching, thereby improving computational and data transmission efficiency. A compact and discriminative FPFH descriptor is calculated for each key point, achieving data compression and rapid matching from the original point cloud to the feature point set, while reducing computational and data volume. The computation flowchart is as follows: Figure 2 As shown, it includes: Based on any key point in the ship-shore candidate point cloud pair Within the preset search radius Perform an inner radius search to obtain its neighborhood point set: ; by Using a radius, the aforementioned neighborhood point set is downsampled to reduce the point cloud density and improve subsequent computational efficiency; For each key point Local plane fitting is performed within the downsampled neighborhood point set. By constructing the covariance matrix and performing eigenvalue decomposition, the eigenvalues ​​are obtained. and and key points normal vector ; Linearity index defined based on eigenvalues Select key points with high geometric differentiation and eliminate redundant points; Combining key points normal vector Construct a local Darboux coordinate system for the neighborhood point set, and calculate the angular features between the key point and the neighborhood point set. α , , θ ); Based on the aforementioned angular features, key points are statistically formed. SPFH histogram (Simple Point Feature Histogram); By performing distance-weighted fusion of SPFH values ​​within the neighborhood of keypoints, the final keypoints are obtained. The FPFH descriptor.

[0023] Specifically, the registration method based on the improved FPFH local feature descriptor reduces the computational complexity from the quadratic complexity of traditional PFH to linear complexity while maintaining strong geometric expressive power, thereby achieving efficient and robust point cloud feature description.

[0024] Based on the FPFH descriptor, fast and high-precision relative pose estimation is achieved through coarse and fine registration, including: The Sample Consensus Initial Alignment (SAC-IA) algorithm was used for preliminary calibration of the point cloud to obtain the low-precision pose of the shipborne LiDAR in the shore-based point cloud map coordinate system. , is represented as: (4) in, The shore-based point cloud provided for shore-based LiDAR is also known as the target point cloud; This represents the relative pose of the shipborne LiDAR to the shore-based system at the current moment. The pose of the shore base in the point cloud map coordinate system; High-precision registration was performed using Small-GICP: Will As the initial value for registration, use Pose sharing with shore-based LiDAR To put the shipborne LiDAR at the current point in time k FPFH descriptor acquired at each moment Convert to target point cloud In the coordinate system, the source point cloud is obtained that is consistent with the coordinate system of the shore-based point cloud map. , is represented as: (5) Source cloud Registration to target point cloud This allows for a more accurate high-precision coordinate system transformation from the shipborne LiDAR pose to the shore-based point cloud map. ; Combined with coordinate system transformation The high-precision relative pose of the shipborne LiDAR in the coordinate system of the shore-based point cloud map is obtained at the current moment. : (6) Since shore-based LiDAR is fixed, then This represents the relative pose of the shipborne LiDAR to the shore-based system at the current moment. Since point cloud registration initialization and continuous matching require time, ship-to-shore fine registration is a low-frequency, high-precision process, while shipborne LiDAR odometry is a high-frequency, low-latency process. Therefore, an asynchronous mechanism is adopted to solve the real-time problem. The high-frequency pose increment provided by the shipborne LiDAR odometry is used to reconstruct the low-frequency results of ship-to-shore fine registration, obtaining the final ship-to-shore relative pose estimate at the current moment, expressed as: (7) in, This indicates the current time of the shipborne LiDAR relative to the shore-based coordinate system. S The pose estimation results, express k -1 time unmanned surface vessel coordinate system S The pose estimation results in the middle, and They represent k -1 time and k At any given moment, the pose of the unmanned surface vessel in its own odometry coordinate system is estimated; ) 1 This represents the inverse of the pose transformation matrix, used to achieve inverse transformations between coordinate systems.

[0025] Specifically, this embodiment solves a high-precision six-degree-of-freedom rigid body transformation for each pair of ship-shore candidate point cloud pairs. R , t This refers to the high-precision relative pose of the shipborne LiDAR with respect to the shore-based LiDAR, in order to establish an accurate ship-shore pose correlation. Based on the high-precision relative pose, a ship-shore pose correlation factor is generated, which serves as a ship-shore relative observation constraint and is used for global anchoring.

[0026] Specifically, based on the final ship-shore relative pose obtained by asynchronous reconstruction, and combined with the uncertainty of the registration process estimation, a ship-shore pose correlation factor is constructed. This factor connects the current UAV pose node with the shore-based LiDAR fixed pose anchor node to form a ship-shore relative observation constraint, which is used for subsequent ship-shore collaborative factor graph model optimization.

[0027] S5. The pose of the unmanned surface vessel at each moment is taken as the variable node to be optimized, and the fixed reference pose of the shore-based LiDAR in the global coordinate system is taken as the fixed anchor point node. The LiDAR odometry factor and the ship-shore pose correlation factor are introduced as odometry continuity constraints and ship-shore relative observation constraints, thereby constructing a ship-shore cooperative factor graph model. Specifically, the LiDAR odometry factor is used to ensure the continuity of the trajectory; the ship-shore pose correlation factor is used to anchor the shipborne trajectory to the shore-based global reference.

[0028] In a specific embodiment, to integrate the historical pose of the odometry and the ship-shore relative pose into a unified backend optimization framework, this embodiment employs a factor graph-based optimization method. This method achieves constraint modeling through the construction of factors (edges) and nodes, specifically using the iSAM2 solution strategy. By dynamically updating the decomposition structure of the sparse smooth information matrix, it efficiently completes the estimation of the ship-shore synchronous continuous cooperative pose. This optimization process receives two types of key inputs: one is the coordinate factors from the shipborne LiDAR odometry (i.e., trajectory continuity constraints), and the other is the ship-shore pose correlation factor generated by the ship-shore cooperative relative pose estimation. The optimizer jointly corrects the ship's navigation trajectory based on the above two types of factors, thereby effectively ensuring the consistency and robustness of the system pose estimation.

[0029] The ship-shore collaboration factor diagram model proposed in this embodiment is as follows: Figure 3 As shown, variable nodes represent the status of the ship and shore-based systems, while factor nodes provide odometer data and ship-shore data correlations to aid optimization. Variable Nodes and These represent the attitudes of the ship and the shore-based LiDAR, respectively. For factor nodes, the connection edges for each ship state correspond to LiDAR odometry factors. These factors are LiDAR scan data accumulated over time during continuous positioning and are used to generate attitude trajectories. The constraint edges between the ship's keyframes and the shared data from the shore-based LiDAR constitute the ship-shore pose correlation factor, which is used to constrain the relative transformation relationship between the ship's attitude and the shore-based LiDAR attitude.

[0030] S6. Solve the ship-shore cooperative factor graph model. Under the condition of simultaneously satisfying the odometry continuity constraint and the ship-shore relative observation constraint, minimize the overall residual to obtain the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time. Based on the consistent optimal pose, realize berthing and departure control and navigation, thereby effectively suppressing the cumulative drift error caused by long-term operation. At the same time, receive a new frame of shipborne point cloud and shore-based point cloud, repeat S2-S5, and realize real-time continuous pose estimation.

[0031] Specifically, as a new frame of shipborne and shore-based lidar data arrives, S1-S5 are repeated to continuously update the factor map and perform rolling optimization, thereby achieving real-time, continuous, and high-precision pose estimation under ship-shore collaborative conditions.

[0032] In a specific embodiment, the optimization process of the ship-shore cooperative factor graph model is based on the constraints in the factor graph to find the optimal solution for constructing the factors. This optimization belongs to maximum a posteriori probability inference. Specifically, solving the ship-shore cooperative factor graph model to obtain the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time step includes: Online optimization is performed using the iSAM2 solver, while simultaneously satisfying trajectory continuity constraints and ship-shore relative observation constraints, to minimize the sum of nonlinear least squares errors, i.e., minimize the overall residual, expressed as: (8) in, The error model representing the variable factors, The error model representing the shipborne LiDAR odometry factor has a correlation range of two consecutive state nodes. and ; The error model represents the ship-to-shore LiDAR pose correlation factor, with the correlation range being shore-based. And this ship State nodes, The fixed pose of the shore-based LiDAR in the global coordinate system (shore-based point cloud map coordinate system); and This indicates the weights used in different covariance matrices. The covariance of each variable reflects its degree of uncertainty. This parameter can be set according to the error statistics of the corresponding factor's observations. Represents the set of all variables. This represents the prior terms obtained through marginalization. Specific error models include: LiDAR odometry factor: based on the first k The next scan obtains ship status parameters through successive matching and k +1 estimate, combined with the relative transformation relationship in two consecutive LiDAR odometry coordinate systems. The residuals of the LiDAR odometry coefficients can be expressed as: (9) Ship-shore pose correlation factor: Information shared through shore-based LiDAR can improve the performance of the ship's pose estimation system. The constraint relationship between the state nodes between the ship and shore can be regarded as the ship-shore data correlation factor. This factor combines the ship-shore relative pose transformation matrix. The residual of this factor can be expressed as follows, which represents the state of the ship in the shore-based point cloud map coordinate system. Error between the state derived from the relative pose and the state: (10).

[0033] Specifically, to verify the effectiveness of the method proposed in this embodiment, simulation experiments were conducted using the Gazebo physics simulation environment within the Robot Operating System (ROS). The official Virtual RobotX (VRX) competition environment, jointly developed by Open Robotics (OP) and the Naval Postgraduate School, was employed. Figure 5 As shown, this is not a replica of a specific port, but rather designed specifically to test the performance of USVs in complex maritime missions. The VRX environment encompasses a range of typical maritime challenges, such as waterways, buoys, docks, other dynamic vessels, and a variable marine environment. Its design aims to simulate real-world autonomous navigation, perception, and manipulation tasks, which aligns closely with the research objectives of this embodiment, providing a highly challenging and standardized test platform for the proposed cooperative pose estimation research.

[0034] This embodiment sets up a collaborative sensing system consisting of a mobile shipborne platform and a fixed shore-based platform. Both are equipped with the same hardware suite of core sensors to ensure data consistency and the feasibility of spatiotemporal synchronization. A fixed shore-based platform is set up on open ground near the berth in the VRX virtual environment. This platform is equipped with the exact same sensor suite as the unmanned surface vessel (USV), namely a Velodyne VLP-16 LiDAR (performance parameters are shown in Table 1). To ensure its accuracy as an absolute coordinate reference, a high-precision GNSS base station (the same model as the USV) is also configured for it. This base station can provide millimeter-level static position reference, which is standard practice in marine mapping and collaborative positioning. With its stable and global perspective, the shore-based LiDAR can observe blind spots that may exist in the USV's own sensors (such as when obstructed by other vessels), providing crucial external observation information for collaborative pose estimation.

[0035] Table 1:

[0036] To verify the pose estimation performance of the proposed method in busy ports during berthing and departure, a simple and efficient point-to-point communication network was designed. This network treats the shipborne and shore-based terminals as two equal communicating agents, but their information interaction follows a clear "request-response" pattern. The shipborne terminal, as the requester of the positioning service, actively initiates communication, and its communication framework is as follows: Figure 4 As shown, the shore-based end, acting as the provider of the global reference, responds. The entire coordination process is driven by a series of well-defined messages, forming an event-driven state machine. To achieve modular data exchange, four core message types are defined, and the entire ship-shore coordination communication flow is as follows: Figure 6As shown. The Position Identification Message (PR_MSG) is sent by the shipborne end and contains a global descriptor extracted from the current LiDAR scan frame, used to request the shore-based end to perform fast scene matching. The Relative Pose Request Message (RPE_MSG) is sent by the shore-based end as a response to PR_MSG. This message contains feature descriptors related to the candidate matching region, used by the shipborne end to calculate the accurate relative pose. The Factor Graph Optimization Message (FGO_MSG) is transmitted bidirectionally between the ship and shore. After the ship-shore pose data association (i.e., accurate relative pose) is established, both parties share boundary node information by exchanging FGO_MSG, and jointly perform distributed graph optimization in an iterative manner to achieve global pose consistency. The End Message (END_MSG) is sent by either end to terminate the current communication cycle, indicating that the current collaborative task has been completed or the current communication window is about to close.

[0037] Specifically, the entire simulation system was built on the Ubuntu 20.04 operating system and the ROS Noetic environment. All data communication between modules was conducted through ROS topics and services, ensuring the system's modularity and scalability. The motion control of the unmanned surface vessel (USV) was achieved through the `ros_control` interface. The ship-shore cooperative pose estimation algorithm proposed in this embodiment can subscribe to laser point cloud data from the USV and shore-based platform in real time. Through steps such as position recognition, relative pose estimation, and factor graph optimization, it outputs the optimized USV pose. C++ was used to ensure the algorithm's real-time performance and computational efficiency when processing large amounts of point cloud data.

[0038] Specifically, the experiment compared the accuracy of single-ship berthing status estimation with the accuracy of shore-based berthing status estimation. Evaluation metrics included recall, accuracy, F1-score, and computational efficiency. Recall: The percentage of positive pairs that are successfully retrieved as candidate matches out of all true overlapping pairs. It is a key metric for measuring whether the PR module is "missing" matches and is crucial for capturing valuable collaborative opportunities. It can be calculated using the following formula: (11) Accuracy: The percentage of truly overlapping positive sample pairs among all retrieved candidate matches. It is a key metric for measuring whether the PR module is generating false positives and directly affects the computational load of subsequent modules. It can be calculated using the following formula: (12) in, The number of sample pairs that were successfully retrieved as candidate matches and were indeed true overlaps; N GT This represents the total number of all truly overlapping positive sample pairs; N FPThe number of sample pairs that are incorrectly retrieved as candidate matches but are not actually true overlaps; F1-Score: The harmonic mean of precision and recall, used to comprehensively evaluate the performance of the module, and can be calculated using the following formula: (13) Computational efficiency: The average processing time (milliseconds) for generating a single descriptor and completing a database retrieval directly affects the real-time performance of the system.

[0039] Evaluation metrics for relative pose estimation include relative translation error (RTE), relative rotation error (RRE), success rate, and computational efficiency, among which: RTE measures the error between the estimated translation and the true value for each sample (usually the square of the Euclidean distance), reflecting the accuracy of the translation estimate. RRE measures the error between the estimated rotation and the true value for each sample (rotation angle, in degrees), reflecting the accuracy of the rotation estimate. For successfully registered pairs, the relative translation error and relative rotation error between their estimated relative pose and the true value can be calculated using the following formula: (14) (15) in, t n,GT For the first n The true value translation vector of each sample; No. n The estimated translation vector for each sample; R n,GT No. n The true value rotation matrix of each sample; For the first n The estimated rotation matrix for each sample; N success The number of successfully registered sample pairs; Success rate: The percentage of candidate matching pairs that successfully converge and output a valid pose result; Computational efficiency: The average processing time for each candidate match.

[0040] Evaluation metrics for ship-shore collaborative factor graph model optimization include absolute trajectory error and system operating frequency.

[0041] Absolute trajectory error (ATE): This is the core indicator for evaluating global trajectory accuracy, directly reflecting the cumulative drift of the system throughout its operation. A lower ATE indicates a trajectory closer to the true path and better drift suppression. This embodiment only considers translation error, therefore it can be calculated using the following formula: (16) Where, trans() means taking the translated part of the variable inside the parentheses; N This represents the total number of timestamps or keyframes in the trajectory. For the first i The ground real transformation matrix at each time step (including rotation and translation). No. i The estimated transformation matrix at each time step; System operating frequency (FPS): Evaluate the real-time computing performance of each algorithm to ensure that the improvement in accuracy does not come at the expense of real-time performance; Specifically, position recognition (PR) is the first step in ship-shore collaborative perception and the trigger for the entire collaborative process. Its core task is to quickly and efficiently determine whether there is sufficient overlap between the point cloud data of the ship-borne and shore-based terminals at the same time, which can assist the unmanned surface vessel (USV) in high-precision pose estimation. For pose estimation in USV berthing and unberthing scenarios with extremely high real-time requirements, traditional point cloud registration methods are difficult to apply due to their high computational cost. To verify the effectiveness and superiority of the Scan Context-based global descriptor used in this embodiment in ship-shore dynamic scenarios, a self-collected "ship-shore collaborative berthing and unberthing" virtual dataset was used for testing. This dataset covers the entire process of USV approaching from a long distance (>200m), maneuvering at a medium distance (50-200m), and berthing at a close distance (<50m). To test the invariance of viewpoint, various approach routes were designed, including parallel berthing, turning berthing, and approaching from different angles (port side, starboard side, bow facing). 200 pairs of point cloud frames were selected from the dataset, including 150 pairs of positive samples with actual overlapping regions and 50 pairs of negative samples without overlapping regions. Figure 7 The Scan Context descriptor for the ship-shore point cloud at a distance of 66.73 m is displayed, and the minimum distance value Distance between the two is calculated. SC The value of 0.93 indicates that the scenes detected by ship-to-shore LiDAR are very similar. Figure 8The distribution of these 200 pairs of point cloud frames across different distance ranges and the success rate of the proposed location recognition algorithm for samples at different distances in the dataset are shown. Blue bars represent the total number of samples in the corresponding distance range, orange bars represent the number of successfully recognized samples, and the green curve represents the recognition success rate. It can be seen that, taking the 130m range LiDAR used in this embodiment as an example, the success rate of location recognition significantly decreases when the distance between the shipborne and shore-based LiDARs exceeds 100m.

[0042] To fully verify the effectiveness of the pose estimation method in ship-shore cooperative berthing and unberthing scenarios, another widely used global point cloud descriptor, M2DP, was selected as the baseline method and compared with the method proposed in this embodiment. M2DP is a global descriptor based on multi-view point cloud density distribution, which performs well in structured scenes but is relatively sensitive to viewpoint changes. The performance comparison of the two descriptors on the test dataset is shown in Table 2; Table 2:

[0043] As can be seen, the method proposed in this embodiment exhibits a significant advantage in computational efficiency. Its average processing time is only 24.3 ms, far lower than M2DP's 69.7 ms. This superior efficiency enables the method proposed in this embodiment to meet the high-frequency, low-latency communication requirements in ship-shore collaborative scenarios, providing a solid foundation for the real-time performance of the entire system.

[0044] In terms of accuracy, the method proposed in this embodiment achieves an F1-Score of 0.90, which is superior to M2DP. Although M2DP has a slightly higher accuracy, its recall is significantly lower. This indicates that M2DP is more "conservative" in matching, reducing false alarms but also missing many valid matching opportunities. In dynamic ship-shore scenarios, the rapid movement and variable approach angles of unmanned surface vessels mean that the observation perspective is constantly changing. The Scan Context used in this embodiment, due to its bird's-eye view projection method, has natural robustness to such changes in perspective and distance, thus achieving a higher recall. This means that the system has more opportunities to trigger collaboration, and can reliably identify common scenes even under poor observation conditions.

[0045] like Figure 9 The red area represents the shipborne LiDAR point cloud, the green area represents the shore-based LiDAR point cloud, and the blue area represents the shipborne LiDAR point cloud displayed in the global coordinate system after ship-shore collaborative relative pose estimation. As can be seen from the figure, the ship-shore point clouds exhibit significant rotation angles due to different viewing angles. Therefore, using the optimal rotation alignment result obtained after position recognition as the prior input for the relative pose estimation process can improve the accuracy and speed of pose estimation.

[0046] To comprehensively evaluate the performance of the proposed method in this embodiment, two representative advanced registration algorithms, Small-GICP and KISS-Matcher, were selected as baselines. Small-GICP is an advanced, probabilistic model-based full point cloud registration algorithm, an efficient variant of traditional ICP. It achieves high-precision registration by minimizing the distance between point cloud distributions. KISS-Matcher is a lightweight feature registration method that prioritizes real-time performance, estimating pose by rapidly extracting and matching local features. These two baselines represent two different technical directions: high-precision full point cloud registration and efficient feature matching, respectively, and can be effectively compared with the proposed method in this embodiment from multiple dimensions. Testing was conducted using a self-collected virtual dataset of "ship-shore cooperative berthing and unberthing." 100 pairs of ship-shore candidate point cloud pairs were selected from the dataset, including 50 correct matches and 50 incorrect matches. All three methods were tested on the same ship-shore candidate point cloud pairs to ensure fair comparison. For both baseline methods, coarse poses were used as their initial values ​​to simulate real cooperative scenarios, with ground truth values ​​provided by pose recording nodes in the virtual environment. Table 3 shows a performance comparison of the three methods on the test dataset.

[0047] Table 3:

[0048] As shown in Table 3, the performance of the three methods reflects the characteristics of their respective technical approaches. Small-GICP exhibits the highest absolute accuracy in successful registration, validating its ability as a high-precision full point cloud registration algorithm. However, its success rate is only 62%, the lowest among all methods. This fully exposes the algorithm's high sensitivity to the coarse initial pose of the input. Faced with large initial deviations, Small-GICP is prone to getting trapped in local optima or failing to converge, which is fatal in collaborative systems that need to handle uncertain candidate matches. KISS-Matcher demonstrates a high success rate (90%) and extremely fast processing speed, consistent with its design goal as an efficient feature matching method. However, its registration accuracy (0.09m, 0.22°) is relatively low, possibly due to its simplified feature descriptors being insufficient to capture complex geometric details, leading to matching biases. The method proposed in this embodiment successfully balances accuracy and robustness, achieving a success rate of 96%, significantly better than Small-GICP and slightly better than KISS-Matcher. This is mainly attributed to the robustness of the SAC-IA algorithm, which can find reliable correspondences in the low-dimensional FPFH feature space even with large initial biases, thus providing an excellent initial value for subsequent optimization. In terms of accuracy, through the refinement of Small-GICP, the method proposed in this embodiment (0.06m, 0.18°) significantly outperforms KISS-Matcher and is very close to the theoretical accuracy limit of Small-GICP. This proves the effectiveness of the two-stage registration strategy designed in this embodiment. Regarding computational efficiency, KISS-Matcher is the fastest, requiring only 22.1 milliseconds. The average processing time of the method proposed in this embodiment is 45.7 milliseconds, which, although slower than KISS-Matcher, is far lower than the 158.4 milliseconds of directly applying Small-GICP.

[0049] Specifically, the high efficiency of the method proposed in this embodiment stems from its ingenious process design. The computational cost of FPFH feature extraction and SAC-IA initial alignment is far lower than that of iterative optimization on hundreds of thousands of points. Since SAC-IA provides a high-quality initial pose, the subsequent Small-GICP optimization process can converge quickly with only a few iterations, avoiding time-consuming and potentially unsuccessful long-term searches. Therefore, the method of this invention significantly surpasses high-precision baselines in computational efficiency, while its time consumption fully meets the requirements of real-time applications such as ship-shore collaboration.

[0050] To verify the effectiveness of the optimization method proposed in this embodiment in suppressing trajectory drift, a series of comparative experiments were designed. The experiments simulated a cooperative pose estimation task between a USV and a fixed shore base station. Since this experiment only verifies the performance of the ship-shore cooperative factor graph optimization method, a virtual port scene of approximately 100m × 100m was constructed based on the official VRX competition environment. Figure 10 A partial top-down view of the port scene is shown, indicating the ship's starting position, target berth, shore base location, and berthing path. The USV performs multiple berthing and unberthing operations within the port, providing conditions for collecting effective ship-to-shore berthing and unberthing data. To highlight the advantages of collaboration, two representative advanced single-machine LiDAR sensing systems are selected as baselines, and the collaborative system used to implement the proposed method in this embodiment is directly compared with them. A-LOAM is an advanced and efficient open-source odometry method based on LiDAR. It only uses LiDAR data for point cloud matching and motion estimation, and its performance represents the level of pure odometry without backend global optimization. LIO-SAM is a factor graph optimization method tightly coupled with LiDAR and IMU. It is currently an advanced solution in the field of single-machine SLAM, and can effectively suppress short-term drift by fusing IMU data. Table 4 shows the performance comparison results of the three methods after running continuously for 10 minutes in the VRX scene.

[0051] Table 4:

[0052] Experimental results clearly reveal the significant advantages of ship-shore cooperation in suppressing long-distance drift. A-LOAM exhibits the most severe trajectory drift, with an ATE as high as 8.75 m. This is because the method relies solely on point cloud matching between consecutive frames, lacking any global constraints to correct accumulated errors, causing its trajectory to deviate rapidly from the true path over time. LIO-SAM significantly outperforms A-LOAM, reducing its ATE to 3.21 m. This demonstrates the effectiveness of tightly coupled IMU and LiDAR, with high-frequency IMU data effectively constraining short-term odometry uncertainties. However, as a LiDAR system, it still lacks an external global reference, and its positioning error continues to accumulate with time and distance. The method proposed in this embodiment achieves optimal performance, with an ATE of only 0.42 m, approximately 81% lower than LIO-SAM. This superior performance is attributed to ship-shore pose correlation and factor map optimization. Whenever a ship enters an area mapped by shore-based radar, the front-end system identifies the common-view area and generates cross-platform pose constraints. The back-end treats these cross-platform constraints as global anchor points, periodically and globally correcting the ship's accumulated errors. Therefore, although ships continuously generate new drifts during long voyages, the collaborative optimization mechanism always "pulls" their trajectory back to the globally consistent optimal solution, thereby keeping the drift at an extremely low level. like Figure 11 and 12 As shown, the two representative berthing and departure trajectories of this ship intuitively demonstrate that the trajectory of a single LiDAR system diverges over time, while the trajectory of the method proposed in this embodiment highly coincides with the true value.

[0053] In terms of computational efficiency, the method proposed in this embodiment operates at a frequency of 14.2 FPS, slightly lower than two single-robot baselines. This is because the collaborative system introduces additional computational burdens: firstly, the overhead of location identification and relative pose calculation between ship and shore, and secondly, the iterative calculation of distributed graph optimization for multiple robots. However, an update frequency of 14.2 Hz fully meets the real-time requirements for application scenarios such as ship berthing. This indicates that the method proposed in this embodiment achieves an order-of-magnitude improvement in positioning accuracy while maintaining the real-time operation capability of the system, proving its feasibility in practical applications. 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 them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for estimating the berthing and departure pose of a USV based on ship-shore LiDAR cooperation, characterized in that, The specific steps include: S1. Collect shipborne point cloud data using shipborne LiDAR, collect shore-based point cloud data using shore-based LiDAR, and obtain the fixed reference pose of shore-based LiDAR in the global coordinate system. S2. Perform lidar odometry calculation on two consecutive frames of shipborne point cloud to obtain the motion increment of the unmanned surface vessel at adjacent times, and generate LiDAR odometry factor based on the motion increment. S3. Construct global descriptors for shipborne point clouds and shore-based point clouds respectively to obtain shipborne global descriptors and shore-based global descriptors; perform scene matching between shipborne observations and shore-based observations based on the shipborne global descriptors and shore-based global descriptors, and generate ship-shore candidate point cloud pairs that can be used for cross-platform registration based on the set decision mechanism when the matching is successful. S4. Based on the ship-shore candidate point cloud pair, extract key points and calculate the FPFH descriptor of the key points. Combine the FPFH descriptor and use coarse registration and fine registration methods to solve for the high-precision relative pose of the ship-borne LiDAR relative to the shore-based LiDAR. Reconstruct the high-precision relative pose based on the asynchronous mechanism to obtain the final ship-shore relative pose. Generate the ship-shore pose association factor based on the final ship-shore relative pose. S5. The pose of the unmanned surface vessel at each moment is taken as the variable node to be optimized, and the fixed reference pose of the shore-based LiDAR in the global coordinate system is taken as the fixed anchor point node. The LiDAR odometry factor and the ship-shore pose correlation factor are introduced as odometry continuity constraints and ship-shore relative observation constraints, thereby constructing a ship-shore cooperative factor graph model. S6. Solve the ship-shore cooperative factor graph model. Under the condition of simultaneously satisfying the odometry continuity constraint and the ship-shore relative observation constraint, minimize the overall residual to obtain the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time. Based on the consistent optimal pose, realize berthing and departure control and navigation. At the same time, receive a new frame of ship-borne point cloud and shore-based point cloud, repeat S2-S5, and realize real-time continuous pose estimation.

2. The USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration according to claim 1, characterized in that, In S3, the specific steps for performing scene matching between shipborne and shore-based observations based on the shipborne global descriptor and the shore-based global descriptor, and generating ship-shore candidate point cloud pairs that can be used for cross-platform registration based on a set decision mechanism when a match is successful, include: S321. Calculate the similarity between the shipborne global descriptor and the shore-based global descriptor: For any global descriptor among the global descriptors and shore-based global descriptors, from 0 to N s 1. Perform column shifting, and calculate the distance to another global descriptor after each column shift. The distance formula is defined as: (1) in, k It is the number of column shift steps. k ∈[0, N s 1]; W r It is a radial distance r Relevant weighting factors; | | represents Manhattan distance; The minimum distance is taken as the final distance between the two global descriptors, and this final distance is used as the scene similarity under optimal rotation alignment, expressed as: (2) S322. Based on the established decision-making mechanism, generate candidate ship-shore point cloud pairs that can be used for cross-platform registration, including: Set a distance threshold T dist ; Determine if it is a Distance SC < T dist If so, then perform a time consistency check; Time consistency checks include: Set a sliding time window and check if the Distance condition is met within that sliding time window. SC < T dist If the percentage of frames exceeds a preset ratio, then the two point clouds of the shipborne and shore-based systems at the current moment are determined as ship-shore candidate point cloud pairs.

3. The USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration according to claim 2, characterized in that, In S4, key points are extracted based on the ship-shore candidate point cloud pair, and FPFH descriptors of the key points are calculated. Combining the FPFH descriptors with coarse and fine registration methods, the high-precision relative pose of the shipborne LiDAR relative to the shore-based LiDAR is obtained. The high-precision relative pose is then reconstructed based on an asynchronous mechanism to obtain the final ship-shore relative pose. The specific steps include: Several key points were obtained based on the selection of candidate point clouds from the ship and shore. Calculate the FPFH descriptor for each keypoint, including: Based on any key point in the ship-shore candidate point cloud pair Within the preset search radius Perform an inner radius search to obtain its neighborhood point set: ； by Using a radius, downsample the aforementioned neighborhood point set; For each key point Local plane fitting is performed within the downsampled neighborhood point set. By constructing the covariance matrix and performing eigenvalue decomposition, the eigenvalues ​​are obtained. and and key points normal vector ; Linearity index defined based on eigenvalues Select the key points that meet the requirements and eliminate redundant points; Combining key points normal vector Construct a local Darboux coordinate system for the neighborhood point set, and calculate the angular features between the key point and the neighborhood point set. α , , θ ); Based on the aforementioned angular features, key points are statistically formed. SPFH histogram; By performing distance-weighted fusion of SPFH values ​​within the neighborhood of keypoints, the final keypoints are obtained. FPFH descriptor; Based on the FPFH descriptor, fast and high-precision relative pose estimation is achieved through coarse and fine registration, including: The SAC-IA algorithm was used for preliminary calibration of the point cloud to obtain the low-precision pose of the shipborne LiDAR in the shore-based point cloud map coordinate system. , is represented as: (3) in, The shore-based point cloud provided for shore-based LiDAR is also known as the target point cloud; This represents the relative pose of the shipborne LiDAR to the shore-based system at the current moment. The pose of the shore base in the point cloud map coordinate system; High-precision registration was performed using Small-GICP: Will As the initial value for registration, use Pose sharing with shore-based LiDAR To put the shipborne LiDAR at the current point in time k FPFH descriptor acquired at each moment Convert to target point cloud In the coordinate system, the source point cloud is obtained that is consistent with the coordinate system of the shore-based point cloud map. , is represented as: (4) Source cloud Registration to target point cloud This allows for a more accurate high-precision coordinate system transformation from the shipborne LiDAR pose to the shore-based point cloud map. ; Combined with coordinate system transformation The high-precision relative pose of the shipborne LiDAR in the coordinate system of the shore-based point cloud map is obtained at the current moment. : (5) The high-precision relative pose is reconstructed based on an asynchronous mechanism to obtain the final ship-shore relative pose estimate at the current moment, expressed as: (6) in, This indicates the current time of the shipborne LiDAR relative to the shore-based coordinate system. S The pose estimation results, express k -1 time unmanned surface vessel in shore-based coordinate system S The pose estimation results in the middle, and They represent k -1 time and k At any given moment, the pose of the unmanned surface vessel in its own odometry coordinate system is estimated; ) 1 This represents the inverse of the pose transformation matrix, used to achieve inverse transformations between coordinate systems.

4. The USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration according to claim 3, characterized in that, In S6, the specific steps for solving the ship-shore cooperative factor graph model, minimizing the overall residual and obtaining the consistent optimal pose of the unmanned surface vessel in the global coordinate system at each time step while simultaneously satisfying the odometry continuity constraint and the ship-shore relative observation constraint, include: The ship-shore cooperative factor graph model is solved using the iSAM2 solver, while simultaneously satisfying trajectory continuity constraints and ship-shore relative observation constraints, in order to minimize the sum of nonlinear least squares errors, i.e., minimize the overall residual, expressed as: (7) in, The error model representing the variable factors. The error model representing the shipborne LiDAR odometry factor has a correlation range of two consecutive state nodes. and ; The error model represents the ship-to-shore LiDAR pose correlation factor, with the correlation range being shore-based. And this ship State nodes; and This indicates the weights used when applying different covariance matrices; Represents the set of all variables. This represents the prior terms obtained through marginalization. (8) in, This represents the relative transformation relationship between two consecutive LiDAR odometry coordinate systems. (9) in, Let be the ship-shore relative pose transformation matrix. This indicates the ship's status in the shore-based point cloud map coordinate system.

5. The USV berthing and departure pose estimation method based on ship-shore LiDAR collaboration according to claim 4, characterized in that, In S3, the process of constructing shipborne global descriptors and shore-based global descriptors is the same, including: Divide the 3D point cloud space along the vertical axis into n L A series of equal-height levels are used to transform a 3D point cloud into a 2D maximum height map. The two-dimensional maximum height map is divided according to polar coordinates. N r A radial concentric ring and N s Each angle sector forms N s × N r One grid cell; For any point in the point cloud p i =( x i , y i , z i Convert it to polar coordinates: ( ρ i , i , z i ), where radial distance azimuth ; For each grid cell ( s , r ): Filter out sectors that fall within the corresponding angle. s and radial annular region r For all points within the grid, the maximum Z-coordinate is taken as the value of the corresponding grid cell, defined as follows: (10) Where, Φ s and R r Representing the first s The first angle sector and the first r The boundary range of each radial ring region; If there are no points in the grid cell, the value is assigned to 0; All grid cell values ​​are set according to N s × N r The dimensional arrangement yields the complete global descriptor.

Images