Autonomous Underwater Vehicle Navigation Enhancement by Optimized Side-Scan Sonar Registration and Improved Post-Processing Model Based on Factor Graph Optimization

Abstract

Autonomous Underwater Vehicles (AUVs) equipped with Side-Scan Sonar (SSS) play a critical role in seabed mapping, where precise navigation data are essential for mosaicking sonar images to delineate the seafloor’s topography and feature locations. However, the accuracy of AUV navigation, based on Strapdown Inertial Navigation System (SINS)/Doppler Velocity Log (DVL) systems, tends to degrade over long-term mapping, which compromises the quality of sonar image mosaics. This study addresses the challenge by introducing a post-processing navigation method for AUV SSS surveys, utilizing Factor Graph Optimization (FGO). Specifically, the method utilizes an improved Fourier-based image registration algorithm to generate more robust relative position measurements. Then, through the integration of these measurements with data from SINS, DVL, and surface Global Navigation Satellite System (GNSS) within the FGO framework, the approach notably enhances the accuracy of the complete trajectory for AUV missions. Finally, the proposed method has been validated through both the simulation and AUV marine experiments.

1. Introduction

The utilization of Autonomous Underwater Vehicles (AUVs) equipped with Side-Scan Sonar (SSS) stands as a pivotal technique for seabed scanning [1]. AUVs typically utilize a navigation system that integrates data from the Strapdown Inertial Navigation System (SINS) and the Doppler Velocity Log (DVL) [2]. By amalgamating the measurements from the Side-Scan Sonar (SSS) with the navigation data, seabed acoustic mosaics are generated, which contain navigation information and the positions of significant seabed features [3]. The precision of the AUV’s navigation data holds substantial sway over the accuracy of seabed mapping and the localization of seabed features. Nevertheless, conventional SINS/DVL-integrated navigation methods are susceptible to errors stemming from imprecise DVL measurements, scale factors, and mounting error angles, thereby culminating in positioning discrepancies over time. While post-processing with satellite data before and after diving might yield enhancement, this approach is constrained by the absence of positional data during the underwater phase and limitations in modeling accuracy. Based on our experimental findings, although post-processing with satellite data before and after diving could offer some enhancements, this approach is hindered by the lack of positional data during the underwater phase and limitations in modeling accuracy.

AUV utilizes the SINS/DVL-integrated navigation method to obtain navigation information underwater [4,5]. This is a dead-reckoning method that achieves positioning by fusing and integrating the measurements of DVL and SINS. However, not only the heading errors of SINS would cause overall trajectory deviations, but also there are error sources such as the change in DVL calibration due to sound speed and installation errors between DVL and SINS in the system, leading to accumulated positioning errors of underwater AUVs [6,7].

Performing simple post-processing with GNSS measurements taken before the submerging and after the surfacing of AUV could optimize the trajectory to some extent [8,9], but there are still some issues remaining.

The first limitation is the difficulty in obtaining stable and reliable relative position measurements from Side-Scan Sonar (SSS) images: Relative position measurements from sonar image registration can complement the measurements; conventional feature-based image registration methods are limited in dull environments [10], making them prone to failure in seabed sonar images. Image registration methods based on the Fast Fourier Transform (FFT) show improved performance by leveraging overall image information [11,12], and it is already utilized in image sonar. However, the registration of SSS images presents a greater challenge. Unlike images from conventional image sonar, which typically exhibit similar perspectives, the perspectives of SSS images are usually entirely distinct. This leads to significant discrepancies between images, resulting in inaccuracies in the determination of relative positions in the output and the potential outliers.

The second limitation is that existing methods often lack the estimation of the AUV’s heading and the DVL installation errors. These omissions significantly affect the quality of SSS imaging, for it is the AUV’s heading, rather than its navigational course, which governs the angle of this frame in the stitched map. The conventional map stitching methods do not correct for heading, treating the heading values obtained from the SINS/DVL-integrated navigation as true values [10,13], which limits the maximum achievable accuracy. However, existing methods typically overlook the installation errors between DVL and the SINS [14]. This omission results in errors in the transmission of position correction within the method, casting suboptimal accuracy in heading estimation [15]. Although traditional filtering methods could estimate errors in time series by extending the system model, they often encounter difficulties when the intervals between positional measurements are excessively long [16]. This challenge arises from the nonlinear system, where certain states, despite being unobservable at a given moment, may still be inaccurately estimated by the filter, potentially resulting in system instability. Consequently, this might be a reason why existing methods generally do not incorporate the estimation of these errors.

This study aims to address the two previously mentioned issues in the AUV navigation information for seafloor mapping. On one hand, an improved SSS image registration method based on the FFT is designed and implemented, which enables a more stable and accurate acquisition of relative positions from the SSS. On the other hand, to concurrently estimate the state sequence of the AUV and the constant system error parameters, we propose a method based on FGO. Compared to filter-based methods, which necessitate the sequential estimation of constant states, the FGO-based method allows for the flexible modeling of states as either sequences or constants [17,18], making it particularly suitable for scenarios like this study, where large time intervals between measurements are utilized to estimate constant states such as installation errors.

Ultimately, by using FGO to fuse relative position from SSS images and position from GNSS before submersion and after surfacing, it becomes possible to simultaneously estimate navigation information such as attitude, velocity, and position as time series, along with constant error parameters like DVL scale factors and installation errors. This method offers two significant advantages: first, it allows for the estimation of more accurate AUV attitude and position, thereby enhancing the quality of seafloor images through improved attitudes and positions of the SSS. Second, the derived parameters, such as installation errors, can serve as valuable references for the AUV’s subsequent missions.

The contribution of this study Can be summarized as

• To acquire reliable relative position measurement, an improved Fourier-based image registration technique has been proposed to address the lack of features in SSS seafloor images. This method builds upon traditional registration by incorporating a Gaussian prior to eliminating outliers, and employs bandpass filtering to enhance the accuracy after outlier removal. This approach not only significantly improves the stability of the registration process, but also requires minimal computational resources since both enhancements are applied to the normalized cross-correlation function.
• A post-processing method based on Factor Graph Optimization (FGO) is proposed that facilitates the simultaneous estimation of the varying AUV’s navigation information and constant error parameters, such as DVL scale factors and installation errors. With position measurements, this method not only allows for the optimization of trajectory information in the same manner as traditional post-processing methods, but also enables an effective optimization of heading information.

