An Underwater Multisensor Fusion Simultaneous Localization and Mapping System Based on Image Enhancement

Abstract

As a key method of ocean exploration, the positioning accuracy of autonomous underwater vehicles (AUVs) directly influences the success of subsequent missions. This study aims to develop a novel method to address the low accuracy in visual simultaneous localization and mapping (SLAM) within underwater environments, enhancing its application in the navigation and localization of AUVs. We propose an underwater multisensor fusion SLAM system based on image enhancement. First, we integrate hybrid attention mechanisms with generative adversarial networks to address the blurring and low contrast in underwater images, thereby increasing the number of feature points. Next, we develop an underwater feature-matching algorithm based on a local matcher to solve the feature tracking problem caused by grayscale changes in the enhanced image. Finally, we tightly couple the Doppler velocity log (DVL) with the SLAM algorithm to better adapt to underwater environments. The experiments demonstrate that, compared to other algorithms, our proposed method achieves reductions in both mean absolute error (MAE) and standard deviation (STD) by up to 68.18% and 44.44%, respectively, when all algorithms are operating normally. Additionally, the MAE and STD of our algorithm are 0.84 m and 0.48 m, respectively, when other algorithms fail to operate properly.

1. Introduction

Since the twenty-first century, due to the increasing shortage of land resources, the ocean, as a newly developed treasure trove for humanity, has emerged as a new world center [1,2]. Due to the complexity of the marine environment, the cost of manual operation in this environment is high, and there are various hidden dangers. Autonomous underwater vehicles (AUVs) [3,4], owing to their autonomous navigation capabilities, can effectively replace manual underwater operations such as seabed exploration, seabed archaeology, and seabed minesweeping [5,6], and have gradually become essential tools for executing underwater tasks. The prerequisite for AUVs to perform underwater missions is the availability of a high-precision navigation and localization system. Hydroacoustic localization systems [7,8], inertial navigation and localization systems [9,10], and simultaneous localization and napping (SLAM) technology [11,12] have been proposed as solutions to the underwater navigation challenges faced by AUVs.

The process of the hydroacoustic positioning system is summarized as the interaction between an acoustic transmitting transducer and a receiving transducer to determine the target position. As hydroacoustic positioning systems generally use acoustic waves as the communication carrier, they are affected by the underwater environment, leading to increased noise in the hydroacoustic signal, longer communication delays, and other issues that significantly impact the accuracy and real-time capabilities of navigation and positioning. Additionally, hydroacoustic positioning systems are expensive and difficult to install, further limiting their application scenarios.

Inertial navigation and positioning systems achieve a high degree of covertness and autonomy by using the acceleration and angular velocity data obtained from accelerometers and gyroscopes to perform dead reckoning, thereby determining the current position. In practical applications, this information is generally provided by inertial measurement devices. High-precision navigation equipment is expensive and generally not suitable for civilian use, while low-cost micro-electro-mechanical system inertial measurement units (MEMS IMUs) are less accurate and cannot be used independently for extended periods.

With the development of computer technology, SLAM technology with a laser or camera as the core sensor is increasingly employed. Laser SLAM [13,14], which achieves navigation and positioning by comparing the emitted and reflected laser beams, is gradually being applied in underwater environments. For example, the literature [15,16] proposes the use of single-photon underwater LiDAR for bathymetric measurements, and the measurement results are highly consistent with synchronized sonar data. Visual SLAM [17,18], which determines position through image data, has the advantages of no drift and high stability, and can provide high-precision navigation and positioning for AUVs.

Currently, SLAM algorithms applied to AUVs are broadly classified into two categories: EKF-SLAM, based on the extended Kalman filter (EKF); fast SLAM, based on the particle filter (PF); and graph-SLAM, based on graph optimization.

The computation of EKF-SLAM [19] is divided into four processes: prediction, observation update, data association, and state dimension expansion. A study [20] proposed an algorithm for underwater localization using acoustic signals based on the EKF-SLAM algorithm, which fuses inertial sensors and the direction angle of the acoustic source obtained from Bayesian estimation for position estimation. Authors [21] developed an enhanced EKF-SLAM, and the algorithm uses position error models based on side-scan sonar and AUV inertial navigation systems as input data, rather than directly using the data from inertial sensors and side-scan sonar. EKF-SLAM inherits the advantages and disadvantages of the EKF algorithm. The algorithm’s principle is simple and easy to implement, but the error caused by system nonlinearity may lead to nonconvergence and high computational cost.

The basis of FAST-SLAM is the PF, whose core idea is to use a series of randomly drawn samples and their weights to represent the posterior probability distribution of the state. When the number of samples is sufficiently large, the true posterior distribution can be well approximated by such random sampling. Researchers [22] utilized the mean trajectory graph, a method that reduces computational consumption by retaining only the current estimated position of the AUV in its particles, while all historical states of the AUV are stored in the mean trajectory graph. Others [23] proposed a particle-filter-based bathymetric simultaneous localization and mapping (BSLAM) method with mean trajectory map representation, where the particles of the algorithm retain only the current estimated position of the AUV, while all historical states of the AUV are stored in the averaged trajectory map. The disadvantages of FAST-SLAM include the problem of particle “degeneracy” and the difficulty of finding a general selection criterion for the importance density function used to generate particles.

In graph-SLAM, the position of an AUV is represented as a node or vertex, and the relationships between positions constitute edges. The process is mainly divided into two parts: the front end, responsible for processing the data collected by the sensors, and the back end, which uses a nonlinear optimization algorithm to determine the robot’s position. The literature [24] proposes a collaborative SLAM framework where each AUV generates its own local map while using marginalized pose and sparsified information matrices to reduce the size of communication packets, ultimately achieving collaborative localization and mapping. In a study [25], to enhance the robustness of SLAM, a robust estimator that fuses the robot’s kinematic model with proprioceptive sensors was proposed to propagate the pose when the visual inertial odometer (VIO) fails, ensuring proper localization. Graph-optimization-based algorithms were previously considered too time consuming to meet the real-time requirements of SLAM. However, with the emergence of efficient solving algorithms and the rapid development of related hardware, graph-optimization-based algorithms have regained attention and become a hot topic in the current SLAM research.

