Research on simultaneous localization and mapping for AUV by an improved method: Variance reduction FastSLAM with simulated annealing

Jishn Cui , Dongzhu Feng , Yunhui Li , Qihen Tin

a School of Aerospace Science and Technology, Xidian University, Xi’an 710126, China

b Key Laboratory of Equipment Efficiency in Extreme Environment, Ministry of Education, Xi’an 710126, China

c School of Astronautics, Harbin Institute of Technology, Harbin 150010, China

Keywords:Autonomous underwater vehicle (AUV)Sonar Simultaneous localization and mapping(SLAM)Simulated annealing FastSLAM

ABSTRACT At present,simultaneous localization and mapping(SLAM)for an autonomous underwater vehicle(AUV)is a research hotspot.Aiming at the problem of non-linear model and non-Gaussian noise in AUV motion,an improved method of variance reduction fast simultaneous localization and mapping(FastSLAM)with simulated annealing is proposed to solve the problems of particle degradation, particle depletion and particle loss in traditional FastSLAM, which lead to the reduction of AUV location estimation accuracy.The adaptive exponential fading factor is generated by the anneal function of simulated annealing algorithm to improve the effective particle number and replace resampling. By increasing the weight of small particles and decreasing the weight of large particles, the variance of particle weight can be reduced,the number of effective particles can be increased,and the accuracy of AUV location and feature location estimation can be improved to some extent by retaining more information carried by particles.The experimental results based on trial data show that the proposed simulated annealing variance reduction FastSLAM method avoids particle degradation, maintains the diversity of particles, weakened the degeneracy and improves the accuracy and stability of AUV navigation and localization system.

1. Introduction

Autonomous underwater vehicle (AUV) is a cable-free, autonomous and intelligent underwater vehicle, which has been widely used in military and civil fields, and has enormous potential for development [1-4]. Accurate underwater navigation is the indispensable condition for AUV to accomplish any scheduled mission,which can be divided into two main types: non-autonomous navigation with the help of external signals and autonomous navigation based on self-carried sensors. Autonomous navigation can achieve relatively accurate navigation in complex marine environment without relying on external signal sources only through their own speed (DVL, etc.), attitude (Compass, etc.) and environmental sensing sensors (sonar, etc.). It is a hotspot in the field of AUV navigation [5-8].

Simultaneous localization and mapping (SLAM) is developed from the demand for autonomous navigation of mobile robots[9-11].It is generally impossible to predict the mapping map of the robot’s working area or the valid environmental priori information is very limited.In the case of uncertainty of the mobile robot’s own position,the robot uses the carried sensors(sonar and camera,etc.)to observe the characteristics of the environment repeatedly,so as to complete the robot’s own positioning and feature position correction,and construct the environmental map at the same time.In recent years, SLAM technology has been widely used and developed in the fields of unmanned aerial vehicle(UAV),AUV and so on[12-15],because it can obtain more reliable state estimation and environmental information without the need of terrain information or auxiliary external positioning array.

Extended Kalman filter SLAM (EKF-SLAM) have been widely used for the SLAM applications as the basic framework [16-19].However, the traditional EKF-SLAM has the problems of high computational cost and inconsistent filtering as the linearization of the nonlinear model.In addition,because the covariance matrix of EKF-SLAM has n2elements (n is the number of features), even if a new feature is observed,all n2elements need to be updated,which leads to the computational complexity of EKF-SLAM. Thirdly, EKFSLAM is susceptible to the influence of incorrect data association,which may lead to the divergence of the final estimation once the data association error occurs. Sage-Husa adaptive EKF-SLAM(SHEKF-SLAM) virtualizes the uncertainties of the model and noise statistical characteristics into the process noise of the system,and uses the noise statistical estimator of SHEKF-SLAM to estimate and correct the statistical characteristics of the noise in real time,so as to improve the accuracy of AUV navigation and map construction[20]. However, it is vulnerable to the impact of the initial value of the noise statistical characteristics, the uncertainty of the model,the change of system parameters and the sudden change of the state of the positioning process.Strong tracking EKF-SLAM(STEKFSLAM) is robust to the inaccurate selection of noise statistical characteristics, the simplification of the system model and the change of actual system parameters [21-24].So,it can be invoked as a solution to the problem of SHEKF-SLAM deficiency and track the system state better. The combination of the two methods is called Combined Adaptive EKF-SLAM (CAEKF-SLAM) [25]. But CAEKF-SLAM can not fundamentally solve the fundamental problem that EKF-SLAM is not suitable for model nonlinearity and noise non-Gaussian.

