An Adaptive UKF Based SLAM Method for Unmanned Underwater Vehicle

Abstract

This work proposes an improved unscented Kalman filter (UKF)-based simultaneous localization and mapping (SLAM) algorithm based on an adaptive unscented Kalman filter (AUKF) with a noise statistic estimator. The algorithm solves the issue that conventional UKF-SLAM algorithms have declining accuracy, with divergence occurring when the prior noise statistic is unknown and time-varying. The new SLAM algorithm performs an online estimation of the statistical parameters of unknown system noise by introducing a modified Sage-Husa noise statistic estimator. The algorithm also judges whether the filter is divergent and restrains potential filtering divergence using a covariance matching method. This approach reduces state estimation error, effectively improving navigation accuracy of the SLAM system. A line feature extraction is implemented through a Hough transform based on the ranging sonar model. Test results based on unmanned underwater vehicle (UUV) sea trial data indicate that the proposed AUKF-SLAM algorithm is valid and feasible and provides better accuracy than the standard UKF-SLAM system.

1. Introduction

The simultaneous localization and mapping (SLAM) algorithm [1, 2] was first proposed by Smith, Self, and Cheeseman in 1988 to provide localization and map building for mobile robots and is now widely used in many different mobile robot systems. The SLAM algorithm was first used for unmanned underwater vehicle (UUV) navigation in September 1997 in a collaborative project between the Naval Undersea Warfare Center (NUWC) and Groupe d’Etudes Sous-Marines de l’Atlantique (GESMA). The objective of the trial using SLAM was to get a UUV starting in an unknown location and without previous knowledge of the environment to build a map using its onboard sensors and then use the same map to compute the robot’s location.

Given the recent wider use of UUV in the marine environment, it is notable that truly autonomous SLAM-based UUV navigation is still lacking. However, it is challenging to develop SLAM-based UUV navigation due to factors such as system complexity, weak perception, nonstructure, increasing system noise, and unknown statistical characteristics.

SLAM solutions can be divided into two categories: a nonprobabilistic probability estimation method and methods primarily based on probability estimation; Table 1 gives the summery of SLAM methods. The probability estimation method first developed [3] was the EKF based SLAM algorithm, which suffers difficulty solving data association problems, high computational costs due to the calculation of Jacobian matrices, and inconsistency due to errors introduced during linearization. In trying to reduce storage and computational requirements, Thrun et al. [4] proposed a SLAM algorithm based on sparse extended information filter. However, this method is only applicable for creating feature maps and requires the existence of features that are easy to extract and distinguish in the environment, such as point, line, and face features. More recently, Montemerlo et al. [5] proposed a Rao-Blackwellized particle filter based SLAM method (FastSLAM), where each particle stores its map and robot positioning result. However, this algorithm results in a calculation and storage problem proportional to the number of particles and is unable to avoid the disadvantages of particle degradation and sample dilution.

Table 1 

Summery of SLAM methods.

The unscented Kalman filter (UKF) [6, 7] is a nonlinear filter based on unscented transform (UT). For nonlinear systems, UKFs avoids linearization of the state and measurement equations. Additionally, the UKF principle is simple and easy to implement as it does not require the calculation of Jacobians at each time step, and the UKF is accurate up to second order moments in the probability distribution function propagation whereas the EKF is accurate up to first order moment [8]. However, when an UKF is used in underwater SLAM, it needs to predict mathematical model of the system and a priori knowledge of the noise statistics. In many practical applications, prior statistics of the noise is unknown or not accurate. Even if this information is known, the statistical characteristics easily change due to internal and external uncertainties that reflect strong time-varying characteristics. Thus, a conventional UKF does not have the adaptive ability to respond to changes in the noise statistics, which can lead to large estimation errors and even cause divergence in the case of unknown and time-varying noise statistics [9–11].

In order to solve the above problem, we apply an adaptive UKF (i.e., an adaptive unscented Kalman filter, or AUKF) filtering algorithm to underwater SLAM. By introducing a modified (i.e., suboptimal and unbiased) Sage-Husa maximum a posterior (MAP) noise statistic estimator, the new algorithm provides online estimation of the statistical parameters of unknown system noises and restrains filtering divergence. In addition, the method uses a covariance matching criterion approach to determine the convergence of the filter. When the filter is divergent, the proposed method introduces an adaptive fading factor to correct prediction error covariance, adjust the filter gain matrix 𝐾, and suppress filter divergence, thus enhancing the fast tracking capability of the filter. Test results based on sea trial data of UUV indicate that the proposed AUKF-SLAM algorithm provides better navigational accuracy than a conventional UKF-SLAM algorithm.

2. Adaptive UKF Algorithm

2.1. UKF Algorithm

Unscented Kalman filters were first proposed by Julier and Uhlmann [12]. The algorithm’s main principle is to select a number of sampling points in the state distribution (sigma points), which can completely capture the true mean and covariance of state distribution. Those sigma points are then substituted into the nonlinear function to obtain the corresponding nonlinear function point set, and it can solve the mean and covariance after transformation by these points.

