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.