Graph Matching for Underwater Simultaneous Localization and Mapping Using Multibeam Sonar Imaging

Abstract

This paper addresses the challenges of underwater Simultaneous Localization and Mapping (SLAM) using multibeam sonar imaging. The widely used Iterative Closest Point (ICP) often falls into local optima due to non-convexity and the lack of features for correct registration. To overcome this, we propose a novel registration algorithm based on Gaussian clustering and Graph Matching with maximal cliques. The proposed approach enhances feature-matching accuracy and robustness in complex underwater environments. Inertial measurements and velocity estimates are also fused for global state estimation. Comprehensive tests in simulated and real-world underwater environments have demonstrated that the proposed registration method effectively addresses the issue of the ICP algorithm easily falling into local optima while also exhibiting excellent inter-frame registration performance and robustness.

1. Introduction

In recent years, autonomous underwater robots have been used in extensive applications, such as pipeline inspection, geological structure mapping, rescue operations, and resource exploration [1]. These robots’ autonomous positioning and navigation control in complex environments necessitate environmental modeling and robot state estimation as inputs. Particularly in ocean environments that still need to be mapped, the limitations of communication infrastructure and remote control operations make autonomous exploration capabilities even more critical. Simultaneous Localization and Mapping (SLAM) technology is vital in this context. The Global Navigation Satellite System (GNSS) and other communication systems, such as Wi-Fi or electromagnetic waves, are significantly attenuated and thus unusable in underwater settings. Autonomous Underwater Vehicles (AUVs) primarily rely on dead reckoning for navigation; however, this method tends to drift over time. While cameras can capture detailed underwater scenes, their effectiveness is constrained by water turbidity and manual control or data processing limitations in such environments. Sonar is a commonly used alternative sensor. However, challenges such as low signal-to-noise ratios, low resolution, uneven sound waves, and multi-path reflections hinder the accurate extraction and registration of target features. Therefore, we address these challenges and focus on solving positioning and mapping issues underwater robots face when navigating complex ocean environments.

Sonar-based underwater SLAM has been a subject of research for many years. Earlier studies have described sonar inertia-based SLAM systems for underwater reconstruction and navigation [2,3,4]. Corke et al. [5] compared acoustic and visual underwater positioning methods, demonstrating their applicability in specific scenarios. Subsequently, research on sonar-based underwater SLAM gradually progressed, with Folkesson et al. [6] utilizing blazed array sonar sensors for real-time feature tracking. A system employing resampled features from sub-sonar is detailed in [7]. The sunfish model launching robot described in [8] employs multibeam sonar, a Fiber Optic Gyroscope (FOG) Inertial Measurement Unit (IMU), an Acoustic Doppler Velocimeter (ADV), and a pressure depth sensor. While it has been successfully applied in cave exploration, its inter-frame registration performance remains less than optimal, relying primarily on trajectory estimation for position determination.

In this paper, to enhance the performance of inter-frame registration and address the issue of local optima due to the non-convexity of the Iterative Closest Point (ICP) algorithm, we have augmented the application of the Gaussian clustering and maximal clique algorithms, which have been previously studied, to adapt to the unique challenges faced by multibeam imaging sonar-equipped robots navigating cluttered underwater environments. Our approach encompasses the following:

• We introduce a novel registration algorithm to effectively address the issue of local optima in inter-frame registration due to the non-convex nature of the Iterative Closest Point (ICP) algorithm.
• We employed an enhanced Accelerated-KAZE (AKAZE) algorithm to improve feature-matching accuracy in sparse environments and along extended straight lines, thereby enhancing the robustness of SLAM.
• We integrated an Inertial Measurement Unit and Doppler velocimeter data for the global state estimation of the robot, incorporating various pose design factors into a factor graph for back-end optimization.

2. Related Work

2.1. Point Cloud Registration

Point cloud registration algorithms can be categorized into two main approaches based on probabilistic fields and point-to-point matching. The probabilistic field approach often employs Gaussian Mixture Models (GMMs) to represent point clouds. In contrast, the point-to-point matching approach encompasses the well-established Iterative Closest Point (ICP) algorithm and its various extensions, as detailed by Segal et al. [9]. Besl and McKay conducted pioneering work in the registration of 3D shapes [10], while Chen and Medioni [11] concentrated on aligning range data for object modeling. Since then, numerous ICP variants have emerged. Due to the extensive body of literature on ICP algorithms, a complete and exhaustive introduction is beyond the scope of this paper. Although registration methods that ensure optimality have been published, they are relatively few and primarily rely on the Branch and Bound (BnB) algorithms. For instance, Li and Hartley [12,13] have proposed a 3D registration method that utilizes a Lipschitz-regularized L2 error function, which is minimized through BnB.

The widely recognized technique for field-based scan matching is the Normal Distribution Transform (NDT), which was first introduced by Biber and Strasser for the purpose of aligning two-dimensional point clouds [14]. This method has since been expanded to three dimensions by Magnusson [15]. Saarinen and colleagues developed the Normal Distribution Transform Occupancy Maps (NDT-OM), integrating NDT with the OctoMap framework crafted by Hornung and his team [16]. In response to the challenge of accurately aligning misaligned scans, Bouraine and others [17] have introduced a Particle Swarm Optimization approach for the Direct-to-Direct (D2D) method, known as NDT-P2D-PSO, which ensures a globally optimal solution and is capable of operating in real time for SLAM applications. However, in underwater sonar SLAM, NDT has some limitations. It employs a Cartesian grid to sample point clouds, which may result in an inadequate capture of the underlying structure of the point cloud, particularly when the scanned surface has irregular shapes. Therefore, we propose a new registration algorithm based on ICP, which provides a robust prediction matrix and effectively addresses the local optimal solution problem associated with ICP.

2.2. Underwater Sonar SLAM

In underwater settings, sonar images typically exhibit low resolution, low signal-to-noise ratios, and high ambiguity, characteristics that pose significant challenges to the crucial processes of automatic feature extraction and data association for underwater SLAM. To address these challenges, various methods have been introduced. Santos et al. [18] introduced a method based on topological Graph Matching, which utilizes Gaussian functions and the maximal clique algorithm to describe and match image features, demonstrating good robustness and rotational invariance. Eustice and colleagues proposed the first SLAM implementation using the Exact Sparse Extended Information Filter (ESEIF) [19], with features manually selected from sonar images, focusing on constructing a feature map to ensure real-time performance. Johannsson, Kaess, and Englot introduced the Normal Distribution Transform (NDT) to detect and match FLS image features [20], assuming a planar surface to address the ambiguity in elevation measurements. Similarly, Shin, Lee, and Choi engaged in seabed mapping, utilizing accelerated KAZE (AKAZE) features to process sonar images and demonstrated effective data association [21].

