An Underwater Distributed SLAM Approach Based on Improved GMRBnB Framework

: Multi-vehicle collaborative mapping proves more efﬁcient in constructing maps in unfamiliar underwater environments in comparison to single-vehicle methods. One of the pivotal hurdles of Simultaneous Localization and Mapping (SLAM) with multiple underwater vehicles is map registration. Due to the inadequate characteristics of the underwater grid maps, matching map features poses a challenge, and outliers between maps add to the complexity. We propose an algorithm to solve this problem. This approach employs the Gaussian Mixture Robust Branch and Bound (GMRBnB) algorithm with an interior point ﬁltering technique. Feature point extraction, registration using the GMRBnB algorithm, inlier extraction based on density, and registration of the inlier are performed to obtain a more precise transformation matrix. The results of the simulation and experiments demonstrate that this technique heightens outlier tolerance and reinforces map registration accuracy. The proposed approach surpasses Iterative Closest Point (ICP) and Normal Distributions Transform (NDT) methods with respect to map registration quality.


Introduction
Autonomous Underwater Vehicles (AUVs) have become widely used in various applications including resource exploration [1], environmental surveillance [2], underwater rescue [3], and military operations [4].AUVs require accurate positioning and the ability to map underwater environments in unknown and challenging conditions to ensure safe and functional operation.AUVs can increase their autonomy in task performance by integrating Simultaneous Localization and Mapping technology (SLAM).SLAM algorithms enable AUVs to determine the location of random beacons for navigation purposes [5], to perform 3D surveys of the underwater environment using multibeam sonar to preserve volumetric data of the free space, and to perform planning tasks [6].Vision-based underwater SLAM estimates self-motion by extracting and matching features from successive images and backend position optimization [7,8].However, despite the affordable cost of cameras, vision-based underwater SLAM suffers from significant limitations.Cameras possess limited detection range and can solely be operated in well-lit, hygienic settings.In contrast, sonar emits sound waves in single or multiple directions and analyses the strength and return time of each echo to acquire information concerning the surrounding environment.A representative device is shown in Figure 1.Sonar-based techniques are currently prevalent in the development of underwater SLAM [9].
Underwater map creation is a crucial purpose of underwater vehicles, with tasks such as underwater docking or search necessitating an accurate and efficient mapping process.However, undertaking such tasks poses an array of challenges for a solitary vehicle in vast underwater environments.These include unstructured scenes, sensor error accumulation, and the inability to utilize GPS signals [10], hence, achieving high precision requirements represents a considerable challenge for a single vehicle.In recent years, researchers have emphasized the study of multi-vehicle collaborative mapping, which entails registering local maps generated by individual vehicles to construct a global map.Given the limitations of underwater communication, it is more practical to construct underwater maps offline and align multiple submaps to create a large-scale underwater map.Map registration methods can be classified into two main types: those with known initial poses and those with unknown initial poses.In the instance of unknown initial poses, map registering is achieved primarily through intersection, optimization, and feature matching.Intersection methods use sensor measurements during encounters to calculate the transformation relationships between local maps through optimization algorithms.However, achieving exactitude in both sensor measurements and observation angles is crucial for these methods.Optimization-based techniques chiefly rely on artificial intelligence algorithms such as Genetic Algorithms to scan for overlapping regions between maps.On the other hand, feature matching-based methods predominantly utilize conventional geometric features such as points and lines.The main objective pertains to the circumstances of distributed underwater SLAM scenarios using current forward-looking sonar technology.Within this framework, optimal precision of underwater map registration is of the utmost importance.Specifically, in situations where real-time restrictions are not crucial, the accent is laid on achieving heightened levels of precision in registering underwater cartographic representations.