The disadvantages of underwater images [26,27], such as color deviation and blurring, pose the risk of crashing the SLAM operation. In this study, to meet the demand for AUV underwater navigation and localization, an underwater multisensor fusion SLAM algorithm based on image enhancement was designed by integrating an underwater image enhancement module into VINS-Mono [28], which belongs to the graph-SLAM category, and fusing data from various sensors such as a monocular camera, IMU, and Doppler velocity kog (DVL).

The main contributions of this study are summarized as follows:

•

In this paper, an underwater image enhancement algorithm based on a generative adversarial network [29] is proposed. To improve the quality of underwater images, we designed a hybrid attention module, consisting of channel attention and spatial attention, and applied it to the generator to enhance the underwater image enhancement effect. Additionally, we constructed a multinomial loss function to improve training efficiency.

•

In this paper, a multisensor fusion SLAM algorithm is proposed, based on the VINS-Mono framework, incorporating DVL, and making corresponding improvements to its measurement preprocessing, initialization, and nonlinear optimization components. Additionally, to address the impact of grayscale changes caused by image enhancement on image matching, this paper proposes an underwater image matching algorithm based on a local matcher.

In Section 2, we describe the specific algorithms used in this study. The results obtained from the system are presented in Section 3. Finally, we provide a brief discussion in Section 4 and the conclusions in Section 5.

2. Materials and Methods

In this section, we describe the programs and methods used in this study, divided into the following two aspects: First, the underwater image enhancement algorithm based on a generative adversarial network is presented. Second, the integration of DVL into the VINS-Mono algorithm and the underwater feature-matching algorithm based on the local matcher are introduced.

The algorithm proposed in this paper is based on the traditional VINS-Mono framework and consists of four parts: measurement preprocessing, initialization, nonlinear optimization, and loop closure. The system framework of the proposed algorithm is shown in Figure 1.

2.1. Underwater Image Enhancement Algorithm Based on Generative Adversarial Network

The disadvantages of underwater images, such as partial color, blurring, and low contrast, lead to a significant reduction in the number of extractable feature points, which in turn can cause the collapse of the entire SLAM system. To address this problem, this subsection proposes a generative adversarial network based on a hybrid attention mechanism to enhance underwater images. The hybrid attention module, which serves as the core part of the generator, aims to restore the color and texture of the image and consists of a channel attention module and a spatial attention module. The overall network framework is a generative adversarial network, with the adversarial network framework based on U-Net [30]. The generator network framework and hybrid attention module network framework are shown in Figure 2.

2.1.1. Channel Attention Module

The channel attention module reallocates channel resources to aid in the color restoration of images. It aggregates feature map information using both maximum pooling and average pooling and achieves adaptive selection through learned parameters, ensuring that the two pooling results have different weights to obtain better channel attention maps. The network framework is shown in Figure 3, and the specific process is detailed below:

Step 1: The input feature map $\mathrm{X}\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$ undergoes two pooling operations in two pooling layers. The pooling results are then weighted by the learning parameters $\alpha$ and $\beta$ and summed element-wise with their mean to obtain the feature tensor $f^{add}\in {\mathbb{R}}^{C_{in}\times 1\times 1}$. This process can be expressed by the following equation:

$$f^{add}=\left(\alpha \otimes AvgPool\left(X\right)\right)\oplus \left(\beta \otimes MaxPool\left(X\right)\right)\oplus \left(0.5\otimes \left(AvgPool\left(X\right)\oplus MaxPool\left(X\right)\right)\right)$$

(1)

where $AvgPool$ represents average pooling; $MaxPool$ represents max pooling; ⊕ denotes element-wise summation; and ⊗ denotes element-wise multiplication. The parameters $\alpha$ and $\beta$ belong to $\left(0,1\right]$, and the network achieves adaptive tuning of channel weights through these two learning parameters.

Step 2: The feature tensor $f^{add}$ undergoes dimensionality reduction followed by dimensionality enhancement to achieve cross-channel information interaction, resulting in the feature tensor $f^z\in {\mathbb{R}}^{C_{in}\times 1\times 1}$. The process can be expressed by the following equation:

$$f^z={\Phi }_1\left({\Phi }_2\left(f^{add}\right)\right)=C2D_{1\times 1}^{C_{in}\times d}\left(\delta \left(BN\left(C2D_{1\times 1}^{d\times C_{in}}\left(f^{add}\right)\right)\right)\right)$$

(2)

where ${\Phi }_1$ represents the ascending operation; ${\Phi }_2$ represents the descending operation; $C2D_{1\times 1}^{C_{in}\times d}$ denotes a $1\times 1$ 2D convolution with the number of output channels as $C_{in}$ and the number of input channels as d; $C2D_{1\times 1}^{d\times C_{in}}$ denotes a $1\times 1$ 2D convolution with the number of output channels as d and the number of input channels as $C_{in}$; $BN$ represents batch normalization; and $\delta$ is the $ReLU$ activation function. The parameter d is determined by the hyperparameter r:

$$d=max\left(C_{in}/\phantom{C_{in}r}r,L\right)$$

(3)

In the final step, the feature tensor $f^z$ is normalized and multiplied element-wise with the input feature map $\mathrm{X}$ to obtain the output feature map $Y\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$:

$$Y=\sigma \left(f^z\right)\otimes X$$

(4)

where $\sigma$ is the $softmax$ activation function.

2.1.2. Spatial Attention Module