The mean, estimate variance, and measurement variance obtained from the unscented transform are introduced into a gradually recursive process of Kalman filtering to obtain the UKF. The main steps of a UKF algorithm are as follows.(1) Initialization 𝑥₀=𝐸𝑥₀,𝐏₀=𝐸𝑥₀−𝑥₀𝑥₀−𝑥₀^𝑇. (1)(2) For 𝑘∈{1,…,∞}(i) calculate sigma points 𝜒_𝑘−1=𝑥_𝑘−1𝑥_𝑘−1±𝑛_𝑥+𝜆𝐏_𝑘−1. (2)(ii) UKF prediction 𝜒_{𝑘∣𝑘−1}=𝐟𝜒_𝑘−1,𝑢_𝑘−1,𝑥_{𝑘∣𝑘−1}=^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑚)𝜒_{𝑖,𝑘∣𝑘−1},𝐏_{𝑘∣𝑘−1}=^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}^𝑇𝑧+𝑄,_{𝑘∣𝑘−1}=𝐡𝜒_{𝑘∣𝑘−1},𝑢_𝑘−1,𝑧_{𝑘∣𝑘−1}=^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑚)𝑧_{𝑖,𝑘∣𝑘−1}. (3)(4)(5)(6)(7)(iii) UKF update 𝐏𝑧_𝑘𝑧_𝑘=^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}^𝑇𝐏+𝑅,_{𝑥𝑘𝑧𝑘}=^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}^𝑇,𝐊_𝑘=𝐏_{𝑥𝑘𝑧𝑘}𝐏𝑧⁻¹_𝑘𝑧_𝑘,𝑥_𝑘=𝑥_{𝑘∣𝑘−1}+𝐊_𝑘𝑧_𝑘−𝑧_{𝑘∣𝑘−1}𝐏_𝑘,=𝐏_{𝑘∣𝑘−1}−𝐊_𝑘𝐏𝑧_𝑘𝑧_𝑘𝐊^𝑇_𝑘, (8)(9)(10)(11)(12)where 𝑄 is the system noise covariance, 𝑅 is the observation noise covariance, 𝐊 is the Kalman gain, and 𝑊_𝑖 is the weight of the mean and covariance.

2.2. Adaptive UKF Algorithm

Adaptive filtering technology has become a focus of research attempting to solve the filter divergence problem caused by the inaccurate statistical properties of the noise and the mathematical model itself. Sage-Husa suboptimal unbiased maximum a posterior (MAP) noise estimators have the advantage that their recursive formula is simple, and the principle is clear and easy to implement. Therefore we chose to use this type of system to estimate unknown system noise.

2.2.1. System Noise Estimation Method

Suppose a class of nonlinear dynamic systems is described as 𝑥_𝑘=𝐟𝑥_𝑘−1,𝑢_𝑘−1+𝑊_𝑘−1,𝑧_𝑘=𝐡𝑥_𝑘+𝑉_𝑘, (13) where 𝑥_𝑘 represents the state vector of the system at time 𝑘,  𝑢_𝑘−1 represents the control, 𝑧_𝑘 represents the measurement value of the state at time 𝑘, and 𝑊_𝑘−1 and 𝑉_𝑘 are independent white Gaussian noise with time-varying means of 𝑞_𝑘 and 𝑟_𝑘 and covariances of 𝑄_𝑘 and 𝑅_𝑘, respectively. Note that 𝑄_𝑘 is a nonnegative definite symmetric matrix, while 𝑅_𝑘 is positive definite symmetric matrix.

Emphasis should be placed on recent data when estimating time-varying noise statistics; that is, the algorithm should gradually forget data that is too old. In this paper, we adopt a fading memory weighted exponent method to design a time-varying noise statistics estimator. According to the literature [13], we selected the weighting coefficient to satisfy 𝛽_𝑖=𝛽_𝑖−1𝑏,0<𝑏<1,^𝑘_𝑖=1𝛽_𝑖=1 (14) which can be written as 𝛽_𝑖=𝑑_𝑘𝑏^𝑖−1𝑑,𝑖=1,…,𝑘,_𝑘=(1−𝑏)1−𝑏^𝑘. (15) The recursive formula of a fading memory time-varying noise statistical estimator described by 𝑞_𝑘, 𝑄_𝑘, 𝑟_𝑘̂, 𝑅_𝑘 is 𝑞_𝑘=1−𝑑_𝑘𝑞_𝑘−1+𝑑_𝑘𝑥_𝑘−^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑚)𝐟𝜒_{𝑖,𝑘−1}𝑄_𝑘=1−𝑑_𝑘𝑄_𝑘−1+𝑑_𝑘𝐊_𝑘𝜀_𝑘𝜀^𝑇_𝑘𝐊^𝑇_𝑘+𝐏_𝑘−^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}^𝑇,𝑟_𝑘̂=1−𝑑_𝑘𝑟̂_𝑘−1+𝑑_𝑘𝑧_𝑘−^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑚)𝐡𝜒_{𝑖,𝑘∣𝑘−1},𝑅_𝑘=1−𝑑_𝑘𝑅_𝑘−1+𝑑_𝑘𝜀_𝑘𝜀^𝑇_𝑘−^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)×𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}×𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}^𝑇, (16) where 𝜀_𝑘=𝑧_𝑘−ℎ(𝑥_{𝑘∣𝑘−1}) is the output residual sequence of the UKF.