Related Work
SLAM, which utilizes visual cameras as the primary sensor, has demonstrated impressive results under well-illuminated conditions.To extract line features from continuous sonar data streams in underwater environments, the Hough transform is a viable method [11].Additionally, bespoke feature extraction techniques can be devised to account for distortions arising from motion, continuous data streams, low scanning frequencies, and high noise levels [12].
Utilizing sonar for underwater SLAM provides significant advantages such as a wider range of applicability, longer detection distances, and it overcomes limitations associated with underwater optics [13].This is achieved by extracting environmental features to construct a 2D grid map, utilizing forward-looking sonar.Furthermore, the data from several sensors, including Inertial Measurement Unit (IMU) and Doppler Velocity Log (DVL) data, can be fused to estimate the AUVs orientation.Preprocessing methods such as beam segmentation, noise elimination, and threshold filtering can be used to make Mechanical Scanning Imaging Sonar (MSIS) data appear similar to laser data, thus allowing for the use of established 2D SLAM frameworks [14].A scan formation module that employs sliding windows can be used to input the created scans into a modified SLAM algorithm when dealing with slow-scanning sonars [15].To minimize errors, an Extended Kalman Filter (EKF) technique is used to reduce errors in the relative motion vectors presented in the robot's coordinate system.Sonar-Visual Inertial (SVIn) improves the robust initial method and incorporates depth measurements obtained from pressure sensors into tightly-coupled optimization formulations to address drift and localization loss.This enables the use of sonar data in the OKVIS framework [16].An improved version, SVIn2, utilizes acoustic range data to strengthen reconstruction and localization.It combines depth information for sturdy initialization, scale refinement, and helps mitigate drift through tightly-coupled integration [17].A neural network approach is taken in Ref. [18] for style transfer between sonar and image information, using Super Glue for acoustic feature matching.Calibration algorithms have been developed specifically for sonar and images to boost the precision of extrinsic calibration in low-light situations, resulting in enhanced robustness and localization accuracy [19].
Incorporating Kalman filtering and motion modellng is shown to yield smoother motion trajectories [20].The inclusion of other geographic information, such as magnetic field data, reduces the odometry position drift considerably [21].Ref. [22] presented a module and hierarchical approach to learning policies for exploring 3D environments, utilizing analytical path planners and a learned SLAM module, and employing both global and local policies.Multi-robot underwater SLAM presents significant challenges owing to the difficulty of underwater communication.This challenge makes it arduous for robots to exchange information.Consequently, special update strategies have been devised to collaboratively maintain a pose graph, transmitting the most relevant information within the constraints of communication bandwidth to optimize the quality of the collaborative map [23].Measurement results can be condensed into small-scale acoustic data packets, providing precise localization and mapping outcomes in acoustic channels of low bandwidth, high noise levels, and unreliability [24].The exchange of visual data between agents can be minimized by utilizing low-dimensional hashes, thereby curtailing data transmission time [25].To facilitate inter-robot loop closure, Swarm SLAM employs a novel priority ordering method that reduces communication [26].DRACo-SLAM attains resilience to erroneous loop closures and sustains low bandwidth demands for inter-robot communication by solely conveying scene descriptors between robots, employing the Pairwise Consistency Maximization (PCM) method [27].
In distributed SLAM, submap registration is a pivotal stage.Iterative Closest Point (ICP) proves unreliable in situations with deficient or degraded geometric features, which results in significant trajectory drift in graph optimization solutions.Given the uncertainty of the data in the underwater environment, a probabilistic implementation of ICP is utilized to align the data [28].For the feature registration problem, the first globally optimal algorithm for rigid (Euclidean) registration of two 3D point sets was proposed by Ref. [29].The 2D-3D point set registration problem is addressed in Ref. [30] through the use of a combined approach that integrates a global optimal rotation search and translational grid search.A branch-and-bound approach is used to investigate the solution space of 3D rigid motion.This led to achieving the global optimum for the 3D rigid Gaussian mixture alignment problem with L2 distance metric [31].Another proposed solution method, Gaussian Mixture Robust Branch and Bound (GMRBnB), proved to be globally optimal with improved accuracy and robustness.The Gaussian Mixture Model (GMM) is utilized to build a dependable objective function.In addition, a new relaxation boundary is established, and an approach is put forward to surmount the issue of local minima that plagues traditional optimization methods.The GMRBnB algorithm's sensitivity and dependence on initialization is also addressed.In this study, we utilize AUV to create grid maps of various regions using the SLAM algorithm.We subsequently register multiple grid maps, enhance the GMRBnB algorithm, and successfully accomplish the distributed creation of large-scale underwater maps.

