Integrated LiDAR-Based Localization and Navigable Region Detection for Autonomous Berthing of Unmanned Surface Vessels

Abstract

Autonomous berthing of unmanned surface vehicles (USVs) requires high-precision positioning and accurate detection of navigable region in complex port environments. This paper presents an integrated LiDAR-based approach to address these challenges. A high-precision 3D point cloud map of the berth is first constructed by fusing LiDAR data with real-time kinematic (RTK) measurements. USV pose is then estimated by matching real-time LiDAR scans to the prior map, achieving robust, RTK-independent localization. For safe navigation, a novel navigable region detection algorithm is proposed, which combines point cloud projection, inner-boundary extraction, and target clustering. This method accurately identifies quay walls and obstacles, generating reliable navigable areas and ensuring collision-free berthing. Field experiments conducted in Ling Shui Port, Dalian, China, validate the proposed approach. Results show that the map-based positioning reduces absolute trajectory error (ATE) by 55.29% and relative trajectory error (RTE) by 38.71% compared to scan matching, while the navigable region detection algorithm provides precise and stable navigable regions. These outcomes demonstrate the effectiveness and practical applicability of the proposed method for autonomous USV berthing.

1. Introduction

1.1. Background

With the continuous advancement of unmanned surface vehicle (USV) technology, their applications in maritime tasks such as hydrographic surveying, environmental monitoring, maritime security patrols, and search-and-rescue operations are rapidly expanding, bringing significant improvements in operational efficiency and safety assurance [1,2,3,4]. A core capability for enabling fully autonomous USVs is autonomous berthing, which refers to the complex process of safely and precisely maneuvering a USV to dock at a quay or harbor [5]. Although substantial progress has been achieved in USV navigation within open waters, autonomous berthing remains a formidable challenge due to its stringent requirements for centimeter-level accuracy, real-time responsiveness, and robust perception in cluttered near-shore environments [6].

Existing approaches typically address these challenges in isolation. Positioning methods based on Global Navigation Satellite System (GNSS) are vulnerable to signal blockage and multipath interference [7,8]. Vision-based approaches employing onboard cameras can provide rich environmental information but are highly sensitive to illumination conditions and require large-scale training datasets to overcome the ambiguities of the water surface [9]. Conventional marine radars, while effective over long ranges, lack the high resolution needed for fine-grained maneuvering in close-range operations [10]. Therefore, there is an urgent need for a sensing solution that can simultaneously provide high resolution and all-weather capability in berthing scenarios.

To address this issue, this paper proposes an integrated framework that employs a vessel-mounted 3D LiDAR as the primary sensor to support both localization and navigable region detection during USV autonomous berthing. LiDAR is inherently robust to illumination variations and can generate centimeter-level 3D point clouds, effectively overcoming the limitations of conventional cameras in maritime environments. Based on LiDAR point clouds, two synergistic tasks are realized. First, localization is achieved by registering real-time LiDAR scans with a pre-constructed 3D berth map, enabling robust and high-precision localization without reliance on GNSS. Second, navigable region detection is accomplished through a novel point cloud processing and obstacle segmentation algorithm, which identifies water surfaces and obstacles to construct safe navigable regions in real time, thereby providing reliable support for USV berthing.

1.2. Related Work

Accurate and reliable localization is a prerequisite for autonomous berthing of USVs. Conventional approaches primarily rely on the GNSS, often enhanced with real-time kinematic (RTK) carrier-phase differential techniques to achieve centimeter-level accuracy [11,12]. However, in crowded ports, under bridges, or near large infrastructures, GNSS signals are highly susceptible to blockage, leading to significant degradation in localization performance [13]. To address this limitation, researchers have explored various alternative solutions.

Vision-based localization methods that employ cameras with artificial markers or natural features have been proposed for short-range navigation [14,15]. While effective under favorable lighting conditions, their performance deteriorates severely at night, in rain or fog, or due to water surface reflections. LiDAR-based localization, particularly through LOAM and its variants, has demonstrated remarkable success in land-based autonomous driving applications [16,17,18]. In the maritime domain, some studies have investigated frame-to-frame LiDAR scan matching algorithms for position estimation [19,20]. However, these methods suffer from error accumulation, making them suitable only for short-range localization, with limited accuracy over extended distances.

In parallel with localization, navigable region detection is another critical component for USV autonomous berthing [21,22]. Existing studies can generally be categorized into two groups. The first group analyzes spatial variations in point cloud data to extract channel boundaries or employs curve-fitting techniques to reconstruct waterway lines [23,24]. The second group emphasizes quay wall detection through object recognition, followed by inferring navigable areas based on the distribution of detected quay structures [25,26].

Nevertheless, most of these studies have focused on relatively structured inland waterways, while the unique challenges of port berthing scenarios have received insufficient attention. Compared with inland rivers, port environments are significantly more complex, containing static obstacles such as quay infrastructures and moored vessels, as well as dynamic elements including moving ships and surface disturbances. These factors impose stricter requirements on the accuracy, robustness, and real-time capability of navigable region detection.

