Autonomous Navigation for Autonomous Underwater Vehicles Based on Information Filters and Active Sensing

Abstract

This paper addresses an autonomous navigation method for the autonomous underwater vehicle (AUV) C-Ranger applying information-filter-based simultaneous localization and mapping (SLAM), and its sea trial experiments in Tuandao Bay (Shangdong Province, P.R. China). Weak links in the information matrix in an extended information filter (EIF) can be pruned to achieve an efficient approach-sparse EIF algorithm (SEIF-SLAM). All the basic update formulae can be implemented in constant time irrespective of the size of the map; hence the computational complexity is significantly reduced. The mechanical scanning imaging sonar is chosen as the active sensing device for the underwater vehicle, and a compensation method based on feedback of the AUV pose is presented to overcome distortion of the acoustic images due to the vehicle motion. In order to verify the feasibility of the navigation methods proposed for the C-Ranger, a sea trial was conducted in Tuandao Bay. Experimental results and analysis show that the proposed navigation approach based on SEIF-SLAM improves the accuracy of the navigation compared with conventional method; moreover the algorithm has a low computational cost when compared with EKF-SLAM.

1. Introduction

Autonomous Underwater Vehicles (AUVs) have portable energy and self-control ability which make them different from remote operate vehicles (ROVs). They are suitable for commercial and military tasks underwater, under-ice or in other environments [1,2]. In the past few decades, the world plans to build or has built about 200 AUVs, such as the well-known REMUS by WHOI [3] or MIT Sea Grant’s Odyssey [4]. As the applications of AUVs are spreading to deeper seas and longer distances, high accuracy navigation capability will play a vital role. Inertial navigation systems (INS) are widely used in AUVs, but the navigation errors accumulate over time and waves and currents exacerbate this. Though the errors can be reduced periodically by using GPS, electromagnetic signals decay very quickly in the water, so the navigation of underwater vehicles cannot rely on GPS. As acoustic signals decay extremely slowly in seawater, acoustic navigation is widely used in three ways: long baseline, short baseline, ultra short baseline. However, deployment and recovery of the baseline is time-consuming and expensive, which limits acoustic navigation in large-scale environments [5]. In addition, it is very difficult to obtain prior maps of large-scale complex underwater environments. For all the reasons mentioned above, autonomous underwater navigation is considered one of the most challenging issues for AUVs [6].

Simultaneous localization and mapping (SLAM) does not require the aid of a priori information about the underwater environment, as when the vehicle moves in the water, its onboard sensors perceive the external environment and extract useful information, then form a map of the environment incrementally while positioning itself [7,8]. SLAM has attracted immense attention since the 1990s as the solution for true autonomous navigation [9], and a handful of algorithms have emerged for this purpose such as EKF-based SLAM, PF-based SLAM and IF-based SLAM. Using a parametric model, EKF-SLAM can be described as a posterior probability distribution parameterized by a state vector and covariance matrix. The algorithm consists of two main steps: prediction and update, which can be summarized as an iterative estimate and calibration process [10,11]. However, the map in EKF-SLAM comprises a covariance between the vehicle state and environmental features which must be processed in each estimate and correct step. As a result, It achieves O(n²) (the number of environmental features) computational complexity. To deal with the limitations, people divide the world into a handful of sub-maps; each containing l features [12–15]. Thus, computational complexity can be reduced to O(l²), but the tradeoff is sacrificing the convergence speed.

Particle filtering (PF) is different from KF, which solves SLAM problem by the particles based on the conditional independence. Fast SLAM, proposed by Montemerlo [16,17], is a very popular SLAM algorithm based upon particle filters. Fast SLAM decomposes SLAM into estimation of the robot path and estimate of the location of the features in the map. Once the vehicle path is established, the features are only related with the vehicle pose [18]. The particle filter is efficient in computation and can represent non-linear, non-Gaussian motion modes, but the more complex the environment is, the more particles are required to describe the posterior probability distribution. As a result, the algorithm complexity increases. In addition, the re-sampling will lead to loss of validation and diversity of particles which results in sample depletion.

Extended Information Filter (EIF) is the information form of EKF, parameterized by the information matrix and information vector corresponding to the EKF [19]. The time projection is efficient as it is quadratic in the number of measurements and not the size of the map. However, recovering the mean from the information matrix and information vector requires a costly O(n³), matrix inversion, so the computational complexity is even higher than for EKF. It is found that the vast majority of the information matrix elements are close to zero, while the information matrix is dominated by a small number of diagonal elements [20]. If the elements are made approximately close to zero, that is pruning weak links in the information matrix, an approximate representation which is the so-called Sparse Extended Information Filter (SEIF) proposed by Thrun et al. is obtained [21,22]. All the basic update formulae can be implemented in constant time, irrespective of the size of the map, which greatly improves computational complexity. There are a variety of new information filter-based algorithms such as TJFT [23], Treemap [24], ESEIF [25,26], but theoretical analysis and experimental evidence show that SEIF-SLAM is computationally efficient and consistent in the relative map, so the SEIF-SLAM algorithm is preferred in our work.

