Berthing State Estimation for Autonomous Surface Vessels Using Ship-Based 3D LiDAR

Abstract

Automated berthing remains a critical challenge for autonomous surface vessels (ASVs), necessitating precise berthing state estimation as a fundamental prerequisite. In this paper, we present a novel berthing state estimation method tailored for ASVs and based on 3D LiDAR technology. Firstly, a berthing plane acquisition scheme based on point cloud plane fitting is proposed; the feasibility of the scheme was verified by experiments. The point cloud registration algorithm was used to realize the ship pose estimation. Before registration, the preprocessing technology was used to filter out the noise and outliers in the point cloud data to improve the accuracy of pose estimation. A detailed method for calculating the berthing state information is proposed. This method considers the influence of ship roll, pitch, and yaw during berthing, and ensures the accuracy of the obtained state information. Finally, a real-time ship berthing perception framework was constructed using the Robot Operating System (ROS), enabling the continuous output of vital berthing state information, including berthing distance, velocity, approaching angle, and yaw rate, at a frequency of 10 Hz. To validate the effectiveness of our algorithm, extensive real ship experiments were conducted, yielding highly promising results. The average angle error was found to be less than 0.26°, with an average distance error below 0.023 m.

1. Introduction

1.1. Background

Maritime transport carries 90% of the volume of world trade, and ships play an important role as a tool for maritime transport. However, marine transportation often faces complex and dangerous environments, and any accident may cause serious economic losses and environmental pollution. Studies have shown that more than 75% of maritime accidents can be attributed to human error [1]. Aiming at this problem, the International Maritime Organization (IMO) proposed the concept of intelligent ships, that is, ships that can operate independently and interact with humans to varying degrees [2]. In addition, compared with conventional ships or fully manned ships, autonomous surface vessels (ASVs) show many advantages, such as improving shipping safety, reducing environmental pollution, and saving transportation costs [3,4,5].

To achieve real autonomous navigation at sea, ASVs require many key technological problems to be solved, and automatic berthing is one of them [6]. The berthing task of a ship is complex; it is easy to collide with other ships and port facilities due to the limitation of port space [7,8]. In addition, when the ship is close to the berth, it is difficult to predict the ship’s motion due to the influence of wind, waves, current, shallow water, and bank effects [9,10]. Therefore, it is necessary to accurately estimate the berthing state of a ship during the berthing process to provide a reference for the control module.

1.2. Related Work

To address the aforementioned challenge, extensive research has been conducted by experts and scholars in the field. Initially, the measurement of ship position and speed relied predominantly on global positioning system (GPS) or differential global positioning system (DGPS) sensors, while angle information, such as ship heading, was obtained using magnetic or electric compasses. However, it has been demonstrated that these conventional sensors fail to meet the precision requirements for ship berthing [11,12]. In recent years, novel sensors have emerged as valuable aids in the process, including sonar, cameras, Inertial Measurement Units (IMUs), radar, LiDAR, and more. Sakakibara and Kubo used shore-based sonar to measure the distance and speed of a ship from a berth [13]. Ref. [14] used two stereo cameras to measure the berthing distance of ships. Ref. [15] estimated the distance between the ship and the wharf based on cameras, a DGPS, and an IMU. Ref. [16] used a microwave radar array to detect the distance from the ship to shore.

Although these approaches provide berthing perception information, they are constrained by the inherent limitations of the sensors. Cameras are sensitive to illumination and weather conditions, often leading to substantial measurement errors [17]. Low-resolution sonar and radar can reduce estimation accuracy [18], while IMUs accumulate drift over time, compromising long-term reliability [19]. Moreover, advanced data processing techniques, such as ensemble learning, have shown significant potential in enhancing recognition accuracy in related motion perception tasks, including highway merging scenarios [20].

In contrast to the aforementioned sensors, LiDAR has gained significant prominence in berthing state estimation in recent years. This can be attributed to LiDAR’s ability to furnish more dependable and precise distance measurements compared to the aforementioned sensors. It is important to note that LiDAR sensors are unsuitable for inclement weather conditions such as rain, snow, and heavy fog. Nevertheless, in such harsh weather conditions, ships usually do not engage in berthing operations, thereby mitigating the impact of this limitation on berthing status estimation.

Research on LiDAR-aided autonomous ship berthing primarily falls into two distinct paradigms, distinguished by the deployment location of the sensor: shore-based systems and ship-based systems. In shore-based systems, the LiDAR is fixed on the quay wall, providing a stable reference frame for measuring the relative distance, velocity, and attitude between the berth and the approaching vessel. This configuration is particularly suited for port infrastructure integration. For example, ref. [21] developed a support system that integrates laser rangefinders with marine hydrometeorological sensors, whereby distance measurements are complemented by real-time wind and wave data to enhance situational awareness. Similarly, aiming to improve port efficiency and safety, ref. [22] described a laser docking system for container terminals designed to deliver high-precision berthing information to pilots. Further advancing the capabilities of shore-based sensing, ref. [23] proposed a novel computational method for extracting multi-point motion pose information, enabling the derivation of a vessel’s speed, distance, and heading to assist in complex maneuvers such as parallel berthing.

Compared with the shore-based LiDAR fixed berthing system, the ship-based LiDAR berthing system has a wider coverage and is more flexible. Ref. [24] combined sensor 3D LiDAR, IMU, and GPS data to build a situational awareness system that extracts the geometric features of the floating platform and estimates the position and direction of the ASV relative to the berthing facility. Based on map, LiDAR, and ultrasonic distance sensors data, ref. [25] developed an algorithm to calculate safe operating areas to assist ship berthing. The sensor data can deal with objects such as inaccurate maps and undrawn berthing ships on the map. Ref. [26] proposed a method for calculating ship berthing parameters by combining multi-solid LiDAR point cloud and millimeter wave radar data, and they verified the algorithm using real ship experiments. Ref. [27] proposed a ship-berthing state perception framework based on 3D LiDAR. The framework needs to be based on a shoreline GPS position prior to obtaining more accurate berthing status information. Furthermore, accurately estimating a vessel’s orientation is critical, as evidenced by advances in deep learning for ship detection, where rotation feature decoupling techniques enable precise orientation awareness [28].