Huang and Kaess introduced the Acoustic Sensor-based Feature Motion (ASFM), which reconstructs 3D structures and sensor motion from multiple sonar viewpoints without making specific assumptions about the scene [22]. In their work, statistical analysis was conducted to identify and discuss degenerate cases that lead to ASFM failures, particularly analyzing which motion patterns result in poor constraints for the optimization problem. In 2018, Jie Li proposed a machine-learning-based loop closure method that accounts for the sparsity of sonar image features and the false data association, developing a reliable mathematical association method and demonstrating the robustness of the system [23]. In 2022, the Stevens Institute of Technology proposed an exploration framework based on virtual maps, which demonstrated favorable performance in simulated experiments [24]. However, practical applications necessitate the consideration of the costs associated with the construction and updating of virtual maps, as well as the resolution and noise levels of the sonar equipment.

Our research significantly enhances the navigation capabilities of multibeam imaging sonar-equipped robots in complex underwater environments, the robustness of the SLAM system, and the accuracy of global state estimation by introducing a novel registration algorithm to address the local optima issue of the Iterative Closest Point (ICP) algorithm, improving feature matching accuracy with an enhanced Accelerated-KAZE (AKAZE) algorithm, and integrating Inertial Measurement Unit and Doppler velocimeter data for factor graph optimization.

3. Inter-Frame Registration

To address the issue of ICP falling into local optima, we employ an inter-frame registration method based on the parallel Accelerated-KAZE (AKAZE) algorithm [25] and a newly proposed Graph Matching (GM) registration algorithm. This approach effectively performs sonar feature matching, adapts to rotational and long straight-line scenes, and accurately estimates the robot’s motion state. The two algorithms complement each other’s weaknesses.

3.1. Graph Matching ICP

The Iterative Closest Point (ICP) is the most widely used inter-frame registration algorithm, which is implemented by minimizing the following value function:

$$J\left(x_{ts}\right)=\sum _{{\mathbf{p}}_t\in {\mathbf{P}}_t,{\mathbf{p}}_s\in {\mathbf{P}}_s}{∥{\mathbf{T}}_{ts}{\mathbf{p}}_s-{\mathbf{p}}_t∥}^2=\sum _{{\mathbf{p}}_t\in {\mathbf{P}}_t,{\mathbf{p}}_s\in {\mathbf{P}}_s}{∥{\mathbf{R}}_{ts}{\mathbf{p}}_s+{\mathbf{t}}_{ts}-{\mathbf{p}}_t∥}^2,$$

(1)

where ${\mathbf{p}}_t$ represents a point in the target point cloud; ${\mathbf{p}}_s$ represents a point in the source point cloud; $\mathbf{P}s$ and $\mathbf{P}t$ represent the point sets of the source point cloud and the target point cloud, respectively; ${\mathbf{T}}_{ts}$ denotes the transformation matrix from the source point cloud to the target point cloud; ${\mathbf{R}}_{ts}$ indicates the rotation matrix that aligns the source point cloud with the target point cloud; and ${\mathbf{t}}_{ts}$ signifies the translation vector that positions the source point cloud relative to the target point cloud.

However, ICP can fall into local optimum issues due to its non-convexity [26]. The application of ICP matching for sonar images is often hindered by the low angular resolution of the sonar, the high noise levels of the imaging sonar, and the low accuracy of trajectory calculations. To solve the problem of ICP falling into a local optimal solution, we propose a new Graph Matching (GM) registration algorithm. This algorithm aims to solve the following equation:

$${\tilde{\mathbf{x}}}_s,{\mathbf{T}}_{\theta }=\underset{{\mathbf{x}}_s,{\mathbf{T}}_{\theta }}{argmax}\sum _{{\mathbf{p}}_s\in {\mathbf{P}}_s}⨿\left(d\left({\mathbf{p}}_s\right)/n\le \epsilon \right),$$

(2)

where ${\tilde{\mathbf{x}}}_s$ represents the globally initialized source point cloud pose and ${\mathbf{x}}_s$ denotes the source point cloud pose that is being optimized. ${\mathbf{T}}_{\theta }$ denotes the predicted transformation matrix of ICP. $\epsilon$ is the reprojection error convergence threshold, and n represents the total number of points in the source point cloud. d is the sum of distances and represents the cumulative measure used to search for the nearest point through the course of the last iterative transformation. ⨿ is an indicator function that equals one if the solution is successful and zero if the solution fails. Simultaneously, the definition of $d\left({\mathbf{p}}_s\right)$ is as follows:

$$d\left({\mathbf{p}}_s\right)=\underset{{\mathbf{p}}_t\in {\mathbf{P}}_t}{min}∥{\mathbf{R}}_{ts}{\mathbf{p}}_s+{\mathbf{t}}_{ts}-{\mathbf{p}}_t∥,$$

(3)

where ${\mathbf{p}}_s$ refers to an individual point within the source point cloud, and ${\mathbf{p}}_t$ refers to a point within the target point cloud. The rotation matrix ${\mathbf{R}}_{ts}$ and the translation vector ${\mathbf{t}}_{ts}$ together form the transformation that maps points from the source coordinate system to the target coordinate system, facilitating the alignment and registration process between the two point clouds.

3.1.1. Sonar Image Processing and Feature Extraction

Initially, we employ a Constant False Alarm Rate (CFAR) algorithm [27] for initial filtering of the sonar imagery, followed by the removal of discrete points, and finally apply voxel filtering to downsample the point cloud for feature extraction, as shown in Figure 1. Subsequently, we apply the DBSCAN (Density-Based Spatial Clustering of Applications with Noise) algorithm [28] for clustering. DBSCAN is distinguished by its ability to identify dense regions within a dataset and effectively partition them into distinct clusters, even in the presence of noise.

After downsampling the feature point cloud, clustering is performed using DBSCAN. When the search radius $\epsilon =0.06$ and the minimum number of points in a cluster $point{s}_{min}=4$, the results are depicted in Figure 2. After clustering, we filter the clusters based on the number of point clouds they contain to eliminate discrete noise points. Clusters with several point clouds greater than $Pt{s}_{threshold}$ are retained for subsequent Gaussian generation. Here, $Pt{s}_{threshold}$ represents the threshold for the number of point clouds. Next, a Gaussian block is generated for each cluster by traversing the clusters’ labels. Gaussian blocks are used to characterize the shape of each cluster, and the parameters of the Gaussian are defined as follows:

$$GA=\left\{\mu X,\mu Y,\mu I,\sigma B,\sigma S,\sigma I,\theta ,N\right\},$$

(4)

where $\mu X,\mu Y$ denote the positional center point of the Gaussian block in the image, $\mu I$ is the average intensity of the sonar image within the Gaussian block, $\sigma B$ represents the length of major axis of the Gaussian block, $\sigma S$ is the length of the minor axis of the Gaussian block, $\sigma I$ is the standard deviation of sonar image intensity within the Gaussian block, N is the number of points in the cluster, and $\theta$ is the angle measured clockwise from the positive underwater x-axis to the major axis of the Gaussian block. We restrict $\theta$ to the range of $\left(-{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}\right)$. Consistent with the standard Gaussian block approach, we employ the eigenvalues and eigenvectors derived from the covariance matrix ${\mathsf{\Sigma }}_{xy}$ to calculate $\sigma B$ and $\sigma S$.

Compute the center $(\mu X,\mu Y)$ of the point cloud for the cluster and the covariance matrix as follows:

$$\left(\mu X,\mu Y\right)=\left({\displaystyle \frac{1}{N}}\sum _{i=1}^N{x}_i,{\displaystyle \frac{1}{N}}\sum _{i=1}^N{y}_i\right),$$

(5)

$${\mathsf{\Sigma }}_{xy}=\left[\begin{array}{cc}cov(X,X)& cov(X,Y)\\ cov(Y,X)& cov(Y,Y)\end{array}\right],$$

(6)

where X represents the set of values derived from the X coordinates of the points in the cluster minus $\mu X$, and Y represents the set of values derived from the Y coordinates of the points in the cluster minus $\mu Y$.

The eigenvalues and eigenvectors of the covariance matrix ${\mathsf{\Sigma }}_{xy}$ can be determined using QR decomposition, the Jacobi method, etc. Once solved, the eigenvector ${\mathit{\nu }}_{max}$ corresponding to the largest eigenvalue ${\lambda }_{max}$ represents the direction of the major axis of the Gaussian block, while the eigenvector ${\mathit{\nu }}_{min}$ associated with the smallest eigenvalue ${\lambda }_{min}$ indicates the direction of the minor axis, as shown in Figure 3.

Solving for angles $\theta$ and intercepts b

$$k={\displaystyle \frac{{\mathit{\nu }}_{ma{x}_y}}{{\mathit{\nu }}_{ma{x}_x}}},b=\mu Y-k\mu X,\theta =arctan\left(k\right).$$

(7)

Eventually, the clusters derived from the clustering process are further refined into Gaussian blocks, each endowed with relevant parameter attributes. This facilitates the establishment of relationships between neighbors. Figure 2 illustrates the shapes of Gaussian blocks generated for each cluster. It demonstrates that our algorithm is adaptable to various feature shapes for generating Gaussian blocks.

3.1.2. Gaussian Descriptors and Matching

In the pursuit of robust feature representation in image motion, especially with the challenge of rotation invariance, we introduce a novel approach leveraging Gaussian descriptors and graph theory. Our method aims to encapsulate the spatial relationships between feature blocks through the construction of an undirected graph G. Maximal cliques within this graph serve as a stable structural framework to describe the complex relationships among adjacent feature blocks. These cliques, and their associated descriptors, are crucial for our method as they allow for a detailed characterization of the spatial configuration of Gaussian blocks.

After generating the Gaussian blocks, we construct an undirected graph G and define $G=(V,E)$, where $V=\{G{A}_1,G{A}_2,G{A}_3,\dots ,G{A}_n\}$ is a set of vertices representing the centers of the Gaussian blocks. $E=\left\{E_1,E_2,E_3,\dots E_m\right\}$ is a set of edges in the graph, with each Gaussian vertex connected by edges to describe the direct relationships between Gaussian blocks. The attributes associated with the edges are defined as $E_i=\left\{{\theta }_{e1},{\theta }_{e2},{\rho }_e,G{A}_{source},G{A}_{target}\right\}$, where ${\theta }_{e1}$ is the angle between the positive direction of the major axis of the target Gaussian cloud and that of the source Gaussian cloud, and ${\theta }_{e2}$ is the angle between the positive direction of the major axis of the source Gaussian cloud and that of the target Gaussian cloud. These angles are obtained by substituting characteristic information of the two Gaussian blocks into the following equation, with the range of ${\theta }_e$ limited to $(0,2\pi ]$:

$${\theta }_{e1}=arctan\left(\mu Y_{\mathrm{target}}-\mu Y_{\mathrm{source}},\mu X_{\mathrm{target}}-\mu X_{\mathrm{source}}\right)-{\theta }_{source},$$

(8)

$${\theta }_{e2}=arctan\left(\mu Y_{\mathrm{source}}-\mu Y_{\mathrm{target}},\mu X_{\mathrm{source}}-\mu X_{\mathrm{target}}\right)-{\theta }_{target}.$$

(9)

To address the issue of quadrant unification during the actual solution process, we use the coordinate transformation method, as illustrated in the following formulas:

$$X_{q1}=\mu X_{\mathrm{target}}-\mu X_{\mathrm{source}},Y_{q1}=\mu Y_{\mathrm{target}}-\mu Y_{\mathrm{source}},$$

(10)

$$\left[\begin{array}{c}{X}_{p1}\\ {\mathrm{Y}}_{p1}\end{array}\right]={\left[\begin{array}{cc}cos\left({\theta }_{source}\right)& -sin\left({\theta }_{source}\right)\\ sin\left({\theta }_{source}\right)& cos\left({\theta }_{source}\right)\end{array}\right]}^T\left[\begin{array}{c}{X}_{q1}\\ Y_{q1}\end{array}\right].$$

(11)

Furthermore, we obtain ${\theta }_{e1}$ from Equation (12). If ${\theta }_{e1}<0$, we adjust ${\theta }_{e1}={\theta }_{e1}+2\pi$, ensuring that the coordinates are unified into the interval $(0,2\pi ]$. The method for obtaining ${\theta }_{e2}$ is analogous to that for ${\theta }_{e1}$, with details omitted for the sake of brevity.

$${\theta }_{e1}=arctan\left({\displaystyle \frac{Y_{p1}}{X_{p1}}}\right).$$

(12)

In addition, in the edge constraints, ${\rho }_e$ represents the Euclidean distance between two Gaussian center points, calculated using the following equation:

$${\rho }_e=\sqrt{{\left(\mu X_{source}-\mu X_{target}\right)}^2+{\left(\mu Y_{source}-\mu Y_{target}\right)}^2}.$$