In summary, although significant progress has been made in USV localization and environmental perception, there remains a lack of integrated solutions that can simultaneously address both challenges in demanding berthing scenarios. Existing LiDAR-based approaches typically treat localization and perception as independent tasks. For example, SLAM systems often focus on map construction without actively supporting real-time obstacle avoidance, while perception modules generally assume fully known USV poses. To overcome this gap, this paper proposes a unified framework based on a single 3D LiDAR sensor. The framework enables robust map-based localization in GNSS-denied environments and simultaneously achieves real-time, high-precision detection of navigable waters and obstacles, thereby providing comprehensive support for autonomous berthing.

1.3. Contributions

The main contributions of this paper are summarized as follows:

(1) A unified LiDAR-based system framework is proposed to simultaneously address high-precision pose localization and navigable region detection during USV berthing, thereby avoiding the complexity of multi-sensor fusion.

(2) A map-based localization strategy is developed, which registers real-time point clouds with a prior berth map to provide accurate position estimates in GNSS-denied or signal-degraded environments, ensuring reliable support for autonomous berthing.

(3) A novel navigable water detection algorithm is designed, which integrates point cloud projection, inner-boundary extraction, and target clustering to accurately distinguish quay walls from obstacles and generate safe navigable regions.

(4) Full-scale field experiments are conducted to validate the proposed method. Results demonstrate that the system achieves high accuracy and robustness in complex berthing environments, providing stable and reliable information for autonomous berthing operations.

The remainder of this paper is organized as follows: Section 2 presents the overall framework of the proposed system, focusing on LiDAR-based localization and navigable region detection. Section 3 describes the experimental design and implementation, followed by detailed analysis of the results. Section 4 discusses the advantages, limitations, and potential applications of the proposed method. Finally, Section 5 concludes the paper and summarizes the main research contributions.

2. Localization and Navigable Region Detection System

The berthing localization and navigable region detection system proposed in this study consists of two main modules: a localization module and a navigable region detection module. These two components operate in coordination to provide high-precision pose information and safe navigable regions for autonomous berthing of unmanned surface vessels in complex port environments. The overall framework is illustrated in Figure 1.

2.1. Localization

In the localization module, a high-precision point cloud map is first constructed by integrating LiDAR point cloud data with RTK measurements. Subsequently, real-time point cloud scans are registered with the prior map to achieve high-accuracy position estimation for the USV.

2.1.1. Point Cloud Map Construction

Figure 2 illustrates the complete workflow of point cloud map construction, in which the core processes include data preprocessing, scan matching, loop closure detection, and back-end optimization. The primary functions and roles of each module are described as follows. The detailed pseudocode of the entire mapping procedure is summarized in Algorithm 1, which defines the input/output specifications and termination conditions.

Algorithm 1: Point Cloud Map Construction Procedure

Data Shape Specification

To enhance reproducibility, the data formats and dimensions of the main variables used in the mapping and localization modules are summarized as follows:

• LiDAR scan $P_t$: Each frame $P_t\in {\mathbb{R}}^{N_t\times 3}$ represents a set of $N_t$ 3D points $(x,y,z)$ in the LiDAR coordinate frame, where $N_t$ typically ranges from ${10}^4$ to ${10}^5$.
• RTK position $r_t$: The RTK measurements $r_t\in {\mathbb{R}}^3$ correspond to the absolute geodetic position $(x,y,z)$ of the USV in the world coordinate frame.
• Transformation $\mathbf{T}t,t-1$: The relative pose between consecutive frames is expressed as $\mathbf{T}t,t-1\in SE\left(3\right)$, a $4\times 4$ homogeneous transformation matrix.
• Pose set $\mathbf{X}={\mathbf{x}}_t$: Each pose ${\mathbf{x}}_t\in SE\left(3\right)$, representing the USV’s position and orientation at time t.
• Prior/global map $\mathcal{M}$: The global point cloud map $\mathcal{M}\in {\mathbb{R}}^{N_m\times 3}$ contains $N_m$ 3D points in the global coordinate system.
• Scan Context descriptor $I_t$: Represented as a 2D matrix $I_t\in {\mathbb{R}}^{N_r\times N_s}$, typically with $N_r=20$, $N_s=60$.
• Information matrix $\mathbf{\Omega }$: A symmetric positive definite matrix $\mathbf{\Omega }\in {\mathbb{R}}^{6\times 6}$ weighting observation uncertainties.

This specification ensures that all input/output data structures, including point clouds, transformation matrices, and descriptors, are explicitly defined for reproducibility and implementation clarity.

(1) Data Preprocessing

Data preprocessing involves both point cloud data and RTK data. For point cloud data, voxel filtering is first applied to downsample the raw point cloud, thereby reducing data volume and improving computational efficiency. Subsequently, radius filtering is employed to remove noise and outliers, enhancing the accuracy and reliability of the point cloud. For RTK data, a coordinate transformation is performed to convert it from the world coordinate system to the mapping coordinate system, ensuring a consistent reference framework for subsequent mapping and localization processes.

(2) Scan Matching