The spatial attention module strengthens the network’s learning of information-rich regions, aiding in the recovery of the highly textured parts of the image, and determines the importance of the features in each part of the different channels through learning. To ensure the network pays more attention to the highly textured parts of the image, this paper proposes a feature separation algorithm that divides the features in each channel into important and minor features. The spatial attention module first uses the proposed algorithm to achieve feature separation, then applies average pooling and max pooling operations along the channel dimensions, and finally passes the two pooling results through a series of convolutional layers to obtain the final spatial attention map. The network framework is shown in Figure 4, and the specific flow is detailed below:

Step 1: The input feature map $\mathrm{X}\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$ undergoes adaptive average pooling to obtain the average value of each layer. The part above the average value is defined as important features, and the part below the average value is defined as secondary features. Two tensors are then defined to store the results: one tensor sets the important features to 1 and the secondary features to 0, while the other tensor does the opposite. The two tensors are then multiplied element-wise with the input feature map to obtain the important feature map $F_1\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$ and the secondary feature map $F_2\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$. The Python code is shown in Algorithm 1.

Algorithm 1 Python code for feature separation.

Input:  X Output:  $F_1$, $F_2$   1: def channel_separation(X):   2:       one = torch.ones_like(X)   3:       zero = torch.zeros_like(X)   4:       avg_pool = nn.AdaptiveAvgPool2d(1)   5:       avg = avg_pool(X)   6:       important_tensor = torch.where(X > avg, one, zero)   7:       subimportant_tensor = 1 - important_tensor   8:       $F_1$ = X *important_tensor   9:       $F_2$ = X *subimportant_tensor 10:       return $F_1$, $F_2$

Step 2: The feature maps $F_1$ and $F_2$ undergo a pooling layer, a 7 × 7 shared convolutional layer and a series of nonlinear operations to obtain the corresponding spatial attention tensors $f^{SA1}\in {\mathbb{R}}^{1\times H_{in}\times W_{in}}$ and $f^{SA2}\in {\mathbb{R}}^{1\times H_{in}\times W_{in}}$. The process can be expressed as follows:

$$\begin{array}{c}{f}^{SA1}={\Phi }_3\left(C2D_{7\times 7}^{1\times 1}\left(\left[AvgPool\left(F_1\right);MaxPool\left(F_1\right)\right]\right)\right)\\ f^{SA2}={\Phi }_3\left(C2D_{7\times 7}^{1\times 1}\left(\left[AvgPool\left(F_2\right);MaxPool\left(F_2\right)\right]\right)\right)\end{array}$$

(5)

where ${\Phi }_3$ represents a series of nonlinear operations consisting of a sequence of $BN$ layers and $ReLU$ functions; and $C2D_{7\times 7}^{1\times 1}$ denotes a $7\times 7$ 2D convolution with 1 output channels and 1 input channel.

In the final step, the spatial attention tensors $f^{SA1}$ and $f^{SA2}$ are multiplied with the feature maps $F_1$ and $F_2$, respectively, and the results are summed element-wise to obtain the output feature map $\mathrm{Y}\in {\mathbb{R}}^{C_{in}\times H_{in}\times W_{in}}$. The process can be expressed as follows:

$$Y=\left(f^{SA1}\otimes F_1\right)\oplus \left(f^{SA2}\otimes F_2\right)$$

(6)

2.1.3. Loss Function

In this method, the loss function L is obtained by linearly weighting the adversarial loss $L_{adv}$ [29], the L1 loss $L_1$ [31], the gradient loss $L_{\mathrm{gra}}$, and the color bias loss $L_{col}$. The specific form of the loss function is shown in the following equation:

$$L=L_{adv}+{\lambda }_{g1}{L}_1+{\lambda }_{g2}{L}_{\mathrm{gra}}+{\lambda }_{g3}{L}_{col}$$

(7)

where the scaling factors ${\lambda }_{g1}, {\lambda }_{g2}$, and ${\lambda }_{g3}$ are set to 0.3, 0.3, and 0.4.

Among them, the gradient loss $L_{\mathrm{gra}}$ can represent the clarity and integrity at the image boundary to a certain extent, aiding in the generation of image details, as follows:

$$L_{gra}=E_{x,y}\left\{{∥{\nabla }_{i}y-{\nabla }_i\left[G\left(x\right)\right]∥}_1\right\}+E_{x,y}\left\{{∥{\nabla }_{j}y-{\nabla }_j\left[G\left(x\right)\right]∥}_1\right\}$$

(8)

where ${\nabla }_i,{\nabla }_j$ are the horizontal and vertical gradient operators.

The color bias loss $L_{col}$ is used to encourage the color distribution of the generated image to align more closely with the real image by measuring the degree of color bias in the generated image, as follows:

$$\begin{array}{c}{L}_{col}=D\left(y\right)/M\left(y\right)-D\left(x\right)/M\left(x\right)\\ d_a=\frac{{\displaystyle \sum _{i=1}^M}{\displaystyle \sum _{i=1}^N}a}{MN},d_b=\frac{{\displaystyle \sum _{i=1}^M}{\displaystyle \sum _{i=1}^N}b}{MN}\\ D=\sqrt{d_a^2+d_b^2}\\ M_a=\frac{{\displaystyle \sum _{i=1}^M}{\displaystyle \sum _{i=1}^N}\left(a-d_a\right)}{MN},M_b=\frac{{\displaystyle \sum _{i=1}^M}{\displaystyle \sum _{i=1}^N}\left(d-d_b\right)}{MN}\\ M=\sqrt{M_a^2+M_b^2}\end{array}$$

(9)

where D denotes the average color difference of the image; M denotes the center distance of the image; a and b denote the two-dimensional coordinates of the a channel and b channel of the image in Lab space; and M and N denote the maximum values of the two-dimensional coordinates of the image in Lab space.

2.2. Underwater Multisensor Fusion SLAM Algorithm

2.2.1. DVL Tightly Coupled to SLAM Algorithm

In this paper, ${\left(·\right)}^w$ represents the world coordinate system, ${\left(·\right)}^b$ represents the body coordinate system (defined the same as the IMU coordinate system), ${\left(·\right)}^c$ represents the camera coordinate system, and ${\left(·\right)}^d$ represents the DVL coordinate system. Meanwhile, the rotation matrix $\mathit{R}$ and quaternion $\mathit{q}$ are used to represent rotation.