2.2.2. Filter Divergence Suppression Method

Since suboptimal Sage-Husa filters often diverge, in this paper we judge whether filtering divergence is occurring according to convergence conditions derived from the covariance matching criterion. If the convergence conditions are satisfied, the Sage-Husa algorithm is applied. If filtering divergence occurs, the proposed method calculates an adaptive weighting coefficient 𝜆_𝑘 through a computational fading factor formula and applies this coefficient to correct 𝐏_{𝑘|𝑘−1}; thus, the role of the observables is strengthened and the filter divergence is suppressed.

The convergence conditions can be written as 𝑣^𝑇_𝑘𝑣_𝑘≤𝑆⋅tr𝐸𝑣_𝑘𝑣^𝑇_𝑘, (17) where 𝑆(𝑆≥1) is an adjustable coefficient presetting and 𝑣_𝑘 is the residual sequence, such that 𝑣_𝑘=𝑧_𝑘−𝐡(𝑥_{𝑘∣𝑘−1}).

The correction method of covariance 𝐏_{𝑘|𝑘−1} is 𝐏_{𝑘∣𝑘−1}=𝜆_𝑘⋅^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}×𝜒_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}^𝑇+𝑄_𝑘−1. (18)

The adaptive weighting coefficient 𝜆_𝑘 is calculated based on the fading factor formula [14, 15] 𝜆_𝑘=𝜆₀𝜆₀≥11𝜆₀𝜆<1,₀𝑁=tr^𝑘tr𝑀_𝑘,𝑁_𝑘=tr𝐶_0,𝑘−𝑅^𝑇,𝑀_𝑘=tr^2𝑛𝑥_𝑖=0𝑊_𝑖^(𝑐)𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}𝑧_{𝑖,𝑘∣𝑘−1}−𝑧_{𝑘∣𝑘−1}^𝑇,𝐶_0,𝑘=𝑣_𝑘𝑣^𝑇_𝑘𝑘=1𝜌𝐶_0,𝑘+𝑣_𝑘𝑣^𝑇_𝑘1+𝜌𝑘>1, (19) where tr(⋅) accounts for matrix trace. Here, 0<𝜌≤1 is a forgetful factor (typically about 0.95) used to increase the filter’s tracking ability, with greater values of the factor creating a smaller proportion of information before time 𝑘 and causing a more prominent residual vector effect. This method has a strong tracking ability for sudden status changes but still keeps tracking for slowly varying state and mutation status changes when the filter reaches a steady state.

2.2.3. Realization of Adaptive UKF Algorithm

(1) Initialization. State initialization is done according to (1) and (20) 𝑞₀=𝑞₀,𝑄₀=𝑄₀. (20)

(2) Time Update. Given values for the variables 𝑥_𝑘−1 and 𝐏_𝑘−1, we solve 𝑥_{𝑘|𝑘−1} and 𝑧_{𝑘∣𝑘−1} based on the unscented transform, that is, using (4) and (7), and covariance 𝐏_{𝑘∣𝑘−1} is predicted by (5).

(3) Convergence Judgment. At this stage, the method uses (17) to judge whether the filter is converging. If the filter is converging, move to the next step; otherwise correct 𝐏_{𝑘∣𝑘−1} using (18)~(19).

(4) Measurement Update. Obtain observation covariances 𝐏𝑧_𝑘𝑧_𝑘 and 𝐏_{𝑥𝑘𝑧𝑘} and filter gain 𝐊_𝑘 according to (8)~(10), and then do a measurement update using (11)~(12).

(5) Recursively Estimate System Statistical Noise Characteristics. Recursively estimate the system’s statistical noise characteristics according to (16).

3. SLAM Algorithm Based on Adaptive UKF

3.1. SLAM System Model

3.1.1. AUV Nonlinear Dynamic Model

As seen from Figure 1, 𝐿 is the global coordinate system established with the initial location and bow of the UUV, where 𝑥, 𝑦, and 𝜓 describe the position and heading of the UUV within this system. While 𝑉 is the UUV vessel frame system and 𝑆 is the sonar coordinate system. The North-East coordinates are given by 𝐸, with its North direction based on magnetic North. Obviously, 𝑧_𝜓=𝜓+𝜓_𝐿0, where 𝑧_𝜓 is the heading of the UUV as measured by its OCTANS.