The scan matching method employed in this study is based on the Voxelized Generalized Iterative Closest Point (VGICP) algorithm [27]. VGICP algorithm extends the classical ICP framework by introducing voxel-based statistical modeling, thereby improving both efficiency and robustness in challenging maritime environments. Unlike conventional ICP, which relies on direct point-to-point correspondences, VGICP represents the source and target point clouds as local Gaussian distributions within each voxel, characterized by their mean and covariance. This probabilistic representation alleviates the effects of sparse or uneven point distributions and eliminates the need for repeated nearest-neighbor searches. The objective of VGICP is to estimate the rigid-body transformation matrix $\mathbf{T}\in SE\left(3\right)$ that best aligns the source cloud $P=\{p_1,p_2,p_3,\dots ,p_m\}$ with the target cloud $Q=\{q_1,q_2,q_3,\dots ,q_n\}$. The optimization problem is formulated as minimizing the Mahalanobis distance between the corresponding Gaussian distributions:

$${\mathbf{T}}^{*}=\underset{T}{\arg \min }\sum _{i=1}^N{\left({\overline{q}}_i-\mathbf{T}{\overline{p}}_i\right)}^T{\left({\Sigma }_{q_i}+\mathbf{T}{\Sigma }_{p_i}{\mathbf{T}}^T\right)}^{-1}\left({\overline{q}}_i-\mathbf{T}{\overline{p}}_i\right)$$

(1)

where ${\overline{p}}_i$ and ${\overline{q}}_i$ denote the voxel centroids of the source and target point sets, respectively, ${\Sigma }_{p_i}$ and ${\Sigma }_{q_i}$ are the covariance matrices estimated from voxel neighborhoods, and N represents the total number of voxel correspondences. By using voxel-level statistics, the covariance terms remain constant during iterations, allowing precomputation and thereby accelerating the optimization. Moreover, the Mahalanobis distance weighting makes the method less sensitive to noise, outliers, and partial occlusions, which are common in berthing scenarios. These properties enable VGICP to achieve faster convergence and higher accuracy compared to traditional ICP, particularly in dynamic and cluttered port environments.

(3) Loop Closure Detection

Loop closure detection aims to identify spatial revisit events by matching current sensor observations against historical data. When successful detection occurs, the established inter-frame pose constraints are integrated into the graph optimization backend as additional edges, effectively compensating for accumulated drift and improving overall trajectory consistency. This work utilizes the Scan Context (SC) descriptor for robust loop closure identification [28]. As illustrated in Figure 3, the method transforms raw 3D point clouds into structured representations through polar coordinate discretization. The space around the sensor is partitioned into $N_r$ concentric rings based on radial distance and $N_s$ equiangular sectors, creating a total of $N_r\times N_s$ spatial bins. Each bin is characterized by the maximum height value among contained points, forming a distinctive signature for the local environment. Empty bins are assigned zero values to maintain structural consistency. Given two Scan Context descriptors $I^p$ and $I^q$ from different frames, their similarity is evaluated through column-wise comparison. The fundamental distance metric employs cosine similarity between corresponding column vectors:

$$\begin{array}{c}\hfill d\left(I^p,I^q\right)={\displaystyle \frac{1}{N_s}}\sum _{j=1}^{N_s}\left(1-{\displaystyle \frac{{\mathbf{c}}_j^p·{\mathbf{c}}_j^q}{∥{\mathbf{c}}_j^p∥∥{\mathbf{c}}_j^q∥}}\right)\end{array}$$

(2)

where ${\mathbf{c}}_j^p$ and ${\mathbf{c}}_j^q$ denote the j-th column vectors of descriptors $I^p$ and $I^q$, respectively. To address rotational variations between scans, the system implements a circular shifting mechanism along the column dimension, searching for the optimal alignment that minimizes the overall distance. The framework incorporates a two-tiered search strategy: initial candidate screening using ring key matching followed by precise similarity evaluation through column-wise comparison. This hierarchical approach enables efficient retrieval from large-scale historical databases while maintaining detection accuracy. A loop closure is confirmed when the minimal distance falls below a predefined threshold, ensuring reliable performance in challenging port environments characterized by structural repetitiveness and sensory disturbances.

(4) Back-End Optimization

Back-end optimization aims to integrate multi-source observational data to correct odometric drift and enhance the global consistency of the trajectory. This paper employs pose graph optimization, unifying constraints from LiDAR odometry, loop closure detection, and RTK prior poses within a graph structure. The pose graph consists of nodes and edges: nodes represent the vehicle’s pose states ${\mathbf{x}}_i$ at different timestamps, while edges represent various observational constraints. As shown in Figure 4, loop closure detection provides relative pose observations ${\mathbf{z}}_{ij}$ between non-consecutive frames, forming binary edges connecting nodes ${\mathbf{x}}_i$ and ${\mathbf{x}}_j$. The RTK positioning system provides absolute pose observations ${\mathbf{z}}_i$, forming unary edges connected to individual pose nodes. For a binary edge, the residual is defined as the difference between the observed relative pose and the relative pose computed from the node states:

$${\mathbf{e}}_{ij}=ln{\left({\mathbf{z}}_{ij}^{-1}\left({\mathbf{x}}_i^{-1}{\mathbf{x}}_j\right)\right)}^{\vee }$$

(3)

