Adaptive SLAM Methodology Based on Simulated Annealing Particle Swarm Optimization for AUV Navigation

Abstract

Simultaneous localization and mapping (SLAM) is crucial and challenging for autonomous underwater vehicle (AUV) autonomous navigation in complex and uncertain ocean environments. However, inaccurate time-varying observation noise parameters may lead to filtering divergence and poor mapping accuracy. In addition, particles are easily trapped in local extrema during the resampling, which may lead to inaccurate state estimation. In this paper, we propose an innovative simulated annealing particle swarm optimization-adaptive unscented FastSLAM (SAPSO-AUFastSLAM) algorithm. To cope with the unknown observation noise, the maximum a posteriori probability estimation algorithm is introduced into SLAM to recursively correct the measurement noise. Firstly, the Sage–Husa (SH) based unscented particle filter (UPF) algorithm is proposed to estimate time-varying measurement noise adaptively in AUV path estimation for improving filtering accuracy. Secondly, the SH-based unscented Kalman filter (UKF) algorithm is proposed to enhance mapping accuracy in feature estimation. Thirdly, SAPSO-based resampling is proposed to optimize posterior particles. The random judgment mechanism is used to update feasible solutions iteratively, which makes particles disengage local extreme values and achieve optimal global effects. The effectiveness and accuracy of the proposed algorithm are evaluated through simulation and sea trial data. The average AUV navigation accuracy of the presented SAPSO-AUFastSLAM method is improved by 18.0% compared to FastSLAM, 6.5% compared to UFastSLAM, and 5.9% compared to PSO-UFastSLAM.

1. Introduction

Autonomous underwater vehicles (AUVs) play a vital role in ocean information acquisition, requiring robust and reliable navigation performance [1,2]. With advances in autonomous robotics, the sensor configuration of autonomous vehicle provides many options for vehicle localization and navigation, such as the LiDAR/GNSS/IMU integration systems [3,4], the GNSS/on-board sensors fusion framework [5] and the GPS/INS/DVL combination systems. However, the marine environment is complex and changeable, and the navigation and positioning of AUV are very challenging. When the global navigation satellite system (GNSS) signal fails, simultaneous localization and mapping (SLAM) provides a feasible solution for AUV navigation and positioning in unknown marine environments. The acoustic ranging equipment, such as mechanical scanning imaging sonar (MSIS), equipped with the AUV can identify underwater environmental features for mapping [6,7]. The navigation devices are used for dead reckoning (DR).

The data-fusion methods in SLAM mainly include graph optimization and filtering. The former has higher precision than the latter but slower calculation speed. At present, Graph-SLAM has emerged as a promising method for land vehicles [8,9,10]. Additionally, some researchers have applied graph optimization to AUV SLAM. The novel graph-based C-SLAM algorithm is presented for multi-AUV navigation through side-scan sonar [11]. The BSLAM algorithm based on the multi-beam bathymetry system is proposed for AUV navigation, which adopts the factor graph optimization algorithm [12]. Carrasco [13] presents the integration of a stereo-vision Graph-SLAM system in the navigation and control architecture of the AUV with a stereo camera. However, the above methods are performed by acoustic or visual image registration, which does not work well in MSIS-based AUV SLAM with sparse point features. In contrast, filtering methods are increasingly used in point features SLAM.

Due to the real-time nature and characteristics of the filtering method, filter-based SLAM is widely used in underwater navigation. The Kalman filter (KF) is a classic recursive estimation algorithm that divides the navigation process into state prediction and update [14]. Currently, there are a lot of KF variants to optimize the state estimation [15,16,17,18]. However, KF is the optimal linear model estimation method, which performs poorly for the SLAM problem under the situation of a nonlinear model. The extended Kalman filter (EKF) based SLAM algorithm is proposed to approximate the nonlinear model through Taylor series expansion [19,20]. However, EKF-SLAM suffers from filtering inconsistencies due to the linearization of the model. Moreover, the computational complexity increases exponentially with filtering. The unscented Kalman filter (UKF) based SLAM algorithm is used to estimate the mean and covariance of the state variables through unscented transformation (UT), avoiding the linearization error caused by the first-order Taylor expansion [21]. However, UKF-SLAM lacks accuracy and hinders the completion of high-precision navigation and mapping.

With the advance of scientific research, FastSLAM has gradually become the mainstream algorithm for solving the SLAM problem [22,23]. State estimation is based on Monte Carlo sampling and the Rao–Blackwell particle filter (RBPF) [24]. Unlike the previous algorithms for keeping the linear Gaussian, FastSLAM can be directly applied to the nonlinear process model. The particle filter (PF) is used for posing estimation. The EKF is used to update features. The dimension of state space is limited to two dimensions, significantly reducing the amount of calculation. The development of FastSLAM has gone through two stages: FastSLAM 1.0 and FastSLAM 2.0 [25]. In FastSLAM 1.0, track points are represented by weighted particles. The features are associated and updated based on a single particle to overcome the computational complexity. In FastSLAM 2.0, observations are added to the proposed distribution. The particles in the high likelihood area are selected to improve the SLAM accuracy. However, the EKF algorithm generates linearization error due to the calculation of the multi-dimensional Jacobian matrix when the features are updated. To solve this problem, researchers proposed the UFastSLAM algorithm, in which UKF is used to update features [26,27]. The unscented particle filter (UPF) algorithm is used to construct an important probability density function, which eliminates the linearization error and enhances SLAM performance.

When AUV performs underwater tasks, high-precision navigation and mapping are necessary conditions. The traditional resampling algorithms in SLAM may reduce the diversity of the samples [28,29,30], resulting in the loss of a lot of potentially helpful information. Therefore, many researchers proposed corresponding improved algorithms to increase particle diversity and enhance position accuracy [31]. The adaptive fading UKF-based SLAM method combined with gravitational field optimization was proposed to adjust the proposal distribution [32]. The gravity search algorithm (GSA) is used to improve the resampling process in the AUV SLAM navigation so that particles tend to be in the high likelihood area of the posterior distribution to realize precise estimation [33]. The lion swarm optimization (LSO) algorithm was proposed to strengthen resampling in the rapid positioning, rescue, and mapping of mining robots [34]. However, the above methods may fall into local optimization and lack the global optimal performance, resulting in the late divergence of the filtering process. The navigation effect is not conducive to AUV remote underwater operations.