Figure 1 

Global coordinate system, the UUV vessel frame system and the sonar coordinate system.

Note that there is a distance offset (1.85 m in the 𝑋 direction, 0.65 m in the 𝑌 direction, and a very small deviation in the 𝑍 direction that can be ignored) between the mounting positions of three ranging sonar and the UUV’s center of gravity. The coordinate systems 𝑆 and 𝑉 are shown in more detail in Figure 2. We assume that the positions of the origin of 𝑆 and 𝑉 in 𝐸 are (𝑆_𝑥,𝑆_𝑦) and (𝑉_𝑥,𝑉_𝑦), respectively, and that three ranging sonar are mounted together (i.e., their mounting positions are the same). This gives a mounting deviation of three ranging sonar in 𝑉 of 𝑎=1.85m and 𝑏=0.65m, where 𝑎 is the deviation in the 𝑋 direction and 𝑏 is the deviation in the 𝑌 direction. The mounting angle of the middle sonar is 𝜙, with mounting angles of 𝜙±7.5 for the left and right sonar. According to Figure 2, using the ranging sonar as a reference we get 𝑆_𝑥=𝑉_𝑥+𝑎²+𝑏²∗cos𝜙−𝑧_𝜓,𝑆_𝑦=𝑉_𝑦+𝑎²+𝑏²∗sin𝜙−𝑧_𝜓. (21)

Figure 2 

Installation position and angle deviation of the ranging sonar.

The method uses a simple 4 DOF (degree of freedom) constant velocity kinematics to predict how the state will evolve from time 𝑘−1 to time 𝑘: 𝑥𝑦𝑧𝜓𝑢𝑣𝑤𝑟_𝑘=𝜓𝜓𝜓𝜓𝑢𝑣𝑤𝑟𝑥+𝑢𝑇cos−𝑣𝑇sin𝑦+𝑢𝑇sin+𝑣𝑇cos𝑧+𝑤𝑇𝜓+𝑟𝑇_𝑘−1+𝐧_𝑘, (22) where [𝑥,𝑦,𝑧,𝜓] is the position and heading of the UUV in 𝐿; [𝑢,𝑣,𝑤,𝑟] gives the line velocity and angle velocity of the UUV in 𝑉, and 𝑘 is the sample time. In this equation, 𝐧(𝑘) is the portion of the system noise with a time-varying mean and covariance, with the covariance of vector 𝑛 given by system noise 𝑄𝐸𝑛_𝑘=𝑞_𝑘,𝐸𝑛_𝑘−𝑞_𝑘𝑛_𝑗−𝑞_𝑗^𝑇=𝛿_𝑘𝑗𝑄_𝑘, (23) where 𝛿_𝑘𝑗 is a delta function.

3.1.2. Feature Model

The feature data used in this paper is derived from measurements of the structured port environment, so the algorithm itself is what chooses for line features with which to build a feature map. The line feature model used in the proposed method is 𝐗_{𝑓𝑖,𝑘}=𝐗_{𝑓𝑖,𝑘−1}𝑖=1,…,𝑛. (24)

3.1.3. Observation Model

The UUV uses a Doppler velocity log (DVL), compass, and pressure sensor to provide direct measurements of the vehicle’s velocity, heading, and depth, respectively. Thus, the observation model is linear, and so the common model becomes 𝐳_𝑘=𝐇𝑥_{𝑘∣𝑘−1}+𝐦_𝑘, (25) where 𝐳 is the observation vector, 𝐦 is a white Gaussian with zero mean, and 𝐇 varies with changes in the measurement: 𝐻=0010000000010000000010000000010000000010. (26)

3.1.4. Ranging Sonar Model

The transmission beam of a ranging sonar creates a conical surface, which is a fan in a two-dimensional plane. David Ribas et al. [16] proposed the underwater mechanical scanning imaging sonar model based on the terrestrial single beam ranging sonar model [17]. In this paper, we determine the location of line features in the environment using the measurement data from the ranging sonar.

In Figure 3, 𝛼 represents the horizontal beam width, 𝛽 represents the incidence angle, and 𝜌^𝑆 is the range at which the bin was measured from the sensor. Reference frame 𝑆 defines the position of the transducer head at the moment the sensor obtains a particular bin, where [𝑥^𝐵_𝑆,𝑦^𝐵_𝑆,𝜃^𝐵_𝑆] is the transformation defining the position of 𝑆 with respect to 𝐵 to the chosen base reference. Both [𝑥^𝐵_𝑆,𝑦^𝐵_𝑆,𝜃^𝐵_𝑆] and 𝜌^𝑆 are obtained from information in the data buffer.

Figure 3 

Ranging sonar model to distinguish line features.