Proposed Distributed SLAM Method
To achieve underwater distributed SLAM, this paper presents an enhanced algorithm based on GMRBnB that utilizes a single vehicle with multiple sensors for underwater SLAM.The collected data from various vehicles is then used for distributed SLAM to accomplish map construction of a vast underwater setting.The process of the algorithm is illustrated in Algorithm 1.The corresponding flow chart is shown in Figure 2.  If the density difference is significant, voxel downsampling must be applied.Next, the GMRBnB algorithm is employed to perform registration, followed by the use of the NNDE method for density calculation and detection of the inlier.The GMRBnB is again used to register the inlier to obtain a more precise transformation matrix.The final step involves registering the submaps.

Submap Construction
The process of submap construction utilizes the approach detailed in ref. [32] for the construction of underwater grid maps.Acquisition of sonar images is followed by their transformation into sparse point cloud data, using threshold segmentation and distanceconstrained filtering.Integration of data from DVL, IMU, and Mechanical Scanning Imaging Sonar (MFLS) sources is carried out to produce an accurate occupancy grid map.We set the grid resolution to 0.05 m.

Clustering Filtering and Density-Based Sampling for ORB Feature
The methodology is outlined in Algorithm 2. The ORB feature points are extracted and their corresponding GMM model is calculated.Employing voxel-based density sampling, a collection of evenly dispersed density points is acquired.

Algorithm 2: Density Uniformization for ORB Feature Point Extraction
Input: two submaps Output: ORB feature point set and its GMM model 1 Load the submaps and extract ORB feature points. 2 Cluster the feature points and remove the clear outlier.
3 Calculate the mean distance between the feature points.4 Choose a suitable voxel size based on the mean distance and create a KD-tree.5 Conduct nearest neighbor search to produce a voxel grid and carry out density sampling in each voxel.Randomly select one point from each voxel to serve as a density sampling point.6 Proceed to compute the GMM for the ORB feature point set.7 Return ORB feature points and its GMM model.

GMRBnB Registration
The BnB algorithm, a discrete global optimization framework, is implemented to tackle non-convex and NP-hard problems, including registration problems.It navigates the complete solution space in pursuit of theoretically guaranteed global optima.The algorithm's two guiding principles are recursive partitioning of the solution space through branching and bounding by calculating lower and upper bounds for optimal solutions in each confined sub-box.
The algorithm utilizes these boundaries to refine the solution space and remove subboxes that do not contain the optimal solution.Convergence occurs when the lower and upper limits for the optimal solution are adequately tight, reaching the predetermined threshold.The efficacy of this algorithm lies in the precise estimation of the lower and upper limits during branching.
Based on the registration problem in this paper, the parameterization of the solution space is as follows.The rotation solution space is and the partition strategy involves dividing the solution space along each dimension's center, resulting in 2 3 = 8 subsets after one partition.
For the two-dimensional transformation solution space, considering the center as the origin, the relaxation upper bound RU(B) and lower bound RL(B) of the objective function for the GMM corresponding to sets X and Y are derived.The convergence of these bounds is also proven in Ref. [33].
The probability density of observing a point x given a GMM is defined as is D(θ, t) with a rigid transformation function T(G, θ, t) to represent the rotation and translation.
After expansion, mainly the cross term changes, all terms except for the point-topoint residuals are easy to calculate, so the focus is to limit the point-to-point L2 residuals e L,X i ,Y j (θ, t).The derivation is given and the final residual relationship is obtained [33].
where α is the angle between a particular solution and the origin, β is the range of the rotation solution space, and ρ is the radius of the translation relaxation circle.
where e L,X i ,Y j (θ, t) is the lower bound of residual e X i ,Y j (θ, t).The maximum error in Formula ( 6) is represented by the value of any solution space, serving as an upper bound for calculation purposes.In contrast, Formula (7) denotes the lower bound for a feasible solution, indicating the minimum error value within the solution space.During the iterative process, the solution space undergoes branching and reduction, which leads to a gradual decrease in the solution space.Consequently, the lower bound error exhibits a gradual increase.Formulas ( 6) and (7) will eventually converge as the differences between them gradually decrease.Once the difference reaches a particular threshold or number of iterations, the calculation terminates.Algorithm 3 presents the pseudocode for GMRBnB Registration.