Due to the complex and variable seawater environment, sensors such as the inertial navigation system (INS), Doppler velocity log (DVL), and MSIS equipped with AUV can be subject to noise interference [35]. The default noise covariance does not fully conform to the time-varying noise distribution, leading to false observations and deviations in state and feature estimates. In recent years, some researchers have also proposed innovative ideas for the SLAM algorithm. The adaptive smoothing variable structure filter (ASVSF) was proposed to estimate measurement noise for SLAM systems, which uses the maximum a posteriori probability creation and weighted exponent concepts [36]. However, as a suboptimal estimation algorithm, the SVSF may have a mismatch between the innovation covariance matrix and the theoretical value. The system accuracy based on this algorithm is not high. The AEKF-SLAM method based on maximum likelihood estimation (MLE) and expectation–maximization (EM) creation was proposed to recursively estimate noise mean and covariance [37]. However, the proposed AEKF-SLAM method is still based on the EKF algorithm, which struggles to solve the Jacobian matrix. The variational Bayesian (VB) based UFastSLAM algorithm was presented to estimate measurement noise in UPF and UKF [38]. However, the proposed UFastSLAM algorithm heavily depends on model prior information, in which the empirical noise characteristics might reduce the stability of the filter.

Therefore, it is essential to present a method that combines particle optimization and noise estimation for AUV navigation. In this paper, we present an innovative simulated annealing particle swarm optimization–adaptive unscented FastSLAM (SAPSO-AUFastSLAM) algorithm, which can solve the particle global optimization loss problem, while estimating measurement noise mean and covariance in real time. The proposed algorithm can improve the AUV SLAM accuracy in complex and variable marine environments. The contributions of this paper are as follows:

• To improve AUV navigation accuracy, the SAPSO-AUFastSLAM algorithm is proposed to enhance resampling while estimating time-varying measurement noise;
• The Sage–Husa (SH) based UPF algorithm is proposed to estimate time-varying measurement noise adaptively in SLAM for improving the filtering accuracy. Meanwhile, the SH-based UKF algorithm is proposed in SLAM to enhance mapping accuracy;
• The SAPSO-based resampling algorithm is proposed to optimize posterior particles. The random judgment mechanism is used to update feasible solutions iteratively, which makes particles disengage the local extreme values and achieves optimal global effects;
• The proposed SAPSO-AUFastSLAM algorithm was effectively verified in simulation and sea trials. Compared with traditional algorithms, the proposed algorithm has improved accuracy and stability.

The remainder of this paper is organized as follows. The AUV SLAM model and feature extraction are introduced in Section 2. A detailed description of the proposed SAPSO-AUFastSLAM algorithm is illustrated in Section 3. Section 4 presents our AUV platform and experimental validation, and conclusions are given in Section 5.

2. AUV SLAM Model and Feature Extraction

In this section, the AUV SLAM models is reviewed, and the sonar feature extraction process is described in detail.

2.1. The Review of AUV SLAM Model

The information observed by the sensor is based on the carrier coordinate system, while the state estimation and feature update is expressed in the navigation coordinate system. It is necessary to convert the information of the carrier coordinate system to the navigation coordinate system as shown in Figure 1. The pressure sensor (PS) equipped with AUV can accurately measure absolute depth based on the seawater properties. Therefore, this paper focuses on 2D SLAM navigation.

The AUV SLAM models include the motion model and observation model, which are typical discrete nonlinear system models divided into two parts: state estimation and feature updating [39]. The 2D position and heading angle of the AUV are used to construct the SLAM state space, which can be defined as

$$X_k={\left[\begin{array}{ccc}{x}_k\hfill & y_k\hfill & {\phi }_k\hfill \end{array}\right]}^T,$$

(1)

where $(x_k,y_k)$ represents the eastward and northward coordinates of the AUV in the navigation coordinate system at time k. ${\phi }_k$ represents the yaw angle of AUV at time k.

In this research, we adopt point feature to denote features in the undersea environment. Feature points (i.e., landmarks) are extracted from sonar raw observations, which are detailed in Section 2.2. The feature space includes the 2D coordinates of each landmark as follows:

$$M_k={\left[\begin{array}{cc}{m}_{xi}^k\hfill & m_{yi}^k\hfill \end{array}\right]}^T,$$

(2)

where i indicates the index of particles, and $(m_{xi}^k,m_{yi}^k)$ represents the 2D features position observed at time k. The observation space includes the AUV distance and angle relative to environmental features. The formula can be defined as

$$Z_k={\left[\begin{array}{cc}{r}_k\hfill & {\theta }_k\hfill \end{array}\right]}^T,$$

(3)

where $r_k$ represents the distance between AUV and features at time k, and ${\theta }_k$ represents the angle between AUV and features in the navigation coordinate system at time k. The motion model can achieve recursive estimation of the AUV state. The formula can be defined as

$$X_k=f\left(X_{k-1},u_k\right)+{\omega }_k,$$

(4)

where $X_k$ represents the state space variable at time k, including the AUV position and heading. The state matrix is described as (5)

$$\left[\begin{array}{c}{x}_k\hfill \\ y_k\hfill \\ {\phi }_k\hfill \end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+v_x·t·cos\left({\phi }_{k-1}\right)-v_y·t·sin\left({\phi }_{k-1}\right)\\ x_{k-1}+v_x·t·sin\left({\phi }_{k-1}\right)+v_y·t·cos\left({\phi }_{k-1}\right)\\ {\phi }_{k-1}+{\sigma }_k·t\end{array}\right]+{\omega }_k$$

(5)

The $u_k$ stands for AUV control information at time k, and $(v_x,v_y)$ represents the 2D velocity, measured by sensor DVL. t represents the time interval. ${\omega }_k$ represents the system process noise, where ${\omega }_k\sim N(0,Q_k)$. ${\sigma }_k$ represents the angular velocity of the AUV. The observation model is used to implement the SLAM feature update. The formula is as follows:

$$Z_k=h\left(X_k,M\right)+v_k,$$

(6)

where $Z_k$ represents the observation space variable at time k, including the relative distance and angle between the AUV and feature. The observation matrix is described as (7) as follows:

$$\left[\begin{array}{c}{r}_k\hfill \\ {\theta }_k\hfill \end{array}\right]=\left[\begin{array}{c}\sqrt{{\left(x_k-m_{xi}^k\right)}^2+{\left(y_k-m_{yi}^k\right)}^2}\\ arctan\frac{y_k-m_{yi}^k}{x_k-m_{xi}^k}-{\phi }_k\end{array}\right]+v_k$$

(7)

Here, the relative distance and angle are calculated based on the estimated AUV position and observed feature position. Each feature observed at time k must participate in the calculation, and $v_k$ represents the observation noise of the AUV at time k, where $v_k\sim N(0,R_k)$.

2.2. The Fusion Feature Extraction Algorithm

We arrange experiments near the coast so that point features on the coast wall can be used for sonar observations. The MSIS sensor equipped with the AUV emits sound waves to the surrounding environment, and uses the echo intensity value to identify the position of obstacles ahead. The sonar configuration information is shown in

Table 1. Sonar configuration information.

Sampling Points (Bins)	Scanning Distance (m)	Sampling Interval (cm)
200	20	10

.

The measurement uncertainty is determined based on the observation covariance. In this paper, we set the initial observation noise covariance matrix to be R, which can be defined as follows:

$$R=\left[\begin{array}{cc}{\sigma }_r^2\hfill & 0\hfill \\ 0\hfill & {\sigma }_p^2\hfill \end{array}\right]$$

(8)

where ${\sigma }_r^2$ indicates the distance error and is defined as ${\sigma }_r=0.18$. ${\sigma }_p^2$ indicates the angle error and is defined as ${\sigma }_p=0.052$. Initial parameters are determined based on sonar observation deviation and multiple test evaluations.

Due to the underwater acoustic noise and sensor error, spurious features in sonar observations may impact the accuracy of locating the real features. Furthermore, a long scan cycle can lead to motion distortion. Therefore, we present the fusion feature extraction algorithm that integrates filtering, sparsification, and coordinate transformation [40].

2.2.1. Data Preprocessing Based on Threshold Segmentation

Data preprocessing is crucial to implement efficient sampling, which can be achieved through filtering and sparsification. Here, we set the noise distance threshold based on real experiment to preliminarily filter out spurious observations. Subsequently, the intensity threshold is determined based on the binarization threshold method to further reduce erroneous feature points, which can be defined as follows:

$$B_{Threshold}=\frac{B_{Max}+B_{Min}}{2},$$

(9)

where $B_{Max}$ indicates the maximum intensity value of per $ping$ raw sonar datum. $B_{Min}$ indicates the minimum intensity value. Data within the threshold are retained, and data exceeding the threshold are discarded.

Considering the calculation speed, the filtered sonar data are sparsely processed in order to ensure that the target-related information is retained, followed by removing redundant features. We set the minimum inter-bin distance threshold according to the experimental situation and data volume. Neighboring features whose distance is less than the threshold are halved to achieve sparsification.

2.2.2. Motion Compensation Based on Coordinate Transformation

Since the AUV sails underwater, slow sonar scanning may cause data mismatch problems, resulting in the distortion of sonar images. Therefore, preprocessed features can be converted to the navigation coordinate system, compensating for the above problems. Based on current navigation data and sonar observation information, the feature coordinate transformation formula can be defined as follows:

$$\left\{\begin{array}{c}{x}_{F,G}=x_{R,G}+{\rho }_{F}cos({\theta }_F+{\psi }_{R,G})\hfill \\ y_{F,G}=y_{R,G}+{\rho }_{F}sin({\theta }_F+{\psi }_{R,G})\hfill \end{array}\right.,$$

(10)

where $x_{F,G}$ and $y_{F,G}$ indicate the x-axis and y-axis feature position coordinates in the navigation coordinate system. $x_{R,G}$ and $y_{R,G}$ represent the x-axis and y-axis AUV position coordinates in the navigation coordinate system. ${\rho }_F$ and ${\theta }_F$ indicate the relative distance and angle between the AUV and the feature in the carrier coordinate system. ${\psi }_{R,G}$ stands for the angle between the AUV and the east direction. It should be noted that the effects of the pitch and roll angle on the features are ignored in this experiment since the angles vary little during the mission phase.

3. Innovative SAPSO-AUFastSLAM Algorithm

To improve SLAM system accuracy and stability and then enhance AUV navigation and mapping capabilities in an unknown environment, an innovative SAPSO-AUFastSLAM algorithm is proposed in this paper. The proposed algorithm can achieve better performance than conventional methods.

3.1. Overview of the Proposed Algorithm

The proposed algorithm is based on the UFastSLAM framework [41]. The measurement noise in the importance sampling stage is estimated by the proposed SH-UPF algorithm. The measurement noise mean and covariance in the feature estimation stage are approximated by the proposed SH-UKF algorithm. At the same time, the SAPSO optimization algorithm is presented for optimal resampling. The overall flow chart is shown in Figure 2.

3.1.1. Importance Sampling

Sampling the suggested distribution is a crucial step in SLAM. In the traditional methods, the noise covariance of the observation model is uniquely determined and cannot meet the time-varying requirements. In response, an adaptive UPF algorithm is proposed for importance sampling. The mean and covariance of the measurement noise in the model are estimated in real time using the SH algorithm to meet the actual requirements of the SLAM systems. The improved UPF observation model is as follows:

$$Z_k=h\left(X_k,M_k\right)+v_k^{\prime },$$

(11)

where $Z_k$ represents the observed variable. $v_k^{\prime }$ indicates the time-varying observation noise. The noise mean and covariance can be defined as

$$E\left[v_k^{\prime }\right]=r_k,$$

(12)

$$E\left[\left(v_k^{\prime }-r_k\right){\left(v_j^{\prime }-r_j\right)}^T\right]={\delta }_{kj}{R}_k^{*},$$

(13)

where ${\delta }_{kj}$ represents a trigonometric function. $r_k$ indicates the measurement noise mean. $R_k^{*}$ indicates the observation noise covariance. The improved adaptive UPF is described in detail in Section 3.2.

3.1.2. Feature Estimation

The feature estimation stage is implemented through the proposed SH-UKF algorithm in this paper, including the prediction and update. Like the noise estimation in importance sampling, the SH algorithm is used to construct an adaptive UKF model at this stage to generate a realistic noise distribution, improving the measurement model accuracy. The adaptive UKF is described in detail in Section 3.3.

3.1.3. Resampling

For the problem of particle diversity loss in traditional resampling methods [42,43], researchers proposed some optimization algorithms [44]. However, the particle “precocity” phenomenon in the existing methods may lead to premature convergence during the search process, resulting in filtering trajectory divergence. Therefore, we propose the SAPSO algorithm to replace traditional optimization algorithms, introducing random factors into the particle update stage to obtain the optimal global solution. The proposed optimal algorithm is detailed in Section 3.4.

3.2. Adaptive UPF Algorithm Based on Sage Husa

To eliminate the impact of inaccurate measurement noise covariance on AUV SLAM accuracy, we present an improved SH-UPF algorithm to optimize the SLAM navigation process. The updated proposed distribution is more consistent with the actual distribution by estimating noise mean and covariance in real time and using them in the importance sampling stage. Here, the time update steps of UPF will be described in Appendix A.

When external sensors are available, the observation prediction sigma point set is transferred through the AUV SLAM observation model mentioned in (11). The prediction and the observation prediction sigma point set are as follows:

$${\chi }_{k\mid k-1,j}^{\left[i\right]}=\left[\begin{array}{c}{\chi }_{k\mid k-1,j}^{x,\left[i\right]}\\ {\chi }_{k\mid k-1,j}^{y,\left[i\right]}\\ {\chi }_{k\mid k-1,j}^{\phi ,\left[i\right]}\end{array}\right],j=0,1,\dots ,2m,$$

(14)

$${\widehat{z}}_{k\mid k-1,j}^{\left[i\right]}=h\left({\chi }_{k\mid k-1,j}^{\left[i\right]}\right),$$

(15)

where subscript j indicates the index of the sampling point, and i stands for the index of particles. The left sides of (14) and (15) represent the symbols of predicted sigma point and the observed predicted sigma point. The predicted sigma point includes the AUV position components and the heading angle. The average and cross covariance of the observation prediction generated by weighted summation are represented as follows:

$${\overline{Z}}_{k\mid k-1}^{\left[i\right]}=\sum _{j=0}^{2m}{w}_m^j·{\widehat{z}}_{k\mid k-1,j}^{\left[i\right]},$$

(16)

$$P_{xz,k\mid k-1}^{\left[i\right]}=\sum _{j=0}^{2m}{w}_c^j·\left({\chi }_{k\mid k-1,j}^{\left[i\right]}-X_{k\mid k-1}^{\left[i\right]}\right){\left({\widehat{z}}_{k\mid k-1,j}^{\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{\left[i\right]}\right)}^T,$$

(17)

where $X_{k\mid k-1}^{\left[i\right]}$ indicates the predicted state variable at time k, and $w_m^j$ and $w_c^j$ are used to calculate the mean and covariance. The formula is expressed as follows:

$$\left\{\begin{array}{c}{w}_m^0=\lambda /(m+\lambda )\hfill \\ w_c^0=\lambda /(m+\lambda )+\left(1-{\alpha }^2+\beta \right)\hfill \\ w_m^j=w_c^j=1/2(m+\lambda ),j=1,2,\dots ,2m\hfill \end{array}\right.$$

(18)

Here, $\lambda ={\alpha }^2(m+\kappa )-m$. We take $\kappa =0$, $\alpha =0.002$. As seen above, the sampling of sigma points is divided into three stages, and the number of samples is determined by the state space dimension m, $m=5$. Assuming that the observation noise at time k follows a Gaussian distribution with zero mean and covariance $R_k$, the innovation covariance matrices can be expressed as follows:

$$\left\{\begin{array}{c}{S}_k^{\left[i\right]}=\sum _{j=0}^{2m}{w}_c^j·\left({\widehat{z}}_{k\mid k-1}^{\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{\left[i\right]}\right){\left({\widehat{z}}_{k\mid k-1}^{\left[i\right]}-{\overline{\mathbf{Z}}}_{k\mid k-1}^{\left[i\right]}\right)}^T\hfill \\ P_{zz,k\mid k}^{\left[i\right]}=S_k^{\left[i\right]}+R_k\hfill \end{array}\right.$$

(19)

The filter gain is calculated based on the results obtained from (17) and (19) to update the AUV state and covariance. The formula can be expressed as follows:

$$K_k^{\left[i\right]}=P_{xz,k\mid k-1}^{\left[i\right]}{\left(P_{zz,k\mid k}^{\left[i\right]}\right)}^{-1},$$

(20)

$${\epsilon }_k=Z_k-{\overline{Z}}_{k\nmid k-1}^{\left[i\right]},$$

(21)

$$X_{k\mid k}^{\left[i\right]}=X_{k\mid k-1}^{\left[i\right]}+K_k^{\left[i\right]}·{\epsilon }_k,$$

(22)

$$P_{k\mid k}^{\left[i\right]}=P_{k\mid k-1}^{\left[i\right]}-K_k^{\left[i\right]}·{\left(P_{zz,k\mid k}^{\left[i\right]}\right)}^{-1}·{\left(K_k^{\left[i\right]}\right)}^T,$$

(23)

where $K_k^{\left[i\right]}$ represents the UPF filter gain, and ${\epsilon }_k$ represents an innovation vector used to evaluate the deviation between the observed and predicted values.

To meet the system adaptability, the SH algorithm is introduced for the real-time correction of UPF. The SH algorithm is based on maximum a posteriori estimation and exponentially weighted attenuation and has the unbiased nature of constant value noise estimation [45]. Noise estimation can be implemented recursively. The noise value at time $k+1$ can be estimated from the observation data at time k, and the mathematical formula is expressed as follows:

$$\left\{\begin{array}{c}{r}_k=\left(1-d_k\right)·r_{k-1}+d_k\left[Z_k-\sum _{j=0}^{2m}{w}_m^j·h\left({\chi }_{k\mid k-1,j}^{\left[i\right]}\right)\right]\hfill \\ R_k^{*}=\left(1-d_k\right)·R_{k-1}^{*}+d_k\left[{\epsilon }_k·{\epsilon }_k^T-\sum _{j=0}^{2m}{w}_c^j·\left({\widehat{z}}_{k\mid k-1,j}^{\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{\left[i\right]}\right){\left({\widehat{z}}_{k\mid k-1,j}^{\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{\left[i\right]}\right)}^T\right]\hfill \end{array}\right.,$$

(24)

where $r_k$ and $r_{k-1}$ represent the mean value of estimated noise at k time and $k-1$ time, and $R_k^{*}$ and $R_{k-1}^{*}$ represent the covariance matrix of estimated noise. $d_k$ represents the weighted exponential factor of decaying memory. In noise estimation statistics, it is essential to emphasize the latest data and gradually forget older values. Here, we define $d_k$ as follows:

$$d_k=\frac{(1-b)}{\left(1-b^k\right)}$$

(25)

The value of b must meet the requirements of $0.95<b<0.99$. To meet the system requirements, we take $b=0.98$. As seen from the above, the estimated value of observation noise can be obtained based on the SH-UPF algorithm at time k, which can be used in the observation update process next time. The updated formula for fusion estimation noise can be expressed as follows:

$${\epsilon }_{k+1}=Z_{k+1}-{\overline{Z}}_{k+1\mid k}^{\left[i\right]}-r_k,$$

(26)

$$P_{zz,k+1\mid k+1}^{\left[i\right]}=S_{k+1}^{\left[i\right]}+R_k^{*},$$

(27)

where the calculation of the innovation vector needs to consider the estimated mean value of the observation noise, and the observation noise covariance in the innovation covariance needs to be updated to the estimated value at the current time.

During the SH-UPF filtering process, $R_k^{*}$ and $r_k$ are updated simultaneously. Due to the difference between the SLAM system and pure navigation, more than one feature is observed each time, and each observation value participates in the update. Considering practical applications, noise estimation is performed once for each feature update in the proposed algorithm, and the final estimated value is transmitted to the next moment. The accuracy of AUV underwater SLAM navigation is improved through iterative filtering of the UPF system and fusion of recursive estimates of observation noise.

3.3. Adaptive UKF Algorithm Based on Sage Husa

In the AUV SLAM system, feature estimation is a crucial step that determines mapping accuracy and serves as AUV navigation. In traditional UFastSLAM, feature estimation is based on the UKF algorithm. In order to achieve adaptive mapping performance, we propose an enhanced SH-UKF algorithm for the AUV SLAM, which recursively estimates the time-varying measurement noise. After successful data association, the sigma points are determined to be sampled from the Gaussian distribution and transferred to the non-linear function. After weighted summation, the observation prediction mean and cross-covariance are obtained, with the formula as follows:

$${\overline{Z}}_{k\mid k-1}^{f,\left[i\right]}=\sum _{j=0}^{2m}{w}_m^j·{\widehat{z}}_{k\mid k-1,j}^{f,\left[i\right]},$$

(28)

$$P_{xz,k\mid k-1}^{f,\left[i\right]}=\sum _{j=0}^{2m}{w}_c^j·\left({\chi }_{k\mid k-1,j}^{f,\left[i\right]}-X_{k\mid k-1}^{f,\left[i\right]}\right){\left({\widehat{z}}_{k\mid k-1,j}^{f,\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{f,\left[i\right]}\right)}^T,$$

(29)

where superscript f represents the feature estimation sign. ${\widehat{z}}_{k\mid k-1,j}^{f,\left[i\right]}$ indicates the observation and prediction sigma point, and ${\overline{Z}}_{k\mid k-1}^{f,\left[i\right]}$ represents the observation prediction mean. The filter gain at the update stage of the UKF observation at time k can be expressed as follows:

$$K_k^{f,\left[i\right]}=P_{xz,k\mid k-1}^{f,\left[i\right]}·{\left[S_k^{f,\left[i\right]}+R_{k,f}\right]}^{-1},$$

(30)

where $R_{k,f}$ represents the observation noise covariance matrix at the feature estimation stage. The mean and covariance of the posterior estimation of features can be expressed as follows:

$$X_{k\mid k}^{f,\left[i\right]}=X_{k\mid k-1}^{f,\left[i\right]}+K_k^{f,\left[i\right]}\left(Z_k-{\overline{Z}}_{k\mid k}^{f,\left[i\right]}\right),$$

(31)

$$P_{k\mid k}^{f,\left[i\right]}=P_{k\mid k-1}^{f,\left[i\right]}-K_k^{f,\left[i\right]}·{\left[S_k^{f,\left[i\right]}+R_k\right]}^{-1}·{\left(K_k^{f,\left[i\right]}\right)}^T$$

(32)

Similar to the improved SH-UPF algorithm, this section integrates the SH noise estimator in the UKF algorithm to estimate the noise parameters at the next moment through the statistical data at the k moment and integrates them into the observation update process of the feature estimation to improve the accuracy of the posterior estimation. The noise estimation formula can be defined as

$$\left\{\begin{array}{c}{r}_{k,f}=\left(1-d_k\right)·r_{k-1,f}+d_k\left[Z_k-\sum _{j=0}^{2m}{w}_m^j·h\left({\chi }_{k\mid k-1,j}^{f,\left[i\right]}\right)\right]\hfill \\ R_{k,f}^{*}=\left(1-d_k\right)·R_{k-1,f}^{*}+d_k\left[{\epsilon }_{k,f}·{\epsilon }_{k,f}^T-\sum _{j=0}^{2m}{w}_c^j·\left({\widehat{z}}_{k\mid k-1,j}^{f,\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{f,\left[i\right]}\right){\left({\widehat{z}}_{k\mid k-1,j}^{f,\left[i\right]}-{\overline{Z}}_{k\mid k-1}^{f,\left[i\right]}\right)}^T\right]\hfill \end{array}\right.,$$

(33)

where $r_{k,f}$ and $r_{k-1,f}$ represent the estimated noise mean at time k and $k-1$ during the feature update, respectively. $R_{k,f}^{*}$ and $R_{k-1,f}^{*}$ represent the estimated noise covariance matrices at time k and $k-1$. ${\epsilon }_{k,f}$ represents the innovation vector of UKF. At this stage, the calculation method for $d_k$ is the same as in Section 3.2. The estimated noise data participate in the feature update process at the next moment, making the final estimated feature more approximate to the actual value.

The adaptive SH-UKF algorithm introduces noise estimation into the feature update process, which is simple and easy to implement, making the posterior probability estimation value of the improved algorithm more accurate. The proposed method can improve the AUV SLAM mapping accuracy and avoid the problem of the algorithm being too complex. The flowchart of the proposed algorithm is shown in Figure 3.

3.4. Optimal Resampling Based on Simulated Annealing Particle Swarm Optimization

To improve the global optimization loss of samples during the UFastSLAM resampling phase, we propose an enhanced SAPSO algorithm to achieve the optimal global effect for optimal resampling.

PSO is a sample parallel random optimization method based on iterative thinking, simulating birds foraging and fish clustering behavior [46,47]. Each particle in the sample represents a feasible solution to the problem and searches for the optimal solution in the sample space through information sharing and collaboration. Sample attributes include speed, position, and fitness. If a group of samples includes N particles, the position of each particle in the search space can be expressed as a set $X=\{X_1,X_2,X_3,\dots ,X_N\}$. The speed of particles in the sample can be described as a set $V=\{V_1,V_2,V_3,\dots ,V_N\}$. Then, the state update formula for the particles in each search for the optimal can be defined as

$$V_i^{j+1}=w{t}_i·V_i^j+c_1·r_1·\left(p{b}_i-X_i^j\right)+c_2·r_2·\left(gb-X_i^j\right),$$

(34)

$$X_i^{j+1}=X_i^j+V_i^{j+1},$$

(35)

where j represents the number of iterations, $j=1,2,3,\dots ,T$. $c_1$ and $c_2$ represent learning factors. To adapt to the system requirements, we take $c_1=c_2=2$ while limiting the particle search speed $V_i$ to the range of $[-4,4]$. $r_1$ and $r_2$ represent random factors that follow a random distribution of $[0,1]$. $p{b}_i$ represents the optimal local value searched during sample iteration, and $gb$ represents the optimal global value of the entire sample. $w{t}_i$ represents the inertial weight of the particle. In this paper, we present an improved weight calculation formula [47,48], which can be defined as

$$w{t}_i=\left\{\begin{array}{cc}w{t}_{min}+\left(w{t}_{max}-w{t}_{min}\right)·\frac{f_i-f_{min}}{f_{\mathrm{mean}}-f_{min}},\hfill & f_i⩽f_{\mathrm{mean}}\\ w{t}_{max},\hfill & f_i>f_{\mathrm{mean}}\end{array}\right.,$$

(36)

where $f_i$ represents the fitness function. $f_{mean}$ represents the average fitness of the sample, and $f_{min}$ represents the minimum fitness of the sample. $w{t}_{max}$ represents the maximum weight, which is set to $w{t}_{max}=1.2$. $w{t}_{min}$ represents the minimum weight, which is set to $w{t}_{min}=0.4$. In this article, we adaptively determine the inertia weight of each particle for optimization based on the average fitness of the sample. The calculation formula for the fitness function is as follows:

$$f_i=\sqrt{{\left(X_{i,x}^j-{target}_x\right)}^2+{\left(X_{i,y}^j-\mathrm{target}{}_y\right)}^2},$$

(37)

where the subscript x represents the lateral coordinates of the AUV position in the particle, the subscript y represents the longitudinal coordinates of the AUV position in each particle, and the target represents the mean value of the initial sample position.

It is worth mentioning that there is a problem of premature convergence in the sample iteration process. After a few iterations, the particles’ position in space remains unchanged, limited to a local range, resulting in low accuracy. To this end, the SA algorithm is introduced, a global optimization algorithm based on iterative thinking, using temperature as a control parameter and the value of the objective function as the internal energy [49]. Within the allowable temperature range, it allows samples to accept suboptimal solutions with a certain probability and ultimately obtain a relatively optimal solution through multiple iterations. In this article, according to the sampling principle of SA, particles in the sample space are transferred according to the following improved probability:

$$p_i=\left\{\begin{array}{cc}1,\hfill & f_i\ge f_{min}\hfill \\ e^{\left(-\frac{f_i-f_{min}}{f_i}\right)},\hfill & f_i<f_{min}\hfill \end{array}\right.$$

(38)

When the fitness of the updated particles is less than the optimal value, the new particles are accepted with an exponential probability rather than completely deleting the new particles as is the case with traditional methods.

For optimal resampling, the SAPSO algorithm introduces a random judgment mechanism and combines with the probability jump feature in the search process. When iteratively updating a feasible solution, the improved probability is used instead of the determined judgment condition to accept a solution that may not be optimal. Therefore, it is possible to probabilistically jump out of the optimal local solution and ultimately tend to global optimization. The proposed method can improve the global search ability to a certain extent, thereby obtaining a more accurate AUV state estimation. The proposed SAPSO algorithm can be summarized in Algorithm 1.

Algorithm 1: Proposed SAPSO: 2-D.

Initialize parameters: $Tem{p}^0$,$X_i^0$,$V_i^0$ $\mathbf{for}j=1:T$     $\mathbf{for}i=1:N$           Caiculate fitness: $f_i=distance(X_i^j,target)$     $\mathbf{end}\mathbf{for}$     $f_{min}=min\left[f_i\right]$     $f_{mean}=mean\left[f_i\right]$     $\mathbf{for}i=1:N$        Calculate inertia weight:        $w{t}_i=\left\{\begin{array}{cc}w{t}_{min}+\left(w{t}_{max}-w{t}_{min}\right)·\frac{f_i-f_{min}}{f_{\mathrm{mean}}-f_{min}},& f_i⩽f_{\mathrm{mean}}\\ w{t}_{max},& f_i>f_{\mathrm{mean}}\end{array}\right.$        Update particles velocity:         $\begin{array}{cc}\hfill V_i^{j+1}=& w{t}_i\ast V_i^j+c_1\ast r_1\ast (p{b}_i-X_i^j)+c_2\ast r_2\ast (gb-X_i^j)\hfill \end{array}$        $\mathrm{Update}\mathrm{particles}\mathrm{position}:X_i^{j+1}=X_i^j+V_i^{j+1}$     $\mathbf{end}\mathbf{for}$     $\mathrm{Update}\mathrm{fitness}\mathrm{optimum}.$     $\mathbf{for}i=1:N$        $\mathrm{Calculate}\mathrm{probability}\mathrm{and}\mathrm{sample}:p_i={\mathbf{e}}^{\left(-\frac{f_i-f_{min}}{f_i}\right)}$        $\mathbf{if}{p}_i>rand\left(\right)$            $p{b}_i=X_i^{j+1}$        $\mathbf{else}$            $p{b}_i=X_i^j$        $\mathbf{end}$     $\mathbf{end}\mathbf{for}$     $\mathrm{Annealing}:Tem{p}^{j+1}=Tem{p}^j\ast 0.5$ $\mathbf{end}\mathbf{for}$

4. Experimental Results and Analysis

In this section, we use a simulation platform and sea trial experiments to evaluate the proposed algorithm and verify its effectiveness. The proposed algorithm is compared with existing FastSLAM, UFastSLAM, and PSO-UFastSLAM algorithms. The navigation and mapping accuracy is comprehensively analyzed. The results are presented through result graphs and statistical tables.

4.1. Simulation

Here, we use the open-source simulation environment built by Tim [7] to generate robot trajectory and features. The trajectory path is specified by 17 waypoints with 35 landmarks and starts at (0,0). The experimental operation step size is 8779 steps, and the trajectory length is 658 meters. The initial simulation parameters can be summarized in

Table 2. The initial simulation parameters.

Parameters	Value	Meaning
$r_0$	0	observation noise mean
$R_0$	$diag[0.01,0.0003]$	observation noise covariance
Q	$diag[0.09,0.0027]$	process noise covariance
Particles	20	number of particles
Temperature	10	annealing temperature

.

The effectiveness of the proposed algorithm is evaluated based on 300 Monte Carlo (MC) runs on the simulation case. The simulation results are shown in Figure 4. Figure 4a indicates the trajectory and feature estimation results of an MC experiment. The estimated trajectory and estimated features of the proposed SAPSO-AUFastSLAM algorithm are closest to the actual value. The estimation results of the PSO-UFastSLAM algorithm are the second closest, and the estimation results of the UFastSLAM algorithm and the FastSLAM algorithm are the worst. To evaluate the four algorithms more accurately, we calculate the root mean square error (RMSE) of the motion trajectories of the four algorithms in 300 MC experiments, as shown in Figure 4b. The RMSE of the proposed algorithm in the 300 MC experiments is kept at the lowest, indicating that the algorithm is the most effective to improving the accuracy of AUV underwater navigation.

We add loop closure detection to the initial simulation trajectory and further evaluate the performance of the proposed algorithm compared with other algorithms. This experiment is still based on 300 MC simulations, which is shown in Figure 5. Compared with the experimental results of simulation case 1, the performance of each algorithm is improved. It can be seen in Figure 5a,b that the proposed SAPSO-AUFastSLAM algorithm performs optimally.

Figure 6 and Figure 7 indicate the estimation error of different algorithms in an MC experiment. Figure 6a,b show the position and the feature estimation deviation of different methods from simulation case 1. The estimation errors from simulation case 2 are shown in Figure 7a,b. The errors of simulated path with the closed loop are almost 50% smaller than the open-loop trajectory. At the same time, the suggested algorithms have show the best results. The FastSLAM algorithm has the largest deviation from the true value and lacks stability. The UFastSLAM algorithm has obvious divergence. The PSO-UFastSLAM algorithm and the proposed SAPSO-AUFastSLAM algorithm converge significantly. In contrast, the latter has smaller position and feature estimation errors than the former, while being more stable.

Table 3. Average statistical results of 300 Monte Carlo experiments.

	Parameters	FastSLAM	UFastSLAM	PSO-UFastSLAM	SAPSO-AUFastSLAM
Simulation case 1	RMSE(m)	$10.2574$	$7.8293$	$5.6866$	$3.8979$
	Accuracy(%)	$1.56$	$1.19$	$0.86$	$0.59$
	Run time(s)	$67.249120$	$73.052265$	$82.191906$	$93.437592$
Simulation case 2	RMSE(m)	$6.8969$	$5.2529$	$3.6904$	$2.3454$
	Accuracy(%)	$1.00$	$0.76$	$0.53$	$0.34$
	Run time(s)	$80.270569$	$83.558731$	$90.288240$	$97.048751$

gives the average statistical results of 300 MC experiments from simulation case 1 and simulation case 2. The proposed SAPSO-AUFastSLAM algorithm has the lowest average RMSE and the highest accuracy. In contrast, the FastSLAM algorithm has the highest average RMSE and the lowest accuracy, and the other two algorithms have values in the middle. The accuracy of the proposed algorithm is 32% higher than that of PSO-UFastSLAM and 51% higher than that of UFastSLAM, a 63% improvement over FastSLAM. Therefore, the proposed algorithm has the highest accuracy and the best positioning performance.

To evaluate the robustness of the proposed algorithm in a non-stationary noise environment, we modified the initial observation noise parameters and conducted 100 MC experiments. We set the noise mutation probability $P_m$ and mutation gain $G_m$ in the program to simulate the real ocean environment. The non-stationary noise simulation process is shown in Figure 8.

In this paper, we set $G_m$ to be a random integer at $(1,8)$. $P_m$ is continuously increased to simulate the abnormal degree of dynamic noise. For each case, we performed 100 MC runs for validating. The average RMSE values of the different experiments are shown in

Table 4. The average statistical results of different algorithms in dynamic noise environment.

Variables	Parameters	FastSLAM	UFastSLAM	PSO-UFastSLAM	SAPSO-AUFastSLAM
$P_m=0.1$	P-RMSE (m)	$5.468522$	$5.023337$	$2.262865$	$1.386561$
	F-RMSE (m)	$9.474483$	$7.788813$	$4.403758$	$3.166521$
	$N_{eff}$	$16.291506$	$16.375003$	$16.449501$	$17.843986$
$P_m=0.2$	P-RMSE (m)	$6.923035$	$6.509402$	$3.388484$	$1.780480$
	F-RMSE (m)	$12.996229$	$11.618153$	$9.073089$	$5.959706$
	$N_{eff}$	$15.042108$	$15.542667$	$15.979394$	$17.717086$
$P_m=0.4$	P-RMSE (m)	$9.039256$	$8.816732$	$3.904134$	$2.535749$
	F-RMSE (m)	$18.125908$	$14.764936$	$12.061823$	$7.162695$
	$N_{eff}$	$14.466835$	$15.233064$	$15.601916$	$17.700492$
$P_m=0.6$	P-RMSE (m)	$10.240571$	$9.923318$	$7.624019$	$2.614966$
	F-RMSE (m)	$21.289047$	$18.586555$	$14.790571$	$7.429418$
	$N_{eff}$	$13.184176$	$15.012112$	$15.571266$	$17.070549$
$P_m=0.8$	P-RMSE (m)	$16.355466$	$14.884618$	$11.957723$	$3.137172$
	F-RMSE (m)	$26.446475$	$23.021594$	$16.366015$	$8.421068$
	$N_{eff}$	$12.862332$	$13.192002$	$14.286121$	$16.419210$

. P-RMSE and F-RMSE indicate the average errors of the estimated positions and features in different algorithms. It can be seen that as the mutation probability increases, the proposed SAPSO-AUFastSLAM algorithm remains at a lower error level and is more robust.

As we all know, the number of valid particles is also an important criterion judging the performance of the SLAM algorithm. In this paper, the number of valid particles are referred to as $N_{eff}$, which can be defined as (39)

$$N_{eff}=\frac{1}{{\sum }_{i=1}^N\left(W_k^{\left[i\right]}\right)}$$

(39)

where $W_k^{\left[i\right]}$ indicates the weight of the i-th particle at time k. Too many invalid particles can lead to particle depletion, reducing state and feature estimation accuracy. Therefore, we further evaluate the proposed algorithm by comparing the $N_{eff}$ of different algorithms in a non-stationary noise environment. The average results of 100 MC runs are shown in Table 4. Obviously, the proposed algorithm has the most effective particles as the noise mutation rate increases.

The computational cost and positioning error of the proposed algorithm are further analyzed according to the number of particles. The localization error and running time of different algorithms under different particle numbers are used to evaluate the computational complexity and accuracy of the algorithms. For each test, the results are obtained over 150 Monte Carlo runs. As shown in

Table 5. The RMSE/running time with different particles.

Particle Number	Parameters	FastSLAM	UFastSLAM	PSO-UFastSLAM	SAPSO-AUFastSLAM
10	RMSE (m)	6.9545	5.7820	3.7796	2.8203
	Time (s)	57.795506	63.269216	73.037585	84.306541
20	RMSE (m)	6.8969	5.9955	3.7670	2.7215
	Time (s)	67.052265	73.249120	82.191906	93.437592
30	RMSE (m)	6.3205	5.9082	3.6904	2.6044
	Time (s)	90.446690	97.460251	98.155129	100.351062
40	RMSE (m)	6.2764	5.2701	3.4937	2.5653
	Time (s)	127.754483	128.262471	130.007165	136.641434
50	RMSE (m)	6.2242	5.2555	3.4645	2.4673
	Time (s)	136.921214	137.700600	140.136590	145.603873
60	RMSE (m)	6.1038	5.2529	3.3926	2.3454
	Time (s)	145.610535	147.274797	155.407132	165.645236
70	RMSE (m)	5.9445	4.8277	2.9849	2.2050
	Time (s)	147.782083	152.052470	157.941029	177.088356
80	RMSE (m)	5.6225	4.8889	2.9458	1.8203
	Time (s)	169.021821	173.807379	175.848929	182.919457
90	RMSE (m)	5.4765	4.8415	2.9294	1.8493
	Time (s)	179.571131	179.720729	189.834099	198.945290
100	RMSE (m)	5.4704	4.7750	2.0405	1.8415
	Time (s)	191.781751	191.360097	194.632039	203.538832

and Figure 9, the running time of the four algorithms increases while the positioning error decreases as the number of particles increases. From Figure 9a, it can be seen that the error of the proposed algorithm is always the smallest. Figure 9b shows the average running time of different algorithms. Because the noise statistics calculation and SAPSO iterations increase the computational cost, the running time of the proposed algorithm is relatively high compared to other algorithms. Considering the positioning accuracy and calculation cost, we choose 20 particles as reference in this paper.

4.2. Sea Trial

In order to fully evaluate the practicability of the proposed algorithm, we use two sets of sea test data to evaluate the algorithm performance. The two experiments were carried out at Nanjiang Wharf and Tuandao Bay, respectively.

4.2.1. The Experiment at Nanjiang Wharf

We used No.1 Sailfish-210 AUV (Underwater Vehicle Laboratory, Ocean University of China, Qingdao, China) as the experimental platform to verify the effectiveness of the proposed algorithm. The experiment was carried out at Nanjiang Wharf in Qingdao. Figure 10 shows the No.1 Sailfish-210 AUV during the sea trial and overall design block diagram, in which the global positioning system (GPS) provides real-time position information on the water surface. The IMU, DVL, and MSIS combine to complete the underwater SLAM task, and the IMU measures the heading, roll, and pitch angle. The DVL provides the velocity of the body coordinate system, and the MSIS measures the 2D position of the features in the environment. The detailed specifications of the sensors are shown in

Table 6. The detailed specifications of the sensors on the No.1 AUV.

Sensors	Brand	Precision
IMU	Spatial 1750	bias stability: ≤0.1${}^{°}$/h (maximum), ≤0.05${}^{°}$/h (average)
DVL	Explorer DVL	precision: $\pm 1.15$%, $\pm 0.2$ cm/s
GPS	u-blox LEA-M8T	Horizontal position accuracy: $2.5$ m, Range resolution: $7.5$ mm
MSIS	Tritech Micron	Mechanical resolution: $0.{45}^{°}$, $0.9^{°}$, $1.8^{°}$, $3.6^{°}$

.

The No.1 Sailfish-210 AUV is equipped with a dual-core CPU (Advantech, Taipei, China), which collects the data of each sensor through MOOS-IvP software (Version 12.2) for calculation. The duration of the experiment is 284 s. The experimental results are shown in Figure 11. The estimated trajectory and features of the different algorithms are fully shown in Figure 11a. The enlarged local figure is shown in Figure 11b. It can be seen that the motion trajectory and features of the proposed SAPSO-AUFastSLAM algorithm are closer to the actual value.

Position estimation errors of the different algorithms on the No.1 Sailfish-210 AUV are shown in Figure 12. It can be seen from Figure 12b that the position estimation error of the proposed SAPSO-AUFastSLAM algorithm is the smallest. Figure 12c,d give the position errors in the east direction and in the north direction. The two errors of the proposed algorithm are smaller than those of other algorithms. Therefore, the proposed algorithm can achieve high-precision, long-range autonomous navigation in AUV underwater missions.

4.2.2. The Experiment at Tuandao Bay

We used the No.2 Sailfish-210 AUV as the experimental platform to verify the accuracy of the proposed algorithm. The experiment was carried out at Tuandao Bay in Qingdao. Figure 13 shows the No.2 Sailfish-210 AUV during the sea trial and overall design block diagram, in which the INS, DVL, and MSIS combine to complete the underwater SLAM task, and the INS measures the heading, roll, and pitch angle. The detailed specifications of the sensors are shown in

Table 7. The detailed specifications of the sensors on the No.2 AUV.

Sensors	Brand	Precision
INS	HN100	bias stability: ≤$0.{05}^{°}$/h
DVL	Pathfinder DVL	precision: $\pm 1.15\%$, $\pm 0.2$ cm/s
GPS	u-blox LEA-M8T	Horizontal position accuracy: $2.5$ m, Range resolution: $7.5$ mm
MSIS	Tritech Micron	Mechanical resolution: $0.{45}^{°}$, $0.9^{°}$, $1.8^{°}$, $3.6^{°}$

.

The No.2 Sailfish-210 AUV is equipped with an Advantech-5272 industrial computer (Advantech, Taipei, China) with a CPU of Core i3 1G Hz or higher, 4 GB of memory, and 128 GB of hard disk. In this experiment, the sailing distance is 361.5643 m. The duration of the experiment is 1833 s. Figure 14 and Figure 15 show the results of sea trial experiment on the No.2 Sailfish-210 AUV. Figure 14a,b represent the SLAM motion and feature estimations of the four algorithms. Figure 15b gives the position estimation errors of different algorithms. The east and north position errors of different algorithms are shown in Figure 15c,d. As can be seen from the figures, the proposed SAPSO-AUFastSLAM algorithm have more accurate estimations and fewer errors than traditional algorithms.

The specific statistical results are shown in

Table 8. The specific statistical results of the AUV sea trial.

Datasets	Parameters	FastSLAM	UFastSLAM	PSO-UFastSLAM	SAPSO-AUFastSLAM
Sea trial 1	RMSE (m)	5.653	4.759	4.720	4.322
	ERMSE (m)	4.900	4.442	4.376	3.991
	NRMSE (m)	2.819	1.708	1.768	1.659
	Accuracy (%)	2.230	1.877	1.862	1.705
	Step time (s)	0.0158	0.0177	0.0179	0.0233
	Total time (s)	44.56	50.04	50.58	65.75
Sea trial 2	RMSE (m)	4.539	4.445	4.423	4.310
	ERMSE (m)	1.655	1.475	1.495	1.397
	NRMSE (m)	4.227	4.193	4.162	4.077
	Accuracy (%)	0.965	0.944	0.940	0.916
	Step time (s)	0.0149	0.0191	0.0202	0.0232
	Total time (s)	94.30	120.56	127.66	146.55

Step time represents the single-step running time of the AUV sea trial. Total time represents the total running time of the AUV sea trial. ERMSE indicates the eastern RMSE of AUV localization. NRMSE indicates the northern RMSE of AUV localization.

. The RMSE, accuracy, and running time of the different algorithms are summarized. Obviously, the errors of the proposed SAPSO-AUFastSLAM algorithm are smaller than conventional algorithms, and the accuracy is higher. The running time of the presented algorithm is increased to a certain extent, but they can meet the real-time performance. Therefore, the proposed algorithm can effectively improve the performance of AUV underwater SLAM and thus improve navigation accuracy.

5. Conclusions

This paper proposes an innovative SAPSO-AUFastSLAM algorithm to perform the AUV SLAM process. The adaptive UPF and UKF algorithms are based on Sage–Husa noise estimation and the resampling algorithm based on SAPSO optimization in parallel to improve the accuracy of AUV navigation and mapping. The performance of the proposed algorithm is verified by simulation and sailfish AUV sea trial data and compared with the three conventional algorithms of PSO-UFastSLAM, UFastSLAM, and FastSLAM. The experimental results show the effectiveness of the proposed method. In the sea trial, the average accuracy of the proposed method is significantly improved compared with the conventional method.

Although the calculation time of the proposed algorithm can meet the needs of low-frequency navigation, it is longer than that of conventional algorithms. In the future work, we will investigate increasing the calculation speed and adaptively estimating the system noise parameters to improve the performance of the proposed algorithm further. Subsequently, anomaly detection will be added to improve the coarse impact of the noise estimation algorithm, such as judging the divergence of the filtering process and introducing fading factors and adaptive coefficients for adjustment. Additionally, the impact of ocean currents and other factors on AUV navigation will be considered to improve accuracy.