Adding DVL to VINS-Mono requires corresponding adjustments to pr-integration, initialization, and nonlinear optimization. The following sections describe each of these adjustments in detail.

Based on the IMU kinematic model [32], by adding the DVL information, the kinematic IMU/DVL model in the world coordinate system can be obtained as shown in Equation (10):

$$\begin{array}{c}{\mathit{p}}_{b_{k+1}}^w={\mathit{p}}_{b_k}^w+{\mathit{v}}_{b_k}^w\Delta t+{\int \int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^w\left({\widehat{\mathit{a}}}_t-{\mathit{b}}_a\right)-{\mathit{g}}^w\right]d{t}^2\hfill \\ {\mathit{v}}_{b_{k+1}}^w={\mathit{v}}_{b_k}^w+{\int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^w\left({\widehat{\mathit{a}}}_t-{\mathit{b}}_a\right)-{\mathit{g}}^w\right]dt\hfill \\ {\mathit{q}}_{b_{k+1}}^w={\mathit{q}}_{b_k}^w\odot {\int }_{t\in \left[k,k+1\right]}\frac{1}{2}\Omega \left({\widehat{\mathit{\omega }}}_t-{\mathit{b}}_{\omega }\right){\mathit{q}}_t^{b_k}dt\hfill \\ {\mathit{p}}_{d_{k+1}}^w={\mathit{p}}_{d_k}^w+{\int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^w{\mathit{R}}_d^b\left({\widehat{\mathit{v}}}_t-{\mathit{b}}_d\right)\right]dt\hfill \end{array}$$

(10)

where ${\mathit{p}}_b^w,{\mathit{v}}_b^w,{\mathit{q}}_b^w$ denote the position, velocity, and rotation of the IMU in the world coordinate system; ${\mathit{p}}_d^w$ is the position of the DVL in the world coordinate system; ${\widehat{\mathit{a}}}_t,{\widehat{\mathit{\omega }}}_t,{\widehat{\mathit{v}}}_t$ are the measured values of the accelerometer, gyroscope, and DVL, respectively; ${\mathit{b}}_a,{\mathit{b}}_{\omega },{\mathit{b}}_d$ are the corresponding zero biases; $\Delta t$ denotes the time difference between the two frames; ${\mathit{R}}_t^w$ is the rotation of the IMU in the world coordinate system at time t; $g^w$ is the acceleration of gravity in the world coordinate system; ${\mathit{q}}_t^{b_k}$ is the rotation increase in the IMU in the $b_k$ frame at time t; ${\mathit{R}}_d^b$ is the rotational parameter of IMU-DVL; ⊙ is the symbol for quadratic multiplication; and $\Omega$ is defined as:

$$\Omega \left(\mathit{\omega }\right)=\left[\begin{array}{cc}-\left[\mathit{\omega }\right]\times & \mathit{\omega }\\ -{\mathit{\omega }}^T& 0\end{array}\right],\left[\mathit{\omega }\right]\times =\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& -{\omega }_x& 0\end{array}\right]$$

(11)

Equation (10) is multiplied by ${\mathit{R}}_{\mathit{w}}^{{\mathit{b}}_{\mathit{k}}}$ on both sides to obtain the IMU pr-integration ${\mathit{\alpha }}_{b_{k+1}}^{b_k},{\mathit{\beta }}_{b_{k+1}}^{b_k},{\mathit{\gamma }}_{b_{k+1}}^{b_k}$ and the DVL pre-integration ${\mathit{\eta }}_{d_{k+1}}^{b_k}$:

$$\begin{array}{c}{\mathit{\alpha }}_{b_{k+1}}^{b_k}={\int \int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^{b_k}\left({\widehat{\mathit{a}}}_t-{\mathit{b}}_a\right)\right]d{t}^2\hfill \\ {\mathit{\beta }}_{b_{k+1}}^{b_k}={\int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^{b_k}\left({\widehat{\mathit{a}}}_t-{\mathit{b}}_a\right)\right]dt\hfill \\ {\mathit{\gamma }}_{b_{k+1}}^{b_k}={\int }_{t\in \left[k,k+1\right]}\frac{1}{2}\Omega \left({\widehat{\mathit{\omega }}}_t-{\mathit{b}}_{\omega }\right){\mathit{\gamma }}_t^{b_k}dt\hfill \\ {\mathit{\eta }}_{d_{k+1}}^{b_k}={\int }_{t\in \left[k,k+1\right]}\left[{\mathit{R}}_t^{b_k}{\mathit{R}}_d^b\left({\widehat{\mathit{v}}}_t-{\mathit{b}}_d\right)\right]dt\hfill \end{array}$$

(12)

where ${\mathit{R}}_{\mathit{w}}^{{\mathit{b}}_{\mathit{k}}}={\left({\mathit{R}}_{{\mathit{b}}_{\mathit{k}}}^{\mathit{w}}\right)}^T$, ${\mathit{R}}_{{\mathit{b}}_{\mathit{k}}}^{\mathit{w}}$ represents the rotation of the IMU in the world coordinate system for the k frame.

The first-order Taylor series expansion of Equation (12) is given by

$$\begin{array}{c}{\mathit{\alpha }}_{b_{k+1}}^{b_k}\approx {\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}+{\mathit{J}}_{b_a}^{\alpha }\delta {\mathit{b}}_a+{\mathit{J}}_{b_{\omega }}^{\alpha }\delta {\mathit{b}}_{\omega }\hfill \\ {\mathit{\beta }}_{b_{k+1}}^{b_k}\approx {\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}+{\mathit{J}}_{b_a}^{\beta }\delta {\mathit{b}}_a+{\mathit{J}}_{b_{\omega }}^{\beta }\delta {\mathit{b}}_{\omega }\hfill \\ {\mathit{\gamma }}_{b_{k+1}}^{b_k}\approx {\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathit{J}}_{b_{\omega }}^{\gamma }\delta {\mathit{b}}_{\omega }\end{array}\right]\hfill \\ {\mathit{\eta }}_{d_{k+1}}^{b_k}\approx {\widehat{\mathit{\eta }}}_{d_{k+1}}^{b_k}+{\mathit{J}}_{b_d}^{\eta }\delta {\mathit{b}}_d+{\mathit{J}}_{b_{\omega }}^{\eta }\delta {\mathit{b}}_{\omega }\hfill \end{array}$$