To emulate the effect of the horizontal beam width, the model uses a set of 𝑖 values at a given resolution within an aperture of ±𝛼/2 around the direction in which the transducer is oriented. This set of values is also referred to as 𝜃^𝐵_𝑆, where 𝜃^𝐵_𝑆−𝛼2≤𝜃^𝐵_𝑖≤𝜃^𝐵_𝑆+𝛼2. (27)

Each value 𝜃^𝐵_𝑖 represents a bearing parameter for a line tangent of the arc that models the horizontal beam width. As stated earlier, not only are all lines tangent to the arc candidates for line features, but the ones inside the maximum incidence angle limits of ±𝛽 are also considered candidates. For this reason, the algorithm takes each 𝑘 value at a given resolution for each value of 𝜃^𝐵_𝑖 and within an aperture of ±𝛽; that is, 𝜃^𝐵_𝑖−𝛽≤𝜃^𝐵_𝑖,𝑘≤𝜃^𝐵_𝑖+𝛽. (28)

The results of this operation are 𝑖×𝑘 different values of 𝜃^𝐵_𝑖 for the given aperture. These are the bearings for a set of lines representing all the possible candidates compatible with the bin. Given the geometry of the problem, the range parameter 𝜌^𝐵_𝑖,𝑘 corresponding to each one of the 𝜃^𝐵_𝑖,𝑘 bearings is 𝜌^𝐵_𝑖,𝑘=𝑥^𝐵_𝑆cos𝜃^𝐵_𝑖,𝑘+𝑦^𝐵_𝑆sin𝜃^𝐵_𝑖,𝑘+𝜌^𝑆cos𝜃_𝑖,𝑘. (29)

3.2. Feature Extraction

In the sea trial, the application environment of the SLAM algorithm is a cross-section of the ports, dams, and other environment structures. Note that the scanning surface of the sonar and the vertical wall or other vertical extensions of the surface will create line features in the resulting acoustic image. However, the parameters of such static line features will not change as the sonar position changes.

The most popular line feature extraction methods include the split-and-merge method [18] proposed by Pavlidis, the RANSAC method [19] proposed by Fischler and Bolles, and the Hough transform (HT) [20] proposed by Illingworth and Kittler. Between these choices, HT is the most popular line feature extraction method, and a number of others have developed many methods to improve HT line feature extraction [21–23]. In this paper, we use the HT method to extract line features from the ranged sonar data. The HT is a voting scheme where the distance values of each ranged sonar image accumulates evidence for the presence of a line feature. Units that get the most votes in the HT space correspond to line features in the actual environment.

3.2.1. Data Processing of Ranging Sonar Data

A data buffer helps to separate and manage the stream of measurements produced by the continuous arrival of the sonar beams. The buffer stores variables such as the range and bearing for each bin used in the voting, the position and heading in the North-East coordinate system 𝐸, and the transmit angle of the beams so that 𝜌-𝜃 parameter pairs used to present line features are extracted based on HT.

The steps to process the data set from each ranged sonar scan [24, 25] are given below, using the left sonar as an example. First, the data buffer is set, the data is loaded with the range values, and a 0-1 matrix is built where the units without range values are set to 0 and units with range values are set to 1. In the second step, the transmit angle of the sonar in 𝑆, the time, the position and heading of the UUV in 𝐸, and the position and transmit angle of the sonar in 𝐸 are stored into the data buffer. The third step defines the base frame 𝐵, where 𝐵 is the current position of the sonar head when the voting is performed. Finally, the position and heading of the sonar instrument in 𝐵 at every moment is acquired and stored in the data buffer.

3.2.2. Hough Transform

There are three steps to extract line features from the sonar image data. First, the data from all three sonar instruments are loaded, and distance resolution, angle resolution, and threshold value are defined. Secondly, the accumulator is defined, and the index values of the nonzero elements of the accumulator are found. Finally, we use (27)~(29) to vote, and the 𝜌-𝜃 parameter pairs that get the most votes are used to represent the detected line features in 𝐵.

3.3. Data Association

Once the model has extracted line features in the environment based on the HT algorithm, it needs to create an environment map and improve the state estimation of the UUV by fusing the detected line features. The next step is then to perform data association [26] to determine if a measured line 𝑍^𝑉_𝑖 corresponds to any of the features 𝐹_𝑗,𝑗=1,…,𝑛 already existing in the map and so used to update the system or if it is a new line and has to be incorporated into the map. To make this distinction, the most popular individual compatibility nearest neighbor data association algorithm is used to select the best candidate.

Given the transformation of 𝐵 and 𝑉, the position of the 𝑖th line measurement in 𝑉 can be represented by 𝑍^𝑉_𝑖=𝜌^𝑉_𝑖𝜃^𝑉_𝑖^𝑇. (30)

If we assume the position of the line feature 𝑗 already exists in the map in 𝐿; that is, 𝑋^𝐿_𝑙=𝜌_𝑗𝜃_𝑗. (31)