This paper includes an improved SSS registration method, an enhanced post-processing model, and an optimization implementation based on FGO. The sonar registration algorithm was validated through SSS seabed images, while the optimization method was validated through two sets of experiments using both simulated data and ocean trial data. Section 2 presents the overview, Section 3 presents the improved sonar image registration method and its validation, Section 4 proposes the integrated navigation with measurement from SSS image registration based on FGO, and Section 5 and Section 6 provide comparative simulations and experiments of the existing methods and the improved methods, respectively.

2. Overview

The proposed approach is outlined in Figure 1, which comprises three primary components.

(1)

Registration of SSS images

SSS image pair selection, image pair pre-processing, and Fourier-based registration are included, aiming to establish the spatial relationship between the SSS images. This will be demonstrated with details in Section 3.

(2)

Factor graph optimization

Based on the improved high-accuracy pre-integration model and DVL/SSS measurement residuals, as shown in Section 4, the state of the entire trajectory will be batch-optimized through FGO. The specifics of the FGO formulation are provided in Section 4.1. The DVL measurement model will be presented in Section 4.2. The residuals and Jacobian matrix will be demonstrated in Section 4.3 and Section 4.4.

(3)

Information output

After the states are estimated using FGO, the navigational information for the trajectory can be generated.

3. Fourier-Based SSS Image Registration

When the AUV navigates to a location near its previous path, the survey areas of the SSS will overlap. Pre-processing and registration can be applied to the partially overlapping sonar images to obtain relative position measurements.

This section presents an improved Fourier-based image registration technique. It builds on the standard Fourier-based SSS Image Registration method by adding two key improvements. The two improvements involve the addition of a Gaussian prior and the use of a window function on the normalized cross-correlation function, which, respectively, address significant errors and minor errors in SSS image registration. These methods will be validated using examples of SSS images.

3.1. Standard Fourier-Based SSS Registration

3.1.1. Pre-Process of SSS Images

Prior to registration, it is necessary to perform slant-range correction, essential image enhancement, and longitudinal intensity normalization for SSS images. Assuming a level seabed, the formula for slant-range correction is as follows:

$$L_{feature}=\sqrt{{\left(L_{slope}\right)}^2-{\left(H\right)}^2}$$

(1)

where $L_{feature}$ and $L_{slope}$ represent the distance from a point on the seabed to the midline of the acoustic survey path, and the diagonal distance from that point to the SSS transducer, respectively. $H$ represents the height of the transducer relative to the seabed.

After slant-range correction and necessary image enhancement, a vertical intensity normalization is required due to the inherent vertical intensity unevenness in the images. It can be shown as

$$\begin{array}{l}{I}^{\prime }\left(i,j\right)=I\left(i,j\right)+c_j\\ c_j={\mu }_{global}-{\mu }_{col}\left(j\right)\end{array}$$

(2)

where $I^{\prime }\left(i,j\right)$ and $I\left(i,j\right)$, respectively, represent the pixel intensity values at the row $i$ and column $j$ of the intensity-balanced image and the original image; $c_j$ represents the intensity adjustment value for column $j$; ${\mu }_{global}$ is the global mean intensity of the original image $I$; and ${\mu }_{col}\left(j\right)$ is the mean intensity of column $j$ in the original image.

3.1.2. Fourier-Based SSS Registration

As the scanning routes are nearly parallel, it can be assumed that the two images $g$ and $f$ to be registered only have a translational relationship, as shown in Equation (3).

$$f\left(x,y\right)=g\left(x-t_x,y-t_y\right)$$

(3)

The normalized cross-power spectrum is illustrated in Equation (4). The robustness of Fourier-based registration to noise is attributed to the process-normalized process in Equation (4), which can be viewed as a pre-whitening step.

$$C(u,v)={\displaystyle \frac{F(u,v)G^{*}(u,v)}{\left|F(u,v)G^{*}(u,v)\right|}}=e^{-i(u{t}_x+v{t}_y)}$$

(4)

Subsequently, through the inverse Fourier transform, the Phase Correlation Matrix (PCM) can be obtained as shown in (5):

$$P(u,v)={\mathcal{F}}^{-1}C(u,v)$$

(5)

Finally, the position relationship is determined in Equation (6), which is the relative position measurement corrected by sonar image pairs.

$$\left(t_x,t_y\right)=\underset{\left(u,v\right)}{\arg \max }\left\{P(u,v)\right\}$$

(6)

3.2. The Optimization of SSS Registration

3.2.1. Gaussian Prior-Enhanced SSS Registration

The common features present in SSS images often introduce errors in image registration. Along an AUV track, the intensity of SSS images exhibits a gradient, increasing from the port side to a peak at the centerline, followed by a decrease towards the starboard side. Additionally, the contrast fluctuates between weak and strong, reaching its peak at a moderate distance from the midline of the acoustic survey path. When seabed features are not clearly defined, these fixed characteristics of intensity and contrast can dominate the registration results, causing errors in relative position measurements.

However, it has been observed that even when the underwater features in the images are not clearly defined, the PCM frequently yields local maxima in proximity to the correct registration outcomes, albeit not globally optimal. Consequently, this study suggests leveraging the coarse relative positional data supplied by the AUV navigation system as a Gaussian prior, and integrating this Gaussian prior into the registration process to effectively derive meaningful registration results from the PCM.

When the prior image displacement is $\left(t_x^{*},t_y^{*}\right)$, the two-dimensional normal distribution $f(u,v)$ is defined as

$$f(u,v)={\displaystyle \frac{1}{2\pi {\sigma }_u{\sigma }_v}}\exp \left(-{\displaystyle \frac{1}{2}}\left[{\displaystyle \frac{{(u-t_x^{*})}^2}{{\sigma }_u^2}}-{\displaystyle \frac{2\rho (u-t_x^{*})(v-t_y^{*})}{{\sigma }_u{\sigma }_v}}+{\displaystyle \frac{{(v-t_y^{*})}^2}{{\sigma }_v^2}}\right]\right)$$

(7)

where ${\sigma }_u$ and ${\sigma }_v$ represent the standard deviations of $f(u,v)$ in the $u$ and $v$ directions, determined by the attitude holding capability of the SINS; $t_x^{*}$ and $t_y^{*}$ denote the prior image displacements in the horizontal and vertical directions, respectively.

The PCM with added Gaussian prior is denoted as $P^{Gaussian}(u,v)$.

$$P^{Gaussian}(u,v)=P(u,v)+f(u,v)\cdot (P_{\max }-P_{\min })\cdot k$$

(8)