GMRBnB with NNDE for the Inlier
Due to the inaccuracy of sensor measurements, registering submaps is challenging.The presence of outlier or noise causes deviations in the registration results.In theory, the removal of the outlier could improve the registration effect.This paper uses NNDE for outlier removal.For the preliminary registration outcomes, we apply NNDE for estimating the density of the feature points.For each data point, we search for its closest neighbors in the feature space utilizing a KD tree.We compute the mean distance between the nearest neighbor points, normalize the density estimation values through max-min normalization, and classify the data points as inliers or outliers based on the density threshold.Data points with density estimation values exceeding the threshold are designated as inliers, while those falling below the threshold are deemed as outliers.We identify the associated points in the original point sets and implement registration to achieve enhanced alignment.Algorithm 4 presents the pseudocode for this method.extraction.Subsequently, experiments were carried out in an actual lake environment, with the consistency of the algorithm confirmed through quantitative analysis of distributed SLAM map construction results.
The algorithms were implemented using MATLAB 2017B, PyCharm 2021.3.3 (Professional Edition), and Python 3.7, operating on an Ubuntu 18.04 LTS system with ROS1.All experiments were conducted on a laptop equipped with an Intel(R) Core(TM) i5-7300HQ CPU @ 2.50 GHz and 24 GB of memory.

Convergence Validation
The range of convergence is impacted by the standard deviation σ of the Gaussian components that corresponds to the peak width of the Gaussian distribution, based on the characteristics of the objective function.Thus, after numerous trials and comparisons, the standard deviation σ of the Gaussian components was set to 0.1 in all experiments.For the GMRBnB experiments in this study, the convergence threshold ∈ was established at 0.01.
To exhibit the convergence of the suggested lower and upper limits, a duo of artificial measurement sets (with m = n = 50) served as input.The resultant convergence curve is illustrated in Figure 3, where the difference between the lower bound and upper bound is diminishing.With each iteration, the algorithm achieves the optimal solution.

Registration with Different Outlier and Noise Rates
In this section, the precision of the suggested algorithm is illustrated via four experiments employing synthetic data under varying conditions such as noise levels and outlier ratios.
The first measurement set is created by distributing m random points within a square [−1, 1] 2 .Random rotations within the [−π, π] range and random translations within the [−1, 1] 2 range are applied to the measurement set to obtain the corresponding transformed measurement set Y. To simulate outlier, additional points are randomly added.The measurement set is perturbed by random noise addition to imitate noise levels.We register the point sets Y and X to obtain the rotation and translation, and calculate the error between them and the actual rotation and translation.
The findings are depicted in

Registration with NNDE
The employment of a supplementary registration approach that eliminates anomalies is founded on a widely held view that outlier and noise can deeply affect the enrollment outcome of the GMRBnB algorithm.By eliminating certain anomalies and noise in advance, their interference can be lessened, which can result in more precise translation and rotation matrices and improved enrollment outcomes.To better align the experiments with realworld scenarios, we used the RPLIDAR S3 sensor from Slamtec Corporation for registration.The sensor operates at a scanning frequency of 10 Hz and has a ranging resolution of 10 millimeters.To introduce significant translational and rotational differences in the registration data, we selected data frames with a temporal separation of 10 frames.Due to the rising computational complexity resulting from expanding the number of features extracted for matching grid maps, and the exponential growth in computational load with the point count, we have decided to subsample the data from the LIDAR sensor.Please see Figure 5 for the illustrated results.

Analysis
The following conclusion was derived from the conducted experimentation: • As the outlier rate and noise level increase, the rotation error and translation error gradually increase.However, the algorithm still demonstrates accuracy with outlier and noise, as it converges; • Additionally, higher rates of outlier or noise levels adversely affect computation time due to their influence; • Eliminating the outlier through outlier filtering for the initial registration results can improve secondary registration results in some way.