Then the position of line feature 𝑗 in V is 𝑋^𝑉_𝑙=𝜌_𝑗−𝑥cos𝜃_𝑗−𝑦sin𝜃_𝑗𝜃_𝑗−𝜓. (32)

And line feature 𝑗 corresponds to the 𝑖th line measurement, where 𝑧^𝑉_𝑖,𝑘=ℎ_𝑗𝑥_𝑘,𝐬_𝑖,ℎ_𝑗𝑥_𝑘,𝐬_𝑖=𝜌_𝑗−𝑥cos𝜃_𝑗−𝑦sin𝜃_𝑗𝜃_𝑗−𝜓+𝐬_𝑖. (33) Here, 𝜌^𝑉_𝑖 and 𝜃^𝑉_𝑖 are the parameters of the line features in the 𝑉 frame of reference, 𝜌_𝑖 and 𝜃_𝑖 are the parameters of the line features presented in the 𝐿 frame of reference, and 𝐬_𝑖 is the noise with a zero-mean white noise with covariance 𝐑 affecting the line feature observation.

The proposed method uses an innovation term 𝝂_𝑖𝑗 to calculate the discrepancy between the measurement and its prediction, with its associate covariance matrix 𝐒_𝑖𝑗 obtained by 𝑣_{𝑖𝑗,𝑘}=𝑧^𝑉_𝑖,𝑘−𝑧_{𝑗,𝑘|𝑘−1},𝑧_{𝑗,𝑘|𝑘−1}=ℎ_𝑗𝑥_{𝑘|𝑘−1},𝐬_𝑖,𝑆_{𝑖𝑗,𝑘}=𝐏𝑧_𝑘𝑧_𝑘+𝐑_𝑘. (34)

To determine if the correspondence is valid, an individual compatibility (IC) test [27] using the Mahalanobis distance is carried out 𝐃²_𝑖𝑗=𝜈^𝑇_{𝑖𝑗,𝑘}𝑆_𝑖⁻¹_{𝑖𝑗,𝑘}𝜈_{𝑖𝑗,𝑘}<𝜒²_𝑑,𝛼, (35) where 𝑑=dim(ℎ_𝑗) and 𝛼 is the desired confidence level.

Data association is only performed if and when a line feature is detected based on HT. If the data association is successful, that is, the line feature exists in the map, then the model updates the state estimate. Otherwise, state augmentation is carried out where the new measurement is added to the current state vector as a new feature. However, the algorithm cannot do this augmentation directly because the new feature is represented in 𝑉. Thus, the algorithm must first perform a change of reference using the following: 𝑋(𝑘)=𝑋_𝑉(𝑘)𝑋_𝑓1(𝑘)⋮𝑋_𝑓𝑛(𝑘)⟹𝑋(𝑘)⁺=𝑋_𝑉(𝑘)𝑋_𝑓1(𝑘)⋮𝑋_𝑓𝑛(𝑘)𝑋_𝑉(𝑘)⊕𝑧^𝑉_𝑖(𝑘). (36)

4. AUKF-SLAM Algorithm Verification Based on Sea Trial Data

4.1. Test Conditions

The data used in the test conducted in this work comes from a sea trial completed in October 2010 near Dalian Xiaoping Island using a UUV developed by the authors. The navigation route of the UUV was from point 𝐴 to point 𝐵 in Figure 4. As configured for the trails, the UUV possessed a number of different sensors including DVL, OCTANS, depth sensor, and three ranging sonar which mounted in the horizontal frame as a whole set to observe the environment.

Figure 4 

Satellite image of the sea trial test area.

The initial position of the UUV (point 𝐴) was longitude 121.5231°, north latitude 38.8271°, and the trial ended at longitude 121.5083°, North latitude 38.8328° (position 𝐵). The initial heading of the UUV was −70.70°. During the trail, the UUV stayed near the surface of the water so that GPS was available throughout the trial. The total navigation time was 17 minutes and 6 seconds.

DVL can measure a UUV’s current velocity, bottom tracking speed, and so on. However, in the sea trial DVL was only used to measure the bottom track speed while OCTANS was used to measure the UUV’s heading in real-time, that is, the angle between the front of the UUV and magnetic North. The pressure sensor provided depth data by measuring the water pressure, and three ranging sonar provided online environment perception and measurement. Three ranging sonar mounted in the horizontal frame is shown as Figure 5.

Figure 5 

The ranging sonar mounted in the horizontal frame.

A general description of the AUKF-SLAM algorithm is given in Figure 6.

Figure 6 

Basic principle of the AUKF-SLAM algorithm.

4.2. Acquisition of Embankment Measurement Points Using Ranging Sonar