where $P_{\max }$ and $P_{\min }$ represent the maximum and minimum values of $P(u,v)$, respectively. The parameter $k$ represents the confidence coefficient of the prior information, with values ranging from 0 to 1, typically determined empirically.

By using $P^{Gaussian}$ instead of the original PCM, the registration results with the added Gaussian prior can be obtained.

$$\left(t_x,t_y\right)=\underset{\left(u,v\right)}{\mathrm{argmax}}\left\{P^{Gaussian}(u,v)\right\}$$

(9)

3.2.2. Bandpass Filter-Aided SSS Registration

Although the inclusion of prior information can reduce errors introduced by the inherent characteristics in SSS images, the presence of noise and perspective effects still compromise the precision of registration.

Bandpass filters serve to highlight the seabed features of interest, thereby improving the quality of registration. Moreover, since Fourier-based image registration operates in the frequency domain, the enhanced registration results achieved through bandpass filtering can be obtained with minimal computational effort. This is achieved by simply computing the Hadamard product of the normalized cross-power spectrum with a designed window function.

The frequency limits for FIR bandpass filters can be set independently along the horizontal and vertical axes.

The frequency limits for FIR bandpass filters can be individually set for both the horizontal and vertical dimensions. This entails defining the upper frequency limit $f_{high,row}$ and the lower frequency limit $f_{low,row}$ for the horizontal direction, as well as the upper frequency limit $f_{high,col}$ and the lower frequency limit $f_{low,col}$ for the vertical direction.

$$\begin{array}{l}0\le f_{low,row}<f_{high,row}\le 1\\ 0\le f_{low,col}<f_{high,col}\le 1\end{array}$$

(10)

Then, these parameters are used to define the row and column dimensions of the bandpass filter.

$$\begin{array}{l}{H}_{\mathrm{row}}=⌊f_{\mathrm{high},\mathrm{row}}\times m⌋\\ L_{\mathrm{row}}=⌊f_{\mathrm{low},\mathrm{row}}\times m⌋\\ H_{\mathrm{col}}=⌊f_{\mathrm{high},\mathrm{col}}\times n⌋\\ L_{\mathrm{col}}=⌊f_{\mathrm{low},\mathrm{col}}\times n⌋\end{array}$$

(11)

where $m$ represents the number of rows in the image, $n$ is the number of columns in the image, and $⌊·⌋$ denotes the floor function, which rounds down to the nearest integer.

Then, the window function W can be determined.