FastSLAM uses sample form and is not restricted by system nonlinearity and noise non-Gaussian [26-29]. Its basic framework is Rao-Blackwellised particle filter, which decomposes SLAM into AUV path estimation and feature location estimation based on AUV path estimation. The posterior distribution of the whole AUV path is estimated by the particle filter, and the feature location is estimated by EKF, which greatly reduces the complexity of the algorithm. The data association of FastSLAM is carried out for each particle separately, and multiple data association can be made at the same time. Particles that produce false data association are easily discarded in the process of resampling, so the impact of false data by association on Fast-SLAM is relatively small. Researchers at Tokyo University first used particle filter (PF) to complete SLAM in 2006, and successfully realized the application of FastSLAM in AUV navigation and positioning [30]. Because PF uses sample form instead of function form to estimate the system state and does not consider the statistical characteristics of noise, the prediction model in practical application is inaccurate. The traditional FastSLAM chooses the prior distribution as the recommended distribution[31,32], which results in the serious degradation of particles without considering the latest measurements. Resampling reduces the degree of particle degradation but results in particle dilution. As the AUV path and map history information carried by abandoned particles are deleted, the accuracy of navigation positioning and feature location estimation of AUV by FastSLAM decreases.

In summary, aiming at the problem of sample degradation and dilution in FastSLAM, this paper proposes that simulated annealing variance reduction FastSLAM select EKF as the recommended distribution, incorporate the latest measurements into the proposed distribution to reduce the particle degradation to a certain extent, at the same time, replace the resampling process by sample weight variance reduction operation, and reduce the adaptive exponential fading factor by using the anneal function of simulated annealing to ensure the diversity of chestnut sample.

Fig.1. Relationship of coordinates.

The remainder of this paper is organized as follows: In Section 2 gives a description of the relevant system models.Propose the simulated annealing variance reduction FastSLAM in Section 3. Then,provides experiments to examine the method in Section 4. At last, Section 5 gives the conclusion.

2. System model construction

2.1. System model of AUV SLAM

2.1.1. Kinematic model of AUV

Because the sensing behavior between the sensor and environmental characteristics carried by AUV occurs in the sensor coordinate system S-XSYSZS,and the velocity measurement is in the AUV motion coordinate system V- XVYVZV. Final environmental map construction needs to be expressed in the global map GXGYGZG. Therefore, before AUV synchronous positioning and mapping, it is necessary to establish various coordinate systems and determine the relationship between them,so as to facilitate the real-time integration of various measurement data into the global coordinate system.In this paper,the global coordinate system,AUV motion coordinate system and sensor coordinate system are established as showed in Fig.1.

In Fig. 1, ψ is the angle between the initial AUV heading and magnetic north in the earth global frame.A simple 4 DOF constant velocity kinematic model is used to predict how the state will evolve.

Which, T is sampling time. ［x，y，z，ψ］ represents the position and heading direction of AUV in global coordinate system G. ［u，v，w，r］represents the corresponding linear and bow angular velocities in VX,VYand VZdirections in the carrier coordinate system V.n=［nu，nv，nw，nr］ represents the additional Gaussian noise on the line velocity and angular velocity, which acts on the velocity term in the form of acceleration. Specific form as follows:

2.1.2. Feature model

Feature maps are used to represent the environment,and some geometric prototypes are required to represent the objects in the environment. AUV can perceive the characteristics of the environment through its own sonar.Taking a structured environment as an example, common feature models are corner, edge, surface and column, etc. Considering just the plane environment, the feature maps composed of these geometric prototypes are used to represent the environment. This paper mainly focuses on the static environment, so the position of various types of features is unchanged,that is,the state of the latter moment is the same as that of the previous moment, so the kinematics model of features is as follows:

Among them,Xfirepresents the state(location)of the ith feature and n is the number of features.

2.1.3. Measurement model

AUV is deployed with compass, DVL and a pressure sensor to supply heading, velocity and depth in the state vector, so the measurement model is linear. Their common model is written as:

Wherein, z is the observation vector, H changes according to different measurement. m is the noise which affects the measurement process.

Mechanically scanning image sonar is used to apperceive the surroundings whose return is represented in the sonar coordinate system when the line was detected.Data association process will be performed in order to determine whether the measurement is new or visited.First the feature in the map should be transformed to the vehicle coordinate currently:

2.2. Feature extraction

In this paper, Hough transform is used for feature extraction[33]. In order to solve the problem of large storage and low extraction efficiency of Hough transform in extracting straight line features from acoustic images, the possibility of data points belonging to a straight line is judged by the principle of minimax fuzzy reasoning, and voting points are selected adaptively compared with the threshold set. The sonar data of mechanical scanning imaging are introduced. The specific form and the processing method of sonar data are given.2.2.1. Data preprocess

First, the beam data must be processed in advance:

(1) A reasonable threshold of acoustic strength should be set,and the bins whose intensity value are smaller than the threshold should be removed. This procedure preprocesses sonar data to eliminate background noise and eliminate false data.

(2) The local maxima of the rest bins will be picked and stored.

(3) If the distance between of two bins is smaller than a certain value according to “minimum distance” criterion, they should correspond to the same object and hence,one will be discarded.

2.2.2. Data buffer

A data buffer should be built in order to store the continuous beams. The 180°sector is the largest sector of a scan line can be covered,so there is no need for storing more data.The latest beam which falls inside the 180°sector is stored. When the preprocess determines that the newly measured beam contains one or more high-intensity bins that must participate in the voting process, a reference frame B should be set, and time, bins, pose of both AUV and sonar under B should be put in the data buffer.

The choice for B is the current position of the sonar head when the voting is performed.An advantage of choosing this basis is that,a detected line feature can be directly integrated into the SLAM framework as an observation of one of the features already in the map or incorporated as a new feature after compounding it with the current AUV position after the voting, because its parameters are already represented in the sonar frame. In order to efficiently conduct he voting process and the search for maxima, the Hough space should be built, in which line features are described by two parameters ρBand θB.

Fig. 2. Sonar model of line features.

Each bin represents the strength of the echo intensity, which return in a particular place within the insonified area. A common approach is to treat the measurement as an arc whose aperture represents the uncertainty of the beam width. The set of line features compatible with a particular bin consists not only of those lines tangent to the arc defined by α, but also of all the lines intersecting with the arc whose incidence angle is smaller than±β.This process will now be described in Fig. 2. The position of the transducer head defined by the reference frame S when a particular bin was obtained, withbeing the transformation which defines the position of S with respect to the chosen base reference B,and ρSis the range at which the bin was measured from the sonar.

2.2.3. Feature extraction and data association

After the determining the bins that will take part in voting,the process of voting in Hough space is as follows:

1) The Hough space should be quantized according the angular and

linear resolutions of sonar;

3) For every θSj, define, let

Search for the unit which approximatesin Hough space,and vote to it. Eventually, the unit which obtains the most votes corresponds to the parameters of the extracted line.

The nearest neighbor data association method is selected to determine whether the extracted feature is visited or new. The update of AUV or the augmentation of mapping should be performed.

3. Simulated annealing variance reduction FastSLAM

3.1. Condition and method of variance reduction of particle weight

Wherein:

After many recursions, weight of most particles becomes small except one or several particles and their variance becomes bigger.A majority of computational resource is occupied by the particles which have little influence on the estimation solution, and this is referred to as the degeneracy phenomenon. Variance reduction is involved in order to deal with the degeneracy [34].

Effective particle number Neffis used to measure the degeneracy degree of particles:

Since:

Where the particle number M is a constant, Neffis only related to the variance of the particle.Obviously,1 ≤Neff（~wt）≤M.The larger variance results in a smaller Neff（~wt）. Which indicates the degeneracy is more serious.In order to alleviate the phenomenon,the variance of particle weight needed to be reduced.

The principle of particle weight variance reduction is as follows:

(1) For the particles which have relatively high weights, their weights should be decreased;

(2) For the particles which have relatively low weights, their weights should be increased;

(3) Keep the original order of all particle weight remains unchanged, from high to low.

Theorem 1.Denote w1，w2，…，wMas a set of particle weight which is unnormalized, and 0 ≤w1≤w2…≤wM, there is:

In which wM>0，0<α<β<1, wα=（w1）α，（w2）α，…（wM）αand

Proof.Letaccording to Eq. (8). The proposition is equal to f（α）≥f（β） for 0<α<β<1.That is to prove that f（x）decreases monotonously with x in interval （0，1）.

Since:

We only need to study:

Then take advantage of mathematical induction to solve the problem.

(1) If M = 1, then g1（x） = 0;

(2) If M =2g2（x） =（w1w2）xln（w1/w2）［（w2）x-（w1）x］≤0,only when w1=w2the equality holds.

(3) Considering the case that M =k-1 and gk-1（x）≤0, the problem will be solved if gk（x）≤0 when M = k.is set, and the original value of particle weight is registered= wt;

(2) Effective particle number Neffis computed;

(3) When Neff

In view of the above steps (1)-(3), for any natural number MgM（x）≤0,and if w1=w2=… =wM,the equality holds.So f（x）decreases monotonously with x in interval（0，1）,and if 0< α< β<1, f（α）≥f（β） holds.

3.2. The adaptive selection of the exponential fading factor

A threshold should be set firstly.If the effective particle number is lower than the threshold in certain iteration, the variance reduction should be performed.Based on Theorem 1,compute the exponential fading factor α by solving the equation:

However, it is difficult to solve the above equation analytically by any conventional methods,and the computational burden is also too heavy, especially in the case when M is a large number. If the variance of particle weight is large or the effective particle number is small, the particle weight could be adjusted by selecting an exponent α∈（0，1）.The smaller α is,the larger the effective particle number will be.Therefore,the fading factor α should be decreased step by step from 1 to 0.A reasonable α should be selected in order to produce a group of new particles,and the supporting area of the particles could be augmented.

In non-homogeneous simulated annealing [35], the function of annealing could be tk=λ/lg（k+k0）whose temperature is inversely proportional to time. The selection of particle weight exponent factor makes reference with this idea. After obtaining the particle weight at time t,the exponent fading factor function is established:

Then the basic steps of variance reduction are as follows:

(1) The parameter of rate of variance reduction should be selected firstly.A threshold Nthrfor effective particle number

(4) The particle weights should be normalized;

(5) The new particle set is obtained.

In application, the parameter Nthris usually set as a constant,less than the particle number M(for example, Nthr= 0.8M). Let k0=1,λ determines the rate of variance reduction.If λ is larger,the rate of reduction will be slow and vice versa.However,the slow rate will induce large computation burden,and the higher velocity will make lower accuracy. Consequently, the parameterλ is related to the degree of degeneration of particles. λ should be smaller when effective particle number decreases dramatically.

3.3. FastSLAM based on variance reduction

Fig. 3. Illustration of AUV FastSLAM.

According to the idea of Rao-Blackwellised particle filter, Montemerlo made the FastSLAM posterior decompose into a product of K +1 recursive estimators, that is one estimator over AUV paths,and K independent estimators over landmark positions, each conditioned on the path estimate. As shown in Fig. 3, the real line represents the actual trajectory of AUV,the dashed line represents the estimated trajectory,“×”is the environmental feature,and the ellipse represents the recommended distribution at each update stage. Moving AUVs in partially or completely unknown underwater environment, due to the influence of noise disturbance and model uncertainty, ellipse becomes larger and larger, and position estimation error becomes larger and larger in the course of dead reckoning. Through repeated observation of the surrounding features, we can judge whether the current features have been observed or not.If they belong to the observed features,we can use the features to predict the location and reality. The deviation between the measured positions can correct the position of itself and the feature position, the ellipse becomes smaller, and the position of AUV and feature is updated; otherwise, the new feature will be integrated into the map to realize the expansion of the map. The map in a particle depends on the accuracy of the trajectory,and the probability model of the AUV position can be obtained from multiple such trajectories.