From the analysis above, we can see that, although some successful schemes for the research of ship-borne LiDAR berthing state estimation exist, these schemes either require a combination of multiple sensors, are complex, or require prior port GPS positioning, resulting in measurement errors. This paper proposes a new ship-borne 3D LiDAR berthing state estimation scheme for ASVs. The implementation of this scheme only needs a LiDAR sensor, which has high accuracy and can output berthing state information with a frequency of 10 Hz in real time. The proposed method takes into account the roll, pitch, and yaw of the ship and does not require GPS position prior to this.

1.3. Contributions

The main contributions of this paper are as follows:

(1)

Our foremost contribution is the proposition of a novel method that directly utilizes point cloud data to calculate berthing state information. By extracting a berthing plane from point clouds, we eliminate the need for prior shoreline GPS positions, and more accurate state estimation is achieved.

(2)

We introduce a detailed methodology for calculating berthing state information solely based on point cloud data. These parameters encompass berthing distance, berthing angle, berthing speed, and yaw rate. Importantly, our approach accounts for the dynamic changes in ship pose during berthing, ensuring the accuracy and reliability of state estimation.

(3)

To facilitate seamless integration and real-time calculations, we construct a berthing state perception framework based on the renowned point cloud library (PCL) and the versatile Robot Operating System (ROS). This framework empowers ASVs with the capability to continuously compute four essential types of berthing state information in real time.

(4)

Our proposed algorithm underwent rigorous verification through real ship experiments, meticulously evaluating its performance and accuracy. The evaluation results affirm the real-time performance and accuracy of our berthing state estimation framework.

The rest of the paper is organized as follows: Section 2 introduces the principle of the plane fitting algorithm and the berthing state estimation algorithm. The real ship experiment is covered in Section 3, including the experimental environment and equipment, experimental design, experimental results, and results evaluation. Section 4 discusses the advantages and disadvantages of the plane fitting algorithm and the advantages and limitations of the berthing state estimation scheme. Finally, Section 5 presents the conclusions of this research and future work.

2. Plane Fitting Methods and Berthing State Estimation

Figure 1 presents the overall workflow of the berthing state estimation algorithm. The algorithm is composed of three main modules: berthing plane fitting based on 3D LiDAR, the estimation of a ship’s own state, and the estimation of a ship’s state relative to the berth. First, the berthing plane is extracted from the LiDAR point cloud by applying a plane fitting algorithm, and the corresponding plane equation is derived. Subsequently, the point cloud is preprocessed through radius filtering and voxel filtering, and the ship’s pose is obtained using a point cloud registration algorithm. Finally, by combining the berthing plane equation with the estimated ship pose, key berthing parameters are computed through the berthing state estimation algorithm, including berthing distance, berthing angle, berthing speed, and yaw rate.

Figure 1. Flow box diagram of the berthing state estimation algorithm.

2.1. Berthing Plane Fitting Methods

Random Sample Consensus (RANSAC) [29] and its variants, least squares (LS) [30,31], Least Median of Squares (LMedS) [32], and Principal Component Analysis (PCA) [33] are common plane fitting methods. The RANSAC algorithm and its variants’ processes are similar. Therefore, this part only introduces the four algorithms of RANSAC, LS, LMedS, and PCA to fit the berthing plane process.

2.1.1. RANSAC

RANSAC is a random parameter-estimation algorithm. RANSAC divides data points into inner points and outer points. The ultimate goal is to eliminate the outer points as much as possible and retain the inner points as much as possible. Unlike other algorithms, the RANSAC algorithm does not use all the initial data to obtain an initial solution and then eliminate invalid data. Instead, it uses as few initial data points as possible to meet feasible conditions and uses a consistent data set to expand it. This idea is about finding models to fit data and has a wide range of applications in the field of computer vision [34].

The RANSAC algorithm can be summarized into two steps: first, a minimum sample subset that meets the conditions is randomly selected from the existing data samples, and then the corresponding model parameter values are calculated using the initial subset; finally, the deviation between all sample data and this model is calculated. A pre-set threshold is used to compare with this deviation. When the deviation is less than the threshold, the point is determined to be an inner point; otherwise, the outer point is removed. The number of inner points is recorded during the determination process, and this process is repeated until the end of all point cloud determination. In this way, an iteration is completed, and the total number of interior points and the parameter values of the model are recorded at the end of each iteration. After the whole iteration, the calculated optimal model parameters are the final model parameter estimators. The process of plane fitting using RANSAC combined with the eigenvalue algorithm is as follows:

(1)

Three points were randomly selected to calculate the model parameters according to the plane equation ax + by + cz + d = 0.

(2)

Calculate the distance from the remaining point to the plane equation, and compare the distance with the set threshold. If it is less than the threshold, the point is the interior point; otherwise, it is the outer point. Count the number of interior points under the parameter model.

(3)

Continue to perform the above two steps. If the number of interior points of the current model is greater than the maximum number of interior points that have been saved, the new model parameters are changed, and the model parameters are always those with the largest number of interior points.

(4)

Repeat the above three steps, iterate until the iteration threshold is reached, find the model parameters with the largest number of interior points, and finally, use the interior points to estimate the model parameters again to obtain the final model parameters.

2.1.2. Least Squares

The least square method is used to find the best function matching regarding data by minimizing the sum of squared errors, which is often used in a straight line, curve fitting, plane fitting, and so on. In the case of fitting a plane, the goal is to find a plane that minimizes the sum of Euclidean distances between the plane and all data points.