$$\begin{gathered} RowPass\left(u,L_{\mathrm{row}},H_{\mathrm{row}},m\right)=\left\{\begin{array}{cc}1\hfill & \mathrm{if} u\in (L_{\mathrm{row}},H_{\mathrm{row}})\cup (m-H_{\mathrm{row}}+1,m-L_{\mathrm{row}}+1)\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right. \\ ColPass\left(v,L_{col},H_{col},n\right)=\left\{\begin{array}{cc}1\hfill & \mathrm{if} v\in (L_{col},H_{\mathrm{col}})\cup (n-H_{\mathrm{col}}+1,m-L_{\mathrm{col}}+1)\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right. \\ W(u,v)=RowPass\left(u,L_{\mathrm{row}},H_{\mathrm{row}},m\right)\times ColPass\left(v,L_{col},H_{col},n\right) \end{gathered}$$

(12)

where $u$ and $v$ represent the indices for the rows and columns, respectively.

Subsequently, the enhanced frequency domain results can be achieved by using the Hadamard product between the window function and the normalized cross-power spectrum.

$$C^{FIR}(u,v)=C(u,v)\odot W(u,v)$$

(13)

where $\odot$ denotes the operation of taking the Hadamard product of two matrices.

After this step, the time-domain results, which are also the optimized PCM, can be derived through an inverse Fourier transform, following the same procedure as standard Fourier-based image registration.

$$\begin{array}{l}{P}^{\mathrm{FIR}}(u,v)={\mathcal{F}}^{-1}{C}^{FIR}(u,v)\\ \left(t_x,t_y\right)=\underset{\left(u,v\right)}{\mathrm{argmax}}\left\{P^{\mathrm{FIR}}(u,v)\right\}\end{array}$$

(14)

3.3. Evaluation of Optimized Fourier-Based SSS Registration

To validate and verify the performance of the improved registration method, the following four registration approaches will be applied to the same pairs of sonar images:

• Classic Fourier-based image registration method;
• Fourier-based registration method optimized with Gaussian prior;
• Fourier-based registration method optimized with bandpass filter;
• Fourier-based registration method optimized with both Gaussian prior and bandpass filter.

The SSS image pairs employed in the experimental study were meticulously chosen to comprise seabed visuals with ambiguous characteristics, acquired by an unmanned semi-submersible along contiguous survey transects. The two images only have a translational relationship between them. The pre-registered SSS image pairs, which have undergone image enhancement and intensity balancing, are shown in Figure 2. Each pixel in the image represents 0.1 m. According to GNSS measurements, in correct registration, the left half of Figure 2b should overlap with the right half of Figure 2a, indicating a minimal vertical displacement and a lateral displacement approximately equal to half the width of the image.

3.3.1. Fourier-Based Image Registration

Under the classic Fourier-based image registration, the PCM and the overlapping result of the two images are shown as Figure 3.

This registration attempt was unsuccessful, with an error of 76.2 m, rendering it unsuitable for positional measurements.

In the classic Fourier-based registration algorithm, the coordinates of the maximum value of the normalized cross-correlation function on the upward and rightward axes represent the relative pixel displacement of the two images in the upward and rightward directions, respectively. However, as shown in Figure 3a, the maximum of the PCM is located on the ridge with a rightward displacement close to zero, rather than at the correct registration position. The overlap image of the registration result is shown in Figure 3b, where it can be observed that due to the influence of the longitudinal features, the registration algorithm considers the two images to be almost completely overlapping. This is far from the reference position provided by GNSS. This is a failed registration with an error of 76.16 m, which is unacceptable.

The two unfavorable conditions for this registration can be observed in the PCM. Firstly, there is a ridge at a rightward displacement close to zero, which is generated by the influence of residual longitudinal features in the image. Secondly, the PCM contains numerous small spikes, which are caused by high-frequency noise in the image and lead to local registration errors. The PCM’s maximum value, on which this failed registration is based, corresponds to a small spike on the ridge caused by high-frequency noise due to the longitudinal features. However, at the end of the rightward axis of Figure 3a, there is a noticeable but less prominent peak, the position of which aligns with the reference provided by GNSS. This indicates that a frequency domain analysis is capable of extracting the correct displacement information, but it is overwhelmed by misleading information. This demonstrates the feasibility of optimizing the registration process.

3.3.2. Gaussian Prior-Optimized Registration

Under the registration method optimized with Gaussian prior, the PCM and the overlapping result of the two images are shown as follows.

This is a successful but less precise registration, with an error of 0.9 m as referenced by GNSS measurements.

Figure 4a shows that, compared to the PCM in the classic registration method, the PCM with Gaussian prior optimization has added convexities due to the prior, which weights the maxima near the prior. This highlighting makes the peaks, which were overwhelmed by misleading information in the classic method, stand out and become more likely to be the global maximum. Therefore, Gaussian prior optimization in registration can largely avoid gross errors caused by the subtlety of seabed features, making the output registration results more reliable. Figure 4b also illustrates this point; the overlap shown in the registration result is more similar to the reference value provided by GNSS.

However, there are two shortcomings to the registration method that uses only Gaussian prior optimization. On one hand, the effectiveness of this method is inherently influenced by the accuracy of the prior information. When the confidence in the prior information is not high, the two-dimensional Gaussian distribution will strengthen the maxima of the cross-correlation function over a broader range. Since the original PCM contains multiple maxima, if the prior is close to a wrong maximum, it could also lead to an incorrect registration result. For example, there are local maxima caused by low-frequency features in Figure 4a, which might compete with the correct registration maximum when the prior information is not highly confident or is biased. On the other hand, when we focus on the local area of the PCM as shown in Figure 4c, it can be observed that due to the high-frequency noise in the acoustic imagery, there are numerous noise spikes around the maximum value, which can interfere with the accuracy of local registration. Figure 4d displays the enlarged details of the overlap, where it can be seen that, compared to the overall features, it is the small bright spots that align more precisely.

3.3.3. Bandpass Filter-Optimized Registration

Under the registration method optimized with a bandpass filter, the PCM and the overlapping result of the two images are shown as follows.

This represents a failed registration with an error of 60.07 m.

From Figure 5a, it is evident that the PCM optimized with bandpass filtering retains only two groups of peak values. These include a local maximum with a larger rightward value, which corresponds to the correct registration we expect, and another peak at a rightward displacement close to zero, which is due to the incorrect registration caused by longitudinal features. In contrast to the traditional method and the Gaussian prior optimization method, the cross-correlation function near the correct registration position in the PCM with bandpass filtering optimization is clearer. This showcases the noise suppression and peak optimization performance of the bandpass filtering.

However, due to the subtle features of this image pair, the maximum value of the cross-correlation function still lies at the incorrect peak. Although bandpass optimization can filter out most of the competing erroneous registration results, it cannot guarantee that the registration of interest becomes the global maximum without individually tuning parameters for each image pair. Considering that the image pairs to be registered may not always have visually identifiable features, the effectiveness of using bandpass filtering optimization alone for registration is limited.

3.3.4. Gaussian Prior- and Bandpass Filter-Integrated Registration

Under the registration method optimized with both a Gaussian prior and bandpass filter, the PCM and the overlapping result of the two images are shown as follows.

This is a successful registration, with a reference GNSS measurement error of 0.2 m. Given that each pixel in the image represents 0.1 m, the precision of this registration is satisfactory.

In Figure 6, it can be observed that with the complementarity of the two optimization methods, the cross-correlation function value at the correct registration position successfully becomes the global maximum and is no longer affected by noise spikes and potential competing local maxima. The Gaussian prior optimization can exclude erroneous registrations with gross errors, while the bandpass optimization can refine the registration near the prior position. This results in the most accurate registration outcome.

The registration results for the four methods are shown in

Table 1. The registration results for the four methods.

Registration Method	Precision
Classic Fourier-based image registration	16.2 m
Registration optimized with Gaussian prior	0.9 m
Registration optimized with bandpass filter	60.7 m
Registration optimized with both Gaussian prior and bandpass filter	0.2 m

below.

In conclusion, in the experiments with the four methods, it can be observed that the classic Fourier-based image registration is susceptible to the influence of high-frequency noise and residual longitudinal features in the SSS images, making it prone to failure when dealing with images lacking distinct features.

Optimization with Gaussian prior can eliminate large errors during registration, but still can be misled by local maxima, resulting in registration errors.

Optimization with bandpass filtering can focus on the features at the desired scale in the image, reducing noise spikes and some misleading local maxima. However, it cannot guarantee that the peak value of interest in the registration result is the global maximum. Hence, it may still be influenced by the shared features in the SSS images.

By combining the optimization based on Gaussian prior and the optimization based on bandpass filtering in a single registration, the strengths of both methods can be leveraged. This approach not only eliminates gross errors to improve the likelihood of correct registration but also reduces the risk of being misled by local maxima near the prior registration position. The integrated optimization method of Gaussian prior and bandpass filtering requires minimal computational resources while greatly improving the stability of the registration process.

4. SSS-Integrated Navigation Method Based on FGO

A navigation data processing method based on FGO is introduced, which integrates SSS position measurements and incorporates estimates of DVL scale factors and installation errors. This section includes updated factor graph structures, definitions of residuals, and calculations of the Jacobian matrix. In addition, the IMU pre-integration method used in the approach is an improved method that takes into account the Earth’s rotation [19]. The frame nomenclature is provided in

Table 2. Nomenclature.

Symbol	Description
$n$	Ideal local-level navigation frame
$b$	Body frame
$e$	Earth frame
$i$	Nonrotating inertial frame

.

4.1. Formulation of FGO

In Figure 7, the structure of the proposed factor graph is depicted, using circles to represent the states $x_k$, and squares to represent the factors or the residuals. The corresponding factor is explicitly defined in the subsequent sections.

At time step k, the state $x_k$ is defined as Equation (15),

$$x_k=\left[\begin{array}{ccccc}{C_b^n}_k& v_k& p_k& {\epsilon }_k& {\nabla }_k\end{array}\right]$$

(15)

where ${C_b^n}_k$ represents the attitude Direction Cosine Matrix (DCM), $\delta v_k$ is velocity error, $\delta p_k$ is the position error, and ${\epsilon }_k$ and ${\nabla }_k$ are the bias of gyros and accelerators, respectively.

Then, the state encompassing each time step in the factor graph can be expressed as (16).

$$X=\left(\begin{array}{cccccccc}{x}_2& x_2& x_3& \cdots & x_{n-1}& x_n& {\varphi }_{DVL}& \delta k\end{array}\right)$$

(16)

In this context, ${\varphi }_{DVL}={\left[\begin{array}{ccc}\alpha & \beta & \gamma \end{array}\right]}^{{\rm T}}$ denotes the DVL installation error angles consisting of the roll, heading, and pitch. $\delta k$ denotes the scale factor error of the DVL. Both ${\varphi }_{DVL}$ and $\delta k$ are defined as time-invariant constants.

4.2. DVL Measurement Model

Errors in DVL velocity measurements can be classified into systematic errors and non-systematic errors. Systematic errors include scale factor errors and installation errors, which can be calibrated out. See Figure 8.

Define the DVL velocity measurement as $v_{DVL}={\left[\begin{array}{ccc}{v}_x^d& v_y^d& v_z^d\end{array}\right]}^T$; the measurement model of the DVL can be expressed as

$${\tilde{v}}_{DVL}=C_d^b\left(1+k\right)v_{DVL}=\left(I_{3\times 3}-{\varphi }_{DVL}^\right)\left(1+\delta k\right)v_{DVL}+\delta v_{DVL}$$

(17)

The DCM for transforming from the DVL-fixed coordinate to the vehicle coordinate is denoted by $C_d^b$. ${\tilde{v}}_{DVL}$ represents the measurement obtained from the DVL, whereas $\delta v_{DVL}$ is the measurement noise of DVL, typically considered as white noise. $\delta k$ denotes the DVL scaling factor error; ${\varphi }_{DVL}^$ is the skew-symmetric matrix generated from ${\varphi }_{DVL}={\left[\begin{array}{ccc}\alpha & \beta & \gamma \end{array}\right]}^{{\rm T}}$.

$${\varphi }_{DVL}^=\left[\begin{array}{ccc}0& -\gamma & \beta \\ \gamma & 0& -\alpha \\ -\beta & \alpha & 0\end{array}\right]$$

(18)

4.3. Definition of Residuals

(1).

Residuals of pre-integration factors

The INS residuals $r_{\Delta INS}={\left[\begin{array}{cccc}{r}_{\Delta R_{ij}}& r_{\Delta v_{ij}}& r_{\Delta p_{ij}}& \begin{array}{cc}{r}_{b_g}& r_{b_a}\end{array}\end{array}\right]}^T$ can be expressed as (19); the $r_{\Delta R_{ij}}$, $r_{\Delta v_{ij}}$, $r_{\Delta p_{ij}}$, $r_{b_g}$, and $r_{b_a}$ represent the attitude, velocity, position, accelerometers, and gyroscope residuals, respectively.

$$\begin{array}{l}{r}_{\Delta R_{ij}}=Log\left(\Delta {\tilde{R}}_{ij}^T{\left(C_{n\left(i\right)}^{n\left(j\right)}{C}_{b\left(i\right)}^{n\left(i\right)}\right)}^{-1}{C}_{b\left(j\right)}^{n\left(j\right)}\right)\\ r_{\Delta v_{ij}}=v_{en\left(j\right)}^{n\left(j\right)}-v_{en\left(i\right)}^{n\left(i\right)}-g^n\Delta T-\Delta {\tilde{v}}_{ij}\\ r_{\Delta p_{ij}}={\displaystyle \frac{1}{M_{pv}}}\left(p_j-p_i\right)-v_{en(i)}^{n(i)}\Delta T-{\displaystyle \frac{1}{2}}{g}^n\Delta T^2-\Delta {\tilde{p}}_{ij}\\ r_{b_g}=b_{g,j}-b_{g,i}\\ r_{b_a}=b_{a,j}-b_{a,i}\end{array}$$

(19)

(2).

Residual of the DVL measurement factor

The DVL measurement residuals $r_{v_{DVL}}$ and the SSS measurement residuals can be expressed as (22); the $\Delta {\tilde{p}}_{SSSab}$ indicates the relative position error calculated by (20).

$$r_{v_{DVL}}=\left(v_i-C_{b(i)}^{n(i)}{v}_{\mathrm{DVL}}\right)$$

(20)

(3).

Residual of SSS measurement factor

The residuals related to the relative position measurements from the SSS are expressed as follows,

$$r_{\Delta p_{SSSij}}=\left({\displaystyle \frac{1}{M_{pv}}}\left(p_j-p_i\right)-\Delta {\tilde{p}}_{SSSij}\right)$$

(21)

where $\Delta {\tilde{p}}_{SSSij}={\overline{C}}_{b(i)}^{n(i)}\Delta {\tilde{p}}_{SSSij}^{b(i)}$. ${\overline{C}}_{b(i)}^{n(i)}$ represents the rotation matrix derived from the average heading over a period of time centered at time $i$. The duration of this time period is determined by the length of the corresponding SSS image during image matching, which is fixed at 50 s in this paper.

(4).

Surface GNSS measurement factor

$$r_{\Delta p_{GPS}}={\displaystyle \frac{1}{M_{pv}}}\left(p_i-p_{GPS}\right)$$

(22)

Finally, the whole state can be optimally estimated by the following expression,

$$X^{*}=\arg {\min }_X{\displaystyle \sum _k{‖r_{\Delta INS}‖}_{{\sum }_k^{INS}}^2+{‖r_{v_{DVL}}‖}_{{\sum }_k^{DVL}}^2+{‖r_{\Delta p_{SSSij}}‖}_{{\sum }_k^{SSS}}^2+{‖r_{\Delta p_{GPS}}‖}_{{\sum }_k^{GPS}}^2+{‖r_{PRIOR}‖}_{{\sum }_k^{PRIOR}}^2}$$

(23)

where ${\sum }_k^{INS}$, ${\sum }_k^{DVL}$, ${\sum }_k^{SSS}$, ${\sum }_k^{GPS}$, and ${\sum }_k^{PRIOR}$ are all convex matrices. ${\sum }_k^{INS}$ is the weight of the INS residuals, determined by the accuracy of the IMU; ${\sum }_k^{DVL}$ is the weight of the DVL measurement residuals, determined by the measurement noise of the DVL; ${\sum }_k^{SSS}$ is the weight of SSS measurement residuals, which is determined by the matching noise of the SSS; ${\sum }_k^{GPS}$ is the weight of GPS measurement residuals and determined by the measurement noise of GPS; and ${\sum }_k^{PRIOR}$ can be obtained through the marginalization or set directly.

4.4. Jacobian Matrix

To solve the nonlinear least squares problem presented in Equation (23), it is necessary to compute the detailed Jacobian matrix. The corresponding derivation of the Jacobian matrix is presented as follows.

(1).

Jacobian matrix of IMU pre-integration

The Jacobian matrix of the pre-integration factor for each state is as follows:

$$\begin{array}{l}{J}_{IMU\_PRE}=\\ \left[\begin{array}{cccccccccc}-J_r^{-1}\left(r_{\Delta {\tilde{R}}_{ij}}\right){\left(C_{b\left(j\right)}^{n\left(j\right)}\right)}^{-1}\left[I-T({\omega }_{in}^n^)\right]C_{b(i)}^{n(i)}& 0_{3\times 3}& 0_{3\times 3}& -J_r^{-1}\left(r_{\Delta {\tilde{R}}_{ij}}\right)\Delta {\tilde{R}}_{ij}^T{\left(\left[I-T({\omega }_{in}^n^)\right]C_{b\left(i\right)}^{n\left(i\right)}\right)}^{-1}{J}_r\left({\displaystyle \frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}}\right)& 0_{3\times 3}& J_r^{-1}\left(r_{\Delta {\tilde{R}}_{ij}}\right)& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}& -I_{3\times 3}& 0_{3\times 3}& -{\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}}& -{\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}& -\Delta T& -I_{3\times 3}& -{\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}}& -{\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\end{array}\right]\end{array}$$