So the FastSLAM posterior could be represented as:

Wherein,superscript t represents the set of all the variables from 1 to t. st=s1，s2，…stis the path estimate of AUV; zt= z1，…ztis the observation of each landmark; ut=u1，…utis the control vector;Θ=｛θ1，…θK｝ is the set of all the landmark; nt∈｛1，…K｝ is the index of landmark which is perceived at time t.

Variance reduction particle filter is used to estimate the path of AUV in variance reduction FastSLAM which can sample in sample space effectively. EKF is used to estimation the location of landmarks.Since the estimate of landmarks depends on the estimate of the AUV path,each particle has its own local landmark estimate in variance reduction particle filter.Therefore,if there are M particles and K landmarks, there will be M×K extended Kalman filters in total.

3.3.1. Estimate of AUV path with variance reduction particle filter

Variance particle filter is used to estimate the posterior distribution of AUV pathin variance reduction FastSLAM.A set of particles are used to represent this posterior, and will be written as St. Each st，［m］∈Stdenotes an estimate of AUV paths:

Which, the superscript ［x， y， z， ψ］ represents the m-th particle in set.

Particle set Stis calculated incrementally from particle set St-1at time t-1,control vector utand the observation vector zt.Since we could not draw samples from the SLAM posterior directly at time t,we will draw samples from a simpler distribution called the proposal distribution instead,and correct for the difference using a technique called importance sampling.

The proposal distribution of FastSLAM generates guesses of the AUV’s pose at time t given each particle st-1，［m］. This guess is obtained by sampling from the motion model:

3.3.2. Estimate of landmark location based on EKF

EKF is used to estimate the location of landmarks in variance reduction FastSLAM.Since the landmark estimates are conditioned on the AUV’s path, EKFs are attached to each particle in St. So the posterior of the entire path and landmarks is represented with particle set as follows:

The posterior over the k-th landmark position is easily obtained.Its computation depends on whether nt=k.If nt=k,the landmark θkwas observed at time t. We can get:

If nt≠k, the landmark θkwas observed first time, then

This process is data association. nt=k means that the new measurement corresponds to the feature that was already in the map,and EKF is used to update the location of the landmark.nt≠k means that this measurement doesn’t correspond to any feature in the map, and the augmentation of mapping will be performed.

Samples extracted from the proposed distribution obey thedistribution, which does not exactly match the expected target distribution. The deviation is corrected by the weight of importance.

In practical application, the importance weight is usually calculated based on the residual Vt(the difference between the actual measured value ztand the predicted measured value).The residual is a Gauss distribution with zero mean and Ztvariance.The probability distribution of the measured value ztis the same as that of, so the weight is:

Particles are prone to degenerate after multiple recursions,especially when the prior distribution matches the target distribution poorly. Firstly, the degeneration degree of particles is measured according to the number of effective particles (e.g. Eq.(8)). If the number of effective particles is less than the set threshold,then the adaptive exponential fading factor generated by the cooling function of simulated annealing is used to reduce the weight of large-weighted particles and improve the smallweighted particles. In order to increase the number of effective particles, the variance of the weight is reduced. If the number of effective particles is larger than the set threshold, the variance reduction operation is not performed.

3.3.3. The algorithm process

Fig. 4. Flowchart of algorithm principle.

Fig. 5. Experimental environment and equipment: (a) Satellite image of experimental environment; (b) Ictineu AUV.

In this structured data set of harbor environment,the measured values of environmental perception obtained by mechanical scanning imaging sonar (MSIS) intersect with the extended surface in the acoustic image and show line features. Because the position parameters of these line features do not change with the change of sonar measurement position,only their visibility is affected by the incident angle of the sonar beam and the clarity of sea water. The influence of reflector material is only a static line feature, so it is appropriate to detect the line feature in the image by transformation.The advantage of transformation is that it is insensitive to random noise and robust enough to extract partially covered lines or false measurements.

