^{1}

^{*}

^{1}

^{1}

^{1}

^{2}

^{*}

^{3}

^{1}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

Navigation technology is one of the most important challenges in the applications of autonomous underwater vehicles (AUVs) which navigate in the complex undersea environment. The ability of localizing a robot and accurately mapping its surroundings simultaneously, namely the simultaneous localization and mapping (SLAM) problem, is a key prerequisite of truly autonomous robots. In this paper, a modified-FastSLAM algorithm is proposed and used in the navigation for our

An AUV is an untethered underwater robot which can navigate itself in the complex undersea environment. They are widely applied in military missions, resource exploration and oceanographic surveys. Generally, a mobile robot estimates its own position and posture by testing the internal state or perceiving the external environment, and adjusts its position and realizes accurate positioning under the guidance of a prior map, but in unknown complicated underwater environments, the prior map is not always available, then the AUV can only perceive the environment through on-board sensors such as cameras and sonar, obtain the useful information to construct the environmental map and estimate the vehicle location [

Smith ^{2}) (the number of environmental features) computational complexity. To deal with the limitation, people divide the world into a handful of sub-maps; each contains ^{2}), but the trade-off is sacrificing the convergence speed.

Thrun ^{2}). Measurement and movement can be realized in a constant time. The weakness of the algorithm lies in the difficult data association, bad consistency and impracticable covariance [

Arulampalam

Currently, a lot of research groups have their own AUVs for studying SLAM algorithms. In the ACFR of University of Sydney, the

Taking the advantages of the FastSLAM algorithm, a modified algorithm based on the improved particle filter (PF) is presented in this paper, which employed the data association method by combining the single particle maximum likelihood method with a modified

The

The AUV has good maneuverability due to five DOFs, including surge, heave, roll, pitch, and yaw. The thrust system of this platform consists of five propeller thrusters, where two thrusters paralleling to the bow direction are installed in the abdomen to provide horizontal thrust, mainly for controlling the surge and yaw, while the other three thrusters, two of which are installed on both sides of the bow, and the remaining one is installed on the rear of the vehicle, are employed to provide vertical thrust to control the heave, roll, and pitch. The upper hull of

A number of sensors are installed on the

Mapping-Related Sensor: Active Imaging Sensor

A mechanically scanning forward-looking sonar (Super Seaking DST, Tritech) used for active sensing of environment features is installed at the front top of C-Ranger. It is the principal sensor of C-Ranger AUV. The operating frequency of the sonar is 675 kHz, and its working range is up to 300 meters.

The transducer head of this kind of sonar usually needs a considerable period of time to perform a 360° rotation. This is an important issue that has to be taken into account when operating with such sonar mounted on an AUV, since the resulting acoustic images can get distorted as a consequence of the vehicle's motion. Generally, this effect can be ignored for low velocities but for higher velocities it has to be dealt with. Feature extraction is an important process, e.g., Forouher

Velocity Sensor: DVL

The DVL (NavQuest600, LinkQuest) is used to provide the velocities of the vehicle relative to the seabed. In addition, the NavQuest600 can provide other information: pitch angle, roll angle, heading, altitude, depth, temperature and velocities relative to the ocean currents.

Angular Sensors: AHRS and Gyro

The AHRS (M2, Innalabs) is used to produce attitude information, and a gyro (VG951D) is used to measure angular velocity in the AUV navigation process. AHRS M2 is a low-cost high-performance inertial navigation system, and magnetic interference will not affect the accuracy of headings over short times.

Positioning Sensor: GPS

To evaluate the navigation performance of the

The goal of SLAM is to simultaneously localize a robot and determine an accurate map of the environment, that is, to determine the posterior ^{t}^{t}^{t}^{t}^{t} = {_{1}_{t}_{1},…,_{m}^{t} = {_{1},…,_{t}^{t} = {_{1},…,_{t}^{t} = {_{1},…,_{t}}. The data associations describe the mapping of observations to the landmarks in the map.

FastSLAM is an efficient algorithm for the SLAM problem which partitions the SLAM posterior into a localization problem and

In FastSLAM, a particle filter is used to approximate an ideal recursive Bayesian filter for the mobile vehicle's pose estimation ^{t}^{t}^{t}^{t}^{t}^{t}^{t}^{t}

In ^{t,n} is the _{i}, attached to the

Sample a new pose for each particle.

Update the landmark observed with EKF in each particle.

Calculate an importance weight.

Importance resampling.

Our algorithm has made improvements to the FastSLAM2.0 and we call it modified-FastSLAM. This modified algorithm takes advantage of the FastSLAM 2.0 algorithm, which employs a data association method combining the single particle maximum likelihood method with a modified

Sampling A New Pose for Each Particle

The vehicle pose are sampled conditioned the motion model incorporating the most recent measurement data _{t}_{t}

The new particles are distributed according to:

The distribution is referred to as the proposal distribution of particle filtering.

Updating the Estimation of observed Landmark

The FastSLAM represents the landmark estimates ^{t}, z^{t}, u^{t}, n^{t}_{i}_{i}_{t}

The algorithm achieves the update

For _{t}

The classical solution to the data association problem is to choose _{t}_{t}

Data association can be made on a per-particle basis because each particle represents a different predicted path. Montemerlo had put forward a per-particle ML (maximum likelihood) [

In order to correctly identify the false landmarks, and ensure accurate data association, this paper sets information variables to track observed landmarks and judges the authenticity of landmarks in the map in accordance with an acceptance gate and discard gate.

All the observed landmarks are added into a preprocessing landmark set and we use variable ^{m}^{m}

If _{m}_{m}

Each time only landmarks in the local area around the robot can be re-observed and thus we only need to make the observations associated with the landmarks in the local map. The local map can be determined according to the detection range of the sensor and the robot pose using _{x}_{y}_{x}_{y}

Calculating Importance Weights of Each Particle

Before importance resampling, the importance weights must be calculated. The proposal distribution ^{t−1}‖^{t−1},^{t−1},^{t−1})_{t}^{t−1},^{t},^{t},^{t}) do not match the desired posterior ^{t}‖^{t},^{t},^{t}). The importance weight of each particle is calculated as follows:

Rank-Based Resampling

Resampling, which tends to eliminate particles with low weights, plays an important role but causes the particle depletion problem in FastSLAM. The literature [

In our algorithm, a rank-based resampling algorithm is adopted from the rank-based selection in GA to resolve the particle depletion problem. Differing from RBR algorithm, our algorithm generates new particles with two selected particles, rather than simply by replacement using a selected particle, and that way will ensure better particle diversity. The pseudo code of our resampling algorithm is shown in

In this section, simulation experiments are carried out to evaluate the performance of the modified-FastSLAM algorithm in comparison with the original method. The FastSLAM2.0 simulator obtained from [_{t}_{x}_{y}_{t}

The algorithms are run with 50 particles while the number of effective particles is three-quarters of the total number of particles. Results in the following are obtained by averaging over 50 Monte-Carlo trials. Pose errors, RMS, NEES, will be produced to evaluate the estimation performance in the next section.

The data association performances of the two algorithms are shown in

Comparison of the loss of particle diversity between original FastSLAM and modified FastSLAM is shown in

The robot pose errors are shown in

In order to evaluate the consistency of the two algorithms, we adopt the

RMS errors of robot position and heading are shown in

We use the average NEES to characterize the filter performance. NEES at time _{t}_{t}_{t}^{2}(chi-square) distributed with the dim (_{t}

The dimension of the robot pose is 3, with N = 50, the 95% probability concentration region for _{t}

All of the experimental results confirm the effectiveness of the modified algorithm. Its main advantage is that the proposed method is more consistent than the original FastSLAM. Also, the deviation of the modified FastSLAM algorithm is far less than that of the original method. In addition, the modified-FastSLAM has good robustness. On the whole, the modified-FastSLAM outperforms the original one.

In the _{x}_{y}^{T}_{x}_{y}

State vector _{x} v_{y}^{T}^{T}_{x}_{y}^{n} = [^{T}^{n}, and the nonlinear portion ^{1} = [_{x} v_{y}^{T}^{1}, where ^{n} = [_{x}_{y}^{T}^{n}^{l}_{φ}_{x}_{y}^{T}^{l}

Like the state vector, the observation vector also is divided into two parts ^{n} ^{1}]^{T}^{n} is from sonar and ^{1} is from compass and DVL. To map its environment, the AUV senses landmarks by imaging sonar. It is able to measure range and bearing to landmarks denoted as ^{n} = [^{T}_{x}_{y}^{T}^{n}

The compass and DVL measurements
_{x}_{y}^{l}

^{n} is independent of ^{1}, and ^{1} is independent of ^{n}, using Bayes' theorem the posterior can be factored into

For the whole system, the first term in

Firstly, initialize state vector. Employ AHRS and gyro data to predict the AUV's next location. Then extract point features from sonar data to update or augment the map. After that, the robot pose will be updated and importance weight will be computed. Lastly do resampling. It is a recursive process. The linear state ^{1} is estimated using a standard Kalman filter. It is updated twice. First it is updated by using observation ^{1}, and then ^{n} is treated as an observation to update ^{1}.

The experiment was performed at Tuandao Bay (Qingdao, China) and the sea trial dataset samples were obtained from the on-board sensors introduced in Section 2. A sonar operating with 180° scanning-sector and 100 m-range is used to sense the environment, and features in the environment are extracted and used to build the global map of the environment based on the algorithm proposed in this paper.

In

The resulting map of the sea trials combined with the environmental map is shown in

In order to get a clear demonstration of the presented algorithm, the position RMS errors by the dead-reckoning, the original FastSLAM (the red line) and the modified-FastSLAM (the blue line) are compared in Figure 13. It can be noted clearly that the overall trend of the error by dead-reckoning is divergent. The curves of the original FastSLAM and modified-FastSLAM have similar trends, and they both increase in the beginning phase due to the cumulative errors, and then they will decrease because landmarks are repeatedly observed so that the trajectory and the map are updated. It is obvious that the position error of the Modified-FastSLAM is less than that of the original FastSLAM, the maximum error of the original FastSLAM trajectory is up to about 16 m, while the Modified-FastSLAM is only about 10 m.

To further evaluate the performance of the Modified-FastSLAM, the RMS errors of AUV positions over the results of 50 runs are presented in

The maximum errors, the minimum errors and the averaged RMSE of the algorithms are compared in this figure. It can be seen clearly that the errors of modified-FastSLAM are much smaller than those of the other two algorithms. The standard deviations of estimation errors are summarized in

The Modified-FastSLAM also shows the best performance compared with the other two algorithms. Furthermore, the comparison of pose errors on x-axis, y-axis and heading angle are shown in

Although the Modified-FastSLAM algorithm is better than the original FastSLAM algorithm according to simulations and field experiments, there are still some problems. Compressing point features in the feature extraction process is time-consuming as a result of the large number of point features. It is also a time-consuming process to determine resampling when compared with standard FastSLAM. However, information compression of point features can reduce the time-cost of data association eventually; especially the employed resampling condition can limit the frequency of resampling, so the total computational cost of the algorithm does not increase.

The proposed algorithm does not fundamentally solve the consistency problem though it is more consistent than the original one, so it might face the problem in applications involving large scale environments. Some algorithms have been proposed to deal with this problem. Among them, the submap-based SLAM is a more effective one. In future works we will try to apply the modified-FastSLAM to combine with EIFSLAM. The modified-FastSLAM can be used to produce local maps, which are periodically fused into an EIFSLAM algorithm. Modified-FastSLAM may avoid linearization of the robot model during operation and provide a robust data association, while EIFSLAM would improve the overall computational speed.

The mechanical-scanning sonar used on the

This paper describes the

This work is partially supported by the High Technology Research and Development Program of China (2006AA09Z231), Natural Science Foundation of China (41176076, 51075377), and the Science and Technology Development Program of Shandong Province (2008GG1055011, BS2009HZ006) and Zhejiang Natural Science Foundation (R1100015), Open foundation (GZKF-201017) of State Key Lab of Fluid Power Transmission and Control in Zhejiang University.

. (

Control architecture of the C-Ranger.

Effect of the vehicle motion on acoustic images. (

Results of FastSLAM and modified-FastSLAM obtained by averaging over 50 Monte-Carlo trials.

Data association results.

Number of distinct particles over time. The particle number used is 50.

Estimation pose error with 2-σ uncertainty bound of the original FastSLAM and the modified FastSLAM algorithm.

RMS error of the two algorithms.

Average NEES of the two algorithms.

(

The modified FastSLAM resulting map combined with the environmental map.

Comparison of the trajectories for GPS, Dead-Reckoning, Original FastSLAM and Modified-SLAM by averaging over 50 independent runs.

The position RMSE of Dead-Reckoning, original FastSLAM and modified-FastSLAM by averaging over 50 independent runs.

. RMSE summarization of Dead-Reckoning, original FastSLAM and modified-FastSLAM over 50 runs.

Pose errors between estimated pose and GPS ground truth. (

Resampling algorithm.

If Neff<Nparticles*0.75&&Weight_covariance>Mean_weight && residual_inconsistency |

newparticles=resampling(Particles,weight,Nparticles,Nnew_particles); |

end |

Function newparticles=resampling(Particles,weight,Nparticles,Nnew_particles) |

Nr=zeros(1,Nparticles); |

U=rand()/Nparticles; |

Cmax=1+rand(); |

Cmin=2-Cmax; |

Ws,rank=sort(w,'descend'); |

for i=1 to Nparticles |

K=rank(i); |

ps(k)=(Cmax-(Cmax-Cmin)*(i-1)/(Nparticles-1))/Nparticles; |

Nreplace(i)=fix((ps(i)-U)*N)+1; |

U=U+Nreplace(i)/N-ps(i); |

end |

index=find(Nr>0); |

Len=length(jj); |

for i=1:Nnew_particles |

index1=index(round(rand()*Len)); |

index2=index(round(rand()*Len))å |

Rand_value=rand(); |

New_particles(i)=Rand_value*particles(index1)+(1- Rand_value)*(particles(index2); |

end |

Parameter settings used in the simulation.

4 m | (0.1 m/s, 0.1 deg) | ||

3 m/s | 40 HZ | ||

30 deg | 5 HZ | ||

(0.3 m/s, 3 deg) | 30 m |

The SLAM algorithm in the C-Ranger AUV.

for i=1:num_of_particles |

Initialization particles |

end for |

while sensors data are ready do |

for t=1 to num_of_particles |

Predict the state |

Compute the weight of each particle and normalize |

Update ^{l} using ^{n} |

end for |

/*Extract point features from sonar data for mapping and update the vehicle's state.*/ |

if sonar data are ready then |

for i=1 to num_of_particles |

Particles(i),n=data_association |

/*n is the data association results.*/ |

for num =1:length(n) do |

if n(num) is a new feature then |

Augment the map Θ |

else |

Update ^{n} using particle filter |

end if |

end for |

If _{m} |

remove m-th spurious feature |

end if |

Update ^{l} using ^{n} |

end for |

end if |

Employ Rank-based resampling to do resampling |

end while |

Standard Deviation of Estimation Errors.

| |
---|---|

Deadreckoning | 0.1979 |

Original FastSLAM | 0.0846 |

Modified FastSLAM | 0.0595 |