Assuming that there is a discrete point cloud $\mathbf{P}=\{{\mathbf{p}}_1(x_1,y_1,z_1),{\mathbf{p}}_2(x_2,y_2,z_2),\dots$, ${\mathbf{p}}_i(x_i,y_i,z_i),$$\dots ,{\mathbf{p}}_n(x_n,y_n,z_n)\}$ in the space, plane fitting is performed. The plane equation is

$$\begin{array}{c}\hfill \begin{array}{c}\hfill ax+by+cz+d=0\end{array}\end{array}$$

(1)

where a, b, c, and d are the parameters. Suppose $c\ne 0$; then,

$$\begin{array}{c}\hfill \begin{array}{c}\hfill z=a_{0}x+a_{1}y+a_2\end{array}\end{array}$$

(2)

where $a_0=-{\displaystyle \frac{a}{c}}$, $a_1=-{\displaystyle \frac{b}{c}}$, and $a_2=-{\displaystyle \frac{d}{c}}$. According to the principle of least squares, the sum of squared errors is minimized, namely

$$\begin{array}{c}\hfill \begin{array}{c}\hfill min\sum _{i=1}^n{\left(z-z_i\right)}^2=min\sum _{i=1}^n{\left(a_{0}x+a_{1}y+a_2-z_i\right)}^2\end{array}\end{array}$$

(3)

The partial derivatives of $a_0$, $a_1$, and $a_2$ are derived, and the derivative is 0. A simplified version can be obtained:

$$\left[\begin{array}{ccc}\sum _{i=1}^n{x_i}^2& \sum _{i=1}^n{x}_i{y}_i& \sum _{i=1}^n{x}_i\\ \sum _{i=1}^n{x}_i{y}_i& \sum _{i=1}^n{y_i}^2& \sum _{i=1}^n{y}_i\\ \sum _{i=1}^n{x}_i& \sum _{i=1}^n{y}_i& n\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}\sum _{i=1}^n{x}_i{z}_i\\ \sum _{i=1}^n{y}_i{z}_i\\ \sum _{i=1}^n{z}_i\end{array}\right]$$

(4)

By solving Equation (4), $a_0$, $a_1$, and $a_2$ can be obtained, and the fitted plane equation can be obtained by intergrating into Equation (2).

2.1.3. LMedS

Like the RANSAC algorithm, the LMedS algorithm can estimate the parameters of the mathematical model iteratively from a set of observation data sets containing outliers. The algorithm was first proposed by Rousseeuw in 1984. Its basic idea is given by Equation (5). It is a random parameter estimation algorithm, and it is robust against noise and abnormal data.

$$\begin{array}{c}\hfill \begin{array}{c}\hfill min\underset{i}{\mathrm{med}}{r_i}^2\end{array}\end{array}$$

(5)

where $\underset{i}{\mathrm{med}}{r_i}^2$ is the median of the residual squares of each subset and all other samples in the sample set, and the one with the smallest median of the residual square corresponding to all subsets is selected as the model to be estimated. Subsequently, adaptive discrimination between inliers and outliers is achieved by calculating weights for each data point based on a robust standard deviation, as defined in Equation (6):

$$\begin{array}{c}\hfill \begin{array}{c}\hfill \sigma =1.4826\times \left(1+{\displaystyle \frac{5}{n-p}}\right)\times \underset{i}{\mathrm{med}}{r_i}^2\end{array}\end{array}$$

(6)

$\sigma$ represents the robust standard deviation, n is the total number of samples, and p represents the minimum number of samples required to calculate the model parameters. The final classification is performed using the screening criterion specified in Equation (7):

$$\begin{array}{c}\hfill \begin{array}{c}\hfill w_i=\left\{\begin{array}{c}1,{\displaystyle \frac{\left|r_i\right|}{\sigma }}\le 2.5\\ 0,{\displaystyle \frac{\left|r_i\right|}{\sigma }}>2.5\end{array}\right.\end{array}\end{array}$$

(7)

$w_i$ is the weight of the ith sample, the weight of 1 is the internal point, and the weight of 0 is the external point; $r_i$ represents the residual between the ith sample and the estimated model. Furthermore, the number of iterations, k, for this algorithm can be determined using Equation (8).

$$\begin{array}{c}\hfill \begin{array}{c}\hfill k={\displaystyle \frac{lg(1-j)}{lg\left(1-{\epsilon }^q\right)}}\end{array}\end{array}$$

(8)

where j represents the minimum number of samples required to compute the model parameters; q denotes the probability of obtaining at least one good sample subset, typically set between 0.95 and 0.99; and $\epsilon$ is the inlier ratio, defined as the proportion of inliers within the total sample set. It should be noted that LMedS can estimate the sample set containing 50% noise at most, without affecting the reliability of the results.

2.1.4. PCA

PCA is a common data analysis method that is often used for the dimensionality reduction of high-dimensional data, and it can be used to extract the main feature components of data. PCA minimizes the variance of the data by subtracting the mean from the corresponding variables and then performs eigenvalue decomposition (EVD) on the covariance matrix. Since the covariance matrix is symmetric, its eigenvalues and eigenvectors can be directly obtained through EVD, which yields the required number of principal components.

The plane equation is shown in Equation (1), and the observed covariance matrix is

$$\begin{array}{c}\hfill \begin{array}{c}\hfill \mathbf{E}={\displaystyle \frac{1}{n}}\sum _{i=1}^n\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right){\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right)}^T\end{array}\end{array}$$

(9)

$$\begin{array}{c}\hfill \overline{\mathbf{p}}={\displaystyle \frac{1}{n}}\sum _{i=1}^n{\mathbf{p}}_i={\displaystyle \frac{1}{n}}\sum _{i=1}^n\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right].\end{array}$$

(10)