Experiments in Real Environments
This section presents field experiments conducted to evaluate the proposed GMRBnB algorithm using inlier filtering and compare it with ICP and Normal Distributions Transform methods.The experiments were conducted at Lake Liquan in Shaanxi Province, China.The satellite map of the area is displayed, and the area is subdivided into three areas in Figure 6.Data is collected from each using a BlueRov in Figure 7.The sensor data is saved as a rosbag package, SLAM is performed on each sub-area, and a grid map is obtained.One AUV is used to simulate the collection of data by multiple AUVs in sub-areas.This approach avoids data errors due to differing AUV performance and sensor installation deviations, eliminates the requirement for sensor calibration, and reduces the cost and difficulty of experiments.During the experiment in Figure 7, a small watercraft was nav-igated towards the target environment where the BlueROV was controlled to conduct SLAM experiments underwater.The working frequency of m750d is selected as 1.2 MHz.The sensor possesses a maximum detection range of 40 m and a minimum detection range of 0.1 m, while the distance resolution stands at 2.5 mm.The sensor features a horizontal measurement angle of 130 • and a vertical measurement angle of 20 • .Figure 8 displays the results for the registration of the grid map.We mark the boundaries of the submaps with a red dotted box to highlight them.The white color indicates the passable area, while the grey and green shade represents the unknown area where it is uncertain whether the grid can be accessed.The black areas in the lower left and upper right corners of the map indicate missing data when registering the submaps.
The resemblance between the map registering the outcome and the map produced from an individual mapping procedure can be assessed using measures such as Mean Squared Error (MSE) and Structural Similarity (SSIM).MSE is employed to indicate differences between images.It computes the squared value of the mean difference between corresponding pixels in two images.The smaller the MSE value, the more similar the two images are.SSIM is utilized to evaluate the similarity between the structure and quality of the two images considering not only brightness and contrast but also the structural information.The value of SSIM is within the range of −1 to 1.The closer the value is to 1, the more similar the images are. Figure 9 illustrates the results, showing that the proposed method in this paper achieves higher SSIM values compared to ICP and NDT.Moreover, the MSE of our method is lower.Higher SSIM values indicate greater structural similarity between the reconstructed image and the ground truth, highlighting the accuracy and reliability of our method.Additionally, the lower MSE reinforces the effectiveness and precision of our method in minimizing reconstruction errors.These findings demonstrate that our method outperforms ICP and NDT in terms of these metrics.Multi-AUVs registration sequencing is an important issue as registration errors can accumulate.Our experiment demonstrated insignificant effects as only three maps were employed.However, if the AUV map registration encompasses more maps, sequential registration strategies ought to be deliberated to minimize the overall registration error of multiple maps.The proportion of overlap between AUV search regions is another critical aspect of multi-AUVs research, and a minimum level of overlap is necessary to guarantee reliable registration performance.We take consideration on this matter, yet have not come to a conclusion.The extent of overlap is linked to parameters such as the precision of single-vehicle SLAM, sensor functionality, and registration algorithm efficacy, and demands further quantitative analysis.

Conclusions
This study centers on the mapping of multi-vehicle distributed SLAM.The paper introduces a notion of mapping each vehicle individually in advance within a pre-determined search area and then combining these individual maps to generate a complete map while doing away with inter-vehicle communication.A 3-DOF based registration approach is suggested to tackle the point-set registration issue.The article outlines a distributed approach for constructing an underwater map, incorporating a dual-utilization of GM-RBnB algorithm and an inlier selection strategy.Firstly, the grid map features of several vehicles are extracted and the GMRBnB algorithm is applied for the initial registration.The outcomes of this preliminary registration undergo inlier selection, grounded on density.Subsequently, another registration is executed, resulting in an improved transformation matrix.The algorithm overcomes the obstacles presented by inadequate feature quality by grid maps and probable anomalies in overlapping submap regions, hence successfully accomplishing registration.When compared with ICP and NDT methods, the algorithm performs better in terms of MSE and SSIM metrics.The proposed approach has proven successful in constructing underwater grid maps and is appropriate for distributed underwater grid map construction.