(13)

where ${\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k},{\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k},{\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k},{\widehat{\mathit{\eta }}}_{d_{k+1}}^{b_k}$ are the IMU/DVL preintegrated measurements; $\delta {\mathit{b}}_a,\delta {\mathit{b}}_{\omega },\delta {\mathit{b}}_d$ are the zero-bias errors of the accelerometer, gyroscope, and DVL; and $\mathit{J}$ denotes the respective Jacobian matrix.

The estimation of the rotational parameter ${\mathit{R}}_c^b$ and the gyroscope zero bias during initialization is consistent with that of VINS-Mono. Therefore, only the estimation of the accelerometer’s zero bias and the DVL’s zero bias is presented in this paper. Considering scale s, the pose of the IMU/DVL in the world coordinate system is

$$\begin{array}{c}{\mathit{R}}_{\mathit{b}}^{\mathit{w}}={\mathit{R}}_{\mathit{c}}^{\mathit{w}}{\left({\mathit{R}}_{\mathit{c}}^{\mathit{b}}\right)}^{-1}\hfill \\ s{\mathit{p}}_b^w=s{\mathit{p}}_c^w-{\mathit{R}}_b^w{\mathit{p}}_c^b\hfill \\ s{\mathit{p}}_d^w=s{\mathit{p}}_c^w-{\mathit{R}}_b^w{\mathit{R}}_d^b{\mathit{p}}_c^d\hfill \end{array}$$

(14)

where ${\mathit{R}}_{\mathit{c}}^{\mathit{w}}$ and ${\mathit{p}}_c^w$ are the attitude and position of the camera in the world coordinate system; ${\mathit{p}}_c^b$ and ${\mathit{p}}_c^d$ are the translational parameters of the IMU-camera and DVL-camera; and ${\mathit{R}}_d^b$ is the rotational parameter of the IMU-DVL.

The state variables are selected as ${\mathit{\chi }}_0=\left[{\mathit{v}}_{b_0}^{b_0},\cdots ,{\mathit{v}}_{b_k}^{b_k},{\mathit{v}}_{b_{k+1}}^{b_{k+1}},{\mathit{g}}^w,s\right]$; Equation (10) and Equation (14) are combined to obtain Equation (15), which is solved to obtain the external parameter estimates of velocity, gravity vector, and scale factor ${\widehat{\mathit{\chi }}}_0=\left[{\widehat{\mathit{v}}}_{b_0}^{b_0},\cdots ,{\widehat{\mathit{v}}}_{b_k}^{b_k},{\widehat{\mathit{v}}}_{b_{k+1}}^{b_{k+1}},{\widehat{\mathit{g}}}^w,\widehat{s}\right]$.

$$\begin{gathered} \left[\begin{array}{cccc}-\mathit{I}\Delta t& 0& \frac{1}{2}{\mathit{R}}_w^{b_k}\Delta t^2& {\mathbf{2}\mathit{R}}_w^{b_k}\left({\mathit{p}}_{b_{k+1}}^w-{\mathit{p}}_{b_k}^w\right)\\ -\mathit{I}& {\mathit{R}}_w^{b_k}{\mathit{R}}_{b_{k+1}}^w& {\mathit{R}}_w^{b_k}\Delta t& 0\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{b_k}^{b_k}\\ {\mathit{v}}_{b_{k+1}}^{b_{k+1}}\\ {\mathit{g}}^w\\ s\end{array}\right] \\ =\left[\begin{array}{c}{\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}+{\widehat{\mathit{\eta }}}_{b_{k+1}}^{b_k}-\left(\mathit{I}-{\mathit{R}}_w^{b_k}{\mathit{R}}_{b_{k+1}}^w\right)\left({\mathit{p}}_c^b+{\mathit{R}}_d^b{\mathit{p}}_c^d\right)\\ {\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}\end{array}\right] \end{gathered}$$

(15)

To reduce the effect of gravity on the estimated accelerometer zero bias and DVL zero bias, a gravity refinement step is required.

$${\mathit{g}}^w=g\frac{{\widehat{\mathit{g}}}^w}{∥{\widehat{\mathit{g}}}^w∥}+\left[\begin{array}{cc}{\mathit{b}}_1& {\mathit{b}}_2\end{array}\right]\left[\begin{array}{c}{m}_1\\ m_2\end{array}\right]=g\frac{{\widehat{\mathit{g}}}^w}{∥{\widehat{\mathit{g}}}^w∥}+\mathit{bm}$$

(16)

where ${\mathit{b}}_{\mathbf{1}}$ and ${\mathit{b}}_{\mathbf{2}}$ are the two orthogonal bases in the gravity tangent plane, and $m_1$ and $m_2$ are their respective lengths.

The state variables are selected as ${\mathit{\chi }}_0=\left[{\mathit{v}}_{b_0}^{b_0},\cdots ,{\mathit{v}}_{b_k}^{b_k},{\mathit{v}}_{b_{k+1}}^{b_{k+1}},\mathit{m},s,\delta {\mathit{b}}_a,\delta {\mathit{b}}_d\right]$. Equations (13), (15), and (16) are combined to obtain Equation (17), which is solved to obtain the external parameter estimates of the zero bias of the accelerometer and the zero bias of the DVL.