where $ln{(·)}^{\vee }$ maps the pose difference on $SE\left(3\right)$ to its Lie algebra $\mathfrak{se}\left(3\right)$. For a unary edge, the residual is the direct difference between the RTK observation and the corresponding node state:

$${\mathbf{e}}_i=ln{\left({\mathbf{z}}_i^{-1}{\mathbf{x}}_i\right)}^{\vee }$$

(4)

The optimization objective is to minimize the weighted sum of squared residuals from all constraints:

$${\mathbf{X}}^{*}=arg\underset{\mathbf{X}}{min}\left(\sum _{(i,j)\in \mathcal{C}}{\mathbf{e}}_{ij}^T{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}+\sum _{i\in {\mathcal{V}}_{\mathrm{RTK}}}{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{e}}_i\right)$$

(5)

Here, $\mathcal{C}$ denotes the set of all binary constraint edges, ${\mathcal{V}}_{\mathrm{RTK}}$ denotes the set of nodes with RTK observations, and $\mathbf{\Omega }$ represents the information matrix associated with each observation, weighting its reliability. This nonlinear least-squares problem is solved iteratively using methods like the Gauss-Newton algorithm, ultimately yielding the globally consistent optimized pose estimates ${\mathbf{X}}^{*}$.

(5) Global Map Generation

After completing back-end optimization and obtaining a globally consistent trajectory of the USV, the point cloud data collected at different timestamps are fused into a unified coordinate framework to construct a high-precision global point cloud map. Specifically, each frame of point cloud is first subjected to a rigid-body transformation according to the optimized poses ${\mathbf{X}}^{*}$, mapping it from the sensor coordinate system to the global coordinate system. Subsequently, a time synchronization mechanism is applied to align the point cloud data with RTK observations, ensuring spatial and temporal consistency between the poses and the corresponding point clouds. During the fusion process, a voxel grid strategy is employed to downsample the global point cloud, thereby reducing data volume and improving storage and retrieval efficiency. In addition, filtering and consistency checks are performed in overlapping regions to remove redundant and abnormal points, further enhancing the density and accuracy of the map. The resulting global point cloud map not only provides a complete geometric representation of the port environment but also serves as a high-precision environmental foundation for subsequent tasks such as USV berthing localization.

2.1.2. Localization Based on Prior Map

Localization based on a prior map primarily relies on the vessel-mounted LiDAR to scan the surrounding environment, combined with a pre-constructed point cloud map, to achieve accurate positioning. Figure 5 clearly illustrates the overall process of USV localization using a map. The detailed pseudocode of the localization based on a prior point cloud map is provided in Algorithm 2.

Algorithm 2: Localization Based on a Prior Point Cloud Map

First, pose initialization of the USV is a critical step in the process. Pose initialization typically involves determining the initial position and orientation of the USV within the prior map. Next, the onboard LiDAR begins operation, continuously acquiring real-time point cloud data of the surrounding environment. These point clouds capture the shapes and spatial distribution of objects around the USV. Simultaneously, the system loads a local map corresponding to the current location. The local map is usually a high-precision point cloud map constructed based on prior knowledge, containing accurate information on the positions and shapes of environmental objects. The acquired LiDAR data are then matched with the loaded local map. Through the matching algorithm, the motion state of the USV relative to the local map, including changes in position and orientation, can be determined. This information is subsequently used to update the current pose of the USV, thereby enabling autonomous localization.

2.2. Navigable Region Detection

For USVs operating within confined port regions, accurate detection of navigable regions is crucial for ensuring safe navigation. The navigable region for a moving USV is strictly constrained by port boundaries and obstacles such as moored vessels. To avoid collisions, the USV must operate within these safe regions.

When LiDAR emits laser beams, reflections are received from obstacles and quay walls, while beams directed at the water surface are largely absorbed and not detected by the LiDAR. As a result, the water surface is automatically filtered out by the LiDAR system, making LiDAR highly suitable for navigable region detection.

The navigable region detection algorithm proposed in this chapter consists of three main steps: point cloud inner-boundary extraction, inner-boundary target modeling, and generation of the USV navigable region. The overall framework is illustrated in Figure 6, and each module is described in detail below.

2.2.1. Point Cloud Inner Boundary Extraction

Point cloud inner boundary extraction refers to the process of identifying and extracting key edge points located at the interior boundaries of objects from point cloud data.

First, since raw point clouds usually contain noise and scattered points, a point cloud filtering algorithm is applied to improve data accuracy. The filtering method in this section is based on the point cloud preprocessing described in Section 2.1.1. On this basis, a PassThrough Filter is employed to remove scattered points such as floating debris and wake traces on the water surface. As these scattered points are typically close to the water surface, with relatively small z-axis coordinates, their removal via the PassThrough Filter is both reasonable and effective.

Next, the filtered point cloud is projected onto the horizontal plane. Using the origin of the projected point cloud as a reference, the points are segmented according to their emission angles. Specifically, the arctangent function is used to calculate the angle between the line connecting each point to the origin and the positive x-axis, and points are then assigned to the corresponding angular segment based on this angle.

Finally, within each segmented region, the point closest to the origin is identified. The collection of these nearest points constitutes the desired inner boundary point cloud, as illustrated in Figure 7.