The C-Ranger AUV was developed in Underwater Vehicle Laboratory in Ocean University of China. It is equipped with a variety of onboard equipment for sensing vehicle pose and environment. In this paper we mainly address the autonomous navigation method for the C-Ranger AUV. To verify the advantages of SEIF-SLAM, the application of SEIF-SLAM in AUV navigation via sea trial experiments has been studied; also the data processing approach of sonar had been presented in this work. For the mechanical scanning imaging sonar chosen as the principal sensor for active sensing of undersea obstacles, a compensation method based on feedback of the AUV pose had been used to overcome distortion of the acoustic images due to the vehicle motion. Sea trials in Tuandao Bay were carried out to verify the feasibility of the proposed methods. The experimental results and analysis show that the proposed navigation approach based on SEIF-SLAM improved the accuracy of navigation when compared with conventional method; moreover the algorithm has a low computational cost when compared with EKF-SLAM.

The remainder of the paper is organized as follows: in the next section, the SEIF-SLAM algorithm for our C-Ranger AUV is presented, including the main processing steps. Section 3 describes the C-Ranger AUV and on-board sensors employed in the SLAM module. Further, sonar data processing is discussed in Section 4, including point-feature extraction and motion-distortion compensation. The sea trial experiments is described in Section 5, and the performance of the proposed navigation method is evaluated; then the results and possible future improvements are discussed in Section 6. Finally, we summarize the results and draw the fundamental conclusions.

2. A SEIF-SLAM Algorithm for the C-Ranger AUV

AUV travels undersea at a certain depth in most cases, so the bidimensional vehicle-landmark model was adopted to represent the AUV and landmarks (also called features) in the undersea environment. The vehicle pose x_t = (x_t, y_t, θ_t)^T and the set of map features M = (m_i, 1 ≤ i ≤ N)^T are stored in the state vector:

$${\xi }_t={({\mathbf{x}}_t^T,{\mathbf{M}}^T)}^T$$

(1)

Like in Smith et al. [27], a first-order linearization of the motion and measurement models is employed considering the uncertainty in the data as independent, white Gaussian noise. Corresponding to the Gaussian distribution in Appendix, SEIF-SLAM can be presented as the following posterior probability distribution:

$$p\left({\mathbf{x}}_t,\mathbf{M}|Z^t,U^t\right)=N({\mathbf{\mu }}_t,{\mathrm{\Sigma }}_t)=N\left(\left[\begin{array}{l}{\mathbf{\mu }}_{x_t}\hfill \\ {\mathbf{\mu }}_M\hfill \end{array}\right],\left[\begin{array}{ll}{\mathrm{\Sigma }}_{x_t{X}_t}\hfill & {\mathrm{\Sigma }}_{x_{t}M}\hfill \\ {\mathrm{\Sigma }}_{M{x}_t}\hfill & {\mathrm{\Sigma }}_{\mathit{MM}}\hfill \end{array}\right]\right)=N^{-1}({\mathbf{\eta }}_t,{\mathrm{\Lambda }}_t)=N^{-1}\left(\left[\begin{array}{l}{\mathbf{\eta }}_{x_t}\hfill \\ {\mathbf{\eta }}_M\hfill \end{array}\right],\left[\begin{array}{ll}{\mathrm{\Lambda }}_{x_t{X}_t}\hfill & {\mathrm{\Lambda }}_{x_{t}M}\hfill \\ {\mathrm{\Lambda }}_{M{x}_t}\hfill & {\mathrm{\Lambda }}_{\mathit{MM}}\hfill \end{array}\right]\right)$$

(2)

where, μ_t is the mean of the state vector and η_t is the information vector, Σ_t and Λ_t denote the covariance and information matrix respectively, Z^t and U^t are the history of observational data and motion control inputs. To calculate the probability distribution, the algorithm mainly includes motion update, measurement update, sparsification, mean recovery and other steps. Figure 1 shows the structure of the algorithm.

(A) Motion Update Step

The motion update step predicts the distribution over the new robot pose from time t − 1 to time t and subjects it to a Markov model, in general, a nonlinear function f of the previous pose x_t−1 and the control inputs u_t. Equation (3) is the first-order linearization about the mean robot pose μ_xt–1, F denotes the Jacobian matrix about the state vector at time t − 1 and the term v_t ∼ N(0,Q_t) represents the white Gaussian noise in the model, Q_t is the noise covariance:

$${\mathbf{x}}_t=\mathrm{f}\left({\mathbf{x}}_{t-1},{\mathbf{u}}_t\right)+{\mathbf{v}}_t\approx \mathrm{f}\left({\mathbf{\mu }}_{x_{t-1}},{\mathbf{u}}_t\right)+\mathrm{F}\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{x_{t-1}}\right)+{\mathbf{v}}_t$$

(3)

The motion update step can be implemented in two sub-steps. First, as in Figure 2(a), the state vector is grown to include the new robot pose ${\widehat{\mathbf{\xi }}}_t={\left({\mathbf{x}}_{t-1}^T,{\mathbf{x}}_t^T,{\mathbf{M}}^T\right)}^T$. According to the work by Eustice et al. [25,26], the augmentation of the information matrix and vector are given in Equations (4) and (5):

$${\widehat{\mathrm{\Lambda }}}_t=\left[\frac{({\mathrm{\Lambda }}_{x_{t-1}{x}_{t-1}}+{\mathrm{F}}^{\mathrm{T}}{\mathrm{Q}}_t^{-1}\mathrm{F})}{\begin{array}{l}-{\mathrm{Q}}_t^{-1}\mathrm{F}\hfill \\ {\mathrm{\Lambda }}_{{\mathit{Mx}}_{t-1}}\hfill \end{array}}|\frac{\begin{array}{cc}-{\mathrm{F}}^{\mathrm{T}}{\mathrm{Q}}_t^{-1}& {\mathrm{\Lambda }}_{x_{t-1}M}\end{array}}{\begin{array}{cc}{\mathrm{Q}}_t^{-1}& \mathbf{0}\\ \mathbf{0}& {\mathrm{\Lambda }}_{\mathit{MM}}\end{array}}\right]=\left[\frac{{\widehat{\mathrm{\Lambda }}}_t^{11}}{{\widehat{\mathrm{\Lambda }}}_t^{21}}|\frac{{\widehat{\mathrm{\Lambda }}}_t^{12}}{{\widehat{\mathrm{\Lambda }}}_t^{22}}\right]$$

(4)

$${\widehat{\mathbf{\eta }}}_t=\left[\frac{{\mathbf{\eta }}_{x_{t-1}}-{\mathrm{F}}^{\mathrm{T}}{\mathrm{Q}}_t^{-1}\left(\mathrm{f}\left({\mathbf{\mu }}_{x_{t-1}},{\mathbf{u}}_t\right)-\mathrm{F}{\mathbf{\mu }}_{x_{t-1}}\right)}{\begin{array}{c}{\mathrm{Q}}_t^{-1}\left(\mathrm{f}\left({\mathbf{\mu }}_{x_{t-1}},{\mathbf{u}}_t\right)-\mathrm{F}{\mathbf{\mu }}_{x_t}\right)\\ {\mathbf{\eta }}_M\end{array}}\right]=\left[\frac{{\widehat{\mathbf{\eta }}}_t^1}{{\widehat{\mathbf{\eta }}}_t^2}\right]$$

(5)

Secondly, x_t is marginalized from the posterior probability distribution to achieve the desired distribution over ${\widehat{\mathbf{\xi }}}_t={\left({\mathbf{x}}_t^T,{\mathbf{M}}^T\right)}^T$ according to Appendix. The predicted information matrix Λ̄_t and information vector η̄_t are:

$${\overline{\mathrm{\Lambda }}}_t={\widehat{\mathrm{\Lambda }}}_t^{22}-{\widehat{\mathrm{\Lambda }}}_t^{21}{\left({\widehat{\mathrm{\Lambda }}}_t^{11}\right)}^{-1}{\widehat{\mathrm{\Lambda }}}_t^{12}$$

(6)

$${\overline{\mathbf{\eta }}}_t={\widehat{\mathbf{\eta }}}_t^2-{\widehat{\mathrm{\Lambda }}}_t^{21}{\left({\widehat{\mathrm{\Lambda }}}_t^{11}\right)}^{-1}{\widehat{\mathbf{\eta }}}_t^1$$

(7)

(B) Measurement Update Step

Sonar senses environment features actively in the experiment and observations of features are the key to reduce the uncertainty in the estimates for the robot pose and the map. The measurement step is also subject to a Markov model which is a nonlinear function h of the state vector ξ_t. Equation (8) shows the first-order linearization about the mean state vector μ_t and observed features with the Jacobian H evaluated at this mean. The term w_t ∼ N (0, R_t) represents the white Gaussian noise in the model, R_t is the noise covariance:

$${\mathbf{z}}_t=\mathrm{h}({\mathbf{\xi }}_t)+{\mathbf{w}}_t\approx \mathrm{h}({\mathbf{\mu }}_t)+\mathrm{H}({\mathbf{\xi }}_t-{\mathbf{\mu }}_t)+{\mathbf{w}}_t$$

(8)

When a feature is observed repeatedly, it will be used to update the state estimation. According to [19] by Thrun et al., the information matrix Λ_t and the information vector η_t can be obtained:

$${\mathrm{\Lambda }}_t={\overline{\mathrm{\Lambda }}}_t+{\mathrm{H}}^T{\mathrm{R}}_t^{-1}\mathrm{H}$$

(9)

$${\overline{\mathbf{\eta }}}_t={\overline{\mathbf{\eta }}}_t+{\mathrm{H}}^T{\mathrm{R}}_t^{-1}({\mathbf{z}}_t-\mathrm{h}({\mathbf{\mu }}_t)+\mathrm{H}{\mathbf{\mu }}_t)$$

(10)

with:

$$\mathrm{H}=\left(\begin{array}{llllllll}\frac{\partial \mathrm{h}}{{\partial \mathbf{x}}_t}\hfill & 0\hfill & \cdots \hfill & 0\hfill & \frac{\partial \mathrm{h}}{{\partial \mathbf{m}}_i}\hfill & 0\hfill & \cdots \hfill & 0\hfill \end{array}\right)$$

The observation model is only a function of the vehicle state x_t and the observed features m_i (1 ≤ i ≤ N) at time t, so H is zero everywhere except at positions associated with x_t, m_i. The measurement update step only strengthens the links between vehicle pose and features to be updated, so the sparseness of information matrix can never be changed.

(C) Sparsification Step

To make information sparse, SEIF’s strategy for sparsification is based on partitioning the map into a union of three disjoint sets:

$$\mathbf{M}=\{{\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0\}$$

where M⁺ is the current active features that will remain active after sparsifying, M⁻ denotes the passive features that will remain passive, M⁰ comprises the active features that will be made passive [28]. So the SLAM posterior can be factored into:

$$\begin{array}{ll}p({\mathbf{x}}_t,\mathbf{M}|Z^t,{\mathrm{U}}^t)\hfill & =p({\mathbf{x}}_t,{\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0|{\mathrm{Z}}^t,{\mathrm{U}}^t)\hfill \\ & =p({\mathbf{x}}_t,|{\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0,{\mathrm{Z}}^t,{\mathrm{U}}^t)p({\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0|{\mathrm{Z}}^t,{\mathrm{U}}^t)\hfill \end{array}$$

(11)

Due to the conditional independence between the pose and M⁻ of the vehicle, hence we can set M⁻ to an arbitrary value. Here, it is simply chosen as 0. The evolution of information matrix is shown in Figure 2.

Figure 2(a) illustrates the situation before sparsification: according to the partition, M⁺ = {m₁, m₂, m₃, m₄}, M⁻ = {m₄} at time t, the information matrix tends to be dense after time-projection step (state augmentation and marginalization) which is the main reason for creating weak links.

By eliminating the weak links between the features and vehicle pose in the information matrix, a sparse approximation that allows for efficient SLAM can be achieved. As shown in Figure 2(b), m₁ is made passive before state augmentation and marginalization. It is obvious that the information matrix is sparse. To sparsify the information matrix, the posterior is approximated by dropping the dependence on M⁰ in the first term of Equation (11):

$$\begin{array}{ll}p({\mathbf{x}}_t,\mathbf{M}|{\mathrm{Z}}^t,{\mathrm{U}}^t)\hfill & \approx p({\mathbf{x}}_t|{\mathbf{M}}^{+},{\mathbf{M}}^{-}=0,{\mathrm{Z}}^t,{\mathrm{U}}^t)p({\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0|{\mathrm{Z}}^t,{\mathrm{U}}^t)\hfill \\ & =\frac{p({\mathbf{x}}_t,{\mathbf{M}}^{+}|{\mathbf{M}}^{-}=0,{\mathrm{Z}}^t,{\mathrm{U}}^t)}{p({\mathbf{M}}^{+}|{\mathbf{M}}^{-}=0,{\mathrm{Z}}^t,{\mathrm{U}}^t)}p({\mathbf{M}}^{+},{\mathbf{M}}^{-},{\mathbf{M}}^0|{\mathrm{Z}}^t,{\mathrm{U}}^t)\hfill \end{array}$$

(12)

The information matrix for the distribution p(x_t, M⁺, M⁰ | M⁻ = 0) is:

$${\tilde{\mathrm{\Lambda }}}_t=S_{x,M^{+},M^0}{S}_{x,M^{+},M^0}^T{\mathrm{\Lambda }}_t{S}_{x,M^{+},M^0}{S}_{x,M^{+},M^0}^T$$

(13)

where S_{x,M⁺, M⁰} denotes a projection matrix, used to extract the submatrix of all state variables except M⁰. Let ${\mathrm{\Lambda }}_t^1$, ${\mathrm{\Lambda }}_t^2$, ${\mathrm{\Lambda }}_t^3$ denote the information matrices for the terms p(x_t, M⁺ | M⁻ = 0, Z^t, U^t), p(M⁺ | M⁻ = 0, Z^t, U^t) and p(M⁺, M⁻, M⁰ | Z^t, U^t) respectively, so the sparse information matrix can be obtained as Equation (14) by putting these matrices together according to Equation (12):

$$\begin{array}{lll}{{\mathrm{\Lambda }}^{\prime }}_t\hfill & =\hfill & {\mathrm{\Lambda }}_t^1-{\mathrm{\Lambda }}_t^2+{\mathrm{\Lambda }}_t^3\hfill \\ & =\hfill & {\mathrm{\Lambda }}_t-{\tilde{\mathrm{\Lambda }}}_t{S}_{M^0}{(S_{M^0}^T{\tilde{\mathrm{\Lambda }}}_t{S}_{M^0})}^{-1}{S}_{M^0}^T{\tilde{\mathrm{\Lambda }}}_t\hfill \\ & & +{\tilde{\mathrm{\Lambda }}}_t{S}_{x,M^0}{(S_{x,M^0}^T{\tilde{\mathrm{\Lambda }}}_t{S}_{x,M^0})}^{-1}{S}_{x,M^0}^T{\tilde{\mathrm{\Lambda }}}_t-{\mathrm{\Lambda }}_t{S}_x{(S_x^T{\mathrm{\Lambda }}_t{S}_x)}^{-1}{S}_x^T{\mathrm{\Lambda }}_t\hfill \end{array}$$

(14)

The resulting information vector is now obtained by the following simple consideration:

$${{\mathbf{\eta }}^{\prime }}_t={{\mathrm{\Lambda }}^{\prime }}_t{\mathbf{\mu }}_t=({\mathrm{\Lambda }}_t-{\mathrm{\Lambda }}_t+{{\mathrm{\Lambda }}^{\prime }}_t){\mathbf{\mu }}_t={\mathbf{\eta }}_t+({{\mathrm{\Lambda }}^{\prime }}_t-{\mathrm{\Lambda }}_t){\mathbf{\mu }}_t$$

(15)

(D) Mean Recovery

The information form of the Gaussian is parameterized by its information vector and information matrix. However, the linearization, sparsification, measurement update steps require the mean of the state vector. According to the duality of Gaussian distribution in Appendix, we can get the mean vector:

$${\mathbf{\mu }}_t={\mathrm{\Lambda }}_t^{-1}{\mathbf{\eta }}_t$$

(16)

Due to the great cost of full state recovery and the steps that require the mean of state vector oftentimes only require a subset of the state mean, the mean vector can be recovered by dividing it into two sets:

$$\left[\begin{array}{cc}{\mathrm{\Lambda }}_{\mathit{ll}}& {\mathrm{\Lambda }}_{\mathit{lb}}\\ {\mathrm{\Lambda }}_{\mathit{bl}}& {\mathrm{\Lambda }}_{\mathit{bb}}\end{array}\right]\left[\begin{array}{c}{\mathbf{\mu }}_l\\ {\mathbf{\mu }}_b\end{array}\right]=\left[\begin{array}{c}{\mathbf{\eta }}_l\\ {\mathbf{\eta }}_b\end{array}\right]$$

(17)

where μ_l is the “local portion” that needs to be recovered and μ_b is the “begin portion” of the map. If current estimation for μ_b is kept fixed, an estimate of μ_l can be obtained as:

$${\widehat{\mathbf{\mu }}}_l={\mathrm{\Lambda }}_{\mathit{ll}}^{-1}({\mathbf{\eta }}_l-{\mathrm{\Lambda }}_{\mathit{lb}}{\widehat{\mathbf{\mu }}}_b)$$

(18)

3. The C-Ranger AUV and On-Board Sensors

3.1. C-Ranger AUV

The C-Ranger is an open-frame AUV with dimensions of 1.6 m × 1.3 m × 1.1 m (length, width and height), as shown in Figure 3. There are two electronic cabins and five underwater propeller thrusters. The control architecture of C-Ranger is illustrated in Figure 4. The AUV has good maneuverability due to its five DOFs, including surge, heave, roll, pitch, and yaw. The thrust system of this platform consists of five propeller thrusters, where two thrusters are parallel to the bow direction, and installed in the abdomen to provide horizontal thrust for mainly controlling the surge and roll, The other three thrusters are employed to provide vertical thrust to control the heave, roll, and pitch, two of which are installed on both sides of the bow, and the remaining one is installed on the rear of the vehicle. The upper hull of the C-Ranger is the instrument compartment housing sensors, two industrial computers, a communication module, internal monitoring module and other equipment, while the lower hull is the power and thrust system composed of lithium-ion batteries, power management module, motor-driver module, etc. The maximum speed of the C-Ranger is 3 knots, and it can operate for up to 8 hours when fully charged (tested at speed of one knot).

3.2. On-Board Sensors

A number of sensors are installed on the C-Ranger, some of them are explicitly related to SLAM. These sensors are basically divided into two groups: the internal and the external. Internal sensors include digital compass, gyro, Attitude and Heading Reference System (AHRS) and pressure sensor. External sensors include mechanical scanning sonar, Doppler Velocity Log (DVL), altimeter, CCD camera and GPS.

(A) Mapping-Related Sensor: Active Imaging Sensor

A mechanical-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 the C-Ranger AUV. The operating frequency of the sonar is 675 kHz, and its maximum range is 300 meters. Generally the scanning rate of this kind of sonar is slow, which would make the acoustic image distorted. A compensation for motion-induced distortion will be addressed in the next section.

(B) 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.

(C) Angular Sensors: AHRS and Gyro

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

(D) Positioning Sensor: GPS

To evaluate navigation performance of the C-Ranger, a high-precision and high-dynamic GPS receiver is employed. In the absence of SA, the positioning accuracy is up to 1.1 m (CEP), and the data update rate is up to 20 Hz. The GPS sensor can offer a benchmark to evaluate the estimation of robot trajectory.

4. Data Processing of Sonar

Raw data measured by sensors should be pretreated before it is used in the navigation algorithm. The amount of the raw data from most sensors is not very high, so they can be used in SLAM simply after denoising and synchronizing. Sonar is very essential for the environmental map building, but the large amount of sonar data constrains the real-time navigation. Furthermore, its long scan-cycle will lead to motion-distortion. For the reasons, it is vital to handle sonar data appropriately before being used in SLAM algorithm, thus the data processing of sonar is presented in this section.

4.1. Feature Extraction of Sonar Data

Currently methods based on image processing are popular ones applied to extract features from raw sonar data [29,30], but they are generally too slow for the applications such as AUV navigation using mechanical-scanning-sonar. A real-time data processing is proposed in the next paragraphs.

Sonar transmits sound waves stepwisely and receives them after encountering obstacles; we call each sound wave beam one ping and every ping can be divided into several bins. The relative position of a bin reflects the distance between the sonar and the obstacle, the larger the intensity of bins are, the more obvious features there are, and vice versa [31]. As Figure 5 shows, sonar scans stepwisely in a given sector, and finds one obstacle in the k – th bin of the i – th and (i + 1) – th ping, respectively. Then the intensity and the relative position for every bin whose intensity is beyond a predefined threshold will be translated into corresponding feature information.

Noise in raw data should be eliminated, and the number of features extracted should be as low as possible due to the demands of real-time navigation, in other words, we need to make the features sparse, Figure 6(a–e) show the process. In Figure 6(a) the sonar observes N₁, N₂ which are very close to the sonar head (transducer), high-intensity features in two pings, i.e., ping : m = {F₁, F₂, F₃, F₄, F₅}, ping : n = {F₆, F₇, F₈, F₉}, as well as any low-intensity features (not labeled in the figure). First, we eliminate the noise caused by the sonar itself: according to the distance relationship between sonar and the frame of AUV, we remove the noise close to the vehicle as Figure 6(b) shows. The second step is to eliminate background noise: removing features whose intensity is below the predefined threshold, we will retain features F₁∼F₉ with high intensity in Figure 6(c). The third step is the processing of a single-ping: selecting local prominent points in each ping in order to ensure that the target-related information is retained, then removing redundant features near the prominent point by using a distance threshold. Thus we will get features in each ping and effectively ensure that one target will not match more than one feature. In ping m, F5 is within the sparsification threshold (a distance value) of the F4, so are F8 and F7 in ping n. Then because of F4 > F5, F7 > F8 in intensity, we will get the results shown in Figure 6(d).

Generally the retained features are still too dense, even after the above radial processing, so we need to conduct further sparsification between pings. The procedure is similar to the above step 3, whether one feature is retained or not depends on a threshold as well, but such operations are along direction of a circular arc. The results shown in Figure 6(e) can be obtained in this way. The above processing actually can cut off most redundant information, and greatly reduce the number of features without affecting positioning accuracy. The thresholds mentioned above depend on several factors, such as the distribution of environmental features and accuracy of features’ positions, moreover, the requirement by efficiency of SLAM is also taken into account if necessary.

4.2. Compensation on Motion-Induced Distortion for Mechanical-Scanning Sonar

The transducer head of a mechanical-scanning imaging 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. For higher velocities, it has to be specially dealt with. In the case of the C-Ranger, we use position feedback to undistort the data. The principle of data undistortion is shown in Figure 7, where, superscripts ‘G’ and ‘S’ stand for the global coordinate frame and the sonar coordinate frame respectively; subscripts ‘R’ and ‘O’ denote variables for the vehicle and the features, respectively.

Firstly, a global coordinate frame is built at the starting time of every circle of sonar scanning. In this global coordinate frame, the vehicle’s initial position is supposed at the origin (0, 0). Given the vehicle moves to point M after the scan interval t for one Ping. During this period, the vehicle’s velocities v_x and v_v and can offered by SLAM module, and the vehicle’s displacement ( $x_R^G$, $y_R^G$) can be obtained as:

$$\{\begin{array}{c}{x}_{\mathrm{R}}^{\mathrm{G}}={\nu }_x\cdot t\\ y_{\mathrm{R}}^{\mathrm{G}}={\nu }_y\cdot t\end{array}$$

(19)

Simultaneously, the vehicle’s rotation angle ${\theta }_R^G$ can be provided by outputs of the SLAM module. Then, in the vehicle coordinate frame, the feature (marked as pentagram in Figure 7) is detected with a distance ${\rho }_i^s$ from the vehicle (i.e., sonar) and the angle ${\theta }_i^S$, where i stands for index of the Bin in the Ping. Finally, we need to transform the polar coordinates ( ${\rho }_i^s$, ${\theta }_i^S$) to the Cartesian coordinates in the global coordinate frame and incorporate the motion displacement of the vehicle, thus the compensation formula can be presented as:

$$\{\begin{array}{c}{x}_O^G(i)=x_R^G+{\rho }_i^S•\text{sin}({\theta }_i^S+{\theta }_R^G)\\ y_O^G(i)=y_R^G+{\rho }_i^S•\text{cos}({\theta }_i^S+{\theta }_R^G)\end{array}$$

(20)

By parity of reasoning, after the acquisition of every scanning Ping, the position of features will be corrected according to the above formula. Once the sonar completes one circle of scan, the undistorted image will be obtained without doubt.

We use the data in the 49th circle of the Abandoned Marina Dataset [32] to verify the effect of this correction method. Figure 8(a) shows the acoustic image built from the raw sonar data. Since the vehicle’s motion has been ignored during the generation of the image, obvious distortion of the observed features appears when comparing it with the aerial image of the test scenario in Figure 8(c).

The corrected image using the proposed method is shown in Figure 8(b). Obviously the distortion of the image is almost cancelled and a more accurate image is represented. The result demonstrates that our correction approach is effectual.

5. Experiment and Results Analysis

5.1. Experiment in Tuandao Bay

The experiment was performed at Tuandao Bay in Qingdao (China). The satellite map (from Google Earth) and AUV trajectory measured by GPS are shown in Figure 9, where a starting point with direction is marked using a green arrow. The total travel in the experiment is up to 2,812 m with the sailing speed about 1 knot. The scan sector of the principal sonar was configured to 180° with a scanning range of 100 meters.

5.2. Experimental Results

In the experiment, the global coordinates measured by a GPS receiver in the C-Ranger, with a GPS antenna in a buoy connected to the AUV via a short cable, will be taken as the ground truth used for the evaluation of SLAM results. Figure 10 shows the features (marked with blue asterisk) extracted directly from sonar data without using sparsificaton. The point-features in the picture are very dense, and there are many “features” away from the possible objects such as vessels and sea bank, actually a large part of them are noise. As known, the large amount of point-features would increase the computational cost of SLAM and limit the real-time performance of AUV navigation. So the approach presented in Section 4 is necessary to eliminate redundant ones and reduce the number of features. Figure 11 illustrates the result after denoising and sparsifying, the remained features are basically the most representative ones needed to describe the undersea objects.

A comparison of the trajectories for GPS (the red line), EKF (the light blue line) and the SEIF-SLAM algorithm (the blue line) as well as the features (blue points) is shown in Figure 12. Obviously, the deviation of SLAM relative to GPS is smaller than that of EKF, and the deviation of EKF tends to increase gradually. On the other hand, the location of the features obtained by SLAM approach match the actual environment landmarks quite well.

Figure 13 presents the estimation errors of SEIF-SLAM (the red line) and EKF (the blue line) relative to GPS, respectively. In this figure we can see that the error of SEIF-SLAM is always smaller than that of EKF for the whole experiment except for a few tens of seconds in the beginning. The maximum error of SEIF-SLAM is 26.1 m which is only about 7.3‰ of the whole course, and appears at the time of 4,800 seconds after starting off. As shown in Figure 9, the corresponding point at this time is at the position marked as a yellow star. That is because there were many moving targets present such as little boats during the experiment which would impact the sonar observations.

For more clarity, area A marked in Figure 12 is zoomed, as shown in Figure 14, in which the black and blue ellipses denote the uncertainty of vehicle pose and features separately. It is obvious from Figure 14 that the vehicle’s uncertainty increases for some time and then becomes very small suddenly. In fact, we note that the change of the uncertainty is cyclical: at the starting stage, the uncertainty for the pose is small and will increase gradually in one sonar-scanning-cycle because of error accumulation; at the ending point (i.e., the end of one sonar-scanning-cycle), the pose estimation uncertainty becomes small because of the update step in the SEIF-SLAM algorithm. On the other hand, due to the sonar scanning noise, the features uncertainty in Figure 14 seems to not follow any rules. In fact, when some of the features are revisited, their uncertainty will become gradually smaller relative to other features. In general, the more accurate the pose estimation is and the more times the feature is re-observed, then the smaller the feature uncertainty is.

The sparsity of information matrix is the key to the lower computational complexity of SEIF-SLAM. Figure 15 shows the statistical results of the information matrix elements.

Figure 15(a) is the statistical result of the elements magnitude of the information matrix. We can find that the overwhelming majority of information matrix elements is close to zero while only a small number of them are far from zero. Figure 15(b) shows where these elements locate in the information matrix and the blue dot represents the non-zero elements which mainly locate in the diagonal and the two ends of the counter-diagonal of the information matrix. The diagonal elements are the respective variance of the pose and features while the counter-diagonal is the covariance of the last observed features, the links of which are not yet marginalized and approximated to zero.

To compare the performance of computational efficiency, 50 Monte Carlo experiments are carried out. Figures 16 and 17 present the comparisons of average CPU time and memory usage between EKF-SLAM and SEIF-SLAM.

As we can see that SEIF-SLAM is less efficient than EKF-SLAM when there are less than 1,000 features in the map. The reason for this is that when there are only few features, the impact brought by computation in sparsification step is much greater than that brought by the sparsity property. When the number of features is more than 1,000, SEIF-SLAM is more efficient than EKF-SLAM. Figure 17 demonstrates the difference between SEIF-SLAM and EKF-SLAM in storage. We can conclude that SEIF-SLAM need much less storage than that of EKF-SLAM and the gap increases when the number of features increases. In fact, it is because that SEIF-SLAM maintains information matrix which is sparse, which is far superior to the non-sparse matrix in storage.

6. Discussion

Like EKF-SLAM, the consistency of the SEIF-SLAM algorithm is not perfect. There are two reasons for this: on the one hand, error accumulation caused by nonlinear model linearization will easily result in inconsistency, and this problem is the same as the EKF algorithm and exists in most of the SLAM algorithms based on the Gaussian linear filters; On the other hand, the sparsification, which marginalized out the weak links (i.e., the links between vehicle pose and further features), could affect consistency though it has been proved that SEIF-SLAM has relatively good consistency [26,28]. The ESEIF-SLAM algorithm [26] improves the consistency through the modification of the sparsification steps of the SEIF algorithm, but increases a little computation. The more efforts will be made to improve the consistency and reduce computational cost of the information-filter-based SLAM algorithm in our future work.

Secondly, the mechanical-scanning sonar used on the C-Ranger usually has a quite low scanning rate, it needs about 27 seconds to scan a 360° sector. As a result, SEIF-SLAM filter updates happen approximately 13.5 meters apart, and the frequency of measurement update is very slow. Obviously the main reason is the usage of the mechanical sonar. Therefore, the mechanical sonar will be exchanged for a multi-beam sonar to obtain a high update frequency on the next generation platform. High update frequency should be helpful to improve the accuracy of navigation for AUV a lot.

7. Conclusions

In this paper, the autonomous navigation method for the C-Ranger AUV had been addressed. To verify the advantages of SEIF-SLAM, the application of SEIF-SLAM for AUV navigation had been studied, the approach for data processing of sonar had also been presented in this work. The mechanical scanning imaging sonar is chosen as the principal sensor for active sensing of the undersea obstacles, a compensation method based on feedback of the AUV pose has been proposed to overcome distortions of the acoustic images due to the vehicle motion. Sea trial experiments in Tuandao Bay have been conducted to verify the feasibility of the proposed navigation methods for the C-Ranger. The experimental results and analysis demonstrated that the proposed navigation approach based on SEIF-SLAM improved the accuracy of navigation compared with conventional methods; moreover the algorithm achieves low computational cost when compared with EKF-SLAM.