where ${\mathbf{p}}_i$ represents the i-th 3D point as a column vector, $\overline{\mathbf{p}}$ denotes the barycenter value of the observed data, and the eigenvalues are solved by the covariance matrix. The minimum three eigenvalues are ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$$\left({\lambda }_3\ge {\lambda }_2\ge {\lambda }_1\right)$. The corresponding eigenvectors are ${\mathbf{v}}_1$, ${\mathbf{v}}_2$, and ${\mathbf{v}}_3$, respectively. Then, ${\mathbf{v}}_1$ corresponds to the plane normal vector $(a,b,c)$. Using the mean value of the x, y, and z directions of the point cloud, the parameter d can be obtained from Equation (11):

$$\begin{array}{c}\hfill \begin{array}{c}\hfill d=-\left(a\overline{x}+b\overline{y}+c\overline{z}\right)\end{array}\end{array}$$

(11)

Bring a, b, c, and d into Equation (1) to get the fitted plane equation.

2.2. ASV State Estimation Method

Figure 2 depicts a schematic of the ship’s motion during the berthing process. As illustrated in the figure, the ship’s motion encompasses both translational motion along the coordinate axis and rotational motion around the axis. Throughout the berthing procedure, real-time data from the surrounding environment is collected via a LiDAR sensor, enabling the acquisition of the ship’s current motion state. This is accomplished by leveraging the point cloud data from the current frame, as well as the local map point cloud data, through the utilization of a point cloud registration algorithm. It is worth noting that the original point cloud data is prone to containing numerous noise elements and outliers. Incorporating the original point cloud data directly into the registration process would adversely impact both the speed and accuracy of the registration. Consequently, it is customary to preprocess the original point cloud data prior to registration, with the goal of reducing the quantity of point clouds and eliminating outliers. By adopting this approach, real-time and accurate registration can be achieved.

Figure 2. Ship motion diagram.

2.2.1. Point Cloud Preprocessing

In this paper, we employ radius filtering and voxel filtering techniques for preprocessing point cloud data. Radius filtering involves determining the number of neighboring points within a specified radius for each point. Points with a count of neighboring points below a threshold are filtered out. This approach effectively eliminates noise and outliers from the data.

On the other hand, voxel filtering entails the creation of a three-dimensional voxel grid for the input point cloud data. Within each voxel, the center of gravity of all points contained within it is calculated, serving as an approximate representation of the other points within the voxel. By utilizing this method, all points within a voxel are effectively represented by a single center of gravity point. The voxel filtering technique enables downsampling while preserving the underlying geometric structure of the point cloud itself.

In Figure 3a, we present the point cloud diagram before preprocessing during berthing, with the red and green arrows indicating the bow and port directions, respectively. Figure 3b depicts a schematic diagram of the point cloud after preprocessing. As observed from the figure, the number of point clouds is significantly reduced after preprocessing, while the geometric structure remains unchanged.

Figure 3. Point cloud preprocessing.

2.2.2. Point Cloud Registration

This paper implements point cloud registration based on the normal distribution transformation (NDT) algorithm. The basic process is as follows: given the source point cloud $\mathbf{P}=\{{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_m\}$ and the target point cloud $\mathbf{Q}=\{{\mathbf{q}}_1,{\mathbf{q}}_2,\dots ,{\mathbf{q}}_n\}$, we have $\mathbf{P},\mathbf{Q}\subset {\mathbb{R}}^3$. Firstly, the space covered by the target point cloud is divided into voxels. For the j-th voxel, let the set of points contained in it be $C_j=\{{\mathbf{q}}_{j1},{\mathbf{q}}_{j2},\dots ,{\mathbf{q}}_{j{k}_j}\}$. Then, the mean and covariance matrix of the points in voxel $C_j$ are computed as

$$\begin{array}{cc}\hfill {\mathit{\mu }}_j& ={\displaystyle \frac{1}{k_j}}\mathbf{\sum }_{i=1}^{k_j}{\mathbf{q}}_{ji},\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill {\mathbf{\Sigma }}_j& ={\displaystyle \frac{1}{k_j-1}}\mathbf{\sum }_{i=1}^{k_j}({\mathbf{q}}_{ji}-{\mathit{\mu }}_j){({\mathbf{q}}_{ji}-{\mathit{\mu }}_j)}^T.\hfill \end{array}$$

(13)

The probability density function for approximating the distribution of points inside voxel $C_j$ is

$$\begin{array}{c}\hfill F_j\left(\mathbf{x}\right)={\displaystyle \frac{1}{{\left(2\pi \right)}^{D/2}\sqrt{|{\mathbf{\Sigma }}_j|}}}exp\left(-{\displaystyle \frac{1}{2}}{(\mathbf{x}-{\mathit{\mu }}_j)}^T{\mathbf{\Sigma }}_j^{-1}(\mathbf{x}-{\mathit{\mu }}_j)\right),\end{array}$$

(14)

where D represents the dimension. The total likelihood of the transformed source point cloud in the corresponding distributions of all voxels is maximized, and the optimal transformation matrix is

$$\begin{array}{c}\hfill {\mathbf{T}}^{*}=arg\underset{\mathbf{T}}{max}\prod _{i=1}^{m}F\left(\mathbf{T}{\mathbf{p}}_i\right).\end{array}$$

(15)

Finally, the Newton iteration method is used to optimize the parameters and complete point cloud registration.

2.3. Berthing State Estimation Method

The parameters a, b, c, and d of the berthing plane equation can be obtained by the plane fitting algorithm. Figure 4 is a schematic diagram of ship berthing, in which the two-way arrow is the distance from each point on the ship to the berthing plane, ${\theta }_b$ is the angle between the bow and stern line of the ship and the berthing plane, and $MN$ is the plane normal. Figure 5 shows a sketch of ship roll and pitch, respectively, where ${\phi }_b$ is the roll angle, and ${\omega }_b$ is the pitch angle. The specific values of the two can be obtained by the NDT algorithm. Suppose that the coordinates of a point on the ship are ($x_0$, $y_0$, $z_0$). When the roll and pitch of the ship are not considered, the distance $d_b$ from the point to the plane can be obtained by the following formula:

$$\begin{array}{c}\hfill \begin{array}{c}\hfill d_b={\displaystyle \frac{\left|a{x}_0+b{y}_0+c{z}_0+d\right|}{\sqrt{\left(a^2+b^2+c^2\right)}}}\end{array}\end{array}$$

(16)

Figure 4. Ship berthing state diagram.

Figure 5. Roll and pitch.

The length of the ship is $L_{OA}$, the width of the ship is $B_M$, and the height of the ship is $H_S$. If roll and pitch are considered, it can be seen from Figure 5 that when the ship berths on both sides, the distance from the ship sides to the berthing plane is reduced by $OF-{\displaystyle \frac{B_M}{2}}$. Similarly, when the ship berths, the distance from the bow and stern to the berth is reduced by $OH-{\displaystyle \frac{L_{OA}}{2}}$. According to Figure 5,

$$\begin{array}{c}\hfill \begin{array}{c}\hfill OF=OE\times cos{\theta }_f=\sqrt{{\left({\displaystyle \frac{B_M}{2}}\right)}^2+{\left({\displaystyle \frac{H_S}{2}}\right)}^2}\times cos{\theta }_f\end{array}\end{array}$$

(17)

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\theta }_f={\theta }_e-{\phi }_b={tan}^{-1}\left({\displaystyle \frac{H_S}{B_M}}\right)-{\phi }_b\end{array}\end{array}$$