2.2.2. Inner Boundary Object Modeling

The process of inner boundary object modeling begins with the classification of the inner boundary point cloud, dividing it into berth inner boundary point clouds and other inner boundary point clouds. Subsequently, clustering techniques are applied separately to these two categories to accurately extract the berth-front targets and obstacle-front targets. In this study, the classification method first employs the MSAC line fitting algorithm [29] to segment the inner boundary point cloud, efficiently identifying and fitting linear features within the point cloud. Next, by combining the port RTK prior coordinates to obtain the shoreline parameters, the fitted lines are filtered to identify the shoreline inner boundary point clouds within the inner boundary point cloud. The remaining points after filtering are classified as other inner boundary point clouds. The extraction process of the shoreline inner boundary point cloud is illustrated in Figure 8.

Although the above steps provide an initial classification of the inner boundary point cloud, each category may still contain multiple independent point cloud clusters. Therefore, Euclidean clustering is further applied to segment them [30]. To improve clustering efficiency, a point-by-point clustering approach is adopted in this study. The schematic diagram of inner boundary object modeling is shown in Figure 9.

2.2.3. Navigable Region Generation

The generation of the navigable region aims to determine the safe navigation area of the USV during the berthing process based on the detected quay wall and obstacle front-edge targets. First, the inner boundary formed by the extracted target point cloud is used as the preliminary navigation boundary. To further enhance berthing safety, this study incorporates target size and orientation information and proposes a navigable region generation method based on the vertices of the oriented bounding box (OBB), yielding a more accurate and safer navigable region.

For the inner boundary derived from target point clouds, the angle between the line connecting each point to the origin and the positive x-axis is first computed. Points are then sorted according to this angle. By sequentially connecting the sorted points, the enclosed area forms the inner boundary of the target point clouds.

To determine the final navigable region, the vertices of the OBB are first filtered. Specifically, vertices along the direction of the maximum eigenvector of the OBB center and its opposite direction, which are closest to the origin, are selected. Then, based on the angle between the line connecting the OBB center to the origin and the positive x-axis, as well as the angle between each selected vertex and the origin relative to the x-axis, the vertices are sorted. After sorting, the vertices are connected sequentially to form the navigable area, as shown in Figure 10. The following pseudocode (Algorithm 3) summarizes the procedure.

This method not only considers the actual position, size, and orientation of targets, but also ensures the stability and reliability of the generated navigable region, providing safe and high-precision spatial constraints for USV berthing navigation.

Algorithm 3: Navigable Region Generation Using OBB Vertices

Input: Target point cloud $\mathcal{P}$, origin $\mathbf{o}=(0,0)$ Output: Vertices of navigable region ${\mathcal{V}}_{\mathrm{nav}}$ 1  Step 1: Compute OBB 2  Compute oriented bounding box (OBB) for $\mathcal{P}$, obtain vertices $\left\{{\mathbf{v}}_i\right\}$ and center $\mathbf{c}$, maximum eigenvector ${\mathbf{e}}_{max}$. 3  Step 2: Filter vertices along principal direction 4  Select vertices ${\mathbf{v}}_i$ satisfying: $$i=arg\underset{{\mathbf{v}}_i\in \mathrm{OBB}\mathrm{vertices}}{min}\parallel {\mathbf{v}}_i-\mathbf{o}\parallel \mathrm{along}{\mathbf{e}}_{max}\mathrm{and}-{\mathbf{e}}_{max}$$ 5  Step 3: Compute angles to origin 6  For each selected vertex ${\mathbf{v}}_i$, compute $${\theta }_i=arctan2(v_{i,y}-o_y,v_{i,x}-o_x)$$ 7  Step 4: Sort vertices by angle 8  Sort vertices in ascending order of ${\theta }_i$. 9  Step 5: Form navigable region 10  Connect sorted vertices sequentially to define the polygon ${\mathcal{V}}_{\mathrm{nav}}$. 11  return ${\mathcal{V}}_{\mathit{nav}}$

2.3. Mechatronic System Design for Autonomous Berthing

The proposed USV berthing system is developed based on a mechatronic integration framework, which seamlessly combines the shore-based control center, onboard computing center, onboard sensors, and propulsion-rudder control unit into a unified autonomous navigation platform. The overall system architecture is illustrated in Figure 11.

As the remote monitoring and management center of the entire system, the shore-based control center maintains real-time communication with the onboard computing center via a wireless data link. It receives real-time information transmitted from the onboard computing center and can send control commands back to the vessel to enable remote operation and supervision of the USV.

The onboard computing center serves as the “brain” of the USV. Its core hardware is a Jetson embedded computer equipped with Robot Operating System 1 (ROS1) software. ROS provides a powerful software framework that supports modular development and execution of various functional nodes. The computing center initiates sensor nodes to activate and record data from onboard sensors. In addition, it receives control commands from the shore-based control center, performs decision-making and motion planning, and transmits control instructions to the onboard propulsion and rudder control unit.

The onboard sensors are the key components enabling environmental perception, including LiDAR, RTK, and IMU sensors. These sensors continuously collect environmental data-such as obstacle positions, vessel pose, and dynamic surroundings-and transmit the information to the computing center for processing and decision-making.