According to the above basic steps, combined with the fuzzy adaptive transformation feature extraction algorithm and grey prediction data association algorithm, the flow chart of the whole method based on the fuzzy adaptive transformation feature extraction and grey prediction fast switching data association is designed, as shown in Fig. 4.

4. Experiments and results analysis

This paper uses the open data set provided by Girona University of Spain for underwater SLAM algorithm verification. The satellite image of the experimental environment is shown in Fig.5.The data set is derived from the structured port environment,and the port is mainly composed of vertical extension surfaces such as dikes and dams.In the structured data set of port environment,the speed of Ictineu AUV is about 0.2 m/s.The data set consists of the measured data of DVL, Compass and MSIS. The range of the MSIS is 50 m,range and angular resolution is 0.1 m and 1.8°, respectively. The selected particle number M=100, the threshold was set Nthr=0.5M experientially and the reduction rate parameter of weight variance was set λ = 0.8.

Fig. 6. Comparison of particles with traditional resampling and variance reduction.

In order to verify the cancellation of this algorithm, a lot of comparative experiments have been done. In order to describe conveniently, this paper is referred to the SLAM method based on EKF as EKF-SLAM.Sage-Husa adaptive EKF-SLAM method,which is referred to as SHEKF-SLAM. Strong tracking EKF-SLAM method,which is referred to as STEKF-SLAM. Combining SHEKF-SLAM method with STEKF-SLAM, which is referred to as CAEKF-SLAM.Traditional FastSLAM method, which is referred to as FastSLAM.This paper proposes a method of variance reduction in quasiannealing is called VRFastSLAM.

Fuzzy adaptive Hough transform is used to extract the features of acoustic images in the simulation experiment,and the threshold is 0.7. The grey prediction ICNN-JCBB [36] fast switching data association method is used to realize the correlation process between the current observation and the existing features in the map. The parameters of noise statistical characteristics are selected as follows:

Fig. 7. Effective particle change curve.

Fig. 8. Variation curve of diversity particles.

Fig. 9. AUV track and port line feature estimation results.

Fig.10. Error chart of eastward direction.

Fig.11. Error chart of northern direction.

Fig.12. Statistical results of port position parameter ρ error.

Among them, q and Q is the mean and covariance of process noise.rDand RDis the mean and covariance of velocity noise.rCand RCis the mean and covariance of heading noise. rPand RPis the mean and covariance of depth noise.

Fig.6 is a comparison of particle weight in AUV path estimation using particle filter based on weight variance reduction and standard particle filter.It can be seen from Fig.6 that before resampling,the weight of other particles are almost zero except for a few particles with large weight,the variance of weight samples is large and the samples degenerate. After resampling, although the weight of particles increases, there are only two kinds of particles. It can be seen that the large weight particles in the original particle concentration are copied,and the phenomenon of particle depletion is very serious. When variance reduction strategy is adopted, the weight of large weight particles decreases, the weight of smallweight particles increases, the number of particles with zero weight decreases greatly, and the variance of weights decreases significantly, thus the number of effective particles is increased,which not only reduces the degradation degree of samples,but also ensures the diversity of samples.

Fig.7 shows that the variance reduction operation of simulated annealing reduces the weight of large weighted particles,increases the weight of small-weighted particles,and reduces the variance of particle weight (Eq. (8) describes the relationship between the variance of particle weight and the number of effective particles),which makes the number of effective particles change in a small range nearby and improves the particle degradation significantly.

Fig. 8 shows that, compared with standard FastSLAM, VRFast-SLAM can significantly reduce particle degradation,but at the same time, the diversity of particles is almost not lost. It can effectively avoid particle dilution, ensure that all particles participate in theAUV navigation and positioning process, and maximize the historical information of AUV Path carried by particles.

Table 1 Estimation error of characteristic position parameters.