(13)

To construct an effective graph, we implement a criterion for establishing constraint edges: an edge is created between two Gaussian blocks if the Euclidean distance ${\rho }_e$ between their centers is below a predefined threshold $R_{max}$. An increase in the value of $R_{max}$ results in a more densely connected graph. This setting is conducive to establishing a Gaussian descriptor that quantifies the adjacency relationships between Gaussian blocks, as illustrated in Figure 4.

Our method is designed to describe the relationship between adjacent feature blocks, addressing the challenge of rotation invariance during image motion. This is achieved by leveraging the angle ${\theta }_e$ stored in the edge between two vertices, which depends on the relationship between the Gaussian block and the edge. When the vertex corresponds to the source Gaussian block ($G{A}_{source}$), the angle is denoted as ${\theta }_{e1}$, and when it corresponds to the target Gaussian block ($G{A}_{target}$), the angle is denoted as ${\theta }_{e2}$. Stated differently, provided the surrounding environment remains static and maintains its shape, the values of ${\theta }_e$ and ${\rho }_e$ for adjacent Gaussian blocks will remain constant, irrespective of changes in time and the robot’s observational perspective. Figure 4 illustrates the effect of different $R_{max}$ values on graph density, demonstrating that an increase results in denser Gaussian descriptor generation. However, this increased density corresponds to a longer relative computation time. Empirical tests have indicated that a setting of $R_{max}=1.0$ yields optimal results.

We constructed a graph containing an image’s Gaussian information and adjacency relationships through the above process. Then, we further optimize the filtering descriptors by searching for the maximal clique [29], which aids in establishing subsequent matching constraints. Given an undirected graph $G=\left(V,E\right)$, a subset $C=\left(V^{\prime },E^{\prime }\right)$ of G is considered a clique. A maximal clique is a clique that is not a proper subset of any larger clique within the graph. This means no additional vertex from G can be incorporated to form a larger clique. Specifically, when a maximal clique contains the largest number of vertices among all maximal cliques in the graph, it is referred to as a maximum clique.

We employ the recursive Bron–Kerbosch algorithm [30] to identify maximal cliques. Our experiments reveal that a clique with three nodes yields the most stable cluster structure. When the number of nodes in a maximal clique is too small, such as two, the clique is insufficient to adequately describe the adjacency relationships among Gaussian blocks, leading to potential similarities that cannot be effectively distinguished. Conversely, identifying maximal cliques may become difficult when the number of nodes reaches four. This is attributed to the disappearance of some obstacles from the robot’s field of view during its movement, making it unsuitable for environments with sparse features. We seek a stable structure to address this issue by targeting a maximal clique with exactly three nodes. Figure 4 illustrates a schematic of the identified maximal clique.

Following the search for the maximal clique, we utilize it as a reference to extract the vertices it contains and to retrieve the associated edge information. Both the edge length ${\rho }_e$ and the angle ${\theta }_e$ are encapsulated within the descriptor to characterize the maximal clique. The descriptor for the i-th vertex within the maximal clique is formalized in the following equation, where the data in descriptor ${\mathbf{descriptor}}_i$ are sorted in ascending order of angles.

$${\mathbf{descriptor}}_i=\left\{[{\rho }_1,{\theta }_1],[{\rho }_2,{\theta }_2],\dots ,[{\rho }_m,{\theta }_m]\right\}.$$

(14)

The obtained ${\mathbf{descriptor}}_1,{\mathbf{descriptor}}_2,\dots ,{\mathbf{descriptor}}_m$ are then assigned to the corresponding attributes of $Verte{x}_i$ to facilitate indexing the Gaussian center point and stored in $\mathbf{descriptors}$. We propose an algorithm to align two sets of descriptors, which we call the GM (Graph Matching) algorithm.

Through the process above, we obtain the descriptors of the corresponding Gaussian blocks for the two frames. Next, we apply matching constraints to these descriptors: during the traversal of the i-th descriptor ${\mathbf{descriptor}}_{1,i}$ of $\mathbf{descriptors}\mathbf{1}$ in $frame1$ and the j-th descriptor ${\mathbf{descriptor}}_{2,j}$ of descriptors $\mathbf{descriptors}\mathbf{2}$ in $frame2$ [31], we define the similarity score, the index of the best matching vertex $best\_match$, and the optimal distance $d_{best}$. Using a brute-force descriptor matching approach [32], the corresponding vertex $Verte{x}_j$ of the $Verte{x}_i$ is found in turn. The specific algorithm procedure is depicted in Algorithm 1.

Initially, we employ Algorithm 2, a similarity algorithm, to calculate the similarity score between the descriptors ${\mathbf{descriptor}}_i$ and ${\mathbf{descriptor}}_j$ of the two vertices using Formula (15), where m and n represent the m-th and n-th pieces of data in ${\mathbf{descriptor}}_i$ and ${\mathbf{descriptor}}_j$, respectively. By adjusting the given angular threshold ${\theta }_{diff}$ and distance threshold ${\rho }_{diff}$, we effectively control the adjacency relationship between Gaussian blocks and mitigate the uncertainties associated with them. After calculating the similarity score, the optimal pairing vertex is identified using Algorithm 1. $d_{pos}$ is defined as the Euclidean distance threshold of the matched vertices. To enhance robustness, we adjust $d_{pos}$ by the robot’s movement velocity, which influences the keyframe distance generation conditions on the trajectory calculation side, as depicted in Formula (16). This adjustment ensures consistency, preventing matched points from diverging beyond the distance of the dead reckoning keyframe motion. Through these steps, a set of preliminary matching points is obtained. Subsequently, the ICP algorithm [33] is employed to refine this set, resulting in the predictive transformation matrix and addressing the issue of the ICP algorithm converging to a local optimum.

$$\left|{\theta }_{i,m}-{\theta }_{j,n}\right|\le {\theta }_{diff},\left|{\rho }_{i,m}-{\rho }_{j,n}\right|<{\rho }_{diff},$$

(15)

$$\sqrt{{\left(x_i-x_j\right)}^2+{\left(y_i-y_j\right)}^2}\le {\xi }_{pas}.$$

(16)

Algorithm 1 Matching constraint algorithm