The onboard propulsion and rudder control unit is responsible for executing control commands received from the computing center. It employs an Arduino microcontroller and a PCA9685 control module to convert high-level control commands into Pulse Width Modulation (PWM) signals, which drive the servo actuators and propulsion motors. Through this mechanism, precise maneuvering and motion control of the USV are achieved.

2.4. Integration of Perception, Localization, and Control

To establish a coherent linkage between perception/localization and control decisions, a hierarchical control architecture is developed for autonomous berthing, as depicted in Figure 12. The proposed architecture integrates perception and localization outputs with trajectory planning and low-level actuation modules, thereby forming a closed-loop control system that supports real-time adaptive decision-making.

The localization module is responsible for estimating the current pose of the USV, while the navigable region detection module defines the safe berthing area as a polygon. Based on this information, the trajectory planner generates collision-free reference paths from the current position to the target berth, taking into account the spatial boundaries of the navigable area and the locations of surrounding obstacles.

The reference trajectories are executed by the control system, which regulates the USV’s thrusters and rudders to control surge velocity, sway motion, and heading. The control system utilizes real-time feedback from onboard sensors-including the IMU, LiDAR, and high-precision positioning devices-to minimize tracking errors and compensate for environmental disturbances such as wind, waves, and currents.

Sensor measurements are continuously fused to update both the USV’s estimated pose and the detected navigable region. This closed-loop feedback enables the control system to dynamically adjust actuation commands in real time, ensuring safe, smooth, and precise berthing maneuvers under varying marine conditions.

3. Verification Experiment

Field experiments were conducted at Lingshui Port, Dalian, China (latitude: 38°52′18.2″, longitude: 121°32′42″), as shown in Figure 13. The USV used in the experiments was Zhilong-1, with a length of 1.75 m and a width of 0.5 m. To enhance navigation stability, the hull was equipped with anti-rolling devices on both sides. The onboard sensor suite included an R-Fans-16 LiDAR (R-Fans, Shanghai, China), an RTK system (Shanghai South Surveying & Mapping Instrument Co., Ltd., Shanghai, China), and a stereo camera (ZED, Stereolabs, Shanghai, China). The R-Fans-16 LiDAR provides a 360° horizontal field of view, a 30° vertical field of view, and a maximum ranging distance of 200 m. Other key parameters are listed in

Table 1. LiDAR data specifications.

Item	Parameter
Laser beam	16
Vertical field of view	${30}^{\circ }(-{15}^{\circ }$∼${15}^{\circ })$
Vertical angular resolution	$2^{\circ }$
Horizontal field of view	${360}^{\circ }$
Maximum ranging	$200 \mathrm{m}$
Ranging accuracy	$\pm 2 \mathrm{cm}$
Refresh frequency	$5 \mathrm{Hz}/10 \mathrm{Hz}/20 \mathrm{Hz}$

. The experimental hardware platform was equipped with an Intel Core i5-4570 processor, 16 GB of RAM, and an NVIDIA GeForce GTX1650 GPU. The software system was implemented in C++17 and operated within the ROS framework.

To evaluate system performance, two sets of experiments were designed: localization experiments and navigable region detection experiments.

3.1. Localization Experiment

In the localization experiment, the operator manually controlled the USV to complete a full navigation loop within the port to verify the accuracy and reliability of the proposed localization algorithm.

Figure 14 presents a top-down view of the point cloud map generated by the algorithm. The point cloud is color-coded to represent different elevation levels, providing both intuitive and practical visualization. The blue regions correspond to low-lying areas near the water surface; the green areas represent the port frontage and berths; the yellow tones indicate medium-height port facilities and low-rise buildings; while the orange-red areas mark the tallest structures. This color gradient enables clear differentiation of targets at varying heights and enhances the overall visualization quality of the map.

As shown in the figure, the generated point cloud map exhibits no ghosting or distortion. The port contours and building structures maintain good verticality and structural completeness, with fine details accurately represented. These results demonstrate that the proposed fusion algorithm achieves high precision in both data processing and map construction, effectively capturing the three-dimensional structure of the port environment. Overall, the point cloud map shows well-defined layers and clear structural features, confirming the reliability and effectiveness of the proposed mapping algorithm.

Figure 15 illustrates the trajectories obtained during the USV’s navigation within the port using two different localization methods: prior-map-based localization and frame-to-frame scan matching. These trajectories are compared against the actual navigation path. Overall, both methods exhibit good agreement with the ground truth trajectory; however, notable differences can be observed in their detailed performance.

The trajectory derived from LiDAR scan matching primarily depends on real-time registration between consecutive point clouds and the surrounding environment to estimate the USV’s pose. While this method provides reasonably accurate localization results, its precision is constrained by factors such as environmental noise, dynamic object interference, and the inherent uncertainty of scanning data.

In contrast, the trajectory obtained through LiDAR matching with a prior map demonstrates a closer alignment with the actual navigation path. The prior map is preconstructed using high-precision measurement techniques, allowing real-time point clouds to be matched against a stable reference environment. This effectively mitigates the effects of environmental noise and dynamic disturbances, thereby enhancing localization accuracy. As shown in the magnified view, the trajectory obtained from prior-map matching exhibits a significantly higher degree of consistency with the actual path, clearly validating the crucial role of prior maps in improving localization precision.