(18)

Therefore,

$$\begin{array}{c}\hfill \begin{array}{c}\hfill OF=\sqrt{{\left({\displaystyle \frac{B_M}{2}}\right)}^2+{\left({\displaystyle \frac{H_S}{2}}\right)}^2}\times cos\left({tan}^{-1}\left({\displaystyle \frac{H_S}{B_M}}\right)-{\phi }_b\right)\end{array}\end{array}$$

(19)

Similarly,

$$\begin{array}{c}\hfill \begin{array}{c}\hfill OH=OG\times cos{\theta }_h=\sqrt{{\left({\displaystyle \frac{L_{OA}}{2}}\right)}^2+{\left({\displaystyle \frac{H_S}{2}}\right)}^2}\times cos{\theta }_h\end{array}\end{array}$$

(20)

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\theta }_h={\theta }_g-{\omega }_b={tan}^{-1}\left({\displaystyle \frac{H_S}{L_{OA}}}\right)-{\omega }_b\end{array}\end{array}$$

(21)

$$\begin{array}{c}\hfill \begin{array}{c}\hfill OH=\sqrt{{\left({\displaystyle \frac{L_{OA}}{2}}\right)}^2+{\left({\displaystyle \frac{H_S}{2}}\right)}^2}\times cos\left({tan}^{-1}\left({\displaystyle \frac{H_S}{L_{OA}}}\right)-{\omega }_b\right)\end{array}\end{array}$$

(22)

Therefore, the berthing distance $d_b^{\prime }$ is

$$\begin{array}{c}\hfill d_b^{\prime }=\left\{\begin{array}{c}{d}_b-OF+{\displaystyle \frac{B_M}{2}}\hfill \\ d_b-OH+{\displaystyle \frac{L_{OA}}{2}}\hfill \\ d_b-\left(OF-{\displaystyle \frac{B_M}{2}}\right)\times \left|cos{\theta }_b\right|-\left(OH-{\displaystyle \frac{L_{OA}}{2}}\right)\times \left|sin{\theta }_b\right|\hfill \end{array}\right.\end{array}$$

(23)

Remark 1. 

The three formulas in Equation (23) correspond to the berthing distance of two sides of a berthing, bow and stern berthing, and other forms of berthing.

It can be seen from Figure 4 that the approaching angle ${\theta }_b$ = ${\theta }_n$ − ${\displaystyle \frac{\pi }{2}}$, ${\theta }_n$ is the angle between the ship’s bow and stern line $AB$ and the plane normal vector $MN$; then,

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\theta }_b={\theta }_n-{\displaystyle \frac{\pi }{2}}={cos}^{-1}\left({\displaystyle \frac{\overrightarrow{MN}·\overrightarrow{MB}}{|\overrightarrow{MN}|·\overrightarrow{\left|MB\right|}}}\right)-{\displaystyle \frac{\pi }{2}}\end{array}\end{array}$$

(24)

Figure 6 shows the berthing distance and angle. The partial components $v_{bow}$, $v_{port}$, and $v_{port\_stern}$ of the berthing speed can be expressed by Equation (25), and the yaw rate $r_b$ is estimated by Equation (26).

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{v}_{\mathrm{bow}}={\displaystyle \frac{d_{\mathrm{bow}}^{\prime }-d_{\mathrm{bow}}^{\prime \prime }}{\Delta t}}\hfill \\ v_{\mathrm{port}}={\displaystyle \frac{d_{\mathrm{port}}^{\prime }-d_{\mathrm{port}}^{\prime \prime }}{\Delta t}}\hfill \\ v_{\mathrm{port}\_\mathrm{stern}}={\displaystyle \frac{d_{\mathrm{port}\_\mathrm{stern}}^{\prime }-d_{\mathrm{port}\_\mathrm{stern}}^{\prime \prime }}{\mathrm{\Delta }t}}\hfill \end{array}\right.\end{array}$$

(25)

$$\begin{array}{c}\hfill \begin{array}{c}\hfill r_{\mathrm{b}}={\displaystyle \frac{{\theta }_{\mathrm{b}}^{\prime }-{\theta }_{\mathrm{b}}^{\prime \prime }}{\mathrm{\Delta }t}}\end{array}\end{array}$$

(26)

Figure 6. Berthing distance and angle.