$$\begin{gathered} \left[\begin{array}{cccccc}-\mathit{I}\Delta t& 0& \frac{1}{2}{\mathit{R}}_w^{b_k}\mathit{b}\Delta t^2& {\mathbf{2}\mathit{R}}_w^{b_k}\left({\mathit{p}}_{c_{k+1}}^w-{\mathit{p}}_{c_k}^w\right)& -J_{b_a}^{\alpha }& -{\mathit{J}}_{b_d}^{\eta }\\ -\mathit{I}& {\mathit{R}}_w^{b_k}{\mathit{R}}_{b_{k+1}}^w& {\mathit{R}}_w^{b_k}\mathit{b}\Delta t& 0& -J_{b_a}^{\beta }& 0\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{b_k}^{b_k}\\ {\mathit{v}}_{b_{k+1}}^{b_{k+1}}\\ \mathit{m}\\ s\\ \delta {\mathit{b}}_a\\ \delta {\mathit{b}}_d\end{array}\right] \\ =\left[\begin{array}{c}{\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}+{\widehat{\eta }}_{b_{k+1}}^{b_k}-\left(\mathit{I}-{\mathit{R}}_w^{b_k}{\mathit{R}}_{b_{k+1}}^w\right)\left({\mathit{p}}_c^b+{\mathit{R}}_d^b{\mathit{p}}_c^d\right)-\frac{1}{2}{\mathit{R}}_w^{b_k}\frac{{\widehat{\mathit{g}}}^w}{∥{\widehat{\mathit{g}}}^w∥}g\Delta t^2\\ {\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}-{\mathit{R}}_w^{b_k}\frac{{\widehat{\mathit{g}}}^w}{∥{\widehat{\mathit{g}}}^w∥}g\Delta t\end{array}\right] \end{gathered}$$

(17)

After obtaining the accelerometer’s zero bias and DVL’s zero bias, they are repropagated for all IMU/DVL preintegrations, thereby completing the initialization of the SLAM algorithm presented in this paper.

For nonlinear optimization, the cost function after adding DVL is