Given a true trajectory as provided by GPS, we can obtain embankment measurement features by fusing this GPS information with the ranging sonar data and then performing a coordinate transformation for the measurement data in a two-dimensional plane. Assume the position of point 𝑃 in 𝑆 is 𝑋^𝑆_𝑃=𝑥_𝑆𝑦_𝑆^𝑇. Then for each ranging sonar, we have 𝑋^𝑆_𝑃=𝑥_𝑆𝑦_𝑆=𝜌_𝑖𝜃×sin^𝑖×𝜋𝜌180_𝑖𝜃×cos^𝑖×𝜋180, (37) where 𝜌_𝑖 and 𝜃_𝑖 represent distance values and mounting angles of the three sonar instruments. For the sea trial, the mounting angles were 𝜃₁=7.5^∘, 𝜃₂=0^∘, and 𝜃₃=−7.5^∘, corresponding to the left (𝑖=1), middle (𝑖=2), and right (𝑖=3) sonar.

The relationship between 𝑉 and 𝑆 is shown in Figure 2, with the position of point 𝑃 in 𝑉 given by 𝑋^𝑉_𝑃=𝑥_𝑉𝑦_𝑉=𝑥_𝑆𝑦+𝑎_𝑆+𝑏. (38)

The position of 𝑉 relative to 𝐿 is 𝑋^𝐿_𝑉=𝑥_𝐿𝑦_𝐿𝜑_𝐿^𝑇, so we can obtain the position of point 𝑃 in 𝐿 using the following synthesis operator 𝑋^𝐿_𝑃=𝑋^𝐿_𝑉⊕𝑋^𝑉_𝑃=𝑥_𝐿+𝑥_𝑉cos𝜑_𝐿−𝑦_𝑉sin𝜑_𝐿𝑦_𝐿+𝑥_𝑉sin𝜑_𝐿+𝑦_𝑉cos𝜑_𝐿. (39)

Figure 7 gives the embankment measurement, where the green, red, and blue points were acquired by the left, middle, and right sonar, respectively.

Figure 7 

Feature measurement of the embankment.

4.3. Line Feature Extraction of the Embankment Based on Ranging Sonar Model

The proposed model can extract line features using an HT form ranging sonar model based on the following assumptions:(a)angle resolution of the HT space is 5^∘ and the distance resolution is 1 m;(b)the largest number of votes is selected as the threshold;(c)a 10^∘ arc is used to model the horizontal beam width of the ranging sonar; that is, 𝛼=10^∘;(d)the model does not consider uncertainty in the vertical beam width;(e)assume that there is only an echo signal when transmit beam of the ranging sonar is parallel to the vertical extension surface; that is, 𝛽=0^∘.

In the sea trail the algorithm extracted six line features L1~L6. The 𝜌-𝜃 parameter, time, and position of the UUV when the feature was detected are given in Table 2 for each line feature.

Table 2 

Extracted line feature parameters.

4.4. Test Validation Based on Sea Trial Data

To verify the performance of the AUKF-SLAM algorithm, we compared the results of tests using AUKF-SLAM, UKF-SLAM, and EKF-SLAM algorithms against trial data with the UUV and assuming that the statistical properties of the system noise was unknown in all cases.

4.4.1. Test Conditions

The actual statistical characteristics of the system noise during the field trial was unknown, so for this work we assumed that actual system noise behavior was based on two laws, one time-varying and one constant. Using these laws, we conducted both a time-varying noise test and a constant noise test. Table 3 gives the results of the system noise tests and the resulting laws, where 𝑘 is the step number. The observation noise is 𝑅=diag0.04²0.04²0.04²0.04²0.04². (40)

Table 3 

Change law of system noise.

4.4.2. Test Results

Figures 8~15 are the test results given for both time-varying and constant noise conditions. Figures 8 and 12 compare the trajectory estimations from the different algorithms against the UUV’s GPS trajectory with time-varying noise and constant noise, respectively. Figures 9 and 13 compare position errors in the East direction between the different algorithms for the different types of noise, while Figures 10 and 14 provide a comparison of the position errors in the North direction. Figures 11 and 15 show the adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying and constant noise test, respectively.

Figure 8 

Comparison of the trajectory estimations with time-varying noise.

Figure 9 

Comparison of position error in the East direction with time-varying noise.

Figure 10 

Comparison of position error in the North direction with time-varying noise.

Figure 11 

Adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying noise test.

Figure 12 

Comparison of the trajectory estimations with constant noise.

Figure 13 

Comparison of position error in the East direction with constant noise.

Figure 14 

Comparison of position error in the North direction with constant noise.

Figure 15 

Adaptive weighting coefficient of AUKF-SLAM algorithm in constant noise test.

4.4.3. Analysis of Test Results

The heading 𝑧_𝜓 measured by the OCTANS is relative to the magnetic north of 𝐸 coordination system shown in Figure 1, and it should be transformed to the heading 𝜓 in the 𝐿 coordinate system for state updating. The heading 𝜓 is shown as Figure 16.

Figure 16 

Heading of the UUV during the field trial.