In addition to the speed calculated by the above berthing distance, the berthing speed also includes the speed component generated by the yaw rate, as depicted in Figure 7. The overall berthing speed, denoted as $v_{bow}^{\prime }$, $v_{port}^{\prime }$, $v_{port\_stern}^{\prime }$, can be expressed using the following formula.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{v}_{\mathrm{bow}}^{\prime }=v_{\mathrm{bow}}+v_{\mathrm{row}\_\mathrm{bow}}\hfill \\ v_{\mathrm{port}}^{\prime }=v_{\mathrm{port}}+v_{\mathrm{row}\_\mathrm{bow}}\hfill \\ v_{\mathrm{sport}\_\mathrm{stern}}^{\prime }=v_{\mathrm{sport}\_\mathrm{stern}}-v_{r\_\mathrm{stern}}\hfill \end{array}\right.\end{array}$$

(27)

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{v}_{r\_bow}=v_{rb}\times cos{\theta }_{bow}=v_{rb}\times cos{\theta }_b^{\prime }\hfill \\ v_{r\_port}=v_{rp}\times cos{\theta }_{port}=v_{rp}\times cos\left({\theta }_b^{\prime }+{\theta }_{mp}\right)\hfill \\ v_{r\_stern}=v_{rs}\times cos{\theta }_{stern}=v_{rs}\times cos\left({\theta }_b^{\prime }-{\theta }_{ms}\right)\hfill \end{array}\right.\end{array}$$

(28)

$$\begin{array}{c}\hfill \begin{array}{c}\hfill v_{rb}=r_b\times l_{Nb},v_{rp}=r_b\times l_{Np},v_{rs}=r_b\times l_{Ns}.\end{array}\end{array}$$

(29)

Figure 7. Berthing velocity.

3. Experiment

The experimental site is “Lingshui Port” in Dalian, China, as shown in Figure 8a. The experimental ship is “Zhilong 1”, an unmanned boat with double propellers and double rudders.

Figure 8. Port and unmanned boat.

The basic parameters of the USV are summarized as follows: the overall length is 175 cm, the beam is 50 cm, the height is 13.5 cm, and the mass is approximately 40 kg. The vessel is equipped with two propellers and two rudders, with a propeller diameter of 8 cm, and a rudder height of 8 cm. An anti-rolling device is also installed, as shown in Figure 8b. The USV is further equipped with an R-Fans 16-line mechanical LiDAR and a binocular camera. The R-Fans sensor provides a suitable balance between vertical resolution, detection range, and cost, which is especially important for near-field perception in berthing operations. Its 30° vertical field of view and 0.2° angular resolution enable reliable detection of quay walls and mooring structures at close range (within 50–80 m), which meets the requirements of typical berthing scenarios. The detailed parameters of the 3D LiDAR are listed in Table 1.

Table 1. LiDAR data specifications.