(24)

where the ${\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}$, ${\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}$, ${\displaystyle \frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}}$, ${\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}}$, ${\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}}$, ${\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}}$, and ${\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}}$ need to be obtained from recursion, as follows:

$$\begin{array}{l}{\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}=-{\displaystyle \sum _{k=i}^{j-1}\left(C_{n(i)}^{n(k)}{C}_{b(i)}^{n(i)}\left(C_{b(k)}^{b(i)}\left(\Delta v_k+\Delta v_{\mathrm{rot}(k)}^{b(k-1)}+\Delta v_{\mathrm{scul}(k)}^{b(k-1)}\right)\right)^\right)}\\ {\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}}={\displaystyle \sum _{k=i}^{j-1}\frac{\partial \Delta {\tilde{v}}_{ik}}{\partial C_{b\left(i\right)}^{n\left(i\right)}}\Delta t}\\ {\displaystyle \frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}}=-{\displaystyle \sum _{k=i}^{j-1}\left[\Delta {\tilde{R}}_{k+1,j}{J}_{r,k}\Delta t\right]}\\ {\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}}=-{\displaystyle \sum _{k=i}^{j-1}\left[C_{n(i)}^{n(k)}{C}_{b(i)}^{n(i)}\Delta {\tilde{R}}_{ik}\Delta v_k^\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\Delta t\right]}\\ {\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}}=-{\displaystyle \sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{v}}_{ik}}{\partial b_{g,i}}\Delta t\right]}\\ {\displaystyle \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}}=-{\displaystyle \sum _{k=i}^{j-1}\left[C_{n(i)}^{n(k)}{C}_{b(i)}^{n(i)}\Delta {\tilde{R}}_{ik}\Delta t\right]}\\ {\displaystyle \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}}=-{\displaystyle \sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{v}}_{ik}}{\partial b_{a,i}}\Delta t\right]}\end{array}$$