Require: Descriptor set for two pictures $\mathbf{descriptors}\mathbf{1}$ and $\mathbf{descriptors}\mathbf{2}$ and abnormal distance threshold $d_{pos}$   1: $\mathbf{descriptors}\mathbf{1}=\left\{\mathbf{descriptor}\mathbf{1}\_\mathbf{1},\mathbf{descriptor}\mathbf{1}\_\mathbf{2},\dots ,\mathbf{descriptor}\mathbf{1}\_\mathbf{m}\right\}$, $\mathbf{descriptors}\mathbf{2}=\left\{\mathbf{descriptor}\mathbf{2}\_\mathbf{1},\mathbf{descriptor}\mathbf{2}\_\mathbf{2},\dots ,\mathbf{descriptor}\mathbf{2}\_\mathbf{n}\right\}$   2: $\mathbf{connections}=\varnothing$   3: for every $\mathbf{descriptor}\mathbf{1}\_\mathbf{i}$ in $\mathbf{descriptors}\mathbf{1}$ /*i $\in [1,m]$*/   4:        $max\_score\leftarrow -1$   5:        $best\_match\leftarrow -1$   6:        $d_{best}\leftarrow -1$   7:        for every $\mathbf{descriptor}\mathbf{2}\_\mathbf{j}$ in $\mathbf{descriptors}\mathbf{2}$ /*j $\in [1,n]$*/   8:        /*Calculate the similarity between two descriptors*/   9:              $score\leftarrow calculate\_score$ 10:                  if $score>max\_score$ then 11:                        /*Calculate the similarity between two descriptors*/ 12:                        $d^{*}\leftarrow distance(\mathbf{descriptor}\mathbf{1}\_\mathbf{i}, \mathbf{descriptor}\mathbf{2}\_\mathbf{j})$ 13:                        if $d^{*}\le d_{pos}$ then 14:                             $max\_score\leftarrow score$ 15:                             $best\_match\leftarrow j$ 16:                             $d_{best}\leftarrow d^{*}$ 17:                        end if 18:                   else if $score==max\_score$ then 19:                          if $d^{*}<d_{best}\mathbf{and}{d}^{*}<d_{pos}$ then 20:                              $best\_match\leftarrow j$ 21:                              $d_{best}\leftarrow d^{*}$ 22:                          end if 23:                   end if 24:        end for 25:        if $best\_match\ne -1$ then 26:              $\mathbf{connections}\leftarrow \mathbf{connections}\cup \left(best\_match\right)$ 27:        end if 28: end for 29: $\mathbf{connections}\leftarrow remove\_duplicate\_point\_pairs$ 30: return$\mathbf{connections}$

Algorithm 2 Similarity scoring algorithm

Require: Two descriptors $\mathbf{descriptor}\mathbf{1}\_\mathbf{i}=\left\{[{\rho }_{i,1},{\theta }_{i,1}],[{\rho }_{i,2},{\theta }_{i,2}],\cdots ,[{\rho }_{i,p},{\theta }_{i,p}]\right\}$ and $\mathbf{descriptor}\mathbf{2}\_\mathbf{j}=\left\{[{\rho }_{j,1},{\theta }_{j,1}],[{\rho }_{j,2},{\theta }_{j,2}],\cdots ,[{\rho }_{j,q},{\theta }_{j,q}]\right\}$, angle threshold ${\theta }_{diff}$, score distance threshold ${\rho }_{diff}$   1: $score\leftarrow 0$   2: $m,n\leftarrow 0$   3: /*m $\in [1,p]$ , n $\in [1,q]$*/   4: while $m<len(\mathbf{descriptor}\mathbf{1}\_\mathbf{i})$ and $n<len(\mathbf{descriptor}\mathbf{2}\_\mathbf{j})$ do   5:       /*calculate score*/   6:       ${\theta }^{*}\leftarrow \left|{\theta }_{i,m}-{\theta }_{j,n}\right|$   7:       ${\rho }^{*}\leftarrow \left|{\rho }_{i,m}-{\rho }_{j,n}\right|$   8:       if ${\theta }^{*}\le {\theta }_{diff}$ then   9:             if ${\rho }^{*}<{\rho }_{diff}$ then 10:                  $score\leftarrow score+1$ 11:             end if 12:             $m\leftarrow m+1$, $n\leftarrow n+1$ 13:       else if ${\theta }_{i,m}<{\theta }_{j,n}$ then 14:             $m\leftarrow m+1$ 15:       else 16:             $n\leftarrow n+1$ 17:       end if 18: end while 19: return score

3.2. Parallel Inter-Frame Registration Algorithm Using AKAZE and GM Methods

When the environmental feature comprises a long straight line and the robot’s trajectory is parallel to this line, both the GM and ICP algorithms may degenerate and fail. In such scenarios, salient echo points typically emerge at the endpoints of the linear feature, which can be effectively tracked by the AKAZE (Accelerated-KAZE) algorithm [25]. The AKAZE algorithm is proficient in feature detection and description, leveraging the FED (Fast Explicit Diffusion) method to accelerate scale space construction and employing the M-LDB (Modified Local Difference Binary) paradigm to generate descriptors. When the robot’s trajectory is perpendicular to the environmental feature line, the AKAZE algorithm exhibits robust performance and high precision in feature tracking and matching. However, it encounters difficulties matching when the motion involves rotation [34].

The final inter-frame registration process is as follows: after the front end obtains the critical frame information, denoted as $frame$, we initialize the waiting point cloud and set the current frame’s point cloud to ${\mathbf{P}}_s$ and the previous frame’s point cloud to ${\mathbf{P}}_t$. Following initialization, we employ the GM and AKAZE algorithms to extract effective sparse points. We then apply the ICP algorithm for registration estimation on these sparse points and calculate the corresponding reprojection errors ${\epsilon }_{GM}$ and ${\epsilon }_{AKAZE}$ for the transformation matrices ${\mathbf{T}}_{GM}$ and ${\mathbf{T}}_{AKAZE}$, respectively. We select the transformation matrix associated with the smaller error and integrate it into the dense point clouds ${\mathbf{P}}_s$ and ${\mathbf{P}}_t$ for further ICP refinement, obtaining an accurate solution ${\widehat{\mathbf{x}}}_{ts}$.

Descriptor matching in the traditional AKAZE algorithm is performed using the brute force method, with the Hamming distance as a similarity metric, and a global search is conducted to identify the closest descriptor. However, the effectiveness of this approach is diminished when dealing with underwater sonar imagery, which is prone to multipath echoes and higher noise levels. We propose a method to eliminate false matching points to further improve the accuracy of matching. We sort the distance $d_i$ between the matching points from small to large, record it as $d_i=\left[d_1,d_2,\dots ,d_n\right]$, and calculate it using the following formula, thereby eliminating mismatches caused by similar features due to sonar reverberation. In the formula, k is the proportion coefficient, used to eliminate data with low confidence. The scaling factor $\xi$ controls the extent of the culling space.