Item	Parameter
Laser beam	16
Vertical field of view	${30}^{\circ }(-{15}^{\circ }\sim {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}$

3.1. Berthing Plane Fitting Experiment

A berthing process diagram and a single-frame point cloud data map of the ship near the berth are shown in Figure 9. Nine frames of point cloud data from the berthing process were chosen for the berthing plane fitting experiment. The berthing plane was fitted using nine different plane fitting algorithms. The algorithms include RANSAC, M-estimator Sample Consensus (MSAC) [35], Maximum Likelihood Estimator Sample Consensus (MLESAC) [36], Progressive Sample Consensus (PROSAC) [37], Randomized Random Sample Consensus (RRANSAC) [38], Randomized M-estimator Sample Consensus (RMSAC) [34], LS, LMedS, and PCA.

Figure 9. Berthing process diagram and point cloud data map.

For RANSAC, MSAC, MLESAC, PROSAC, RRANSAC, RMSAC, and LMedS, the distance threshold was set to 0.01, the maximum number of iterations was 100, and the other parameter settings were recommended by PCL. The fitting speed of RRANSAC and RMSAC is slow when the proportion of outliers is large. Therefore, the data is filtered before algorithm fitting; that is, the port berthing filters the point cloud of the ship’s right side, and the starboard berthing filters the point cloud of the ship’s left side to improve the fitting speed of the two algorithms.

The results of each algorithm’s plane fitting are shown in Figure 10. The blue irregular points in the figure are the two-dimensional projection of the point cloud. The solid line and the dotted line are the intersection of the algorithm fitting plane and the z-value zero plane. It can be seen from the figure that the MLESAC, LS, and PCA fitting planes are quite different from the berthing plane. The fitting results of RRANSAC and RMSAC are better than the first three algorithms, but the fitting results of these two algorithms are not stable, and the fitting results of some frames are poor. The fitting results of the LMedS algorithm in the first two frames are poor, and the fitting plane in the last seven frames is similar to the berthing plane. The poor fitting in the first two frames is due to the fact that when the ship is far from the shore, the number of laser points in the berthing plane is smaller than when it is closer to the shore, and the proportion of noise points exceeds 50%, leading to reduced reliability for algorithm fitting. The fitting results of the algorithm are not reliable. The fitting results of RANSAC, MSAC, and PROSAC are good, and the fitting plane is close to the berthing plane.

Figure 10. Each algorithm plane fitting 2D projection map.

Figure 11 is a comparison of the average berthing plane fitting time (measured in milliseconds) for each algorithm employed. In ascending order, the algorithms are listed as follows: LS, PCA, PROSAC, RANSAC, MSAC, LMedS, MLESAC, RRANSAC, and RMASC. Notably, the plane fitting times for the initial five algorithms are all below the 100 ms threshold, thus demonstrating their suitability for real-time execution.

Figure 11. Comparison of the average berthing plane fitting time for each algorithm.

In summary, the accuracy and speed of the MLESAC algorithm do not meet the requirements. The LS and PCA plane fitting speeds are fast, but the results are quite different from the real value. LMedS has high accuracy at close distances, but the long-distance fitting result is poor, and the fitting time is slow. RRANSAC and RMASC have accuracy and time bidirectional instability. RANSAC, MSAC, and PROSAC have higher fitting accuracy and faster speed.

3.2. Berthing State Estimation Experiment

RANSAC, MSAC, and PROSAC achieve higher fitting accuracy and faster speed on the berthing plane fitting experiment. To further test the state estimation accuracy of the three algorithms in the process of ship berthing, continuous frame experiments were carried out on the three algorithms. The continuous frame experiment is a plane fitting algorithm that is used to fit the berthing plane in real time and calculate the state of the ship relative to the berth during the whole berthing process.

As shown in Figure 12, four different berthing experiments were designed to verify the performance of the algorithm: port bow berthing, port berthing, starboard bow berthing, and starboard berthing.

Figure 12. Berthing experiment diagram.

In the four berthing trials, the curve of the distance between the ship and the berth with time is shown in Figure 13, illustrating the composition of the berthing distance. For port side berthing, the distances are defined as those from the bow, port side, and port side stern to the berth, whereas for starboard side berthing, they are defined as the distances from the bow, starboard side, and starboard side stern to the berth. It can be observed that the berthing distance varies significantly during the initial stage and gradually becomes smoother thereafter. While the overall variation trends obtained by the three algorithms are largely consistent, the locally magnified view reveals discernible differences in detail. In the case of port side bow berthing, the final berthing distances are reduced in the order of port side stern, port side, and bow, which is consistent with the actual situation. The results obtained from the other three experimental groups also validate the effectiveness of the proposed method.

Figure 13. The distance of the vessel relative to the berth.

Figure 14 is the curve of the speed of the three algorithms with time during the berthing process, where $v_{Ransac\_bow}$, $v_{Ransac\_port}$, and $v_{Ransac\_port\_stern}$ are the relative berth speed changes for the bow, port, and port stern obtained by the RANSAC plane fitting algorithm. The speed of the ship is relatively slow during berthing, and finally tends to 0 after a small increase and decrease.

Figure 14. The velocity of the vessel relative to the berth.

Figure 15 illustrates the variation of the ship’s approaching angle during the berthing process. The approaching angle ranges from 0° to 90°. In the case of port side bow berthing, the ship initially approaches the berth at a relatively small angle. During this process, the bow gradually rotates toward the berth, while the stern swings outward, until the entire vessel comes closer to the berth. In contrast, during starboard side bow berthing, the ship approaches the berth at a larger angle, and the approaching angle gradually decreases as the vessel aligns with the berth. Despite the differences in initial angles, both port side and starboard side berthing follow a similar pattern: the vessel approaches the berth at an angle, and as it nears the berth, it rotates until its longitudinal axis becomes nearly aligned with the berth. The curve shown in Figure 15 is consistent with this description, thereby validating the correctness of the proposed algorithm.

Figure 15. Vessel-approaching angle to the berth.

The variation in yaw rate during berthing is illustrated in Figure 16. For clarity, the curves are smoothed using the Savitzky–Golay filter with a window length of 15 and a polynomial fitting order of 3. In this context, a positive yaw rate indicates a rotation toward the berth, while a negative yaw rate corresponds to a rotation away from the berth. The fluctuations observed in the yaw rate curves are attributed to manual remote control of the unmanned boat during the berthing experiments, where local sharp changes in yaw rate are considered normal.

Figure 16. Vessel angular velocity of yaw when berthing.

The above analysis verifies the feasibility of each algorithm. In order to further analyze the performance of each algorithm, nine frames of data during the bow berthing of the port side are selected at equal intervals. The two-dimensional image is shown in Figure 17, where the blue irregular points are the two-dimensional projection of the point cloud, and the light blue dotted line is the artificial fitting line as the reference value of the berthing plane. Specifically, quay boundary points were manually identified from the LiDAR point clouds by two independent operators with experience in marine data interpretation. A least-squares linear regression was subsequently applied to these boundary points to construct a reference berthing plane, hereafter referred to as the artificially fitted line. This reference line was employed to compute the berthing distance and approach angle for ground truth generation. To ensure the reliability of the annotated reference data, cross-validation was performed by comparing the independent annotations of the two operators, and discrepancies were resolved through consensus. To facilitate the observation of the approaching angle, the fitted line was translated, as indicated by the light pink dashed line in the figure. The angle and distance values in the diagram are the ground truth values of the state estimation, which are the angle between the ship’s center line and the fitted straight line and the distance from the bow, port, and port stern to the fitted straight line. Figure 18 is the image data of the UAV’s perspective during the berthing process, corresponding to Figure 17.

Figure 17. Top view of vessel point cloud at the end of the berthing stage. The light blue dashed line represents the manually fitted berth line, the light pink dashed line is the shifted fitted line for clarity, and the orange, blue, and green solid lines indicate the berthing distances of the bow, port side, and stern, respectively.

Figure 18. Top view of vessel images at the end of the berthing stage.

Figure 19 and Figure 20 illustrate the temporal variations of the vessel approaching angle and the associated errors from 106 s to 130 s. The curves in Figure 20 represent the absolute errors, calculated as the modulus of the difference between the estimated approaching angle and the reference value. Analysis shows that the approaching angles obtained by the RANSAC, MSAC, and PROSAC algorithms closely follow the reference values. The absolute errors range from 0° to 0.7°. Specifically, for the berthing angle, the RANSAC algorithm exhibits a maximum error of 0.4368° and a mean absolute error (MAE) of 0.1944°; the MSAC algorithm shows a maximum error of 0.6905° and a MAE of 0.2576°; and the PROSAC algorithm reaches a maximum error of 0.4368° and a MAE of 0.1947°. The errors of RANSAC and PROSAC are comparable, while MSAC shows slightly larger deviations.

Figure 19. Variation process of vessel-approaching angle.

Figure 20. Variation process of vessel approaching angle error.

To assess potential bias, the algebraic errors (signed differences) were also examined. The distribution of these signed errors indicates that the estimators do not exhibit significant systematic bias, as positive and negative deviations are roughly balanced over time.

Figure 21 and Figure 22 present the berthing distances and their corresponding absolute errors, including distances from the bow, port midsection, and port stern to the berthing plane. The absolute errors, again, are defined as the modulus of the algebraic errors and range from 0 to 0.06 m. The maximum errors for RANSAC, MSAC, and PROSAC are 0.0592 m, 0.0592 m, and 0.0519 m, respectively, while the MAEs are 0.0226 m, 0.0222 m, and 0.0227 m. Analysis of the algebraic errors confirms that the berthing distance estimates are unbiased.

Figure 21. Berthing distance change process.

Figure 22. Berthing distance error change process.

Comparing the absolute error of the approaching angle with that of the berthing distance, we observe relatively stable results obtained by the algorithms. When compared with the results of the manual fitting reference value and the 3D LiDAR algorithm, it is evident that 3D LiDAR can be effectively employed for calculating both the berthing distance and angle. Consequently, the proposed 3D LiDAR berthing state estimation scheme outlined in this paper is deemed accurate and reasonable.

4. Discussion

4.1. Accuracy and Real-Time Performance of Algorithm

This paper presents a comprehensive investigation of nine algorithms, testing their performance in terms of accuracy and real-time performance. An examination of Figure 11 reveals that the algorithms RANSAC, MSAC, PROSAC, LS, and PCA exhibit real-time performance. Additionally, the data illustrated in Figure 10 highlights the superior accuracy of RANSAC, MSAC, and PROSAC. To gain further insights, we conducted a detailed analysis of the berthing state estimation experiment, focusing specifically on the RANSAC, MSAC, and PROSAC algorithms. The experimental outcomes validate the precision of the estimated parameters, such as berthing distance, approaching angle, berthing speed, and yaw rate, as they align closely with the actual values. In the process of evaluating the results, it is noteworthy that the three algorithms demonstrate an average angle error of less than 0.3°, a distance error of fewer than 0.03 m, and a remarkably high level of state estimation accuracy.

4.2. Limitations and Improvement of the Plane Fitting Algorithm

Based on the aforementioned analysis, it becomes evident that RANSAC, MSAC, and PROSAC have exhibited commendable performance in terms of both accuracy and speed among the nine algorithms considered. Conversely, the remaining six algorithms have demonstrated subpar performance. Although LS and PCA yielded unsatisfactory results in terms of plane fitting, their computational efficiency is noteworthy, with a processing time of less than 1 ms per frame. These algorithms may be a preferable choice for systems with limited computational resources or scenarios that demand real-time processing. Further analysis and refinement of these two algorithms would prove advantageous. Experimental observations have indicated that the reliability of the LMedS algorithm diminishes when the noise level exceeds 50%. Hence, it can be improved by filtering out the point cloud on the opposite side of the berthing plane. This improvement can also improve the speed of the algorithm. The three algorithms of MLESAC, RRANSAC, and RMSAC are suitable for dealing with a situation where the proportion of local points in the data points is large. The reason why the three algorithms have poor results in the experiment is that the proportion of local points is small. The three algorithms can also be improved by filtering algorithms.

4.3. Limitations and Improvement of Berthing State Estimation Based on Plane Fitting

This paper introduces a novel ASV berthing state estimation scheme based on plane fitting, substantiated by the reliability and accuracy demonstrated through real ship experiments. However, the scheme may exhibit some limitations. Specifically, when the LiDAR’s height exceeds the height of the port by a significant amount, resulting in an increased scan of laser points above the port plane, more laser points are projected to the plane above the port, and fewer laser points are projected to the berthing plane. At this time, the fitting plane of the algorithm may be the ground above the port, not the berthing plane. To address this issue, the water surface height relative to the ground above the port is estimated using tidal data during berthing. Points above the estimated berthing plane are filtered out, followed by execution of the plane fitting algorithm. Additionally, alternative strategies can be employed: (1) sensor fusion with barometric altimetry to dynamically estimate water surface height; (2) adaptive plane fitting that accounts for variations in point cloud distribution and water level; and (3) linear fitting of the intersection between the port plane and the ground plane, representing the shoreline. These approaches offer practical guidance for implementation and enhance the reliability and completeness of ASV berthing state estimation in future applications.

5. Conclusions

This paper mainly investigates ship berthing state estimation based on 3D LiDAR and proposes a berthing state estimation algorithm based on plane fitting. Firstly, nine common plane fitting algorithms were implemented, including RANSAC, MSAC, MLESAC, PROSAC, RRANSAC, RMSAC, LS, LMedS, and PCA. These algorithms were used to fit the berthing plane, and their performance was evaluated in terms of both accuracy and computational speed. Secondly, a ship berthing state estimation framework based on plane fitting was built, which can output berthing distance, berthing speed, approaching angle, and yaw rate information in real time. Finally, the algorithm was validated through a small unmanned ship berthing experiment.

The plane fitting results indicate that RANSAC, MSAC, and PROSAC achieve the highest accuracy with fast fitting speed. The average berthing angle error of the proposed algorithm is below 0.26°, and the average berthing distance error is below 0.023 m, consistent with the results reported in the abstract. This demonstrates that the proposed berthing state estimation algorithm achieves high accuracy (centimeter level) and can output real-time information at a 10 Hz frequency, fulfilling both accuracy and real-time requirements.

Although the algorithm provides highly accurate berthing state information, some limitations remain. For example, when the ship is large or during high tide, the plane fitting algorithm may struggle to accurately fit the berthing plane. Improving the robustness and stability of the algorithm is the focus of future work.