(25)

(2).

Jacobian matrix of DVL measurement

$${\displaystyle \frac{\partial r_{v_{DVL}}}{\partial x_k}}=\left[\begin{array}{ccccc}{C}_{b\left(i\right)}^{n\left(i\right)}{\tilde{v}}_{DVL}^& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(26)

$${\displaystyle \frac{\partial r_{v_{DVL}}}{\partial {\varphi }_{DVL}}}=-C_{b(i)}^{n(i)}\left(\left(1+\delta k\right)v_{DVL}\right)$$

(27)

$${\displaystyle \frac{\partial r_{v_{DVL}}}{\partial \left(\delta k\right)}}=-C_{b(i)}^{n(i)}{v}_{DVL}+C_{b(i)}^{n(i)}{\varphi }_{DVL}^v_{DVL}$$

(28)

(3).

Jacobian matrix of SSS measurement

$$\begin{array}{c}{\displaystyle \frac{r_{\Delta p_{SSSij}}}{\partial x_i}}=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& -I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\\ {\displaystyle \frac{r_{\Delta p_{SSSij}}}{\partial x_j}}=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\end{array}$$

(29)

(4).

Jacobian matrix related to the surface GNSS measurements

$${\displaystyle \frac{{\partial r}_{pGPS}}{\partial x_k}}=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& {\displaystyle \frac{1}{M_{pv}}}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(30)

5. Simulation and Result Analysis

5.1. Setup of Simulation

The simulation models 3000 s of maneuvers, which corresponds to a typical underwater “Z” route for AUVs, with each side of the route, called a survey line, measuring 500 m. All methods incorporate an initial heading error of 2.5°.

The measurements in the simulation include IMU measurements, DVL measurements, relative position measurements from the SSS, and surface GNSS measurements. The IMU’s data frequency is set to 100 Hz, with the zero biases for the three-axis gyroscopes set at 1.0°/h, 0.9°/h, and −1.1°/h, respectively. Gaussian noise with a standard deviation of 0.05°/s and 0.3 mg is introduced. The zero biases for the three-axis accelerometers are set at 0.2 mg, −0.25 mg, and 0.18 mg. Additionally, angular jitter and linear motion are incorporated across all three axes. The DVL data frequency is 1 Hz. The scale factor is configured to 0.98, and the installation errors for the DVL are set to 0.2°, −0.5°, and −0.5°, respectively. The SSS images are registered every 50 m along the survey line, with the registration results subjected to Gaussian noise with a standard deviation of 5m. The GNSS measurement frequency is 1 Hz and is only valid at the beginning and end of the simulation, with Gaussian noise having a standard deviation of 0.5 m.

A total of six methods are verified as follows:

• EKF: SINS+DVL. This method represents the traditional EKF-based SINS/DVL-integrated navigation approach. It is used to illustrate the effectiveness of the most widely applied filtering methods for AUV navigation.
• FGO: SINS+DVL. This method is based on FGO for SINS/DVL-integrated navigation, allowing for the batch optimization of the entire trajectory using all available SINS and DVL measurement data. It is used to demonstrate the effectiveness of batch processing methods in AUV navigation.
• FGO: SINS+DVL+SSS. This method incorporates relative position measurements from the SSS into method 2. It serves to assess the auxiliary role of the proposed SSS relative position measurements in AUV navigation.
• FGO: SINS+DVL+Position Constraint. This method integrates GNSS position constraints, obtained before descent and after surfacing, into the FGO-based SINS/DVL-integrated navigation model. It is used to demonstrate the effectiveness of post-processing navigation data using a traditional measurement source.
• FGO: SINS+DVL+Position Constraint+SSS. This method incorporates relative position measurements from the SSS into method 4. It serves as a comparison to method 4, assessing the auxiliary role of the proposed SSS relative position measurements in post-processing navigation data.
• FGO: The Proposed Method, or FGO: SINS+DVL+Position Constraint+SSS+ Improved Model. This method builds upon method 5 by incorporating the model with DVL installation errors proposed in this paper. It is used to compare with method 5, assessing the improvements in DVL installation error calibration and heading estimation brought about by the proposed model.

The EKF method is employed because traditional SINS/DVL-integrated navigation systems predominantly utilize Bayesian filtering techniques, with EKF being one of the most widely applied. The primary focus remains on the comparison between different FGO methods.

5.2. Results of Simulation

Figure 9 shows the comparison of the integrated navigation results of several methods, including trajectory comparison, position error comparison, heading angle error comparison, and gyro zero bias and accelerometer zero bias estimation effect comparison.

Table 3. The comparison of average positioning and heading errors.

Method	Positioning Error (m)	Heading Error (deg)
KF: SINS+DVL	47.638	1.601
FGO: SINS+DVL	35.777	0.227
FGO: SINS+DVL+SSS	31.637	0.090
FGO: SINS+DVL+Position constraint	17.256	0.486
FGO: SINS+DVL+Position constraint+SSS	15.853	0.471
FGO: SINS+DVL+Position constraint+SSS +Improved model (The proposed method)	2.234	0.039

shows the comparison of the average position and heading errors of different methods.

Figure 9 illustrates the trajectory comparisons of different methods. It is evident that among the real-time navigation group that does not utilize GNSS position constraints, the EKF: SINS+DVL method performs the worst, as it struggles to correct the initial heading error due to its reliance on a dead-reckoning algorithm. In contrast, the other two batch processing methods for real-time navigation, namely FGO: SINS+DVL and FGO: SINS+DVL+SSS, show slightly better performance, as they optimize information over the entire trajectory. The FGO: SINS+DVL+SSS method benefits from incorporating relative position measurements from SSS. In the post-processing methods that utilize GNSS position constraints, all methods maintain good trajectory accuracy; however, the proposed method using the enhanced model demonstrates superior performance at the end of the “Z” route.

Figure 10 and Table 3 present the details of the positioning and heading errors for each method. Figure 10a provides a clearer depiction of trajectory accuracy. For the comparative methods, while incorporating SSS position measurements and GNSS position constraints reduces position errors, these errors still display a linear trend as the AUV traverses the “Z” route. This linear variation is primarily attributed to errors in the DVL scale factor. The proposed method significantly mitigates this variation, which is further evidenced by the average position errors presented in Table 3. Figure 10b compares the heading errors of different methods, while Table 3 provides the specific values of the average heading errors. Firstly, the FGO methods, which can optimize historical navigation information, show smaller heading angle errors compared to the EKF method, which does not adjust for previous heading values. Secondly, the inclusion of SSS relative position information is found to enhance the accuracy of heading estimation. More importantly, while adding position constraints without utilizing the improved model decreases the average positioning errors from over 30 m to below 20 m, it results in an increase in the average heading error from below 0.25° to above 0.45°. This occurs due to the existing model not being adequate, leading to incorrect feedback into the heading estimates. In contrast, the proposed method addresses this issue, resulting in reductions in both positioning and heading errors.

Figure 11 and

Table 4. Comparison of average gyroscope and accelerometer bias errors.

Methods	Gyro X Axis Bias (deg/h)	Gyro Y Axis Bias (deg/h)	Gyro Z Axis Bias (deg/h)	Acc X Axis Bias Error (mg)	Acc Y Axis Bias Error (mg)	Acc Z Axis Bias Error (mg)
KF: SINS+DVL	0.598	0.553	1.100	0.088	0.152	0.017
FGO: SINS+DVL	0.081	0.026	1.215	0.017	0.032	0.002
FGO: SINS+DVL+SSS	0.081	0.035	0.555	0.018	0.0337	0.023
FGO: SINS+DVL+Position constraint	0.083	0.055	0.572	0.023	0.403	0.003
FGO: SINS+DVL+SSS+Position constraint	0.083	0.058	0.323	0.013	0.047	0.003
FGO: SINS+DVL+SSS+Position constraint +Improved model (The proposed method)	0.008	0.083	0.272	0.037	0.021	0.003

illustrate the estimates of inertial sensor biases for the different methods. It can be seen that the inclusion of SSS measurements enhances the estimation accuracy of the Z axis gyroscope bias.

The summary of this simulation result is as follows:

• Compared to filtering methods, batch processing methods can optimize past navigation data, significantly improving the positioning accuracy of the AUV during surveying tasks.
• The relative position measurements provided by SSS can enhance the positioning and heading accuracy of the AUV, with a more pronounced effect when position measurements are otherwise lacking.
• Methods that do not incorporate DVL error modeling result in GNSS position measurements improving the system’s positioning accuracy at the expense of its heading accuracy. However, this issue does not occur with the model proposed in this paper, as both positioning and heading exhibit higher accuracy.

6. AUV Marine Experiments and Result Analysis

6.1. Setup for AUV’s Marine Experiments

The physical experiment took place in Sanya, Hainan, and was conducted using a semi-submersible vehicle. During operation, the main body of the robot remains underwater, unaffected by surface waves, while an antenna, approximately the same length as the AUV, is kept above the water surface. This setup allows for the real-time transmission of large volumes of data. Additionally, the antenna is equipped with a GPS module that integrates RTK technology, enabling high-precision positioning results.

The semi-submersible vehicle operates at a speed of approximately 3 knots. An initial heading error of 4° is set for the experiment. Figure 12 illustrates the AUV setup and its trajectory during the experiment.

Table 5. Parameters of the MG-300B MEMS-IMU.

Parameters	Value
Gyro bias stability	0.02°/h
Angle random walk	≤0.015°/√h
Rate bias over temperature	≤1°/h (over life)
Gyro factor error	<100 ppm
Acc bias stability	<0.1 mg
Velocity random walk	<30 μg√Hz
Acc bias over Temp	<2 mg (over life)

,

Table 6. Parameters of the SSS ES4590.

Parameters	Value
Range	75 m (900 kHz)
Resolution	0.17 m

,

Table 7. Parameters of the Waterlinked a125 DVL.

Parameters	Value
Ping rate	2–15 Hz
Velocity resolution	0.1 mm/s
Long-term accuracy	±1.01%

and

Table 8. Parameters of the RTK GPS module K823.

Parameters	Value
Position accuracy	15 mm+ (baseline length (KM)/1,000,000) mm
Update rate	20Hz

display the parameters of the sensors.

The referenced true values for the experiment are sourced as follows

• Position trajectory comparison and position error comparison

The GPS antenna of the semi-submersible vessel remains at the water surface at all times, and the positioning results obtained using RTK technology will be directly utilized as the true position values. The absolute position errors of the results from different methods are compared, along with their average absolute position errors.

2.

Heading error comparison

For semi-submersible unmanned vessels, there is no higher-precision attitude reference installed. Therefore, the output of the smoothed result from the integrated navigation of SINS/GNSS is used as the true heading value. Since the heading angle accuracy of the SINS/GNSS integration is significantly higher than that of the SINS/DVL integration, it is reasonable to consider its results as the true values.

Similar to the simulation, an initial heading error of 4° is also introduced for all methods in the experiment. A total of six methods are verified:

• EKF: SINS+DVL.
• FGO: SINS+DVL.
• FGO: SINS+DVL+SSS.
• FGO: SINS+DVL+Position Constraint.
• FGO: SINS+DVL+Position Constraint+SSS.
• FGO: The Proposed Method, or FGO: SINS+DVL+Position Constraint+SSS+ Improved Model.

6.2. Results of Marine Experiments

Figure 13 displays the trajectory results for all methods.

From Figure 13, it is evident that the EKF: SINS+DVL method exhibits significant errors, as the initial attitude error leads to uncorrected positioning errors. Among the FGO methods, FGO: SINS+DVL+SSS demonstrates superior trajectory tracking compared to FGO: SINS+DVL. The inclusion of GNSS position constraints before diving and after surfacing in FGO: SINS+DVL+Position Constraint further enhances positioning accuracy. Additionally, FGO: SINS+DVL+Position Constraint+SSS achieves even better performance. The trajectory tracking of the proposed method is comparable to that of FGO: SINS+DVL+SSS+Position Constraint.

Figure 14 presents a comparison of position and heading errors, while

Table 9. The comparison of average positioning error and heading error.

Method	Average Positioning Error (m)	Average Heading Error (deg)
KF: SINS+DVL	42.333	1.946
FGO: SINS+DVL	40.509	0.477
FGO: SINS+DVL+SSS	29.267	0.554
FGO: SINS+DVL+Position constraint	28.656	0.488
FGO: SINS+DVL+Position constraint+SSS	16.120	0.421
FGO: SINS+DVL+Position Constraint+SSS+Improved Model (The proposed method)	11.548	0.133

displays the average values of these errors for each method. As shown in Figure 14a, the inclusion of SSS reduces position errors. However, in the absence of GNSS position constraints at the beginning and end, the vehicle’s position errors fluctuate over time due to DVL scale factor errors. When GNSS position constraints are added, the overall position errors decrease. On the other hand, Figure 14b indicates that without using the improved model, the addition of GNSS position constraints does not significantly enhance heading accuracy.

Table 9 provides average values that further illustrate this point. When SSS relative position measurements are added to the system, the average positioning error decreases from 40.509 m to 29.267 m. Even with GNSS position constraints at the beginning and end of the trajectory, incorporating SSS relative position measurements further reduces the average positioning error from 28.656 m to 16.120 m.

Regarding heading errors, the EKF method cannot correct past heading deviations, resulting in the largest heading error of 1.946°. After applying the FGO method to process the SINS/DVL-integrated navigation data, the heading angle error is reduced to 0.477° due to the optimization of the previous trajectory. However, adding more measurements does not lead to a further reduction in heading errors. In fact, when comparing the FGO: SINS+DVL method with the FGO: SINS+DVL+Position Constraint method, the latter exhibits a higher average heading error of 0.488°, compared to 0.477° for the former. Upon the analysis, it is concluded that the traditional modeling is not sufficiently accurate, as unaccounted factors such as scale factor errors and DVL installation angle errors contribute to position errors. This results in position residuals being incorrectly fed back into the attitude angle, ultimately leading to erroneous estimates.

In contrast, the proposed method, which employs the improved model, effectively reduces both positioning and heading errors by enhancing the overall modeling accuracy.

The summary of the experimental results is as follows:

• The proposed method of incorporating SSS relative position measurements as a position reference into the SINS/DVL navigation system significantly improves the positioning accuracy of the integrated navigation system, particularly in the absence of position measurements.
• The introduction of GNSS measurements before diving and after surfacing to optimize the underwater trajectory of the AUV greatly enhances positioning accuracy, especially when the accuracy of the inertial devices is slightly lower.
• In methods utilizing the traditional model, although the addition of GNSS position constraints improves positioning accuracy, it does not enhance heading accuracy. However, the proposed method with the improved model raises both positioning and heading accuracy. Under the traditional model, residuals are incorrectly fed back into the heading; with a more robust model, these residuals are instead directed towards the DVL calibration angle and scale factor, resulting in improved heading accuracy.

7. Conclusions

This paper proposes a post-processing navigation method for underwater robots based on FGO that incorporates SSS relative position references. The method employs an improved SSS image registration method to obtain relative positions, and integrates these with other measurements from the AUV using an enhanced model within the FGO framework. This approach simultaneously optimizes the navigation information and DVL error parameters within the trajectory, providing a more accurate attitude reference for the SSS map.

The research first introduces an improved Fourier-based image registration technique that enhances measurement stability and accuracy using Gaussian priors and bandpass filtering, with only low computational resource consumption. Subsequently, an SSS-integrated navigation method based on the improved model FGO is proposed, capable of simultaneously estimating the AUV’s navigation information and system error parameters, such as DVL scale factor errors, thereby optimizing position and heading information. Finally, the effectiveness of this method is validated through simulations and field experiments.

There are three principal conclusions:

• The introduction of relative position measurements from SSS and GNSS measurements significantly enhances the position and heading accuracy of the AUV navigation system, particularly in scenarios lacking position measurements.
• While the traditional model improves positioning accuracy with the inclusion of GNSS position constraints, it does not enhance heading accuracy. In contrast, the proposed improved model effectively mitigates the negative impact of residual errors on heading, resulting in improved accuracy for both position and heading.
• Compared to the Kalman filter method, the batch processing approach optimizes past navigation data, leading to significant improvements in the position accuracy of AUV during survey tasks, especially under the traditional model.