$$\overline{d}={\displaystyle \frac{1}{(1-2k)n}}{\sum }_{kn+1}^{(1-k)n}{d}_i,$$

(17)

$$d_i<\xi \overline{d}.$$

(18)

4. SLAM System

Let ${\mathbf{x}}_i$ represent the state of the robot at the i-th timestamp, where $i\in \left\{0,\cdots ,N\right\}$. The robot state ${\mathbf{x}}_i\triangleq \left[\begin{array}{cc}{\mathbf{R}}_i& {\mathbf{t}}_i\end{array}\right]\in \mathrm{SE}\left(2\right)$, where ${\mathbf{R}}_i\in \mathrm{SO}\left(2\right)$ represents the direction of the robot at the i-th timestamp and ${\mathbf{t}}_i\in {\mathbb{R}}^2$ represents its position [35]. To embed the environmental map into the state space, we depict an entity, such as a point cloud or salient image features, as a two-dimensional point feature. This feature is designated as ${\mathbf{l}}_j\in {\mathbb{R}}^2$, where $j\in \{1,\dots ,M\}$. Then, the measurement result can be represented by ${\mathbf{z}}_k$, where $k\in \{1,\dots ,K\}$, and the landmark ${\mathbf{l}}_{jk}$ observed in the state of ${\mathbf{x}}_{ik}$ during the measurement process usually corresponds to a set of paired associations $\left({\mathbf{i}}_j,{\mathbf{i}}_k\right)$.

The objective of SLAM is to find the robot’s trajectory $\mathbf{X}={\mathbf{x}}_i$, the position of the landmark map, and the landmark map $\mathbf{L}=\left\{{\mathbf{l}}_j\right\}$ based on a set of data from the measurement set $\mathbf{Z}=\left\{{\mathbf{z}}_k\right\}$.

4.1. Sonar’s Field of View

The multibeam sonar M1200d, as utilized in this research, encompasses a three-dimensional spatial field of view for the sonar sensor. Given that the horizontal beam angle is $\theta$, the elevation angle is $\phi$, and the effective detection radius is $\mathit{r}$, as shown in Figure 5a. Imaging sonar transforms the observed three-dimensional space into a two-dimensional plane for visualization, as shown in Figure 5b.

4.2. Underwater Robot Motion Estimation

We harness an Inertial Measurement Unit (IMU), a Doppler Velocity Log (DVL), and a depth meter—three pivotal sensors that contribute to estimating the robot’s underwater motion posture. These three sensors are used to estimate the motion posture of the underwater robot, providing initial value constraints, which serve as tight coupling.

4.3. Factor Graph and Mapping

In the front-end calculations, pose estimation and mapping use the relationship between adjacent frames. However, relying solely on local constraints in a chained algorithm can lead to error accumulation and drift. Back-end optimization selects keyframes globally to mitigate this issue, establishing constraints across a larger spatial and temporal scale. This enables us to utilize the factor graph [31] from GTSAM for back-end optimization [36].

As shown in Figure 6, each keyframe’s pose is denoted by a black square and linked to feature points in the sonar imagery. The factor graph includes Sequential Scan Matching (SSM) and Non-Sequential Scan Matching (NSSM) factors. Sequential factors represent the incremental transformation relative to the previous pose, leading to error accumulation. On the contrary, non-sequential factors act as loop closures, connecting two separate poses to correct accumulated drift.

The fundamental principle of the factor graph is to estimate the maximum a posteriori (MAP) probability of the robot’s trajectory and the state of its surrounding environment based on dead reckoning and various sensor measurements [37]. This estimation is typically formulated as follows:

$${\mathbf{X}}^{*}=arg\underset{\mathbf{X},\mathbf{M}}{max}P(\mathbf{X},\mathbf{M}|\mathbf{U},\mathbf{Z}),$$

(19)

where $\mathbf{U}$ signifies the robot’s control input, $\mathbf{Z}$ represents the measurement information, $\mathbf{X}$ denotes the robot’s posture, and $\mathbf{M}$ indicates the position of the observation point.

We assume that the SLAM system’s system model and observation model follow a Gaussian noise distribution and take into account the scaling factor, while also enhancing the fault tolerance of the SLAM system by incorporating switch variables in the nonlinear iterative process. Furthermore, by assigning weight values to loop closures, the system’s maximum a posteriori (MAP) probability estimation can be formulated as follows:

$$\begin{array}{cc}\hfill {\mathbf{X}}^{*},{\mathbf{S}}^{*}=arg\underset{\mathbf{X},\mathbf{S}}{min}& \sum _i{∥f({\mathbf{x}}_i,{\mathbf{u}}_i)-{\mathbf{x}}_{i+1}∥}_{{\mathsf{\Sigma }}_i}^2+\sum _{ij}{∥\psi \left({\mathbf{S}}_{ij}\right)·\left[f({\mathbf{x}}_i,{\mathbf{u}}_{ij})-{\mathbf{x}}_j\right]∥}_{{\mathsf{\Lambda }}_{ij}}^2+\sum _{ij}{∥{\mathbf{r}}_{ij}-{\mathbf{s}}_{ij}∥}_{{\mathsf{\Sigma }}_{ij}}^2,\hfill \end{array}$$

(20)

where ${\mathbf{X}}^{*}$ denotes the optimal estimate of the robot’s trajectory; ${\mathbf{S}}^{*}$ represents the optimal estimate for the switch variables; $\mathbf{X}$ refers to the trajectory of the robot; $\mathbf{S}$ symbolizes the switch variables that determine whether specific loop closure constraints are activated; $f({\mathbf{x}}_i;{\mathbf{u}}_i)$ is the motion model function that predicts the next state of the robot based on its current state ${\mathbf{x}}_i$ and control input ${\mathbf{u}}_i$ at timestamp i; ${\mathbf{x}}_{i+1}$ denotes the actual state of the robot at the subsequent timestamp; ${\mathsf{\Sigma }}_i$ represents the covariance matrix of the motion model, which characterizes the uncertainty in the predictions made by the motion model; $\psi \left({\mathbf{S}}_{ij}\right)$ is the function of the switch variable ${\mathbf{S}}_{ij}$ that adjusts the weight of the loop closure constraint; ${\mathbf{u}}_{ij}$ is the sequence of control inputs from timestamp i to timestamp j; ${\mathsf{\Lambda }}_{ij}$ represents the covariance matrix associated with the loop closure constraint, which describes the uncertainty of the loop closure; ${\mathbf{r}}_{ij}$ refers to the residual of the loop closure constraint; ${\mathbf{s}}_{ij}$ denotes the actual residual corresponding to the switch variable for the loop closure constraint; and ${\mathsf{\Sigma }}_{ij}$ represents the covariance matrix for the loop closure constraint.