To quantitatively assess the localization performance, two standard trajectory error metrics were employed: Absolute Trajectory Error (ATE) and Relative Trajectory Error (RTE).

The ATE measures the global consistency of the estimated trajectory with respect to the ground truth and is defined as

$$\mathrm{ATERMSE}=\sqrt{{\displaystyle \frac{1}{N}}\sum {i=1}^N{\left|{\mathbf{p}}_i^{\mathrm{est}}-{\mathbf{p}}_i^{\mathrm{gt}}\right|}^2},$$

(6)

where ${\mathbf{p}}_i^{\mathrm{est}}$ and ${\mathbf{p}}_i^{\mathrm{gt}}$ represent the estimated and ground-truth positions at timestamp i, and N denotes the total number of trajectory samples.

The RTE evaluates the short-term drift and local consistency between consecutive poses, and is calculated as

$$\mathrm{RTERMSE}(\Delta t)=\sqrt{{\displaystyle \frac{1}{M}}\sum {j=1}^M{\left|(\mathbf{p}{j+\Delta t}^{\mathrm{est}}-\mathbf{p}{j}^{\mathrm{est}})-(\mathbf{p}{j+\Delta t}^{\mathrm{gt}}-\mathbf{p}{j}^{\mathrm{gt}})\right|}^2},$$

(7)

where $\Delta t$ denotes the time interval for relative comparison and M is the number of valid pose pairs.

Both ATE and RTE were evaluated using root mean square error (RMSE), mean, and median to provide a comprehensive assessment of localization accuracy. The ground-truth trajectory was derived from RTK-GNSS measurements, and all estimated trajectories were aligned to the ground-truth frame via a rigid-body transformation prior to error computation.

Figure 16 and Figure 17 present the comparative results of the two matching methods in terms of ATE and RTE, respectively, under the three evaluation indicators (RMSE, mean, and median). For ATE, the trajectory obtained through the LiDAR-to-prior-map matching approach exhibits substantially smaller errors in all directions compared with the LiDAR scan matching method. Similarly, for RTE, the prior-map-based matching consistently achieves lower errors, demonstrating its superiority in both local and global trajectory accuracy.

Quantitatively, the RMSE of the ATE for the LiDAR-to-prior-map matching method is reduced by 55.29% compared with that of the LiDAR scan matching approach, highlighting the crucial role of the prior map in enhancing global localization accuracy. Furthermore, the RTE RMSE decreases by 38.71%, indicating improved short-term consistency and smoother local motion estimation.

In summary, these experimental results clearly verify the effectiveness and robustness of the LiDAR-to-prior-map matching strategy. By integrating prior map information with real-time LiDAR scans, the proposed method achieves more accurate, stable, and consistent localization performance, offering a reliable solution for high-precision navigation of unmanned surface vessels in complex maritime environments.

3.2. Navigable Region Detection Experiment

In the navigable region experiment, the operator controlled the USV to navigate within the berth and its surrounding area to simulate operational scenarios under realistic berthing conditions. The experiment utilized real-time data acquisition from the vessel-mounted LiDAR sensor to perceive and analyze the surrounding region, aiming to verify the accuracy and stability of the proposed navigable region detection algorithm.

Figure 18 presents the results of the projected point cloud and the extracted inner boundary point cloud processed by the algorithm. It can be clearly observed that the inner boundaries within the point cloud have been accurately identified and extracted, forming complete inner boundary point clouds. This indicates that the proposed algorithm can effectively distinguish the inner boundaries from the surrounding point cloud while ensuring the continuity and accuracy of the boundary points. Through the projection and extraction steps, the algorithm robustly captures critical boundary information even in complex regions and berth structures. Overall, these results validate the high accuracy, robustness, and practical feasibility of the algorithm for inner boundary point cloud extraction.

Figure 19 illustrates the results of inner boundary point cloud target classification. In the figure, red bounding boxes clearly indicate quay wall front targets, while orange bounding boxes accurately enclose obstacle front targets. The results demonstrate that the proposed algorithm can precisely classify inner boundary point clouds, effectively distinguishing between different types of boundary targets. Furthermore, the algorithm exhibits good robustness and stability in complex port environments, maintaining accurate classification even when berth boundaries are irregular or obstacles are densely distributed. This provides reliable foundational data for subsequent navigable region generation and berth analysis for the USV.

Figure 20 presents the results of the proposed algorithm, showing the target point cloud inner boundaries and the navigable region. In the figure, the area enclosed by green lines represents the USV’s navigable region, while the area enclosed by blue lines corresponds to the inner boundary of the target point clouds. As shown, the algorithm can accurately identify boundary points surrounding the USV’s path and generate a safe navigable region based on these points. These results indicate that the algorithm achieves high precision and stability in complex port environments, effectively distinguishing obstacles from navigable regions and providing reliable safety support for USV navigation.