$$\underset{\mathit{\chi }}{\min }\left\{\sum _{\left(l,j\right)\in C}{∥{\mathit{r}}_C\left({\widehat{\mathit{z}}}_l^{c_j},\mathit{\chi }\right)∥}_{{\mathit{P}}_l^{c_j}}^2+\sum _{k\in BO}{∥{\mathit{r}}_{BO}\left({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathit{\chi }\right)∥}_{{\mathit{P}}_{b_{k+1}}^{b_k}}^2+{∥{\mathit{r}}_p-{\mathit{J}}_p\mathit{\chi }∥}^2\right\}$$

(18)

where ${\mathit{r}}_C\left({\widehat{\mathit{z}}}_l^{c_j},\mathit{\chi }\right),{\mathit{r}}_{BO}\left({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathit{\chi }\right),\left\{{\mathit{r}}_p,{\mathit{J}}_p\right\}$ are the visual residuals, IMU/DVL preintegrated residuals, and marginalization constraint. The visual residuals and marginalized residuals are consistent with VINS-Mono, and the IMU/DVL preintegrated residuals is

$${\mathit{r}}_{BO}\left({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathit{\chi }\right)=\left[\begin{array}{c}\delta {\mathit{\alpha }}_{b_{k+1}}^{b_k}\\ \delta {\mathit{\beta }}_{b_{k+1}}^{b_k}\\ \delta {\mathit{\gamma }}_{b_{k+1}}^{b_k}\\ \delta {\mathit{\eta }}_{d_{k+1}}^{b_k}\\ \delta {\mathit{b}}_a\\ \begin{array}{c}\delta {\mathit{b}}_{\omega }\hfill \\ \delta {\mathit{b}}_d\hfill \end{array}\end{array}\right]=\left[\begin{array}{c}{\mathit{R}}_w^{b_k}\left({\mathit{p}}_{b_{k+1}}^w-{\mathit{p}}_{b_k}^w+\frac{1}{2}{\mathit{g}}^w\Delta t^2-{\mathit{v}}_{b_k}^w\Delta t\right)-{\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}\\ {\mathit{R}}_w^{b_k}\left({\mathit{v}}_{b_{k+1}}^w+{\mathit{g}}^w\Delta t-{\mathit{v}}_{b_k}^w\right)-{\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}\\ 2{\left[{\mathit{q}}_{b_k}^{w-1}\otimes {\mathit{q}}_{b_{k+1}}^w\otimes {{\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k}}^{-1}\right]}_{xyz}\\ {\mathit{R}}_w^{b_k}\left({\mathit{p}}_{b_{k+1}}^w-{\mathit{p}}_{b_k}^w\right)+\left({\mathit{R}}_w^{b_k}{\mathit{R}}_{b_{k+1}}^w-\mathit{I}\right){\mathit{p}}_d^b-{\widehat{\mathit{\eta }}}_{d_{k+1}}^{b_k}\\ {\mathit{b}}_{a_{k+1}}-{\mathit{b}}_{a_k}\\ \begin{array}{c}{\mathit{b}}_{{\omega }_{k+1}}-{\mathit{b}}_{{\omega }_k}\hfill \\ {\mathit{b}}_{d_{k+1}}-{\mathit{b}}_{d_k}\hfill \end{array}\end{array}\right]$$

(19)

where $\delta {\mathit{\alpha }}_{b_{k+1}}^{b_k}, \delta {\mathit{\beta }}_{b_{k+1}}^{b_k}, \delta {\mathit{\gamma }}_{b_{k+1}}^{b_k}, \delta {\mathit{\eta }}_{d_{k+1}}^{b_k}$ are the measurements residual of IMU/DVL pre-integration; $\delta {\mathit{b}}_a, \delta {\mathit{b}}_{\omega }, \delta {\mathit{b}}_d$ are the zero bias residual of IMU/DVL.

2.2.2. Underwater Feature-Matching Algorithm Based on a Local Matcher

After the underwater images undergo image enhancement, although their clarity and contrast improve, the grayscale also changes to some extent, as shown in Figure 5, thus putting the optical flow algorithm [33] at risk of failure. To solve this problem, this paper proposes an underwater feature-matching algorithm based on a local matcher.

Assuming that the two consecutive frames of the image are the $c_k$ frame and the $c_{k+1}$ frame, the corresponding IMU measurements are the $b_k$ frame and the $b_{k+1}$ frame, and the DVL measurements are the $v_k$ frame and the $v_{k+1}$ frame. The relative position $\Delta \mathit{p}$ and relative attitude $\Delta \mathit{q}$ between the two frames can be obtained according to the following equation:

$$\begin{array}{c}\Delta \mathit{p}={\mathit{R}}_d^c\left[\frac{\left({\mathit{v}}_k-{\mathit{b}}_d\right)+\left({\mathit{v}}_{k+1}-{\mathit{b}}_d\right)}{2}\Delta t\right]\\ \Delta \mathit{q}={\int }_{t\in \left[k,k+1\right]}\frac{1}{2}{\mathit{R}}_b^c\Omega \left({\widehat{\mathit{\omega }}}_t-{\mathit{b}}_{\omega }\right)q_t^{b_k}dt\end{array}$$

(20)

where ${\mathit{R}}_{\mathit{b}}^{\mathit{c}}$ and ${\mathit{R}}_{\mathit{d}}^{\mathit{c}}$ are the rotational parameters for the camera-IMU and camera-DVL.

Assume that the normalized coordinates corresponding to the projections of a waypoint on the images of the $c_k$ frame and the $c_{k+1}$ frame are ${\mathit{p}}^{c_k}$ and ${\mathit{p}}^{c_{k+1}}$, the camera’s internal reference is $\mathit{K}$ and the pixel coordinates corresponding to ${\mathit{p}}^{c_k}$ is ${\mathit{u}}^{c_k}$. The predicted value ${\widehat{\mathit{u}}}^{c_{k+1}}$ of the pixel coordinate ${\mathit{p}}^{c_{k+1}}$ corresponding to ${\mathit{u}}^{c_{k+1}}$ can be obtained using the following equation:

$$\begin{array}{c}{\mathit{p}}^{c_k}={\mathit{K}}^{-1}{\mathit{u}}^{c_k}\\ {\mathit{p}}^{c_{k+1}}=\Delta \mathit{R}{\mathit{p}}^{c_k}+\Delta \mathit{p}\\ {\widehat{\mathit{u}}}^{c_{k+1}}=\mathit{K}{\mathit{p}}^{c_{k+1}}\end{array}$$

(21)

where $\Delta \mathit{R}$ is the matrix form of $\mathit{q}$.

To apply the predicted value ${\widehat{\mathit{u}}}^{c_{k+1}}$ to feature matching, a local matcher with a search window was designed. First, a search window is created, which is a circular or square area centered on the predicted value ${\widehat{\mathit{u}}}^{c_{k+1}}$, and its radius varies from small to large. Next, candidate features are searched within the small radius area. If the number of candidates exceeds the preset threshold (20 features), then the 20 features with the maximum response value are retained. Otherwise, the radius of the search window is enlarged, and the above search steps are repeated until 20 candidate points are detected or the detection area reaches or exceeds the size of the entire image. Finally, the feature points in the search window are regarded as potential matching candidates, and the feature point with the closest descriptor distance is obtained by brute-force matching in a small range.

Through the local matcher, even if there is a certain discrepancy in the enhancement effect between the two consecutive frames, it can still ensure that the feature points of the former image have a candidate match in the latter image. Then, after applying the RANSAC algorithm [34], a better matching result can be obtained. The flowchart of the underwater feature-matching algorithm is shown in Figure 6.

3. Results

In this study, we verified the localization accuracy and robustness of the proposed algorithm through simulation and physical experiments, using the proposed algorithm as the experimental group and VINS-Mono as the control group.

3.1. Simulation Experiments

In this study, the underwater AFRL dataset [35] was selected for simulation verification. According to the authors’ suggestion, COLMAP [36] was used to generate the reference trajectories of the dataset, which could be considered as the ground truth (GT). To be more representative, the Bus subdataset with poorer image quality in the AFRL dataset and the Cave subdataset with better image quality were selected for simulation experiments. Only the “left eye” informationwas used during the simulation process. Additionally, new datasets including the DVL information were generated based on the two subdatasets according to Figure 7 to validate the SLAM algorithm proposed in this paper.

The simulation experiment results are shown in Figure 8 and Figure 9, and the experimental errors, including mean error (MEAN), standard deviation (STD), and root mean square error (RMSE), are shown in

Table 1. Comparison of the errors of the two algorithms.

Subdataset	Algorithm	MEAN (m)	STD (m)	RMSE (m)
Bus subdataset	VINS-Mono *	-	-	-
	Our	0.24	0.16	0.29
Cave subdataset	VINS-Mono	0.32	0.13	0.35
	Our	0.14	0.06	0.16

* Indicates algorithm could not run properly.

. From Figure 8 and Figure 9, it can be seen that the VINS-Mono algorithm can run normally only on the Cave subdataset, but the absolute pose error (APE) is relatively large. This includes max (maximum error), min (minimum error), std (same as STD), median, mean (same as MEAN), and mse (mean square error). The SLAM algorithm proposed in this paper ran normally on both underwater subdatasets. Although the APE was a little larger on the Bus subdataset than on the Cave subdataset, it was still within a reasonable range.

From Table 1, it can be seen that compared with the VINS-Mono algorithm, the SLAM algorithm proposed in this paper has better adaptability and robustness. On the Bus subdataset, with a total mileage of about 53 m, the VINS-Mono algorithm could not run properly, while the RMSE of the SLAM algorithm proposed in this paper was 0.29 m. On the Cave subdataset, with a total mileage of about 87 m, the RMSE of the VINS-Mono algorithm was 0.35 m, while the RMSE of the SLAM algorithm proposed in this paper was 0.16m, which is 54.29% lower.

3.2. Physical Experiments

In this study, the algorithm proposed in this paper was validated through underwater physical experiments. BlueROV was selected as the experimental platform, carrying sensors such as an underwater camera, an IMU, a DVL, and an underwater global positioning system (GPS). The underwater experiments were divided into open-loop and closed-loop experiments, and the reference trajectories (i.e., GT) were provided by the underwater GPS.

3.2.1. Open-Loop Experiment

The total length of the underwater open-loop experimental trajectory was about 83.18 m. The experimental trajectory is shown in Figure 10. RVIZ is a 3D visualizer for the Robot Operating System (ROS) framework.

The experimental results are shown in Figure 11 and Figure 12, and the experimental errors are shown in

Table 2. Comparison of the errors of the two algorithms.

Algorithm	MAE (m)	STD (m)
VINS-Mono	1.76	0.90
Our	0.56	0.50

.

. From Figure 11 and Figure 12, it can be observed that in the feature-rich region, both the proposed algorithm and VINS-Mono coulie detect the feature points that satisfy the operation of their respective algorithms. However, the proposed image enhancement algorithm significantly increased the number of feature points, which improved localization accuracy to a certain extent. From Table 2, in terms of mean absolute error (MAE) and STD, the proposed algorithm reduced these values by 1.20 m and 0.40 m, respectively, compared to those of the VINS-Mono algorithm.

Building on VINS-Mono, the proposed algorithm uses image enhancement to optimize the original image. Additionally, the DVL used for velocimetry has a certain inhibitory effect on the velocity dispersion of the IMU accelerometer, resulting in better performance.

3.2.2. Closed-Loop Experiment

The total mileage of the underwater closed-loop experimental trajectory wa about 209.04 m. The experimental trajectory is shown in Figure 13.

The experimental results are shown in Figure 14 and Figure 15, and the experimental errors are shown in

Table 3. Comparison of the errors of the two algorithms.

Algorithm	MAE (m)	STD (m)
VINS-Mono *	2.32	2.01
Our	0.84	0.48

* Represents the error of the nonfailed part of the VINS-Mono algorithm.

. From Figure 14 and Figure 15, it can be seen that in feature-scarce regions, VINS-Mono could not run properly due to its inability to detect the required feature points. The image enhancement algorithm proposed in this paper effectively solved this problem by providing enough feature points for the algorithm, thereby guaranteeing its normal operation. From Table 3, in terms of MAE and STD, the proposed algorithm had errors of 0.84 m and 0.48 m, respectively, and these errors are relatively small. (Note: Tte errors of the VINS-Mono algorithm shown in Figure 15 and Table 3 represent the errors before it failed to run.)

In the underwater closed-loop experiments, the proposed algorithm detected the loopback and initiated the repositioning and position optimization session, resulting in a small error near the starting point. Additionally, the proposed algorithm incorporated more image enhancement and DVL constraints compared to the VINS-Mono algorithm, which improved robustness and positioning accuracy.

4. Discussion

To improve the accuracy and robustness of underwater visual SLAM, this paper proposes an underwater multisensor fusion SLAM system based on image enhancement. The system uses an underwater image enhancement algorithm based on a generative adversarial network to optimize the underwater images. An underwater feature-matching algorithm based on the local matcher is used to solve the feature-matching problem due to the grayscale change in the enhanced images. Additionally, the DVL is tightly coupled to the visual SLAM system, which ultimately reduces the accuracy error.

In the experiments, the main reason for the failure of the VINS-Mono algorithm was the low input image quality, which could not provide sufficient feature points. In contrast, the image enhancement part of the proposed algorithm effectively solves this problem. Additionally, the above optimization further improves the navigation accuracy of the proposed algorithm. In the open-loop experiment, the MAE and STD of the proposed algorithm were reduced by 68.18% and 44.44%, respectively, compared to those of VINS-Mono. In the closed-loop experiment, the MAE and STD values of the proposed algorithm were 0.84 m and 0.48 m, respectively.

The proposed algorithm is mainly suitable for areas with rich and easily distinguishable underwater features. However, in special underwater environments, AUVs sometimes move to areas with weak or repetitive textures, such as a flat seabed with all sand, resulting in insufficient or mismatched features. In this case, the proposed algorithm has the possibility of crashing. To solve this problem, in future research, we plan to add Kalman filtering to the algorithm as an alternative navigation scheme. We are investigating the navigation accuracy confidence between the two, the timing of switching between them, and the related problems faced by visual SLAM when rerunning on an existing map. Additionally, to further bolster the algorithm’s robustness, we will explore integrating a magnetometer and a depth gauge into the algorithm, which will help with obtaining more accurate heading angle and depth information. Therefore, the code is not open-source.

5. Conclusions

In this paper, we proposed an underwater multisensor fusion SLAM system based on image enhancement, which uses a generative adversarial network with hybrid attention to enhance underwater images. To obtain feature matching between the enhanced images, based on the a priori information provided by the gyroscope and the DVL, we use the proposed local matcher to accomplish feature tracking. Meanwhile, the addition of the DVL information enhances the system’s robustness and accuracy.

Specifically, the main focus of this study was on underwater image enhancement and tight DVL coupling. Underwater image enhancement is the foundation of the entire algorithm, directly determining whether the algorithm can run normally. Its role is to process poor-quality underwater images into images that meet the system’s requirements, providing enough feature points for the algorithm. The role of tight DVL coupling is manifested in two aspects. First, it provides a priori information for the feature matching of the underwater enhanced image, increasing the speed and accuracy of the matching. Second, it provides real speed information to optimize the initialization and nonlinear optimization of the algorithm, which in turn improve the accuracy of navigation and positioning.

To validate the proposed algorithm, we conducted AFRL dataset simulation experiments and physical experiments. The results of these experiments demonstrated that our proposed algorithm has good accuracy and underwater adaptability. Additionally, our system provides significant improvements in terms of MAE and STD over VINS-Mono.