Future Work
This paper employs an enhanced GMRBnB algorithm to register the grid maps generated by multiple underwater vehicles into a larger underwater map.Future work involves enhancing the mapping accuracy of individual autonomous underwater vehicles.Challenges still exist in extracting complete environmental features due to noise and clutter interference, along with map drift caused by data matching failures.Secondly, it is recommended to add more sensors to enhance the efficiency of underwater vehicle SLAM.For instance, investigating the integration of cameras and image processing to enhance localization optimization may yield positive results.Additionally, refining the fundamental components of the GMRBnB algorithm, particularly by enhancing the upper and lower bounding functions, is crucial.The objective is to adjust these functions to allow for realtime execution on embedded platforms.Finally, the future work regarding collaborative efforts among multiple underwater vehicles in real-time is an important aspect .Possible strategies for advancing underwater mapping and navigation in the context of autonomous underwater vehicle operations could include vehicles ascending to the water surface or exchanging data through communication devices underwater, thus eliminating the need for offline processing.

Figure 1 .
Figure 1.Underwater vehicles and sonar.(a) BlueRov, (b) m750d sonar.The BlueROV has the capability of submerging to a depth of 100 meters and can be equipped with multiple attachments, such as clamps or scanning sonar.The Oculus m750D multi-beam imaging sonar features two working frequencies: 750 kHz/1.2MHz, with the detection range of 120 m/40 m, respectively.

Algorithm 1 :
The overall algorithm process of Underwater Distributed SLAM Input: Data collected by BlueRov Output: A large scale grid map by registering multiple grid submaps 1 Single underwater vehicle SLAM. 2 Calculate the Oriented FAST and Rotated BRIEF (ORB) point of the submaps and save the 2D point set data. 3 Use the GMRBnB algorithm to deduce the rotation and translation matrix, which is applied to the second point set, registering the two point sets.4 The registered point set should undergo the Nearest Neighbor Density Estimation (NNDE) method to determine the density.Set the threshold to identify the inlier that corresponds to regions of greater density.5 Save the interior points along with their corresponding points in the original point set.Next, conduct GMRBnB registration on the interior points to calculate a more precise transformation matrix prior to register the underwater maps.

Figure 2 .
Figure 2.Framework of the GMRBnB method with the inlier selection.The process begins by extracting features from the submaps.If the density difference is significant, voxel downsampling must be applied.Next, the GMRBnB algorithm is employed to perform registration, followed by the use of the NNDE method for density calculation and detection of the inlier.The GMRBnB is again used to register the inlier to obtain a more precise transformation matrix.The final step involves registering the submaps.

E
(B) is the function that requires optimizing.f L (B) represents a lower estimate function of the objective function, while f U (B) represents an upper estimate function.Technical abbreviations such as BnB-based and objective function have to be explained upon first use.f U (B) is typically derived by substituting any value from the solution space into the objective function.The convergence of BnB-based algorithms is assured by the properties of the bounding functions.lim σ(B)→0

Figure 3 .
Figure 3. Convergence curve of GMRBnB algorithm.The gap between the lower and upper bounds is converging to zero, and after hundreds of iterations, the proposed method can converge to the optimal solution.

Figure 4 .
Each subfigure (a)-(d) showcases two digits representing the results attained prior and after registration via the proposed method and distinct parameter settings specified in the subcaptions.The origin point set is displayed in red while the target point set is displayed in black.The rotational error and translation error for each instance are as follow: (a) 0.024 and 0.0044, (b) 0.228 and 0.0087, (c) 0.323 and 0.0113, (d) 0.493 and 0.0127.

Figure 4 .
Figure 4. Registration results with different outlier rates and random noise levels.(a) The proportion of outlier is 0, and the proportion of random noise is 0, (b) The proportion of outlier is 33%, and the proportion of random noise is 0, (c) The proportion of outlier is 50%, and the proportion of random noise is 0, (d) The proportion of outlier is 50%, and the proportion of random noise is 5%.

Figure 5 .
Figure 5. GMRBnB registration via NNDE interior point extraction, (a) Source point set and target point set, (b) First registration result, (c) Inlier extraction using NNDE, (d) Second registration result.

Figure 7 .
Figure 7. Equipment and experiment, (a) is the BlueRov and m750d sonar, (b) is the experiment on the lake.We assembled the BlueRov and m750d in Figure 1 and controlled the BlueRov on the ship to conduct the experiments.

Figure 8 .
Figure 8. Registration quality results.(a) The first and second submaps registered result, (b) the second and third submaps registered result, (c) the result of registering (a,b).
with mixture weights w i , means u i , variances σ 2 i and the number of gaussian components k.The distance between GMM