To provide quantitative validation, we evaluate the algorithm using three widely adopted metrics: Precision, Recall, and Intersection over Union (IoU). The evaluation metrics are defined as:

$$\mathrm{Precision}={\displaystyle \frac{TP}{TP+FP}},\mathrm{Recall}={\displaystyle \frac{TP}{TP+FN}},\mathrm{IoU}={\displaystyle \frac{TP}{TP+FP+FN}},$$

(8)

where $TP$, $FP$, and $FN$ denote the number of true positive, false positive, and false negative boundary points, respectively.

Table 2. Quantitative evaluation of navigable region detection.

Figure 18 Subfigure	Precision (%)	Recall (%)	IoU (%)
Top-left	94.9	93.2	88.9
Top-right	94	89.3	84.6
Bottom-left	95.5	96.5	92.3
Bottom-right	95.5	97	92.9

summarizes the quantitative evaluation results.

The consistently high Precision, Recall, and IoU metrics across all evaluation scenarios confirm the effectiveness and robustness of the proposed algorithm. These quantitative results substantiate the method’s capability to generate accurate, reliable, and stable navigable region boundaries even in complex and cluttered port environments.

4. Discussion

The proposed LiDAR-based system for USV localization and navigable region detection was validated through experiments conducted in a real port environment. The results demonstrate that the method exhibits high reliability and practicality in terms of both localization accuracy and environmental perception. The discussion is presented from the perspectives of experimental results, methodological advantages and limitations, and potential applications.

First, in terms of localization accuracy, the experimental results indicate that the map-based registration approach significantly outperforms pure scan matching. Specifically, the absolute trajectory error was reduced by 55.29%, while the relative trajectory error decreased by 38.71%. These findings confirm that incorporating a prior point cloud map can effectively suppress cumulative errors and enhance both global and local trajectory consistency. This validates the effectiveness of the proposed localization strategy, particularly in port scenarios where GNSS signals are heavily obstructed or completely unavailable, highlighting its significant engineering value. It should be noted, however, that the construction of the prior map relies on high-quality RTK and point cloud acquisition, and practical deployment must account for dynamic environmental changes in ports (e.g., new infrastructure or berthed vessels), which may affect the need for map updates.

Second, with respect to navigable region detection, the proposed method, which integrates point cloud inner boundary extraction and object classification, can accurately distinguish quay walls from obstacles and subsequently generate safe navigable regions. The experimental results show that the method maintains strong robustness even in complex environments characterized by irregular berth boundaries and dense obstacle distributions, thereby providing effective support for autonomous obstacle avoidance and berthing of USVs. Compared with existing approaches that rely solely on geometric fitting or object detection, the proposed combination of projection, boundary extraction, and clustering proves more capable of handling dynamic water surfaces and structurally complex port environments. The current study primarily focused on validating the navigable region detection algorithm based on LiDAR data. In future work, this framework can be integrated into a closed-loop control system for autonomous berthing. Specifically, the detected navigable region and boundary information can serve as real-time constraints or reference inputs for path tracking and motion planning modules, enabling dynamic adjustment of control commands during the berthing process.

Nevertheless, certain limitations remain. The LiDAR sensor used in the experiments was a 16-beam device, whose limited vertical resolution may lead to blind spots in detecting small-scale objects at long distances (e.g., floating debris). Future work may consider employing higher-resolution LiDAR or integrating additional sensors (e.g., millimeter-wave radar) to enhance perception capability. Furthermore, the experiments were primarily conducted in static or semi-static port environments; the impact of dynamic targets such as moving vessels and operational equipment has not yet been thoroughly evaluated and requires further validation in more complex dynamic settings. Moreover, the algorithm can be extended toward multi-sensor fusion frameworks by incorporating IMU, RTK, and marine radar data. Such fusion would further improve robustness against LiDAR occlusion, dynamic water surface interference, and environmental variability. The integration of semantic and geometric information across multiple sensing modalities represents a promising direction for achieving more comprehensive and reliable berthing perception in complex harbor environments. Finally, the construction and maintenance of the prior map remain time-consuming and rely heavily on manual efforts. Achieving dynamic map updating and ensuring long-term adaptability are pressing issues that should be addressed in future research.

5. Conclusions

This paper presents a LiDAR-based integrated system for autonomous USV berthing, addressing the challenges of high-precision positioning and navigable region detection in complex port environments. The proposed method constructs a high-resolution 3D point cloud map by fusing LiDAR and RTK data, and estimates the USV’s pose through real-time map-based LiDAR scan matching. A novel navigable region detection algorithm is introduced, which extracts inner-boundaries from point clouds and classifies quay walls and obstacles to generate safe, navigable regions.

Field experiments conducted in Ling Shui Port, Dalian, demonstrate the effectiveness of the proposed system. The map-based localization reduces ATE by 55.29% and RTE by 38.71% compared to conventional scan matching. The navigable region detection algorithm accurately identifies safe passage areas, even in environments with complex berth structures.

Overall, the experimental results validate that the proposed approach significantly improves positioning accuracy and navigation safety for autonomous USVs. The system provides a reliable, GNSS-independent solution, offering practical value for real-world autonomous berthing operations and laying a foundation for further development of intelligent port navigation technologies.