(1) Performance Analysis of the AUKF-SLAM Algorithm. We calculate the estimation error of the algorithm by 𝛿_𝑘=𝛿_𝑒,𝑘𝛿_𝑛,𝑘=𝑥_𝑘−𝑥_𝑘𝑦_𝑘−𝑦_𝑘, (41) where 𝛿_𝑒,𝑘 and 𝛿_𝑛,𝑘 are the estimation error in the East and North directions at time 𝑘, respectively, while (𝑥_𝑘,𝑦_𝑘) and (𝑥_𝑘,𝑦_𝑘) are the true position and estimated position of the UUV at time 𝑘. Note that |𝛿_𝑒,𝑘| and |𝛿_𝑛,𝑘| represent absolute values of the error in the East and North directions at time 𝑘, respectively. Obviously, small values of |𝛿_𝑒| and |𝛿_𝑛| indicate higher accuracy in the filtering algorithm. Figures 8~10 and Figures 11~14 show that |𝛿_𝑒| and |𝛿_𝑛| for the proposed AUKF-SLAM algorithm are lower than the estimation errors given by the other algorithms.

From Figures 11 and 15, it can be seen that the adaptive weighting coefficient is greater than one at some certain time. The results indicate that once the Sage-Husa UKF-SLAM tends to diverge according to convergence conditions, the adaptive weighting coefficient is introduced to correct covariance and ensure the tracking ability.

(2) Root-Mean Square Error. We use the root-mean square error (RMSE) of the position to compare the performance of the various nonlinear filters RMSE_pos=1𝑇^𝑇_𝑘=1𝑥_𝑘−𝑥_𝑘²+𝑦_𝑘−𝑦_𝑘², (42) where 𝑇 is the total running step and (𝑥_𝑘,𝑦_𝑘) and (𝑥_𝑘,𝑦_𝑘) are the true position and estimated position, respectively, of the UUV at time 𝑘. Table 4 gives the RMSE for each of the tested algorithms. From the table, we can see that the RMSE of the AUKF-SLAM algorithm is the smallest for both time-varying system noise and constant system noise. The RMSE in the case of time-varying noise for the AUKF-SLAM algorithm is smaller than the RMSE found in the constant noise scenario by 2.3534 m. In the time-varying noise test, the RMSE of the AUKF-SLAM algorithm is smaller than the RMSE found using the UKF-SLAM algorithm by 1.9152 m; in the constant noise test the AUKF-SLAM algorithm RMSE is smaller by 0.9855 m.

Table 4 

Comparison of RMSE.

From the above analysis, we can see that the AUKF-SLAM algorithm has good tracking ability and produces a RMSE that is smaller than what the other algorithms are capable of achieving for both time-varying and constant system noise. Also, the AUKF-SLAM algorithm produces a smaller RMSE in the presence of time-varying system noise as compared to when operating in a system with constant noise.

Figure 17 shows a comparison of the line features extracted during the environment and feature measurement of the embankment, while Figure 18 is a simultaneous localization and mapping based on AUKF. During UUV navigation, the algorithm uses the three ranging sonar to perceive the environment, continuously extracting 𝜌-𝜃 parameters based on the Hough transform. The model accomplishes navigation based on the AUKF-SLAM algorithm by repeatedly observing available line features to continuously correct the UUV’s position and update its existing line features map. The AUKF-SLAM algorithm avoids the error acceleration caused by dead reckoning and ensures that a UUV no longer has to periodically float to the surface for GPS correction; one of the most important conditions for UUV’s tasked with long-term missions where they must remain hidden.

Figure 17 

Comparison of line features extracted from the environment and the feature measurement of the embankment.

Figure 18 

SLAM diagram based on the AUKF.

5. Conclusion

The proposed AUKF-SLAM algorithm adopts an improved Sage-Hausa suboptimal unbiased maximum posterior noise statistical estimator to estimate unknown system noise. The algorithm estimates and corrects the statistical character of noise in real time and decreases estimated error. At the same time, the algorithm judges whether the filter is converging and introduces an adaptive forgetting factor to correct the predicted covariance adjust the Kalman gain, and restrain the divergence of the filter when it diverges, therefore increasing the algorithm’s fast tracking ability. The AUKF-SLAM algorithm provides a new method for simultaneous underwater localization and mapping in an unknown environment. Assistant navigation based on the proposed AUKF-SLAM algorithm can help UUV’s fulfil missions requiring marine environment monitoring, marine terrain inspection, and long-term underwater tasks.

Acknowledgments

This research work is supported by the National Natural Science Foundation of China (Grant no. E091002/50979017), the Ph.D. Programs Foundation of Ministry of Education of China and Basic Technology (Grant no. 20092304110008), the Research Operation Item Foundation of Central University (Grant no. HEUCFZ 1026), the Harbin Science and Technology Innovation Talents of Special Fund Project (Outstanding Subject Leaders) (no. 2012RFXXG083), and the New Century Excellent Talents Foundation of Education of China (no. NCET-10-0053).