5. Results

5.1. Underwater Platform and Datasets

We employ the BlueRov2 robot as the primary motion carrier, integrating the Oculus M1200d multibeam sonar, the DVL-a50 Doppler Velocity Log, and an IMU into its hardware suite, as shown in Figure 7a. Communication is enabled through a mini switch and a Raspberry Pi, with signals relayed to a distant PC via a carrier module. The multibeam sonar functions in a high-frequency mode at 2.1 MHz, offering a maximum field of view of 10 m, a horizontal aperture of 80°, a vertical aperture of 12°, and a maximum update frequency of 40 Hz. It boasts a range resolution of 0.007 m and an angular resolution of 0.4°. These sensor characteristics are detailed in

Table 1. Sensor parameter characteristics.

Sensor Type	Model	Frequency	Features
Sonar	Oculus M1200d	12.6 Hz	High resolution, high update rate
Doppler Velocimeter	A50	10 Hz	Minimal size, max speed 2 m/s, ±0.1 cm/s accuracy
Inertial Measurement Unit	BMI055	9.5 Hz	High stability
Motion Capture System	NOKOV	171 Hz	2048 × 2048 resolution, 4.2 M pixels, 5.2 ms latency, 52° × 52° FOV

. Sensor data are aggregated for seamless communication using the Robot Operating System (ROS). This enables the visualization of sonar raw data and image data, as well as the DVL data visualization module, as shown in Figure 7b. This setup is crucial for subsequent validation with the SLAM system.

As shown in Figure 8a, the indoor environmental dataset was collected within a pool, which measures approximately 21 square meters with dimensions of 3.5 m by 8 m by 1.5 m. To enhance the environmental complexity, large water buckets were strategically positioned as underwater obstacles, each measuring 0.31 m by 0.295 m by 0.45 m. An optical positioning tracking system, equipped with 11 optical cameras, was deployed in the experimental setup to accurately capture the robot’s motion trajectory. This setup facilitates error analysis by comparing the captured trajectory with that estimated by the SLAM algorithm. Additionally, a wide-angle camera was installed to monitor the robot’s motion, which is crucial for investigating the factors influencing the quality of sonar images for future enhancements. The sonar’s detection range was configured to 4 m with a gain compensation set at 15. A PID algorithm was implemented to govern the robot’s low-speed movement. To ensure planar motion, the robot received z-depth data from the motion capture system and utilized real-time feedback to maintain its movement at a constant height of 0.55 m above the water surface, enabling it to complete a full circuit.

As shown in Figure 8b, the outdoor environmental dataset was collected in the Dasha River, located within the University Town of Nanshan District, Shenzhen City. The experimental site featured a water depth of 2 m, abundant silt and sand, aquatic life, small harbors along the riverbanks, and various obstacles, including bridge piers, effectively simulating the natural environment for underwater robots. The robot’s buoyancy was adjusted using weight blocks throughout the experiment, and its movement was carefully controlled to maintain a consistent depth. The sonar angle was fine-tuned to face slightly downward to enhance detection capabilities. The robot was maneuvered in a counterclockwise circuit within the river to acquire the dataset.

5.2. Inter-Frame Registration Results

5.2.1. GM Results

After obtaining multiple clusters through clustering, the clusters are further generated as Gaussian blocks, each endowed with relevant parameter attributes to facilitate the establishment of adjacency relationships in the subsequent section. These Gaussian blocks are designed to capture the local geometric properties of the point cloud data, which are crucial for accurate feature matching and pose estimation. Figure 9 shows the shapes of the Gaussian blocks generated by each cluster, which can be seen to adapt to various shapes of features for generating Gaussian blocks. The adaptability of these blocks is a key aspect of our method, as it allows for a more robust representation of the underlying geometry, which is essential for handling complex scenes and varying environmental conditions.

After acquiring the descriptors of the two images $\mathbf{descriptors}\mathbf{2}$ and $\mathbf{descriptors}\mathbf{1}$ [31], the GM algorithm is employed for descriptor registration. Figure 10 illustrates the matching outcome between the source and target point cloud of the fifth frame, demonstrating high accuracy. This high accuracy is a result of the robust Gaussian block representation and the effective descriptor-matching strategy employed by our GM algorithm. The Gaussian blocks are effectively aligned, providing initial points for calculating the predicted transformation matrix $T_0$. The dense point cloud and the expected matrix are then substituted into the ICP for resolution. Figure 11a indicates that the traditional ICP algorithm encounters convergence issues by the 26th iteration, settling into a local minimum with a final error of 0.0623 m. This error is specifically related to the registration accuracy between the two point clouds. Figure 11b displays the result after ICP iterations, where the point clouds are nearly superimposed with an error of 0.036 m, achieving convergence around the 29th iteration. This suggests that the enhanced algorithm effectively addresses the issue of ICP getting stuck in local optima and failing to converge, thereby strengthening the system’s pose estimation accuracy.

5.2.2. AKAZE Results

In a monotonous environment with sparse sonar target features, the quality of adjacency relationship information is compromised, resulting in the generation of descriptor information that is not viable for use, as illustrated in Figure 12. This, consequently, causes the failure of the ICP solution. Moreover, the direct application of the ICP algorithm is typically ineffective in linear environments. Therefore, we employ the AKAZE algorithm to extract features from long, straight-line segments. As depicted in Figure 13b, target point clouds and registered source point clouds are nearly superimposed, indicating successful feature matching. Consequently, employing a frame-by-frame matching approach that leverages both the AKAZE and GM algorithms in tandem effectively addresses sonar feature matching. This method is adept at handling scenarios involving rotation and long, straight lines, facilitating the precise estimation of the robot’s motion state. The complementary nature of these algorithms compensates for the limitations inherent in each.

Figure 14 presents a comparative analysis of the traditional versus the improved culling method at the 37th frame. The prevalent engineering approach for eliminating incorrect matches is two times the Hamming distance method, typically utilized in terrestrial visual matching. However, underwater sonar imagery is subject to multipath echoes and higher noise levels, which render this method less effective, as shown in Figure 14a. Building upon this, our approach further refines the elimination of mismatches induced by echoes, thereby enhancing the registration precision, as shown in Figure 14b. The improved method is observed to effectively eliminate mismatches due to sonar echo characteristics, leading to a marked enhancement in registration accuracy and robustness.

5.3. SLAM Results

5.3.1. Indoor Environment Test Results