From Figs. 9-11, it can be seen that SHEKF-SLAM adaptively estimates and corrects the statistical characteristics of process noise in real time by using time-varying process noise estimators,avoiding the influence of the incomplete matching between system model and real model and the unknown or time-varying statistical characteristics of noise on EKF-SLAM.Thus,the estimation accuracy of position and line feature location is more accurate than that of EKF-SLAM. Degree has been greatly improved. STEKF-SLAM can significantly reduce the position estimation error of AUV compared with EKF-SLAM by weakening the influence of obsolete data on current position estimation and real-time correcting the covariance matrix and gain matrix of state prediction.The estimated results of CAEKF-SLAM are basically consistent with those of SHEKF-SLAM.The AUV positioning curve almost coincides with the feature location estimation.It shows that the CAEKF-SLAM estimates better when the initial value of measurement noise is selected properly.

Compared with SHEKF-SLAM, CAEKF-SLAM, STEKF-SLAM and EKF-SLAM, FastSLAM improves the position estimation accuracy and line feature position parameter estimation accuracy of AUV.The essential reason is that the nonlinear AUV motion system using EKF need to linearize the Taylor expansion of the model, which inevitably has some errors. The non-Gaussian noise is another important reason for the low estimation accuracy of EKF-SLAM.FastSLAM estimates the state of AUV in the form of sample rather than function, which is not affected by the nonlinearity of the system and the non-Gaussian noise. Therefore, the accuracy of estimating the position of AUV is relatively high. The position parameter estimation of features depends on the position estimation of AUV. Therefore, the accuracy of position parameter estimation of features as shown in Table 1, Figs.12 and 13 is bound to improve. The navigation location and feature location estimation results of simulated annealing variance reduction FastSLAM are better than other methods.

Although the estimation error of FastSLAM is much lower than that of EKF-SLAM, there are still some errors after 800s. From the effective particle number curve in Fig. 7, it can be seen that the more frequent of the resampling operation,the more serious of the particle degradation. The reason is that the recommended distribution of the standard is a prior distribution, and the difference between the prior distribution and the recommended distribution lead to the degradation of a large number of particles. This is reflected in the phenomena described in Figs. 7 and 8. VRFastSLAM can significantly reduce particle degradation while effectively maintaining the diversity of particles and improving the accuracy of AUV navigation positioning and feature position estimation.

Fig.13. Statistical results of port position parameter θ error.

In summary, the variance reduction of simulated annealing improves the standard in two aspects: suppressing particle degradation and improving particle depletion. It improves the accuracy of navigation,positioning and feature position estimation.It is of great significance for long-range navigation and concealment tasks.

5. Conclusions

Aiming at the SLAM problem of AUV in some known or completely unknown marine environments, this paper focuses on Fastslam algorithm, and proposes a simulated annealing variance reduction FastSLAM algorithm to solve the problem of particle degradation and dilution that affect navigation positioning accuracy. Experiment with trial data showed that based on simulated annealing, variance reduction FastSLAM performed variance reduction operation by determining whether the effective particle number was smaller than the threshold. The exponential fading factor was selected adaptively to avoid particle degeneracy while the particle diversity was kept. Especially the designed method is robust for the degeneracy caused by the model established not match with the actual kinematic model. What’s more, feature extraction based on Hough transform was designed according to the sonar model, and extracted the line features well. The correctness of AUV location was realized by the revisiting of features,and it was the kernel of SLAM.Accuracy and stability of AUV SLAM system were improved obviously via the variance reduction Fast-SLAM with simulated annealing. It is significant for the AUV to execute long term sea environment monitor and underwater work.The presented method is proved available in middling structured environment, and the SLAM in large unstructured environment is the key point in the future.

Declaration of competing interest

The authors declare no conflict of interest.

Acknowledgments

This work was supported by the National Science Fund of China under Grants 61603034, China Postdoctoral Science Foundation under Grant 2019M653870XB, Beijing Municipal Natural Science Foundation (3182027) and Fundamental Research Funds for the Central Universities, China, FRF-GF-17-B44, and XJS191315. At last thanks to Dr. Jing Wang.