The SLAM algorithm presented in this work is denoted as GM SLAM. Additionally, we compare it with other state-of-the-art methods, e.g., Bruce SLAM [38]. Figure 15a illustrates the indoor positioning and mapping outcomes achieved by the GM SLAM algorithm. Here, colored markers represent mapped obstacles, including the pool walls and water tank obstacles. The gray grid delineates the area recognized as unobstructed space. In the indoor test, as the robot navigates two circles around the pool, the mapping results closely mirror the actual environmental layout, despite some observable positional drift. In contrast, as shown in Figure 15b, the Bruce SLAM algorithm’s performance in similar indoor environments is not as optimal, particularly during limited-range movements within complex or feature-rich settings. The GM SLAM demonstrates a superior ability to delineate the environment, offering enhanced positioning accuracy and robustness. Conversely, the Bruce SLAM’s frame-matching capabilities tend to be suboptimal, often failing to converge effectively and exhibiting a propensity for positional drift, leading to less precise mapping and localization outcomes.

Figure 16a presents a comparison between the predicted trajectory generated by GM SLAM, the trajectory produced by the existing Bruce SLAM algorithm, and the actual trajectory obtained from motion capture data. Utilizing the evo tool [39], the trajectories were aligned for a thorough analysis. The analysis reveals that GM SLAM experiences fewer frame-matching failures and exhibits superior robustness in complex environments. Bruce SLAM, however, shows more drift, indicating feature tracking issues and a higher rate of incorrect frame matches, which underscores the higher positioning accuracy of GM SLAM. Figure 16b presents an error analysis for the x, y, and z axes. The operational height of the robot is set at approximately 0.58 m, and the motion capture system boasts a measurement accuracy of 0.02 m. Given the assumption of horizontal plane movement, maintaining the robot at a consistent depth is challenging, resulting in deviations along the z-axis. These deviations, in turn, impact the accuracy of the x and y directions in SLAM.

We compared the positioning trajectories of GM SLAM and Bruce SLAM with the true trajectory, using Equation (21) to calculate the Absolute Pose Error (APE) and Equation (22) to calculate the Root Mean Square Error (RMSE) for analysis [40]. The smaller the RMSE value, the better the fit between the observation model and the actual observations, indicating that the SLAM algorithm’s estimated pose is more accurate.

$$APE=\sqrt{1/N\sum _{i=1}^N{\Vert \mathbf{trans}\left({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{\mathrm{est},i}\right)\Vert }_2^2},$$

(21)

where N represents the total number of observation points, ${\mathbf{T}}_{gt,i}$ denotes the true pose transformation matrix for the i-th observation point, and ${\mathbf{T}}_{\mathrm{est},i}$ refers to its estimated pose transformation matrix.

$$RMSE=\sqrt{{\displaystyle \frac{1}{n}}\sum _{i=1}^n{(y_i-{\widehat{y}}_i)}^2},$$

(22)

where $y_i$ represents the observed value for each sample and ${\widehat{y}}_i$ represents the actual true value.

The Absolute Pose Error (APE) calculation results are detailed in

Table 2. APE calculation results.

	GM SLAM/m	Bruce SLAM/m
max	0.360	0.944
mean	0.118	0.189
median	0.112	0.143
min	0.014	0.007
rmse	0.132	0.247
sse	9.559	33.796
std	0.059	0.159

, with Figure 17 illustrating the corresponding results for the estimated motion trajectories compared to the true values. Figure 17a illustrates that the main robot errors in GM SLAM occur during turning sections, which require further improvement. By comparing the trajectory errors of the three, it can be seen that the average error (mean) of the predicted trajectory of GM SLAM compared to the true trajectory is 0.118 m, with a root mean square error (rmse) of 0.132 m. In contrast, as shown in Figure 17b, the average error (mean) of the trajectory predicted by the Bruce SLAM algorithm is 0.189 m, with a root mean square error (rmse) of 0.247 m, both of which are greater than the errors of GM SLAM, indicating that the positioning accuracy of the GM SLAM algorithm is superior to that of the Bruce SLAM algorithm. Moreover, the standard deviation (std) of the trajectory of GM SLAM is 0.059 m, while that of the Bruce SLAM algorithm is 0.159 m. The other error items of GM SLAM are also generally smaller than those of Bruce SLAM, further demonstrating that the GM SLAM positioning algorithm has higher robustness and is superior to the Bruce SLAM algorithm.

5.3.2. Outdoor Environment Test Results

The sonar-observed objects from the outdoor experiment are depicted in Figure 18a, with straight lines representing the walls along the shore, circular objects representing underwater bridge piers, and other bright spots representing sand and stones on the seabed. We controlled the robot to navigate counterclockwise within the pool to collect the dataset, verifying the positioning and mapping feasibility using the GM SLAM algorithm. Figure 18b illustrates the mapping process, demonstrating that the algorithm can effectively capture and represent the shape and position of actual walls and bridge piers in the map.

As an outdoor environment lacks a ground truth trajectory, this experiment focuses solely on comparing the mapping effects. Figure 19 presents a comparison between the point cloud mapping results of the GM SLAM and Bruce SLAM algorithms. Figure 19a illustrates that the mapping of GM SLAM closely approximates the actual environment, and the robot’s positioning trajectory aligns closely with the expected controlled path. The trajectory is optimized by incorporating non-sequential factors into the factor graph and is further refined during loop closure detection when the robot revisits known locations. Figure 19b depicts the Bruce SLAM’s mapping, revealing certain discrepancies after a circular traversal, attributed to error accumulation and erroneous frame matching. Comparative analysis indicates that the GM SLAM provides superior mapping quality and greater accuracy than the Bruce SLAM.

6. Discussion and Conclusions

In this paper, we present a robust keyframe-based SLAM framework specifically designed for shallow water environments, employing a multibeam sonar. To combat the propensity of ICP matching to fall into local optima during sonar feature matching, we have introduced a GM algorithm that utilizes Gaussian clustering and maximal clique algorithms, designed to calculate a predictive initial transformation matrix for ICP, effectively reducing the risk of being ensnared in local optima. Furthermore, for cases in which features are sparse, we have implemented an enhanced AKAZE algorithm that filters out erroneous match points and enhances matching precision. Experimental validation has confirmed that our algorithm effectively tackles the ICP local optima issue and exhibits robust performance against rotational motion. A comparative analysis with the Bruce SLAM algorithm highlights the superiority of our approach in terms of both positioning and mapping accuracy, achieving impressive results.

Future endeavors will focus on fully leveraging the textural information of sonar by transforming it into semantic data, as the current method only utilizes edge point information and overlooks the sonar’s textural content.