1. INTRODUCTION
For human exploration of the underwater world, Autonomous Underwater Vehicles (AUVs) are receiving increasing attention globally, and considerable progress has been made regarding their use.
Accurate navigation is a prerequisite for the success of an AUV mission (Cox and Wei, Reference Cox and Wei1995). The particularity of underwater operations makes it impossible for AUVs to use Global Positioning System (GPS) positions, and the accumulation of Inertial Navigation System (INS) errors reduces the navigation accuracy to an unacceptable degree over time. Therefore, Terrain-Aided Navigation (TAN) is required for correcting INS errors (Paull et al., Reference Paull, Saeedi, Seto and Li2014; Groves et al., Reference Groves, Handley and Runnalls2006). With the development of a wide-swath bathymetry system, high-precision seabed-terrain measurements have become possible, and TAN has therefore become a feasible way to solve the navigation precision issue for AUVs (Song et al., Reference Song, Bian and Zielinski2015; Chen et al., Reference Chen, Li, Su, Chen and Jiang2015).
The main problems restricting TAN are the design of the matching algorithm and acquisition of a precise prior map. The matching algorithm of TAN has already been significantly developed. Mok et al. (Reference Mok, Bang, Kwon and Yu2013) introduced an adaptive Extended Kalman Filter (EKF) method for use with a TAN algorithm and proved its reliability through a simulation experiment. Donovan (Reference Donovan2012) proposed a TAN algorithm using a particle filter and proved its good location performance regardless of what type of sonar is used, i.e., Doppler Velocity Log (DVL), or single- or multi-beam sonar. Zhang et al. (Reference Zhang, Li, Zhao and Rizos2014) proposed a robust TAN algorithm that can significantly reduce interference from outliers.
Common methods for the acquisition of prior maps include the use of Underwater Terrain Digital Maps (UTDMs) and bathymetry systems. UTDM is a simple method for obtaining a large amount of underwater terrain features but with low precision owing to its small amount of data per unit area and the presence of interpolation errors (Zhang et al., Reference Zhang, Xu and Xu2015). A bathymetry system has high precision but requires accurate location information for map correction when operated, and thus bathymetry systems at present are generally installed beneath ships along with GPS. However, a bathymetry system installed in an AUV is a more realistic method for deep sea-terrain mapping owing to the limited sonar measuring range (a Seabat 7125 can survey a water depth of up to 500 m, and a GeoSwath Plus (GS+) can survey a water depth of 200 m). Determining the absolute underwater location of an AUV mainly relies on an acoustic array (Caiti et al., Reference Caiti, Corato, Fenucci and Allotta2014); however, the layout of an acoustic array is time-consuming and expensive, and it is therefore difficult to achieve a survey and mapping of an underwater terrain within a large sea area using an acoustic array.
With the increase in the distance of AUV voyages (Bellingham et al., Reference Bellingham, Zhang, Kerwin, Erikson, Hobson and Kieft2010) and advancements of Underwater Wireless Sensor Networks (UWSNs) (Zandi et al., Reference Zandi, Kamarei and Amiri2015), a new mode of thinking has been created regarding underwater terrain surveying and mapping. As shown in Figure 1, at point 1, an AUV obtains its absolute location through Underwater Acoustic Communication (UAC) with sensor nodes (sensor nodes are more like a Long Baseline (LBL) positioning system attached to the seafloor, allowing the AUV to obtain its positioning with high precision) when the AUV is within communication range. At point 2, when the AUV is unable to communicate with the sensor nodes, INS is the only method available for navigation. At point 3, the AUV is again within communication range, and starts to communicate with the sensor nodes to locate and determine the final INS error. Throughout this process, the AUV uses a bathymetry system to survey and draw the underwater terrain map. This situation requires a method that allows INS errors to be corrected at each moment on the path, and based on the final INS error, and without an absolute location, for the newly surveyed map to be corrected.
Process of survey and mapping with sensor nodes.
This method needs to meet the following requirements: 1) work without GPS or other precise location method, 2) work without a prior map, and 3) correct any INS errors incurred for each moment based on the final INS error. With this regard, Bar-Shalom and Bar-Shalom and Chen (Reference Bar-Shalom and Chen2004) put forward a correction method based on the mutual independence among sensors; however, the measurement results of the bathymetry system and the navigation results are not clearly independent for an AUV. In addition, Barkby et al. (Reference Barkby, Williams, Pizarro and Jakuba2009) proposed a type of Bathymetric Particle Simultaneous Localisation and Mapping (BPSlam) method that can achieve a Simultaneous Localisation and Mapping (SLAM) for an AUV without GPS or other precise location method. However, this method requires a low-resolution map generated by a surface vessel, and thus is difficult to use while working in deep-sea waters.
The most traditional method for meeting the three requirements above is to correct the map by distributing the final INS error evenly for each moment according to the time or distance. However, a map obtained through this method has low accuracy and consistency, and therefore cannot meet the needs of a prior map. In this paper, graph optimisation of graph-based SLAM is introduced. Graph optimisation is also known as a SLAM back-end, where a SLAM problem can be transformed into a graph optimisation problem after finishing the frame-to-frame alignment and loop closure detection. The optimal pose sequence can finally be obtained by solving the constraint relation (Matthies, Reference Matthies2012).
Graph-based SLAM was first put forward by Lu and Milios (Reference Lu and Milios1997), the basic idea of which is to retain all data frames and spatial constraints of the frames for estimating the position and pose of a vessel using the maximum likelihood method. This SLAM method can be described using a graph. During the twenty years following its introduction, graph-based SLAM has undergone significant developments. Gutmann and Konolige (Reference Gutmann and Konolige1999) presented a method utilising efficient loop-closure detection and mapping based on the work by Lu and Milios (Reference Lu and Milios1997). A nonlinear optimisation algorithm for fast recovery of a robot trajectory was proposed by Olson et al. (Reference Olson, Leonard and Teller2013), which does not rely on an accurate initial value and can be successfully implemented even with a large initial error. However, most of these methods are based on a camera or forward-looking sonar, which can obtain duplicate information between adjacent points. This is different from a bathymetry system, in which an AUV obtains a line of terrain information at a single time, which is not repeated and will not be obtained twice within a short period of time.
In this paper, TCCM for AUV seabed terrain mapping is investigated. This method includes a front-end and a back-end. First, the AUV predicts the terrain elevation of the bathymetry system at each moment using Terrain Elevation Measurement Extrapolation Estimation (TEMEE) while mapping the terrain to calculate the likelihood function at each moment combined with the actual measured value at the front-end. After the final INS error is obtained, it is distributed evenly to each moment at the back-end. An Improved Terrain Correlation Correction Method (ITCCM) is applied to improve the weakness of the previous method shown in the simulation experiment. This method puts forward a partial offset and a global offset based on the previous method, and the combined effects of these offsets are used to correct the mapping error without applying GPS at the back-end. The experiment results show that ITCCM has a significant correction effect. TAN simulation experiments have been conducted to prove the importance of map correction using ITCCM.
2. TERRAIN CORRELATION CORRECTION METHOD
2.1. System model
The AUV INS path points are
${\hat{\rm X}}= \lcub \hat{\rm x}_{\rm 0}\comma \; \hat{\rm x}_{\rm 1} \comma \; \ldots\comma \; \hat{\rm x}_{n}\rcub $
, and its real path points are
${\rm X}=\lcub {\rm x}_{\rm 0}\comma \; {\rm x}_{\rm 1} \comma \; \ldots\comma \; {\rm x}_{n} \rcub $
and
${\rm x}_{0} ={\hat {\rm x}}_{0} ={\rm x}_{n} +d$
, where
$d={\rm x}_{0} -{\rm x}_{n} $
is the distance between the start and end points on real path. The AUV model can be described as
$$\eqalign{ {\hat{\rm x}}_{i+1} &={\hat{\rm x}}_i +u_i +v_i \cr {\rm x}_{i+1} &={\rm x}_i +u_i\quad (i=1,2,\ldots,n) \cr \sum\limits_{i=0}^n {u_i } &=d} $$
where, u i is the real output of the AUV at point i, and v i is the INS error at point i. The final INS error ε can be obtained at point 3 shown in Figure 1.
$$\eqalign{ \varepsilon &={\hat{\rm x}}_n -{\rm x}_n +d \cr &={\hat{\rm x}}_n -{\hat{\rm x}}_0 +d \cr &=({\hat{\rm x}}_n -{\hat{\rm x}}_{n-1} )+({\hat{\rm x}}_{n-1} -{\hat{\rm x}}_{n-2} )+\ldots+({\hat{\rm x}}_{\rm 1} -{\hat{\rm x}}_{\rm 0} )+d \cr &=\sum\limits_{i=1}^n {{ (\hat{\rm x}}_i -{\hat{\rm x}}_{i-1} } )+d} $$
${\rm x}_{0} +\sum\limits_{i=1}^{n} {u_{i} } ={\rm x}_{n} +d$
,
$\sum\limits_{i=1}^{n} {u_{i} } =d$
and
$\varepsilon =\sum\limits_{i=1}^{n} {v_{i}}$
. Thus, the start and end points can be reduced to the same point and generate
$\sum\limits_{i=1}^{n} {u_{i} } =0$
. In addition, error ε consists of errors between the real AUV position at point i + 1 and the AUV position predicted based on the position at point
$i\lpar i=1\comma \; 2\comma \; \ldots\comma \; n\rpar $
. The system noise
$v_{i} \lpar i=1\comma \; 2\comma \; \ldots\comma \; n\rpar $
is white noise and for easier calculation has a distribution of
$p_{v_{i} } \lpar x\rpar ={N}\lpar \mu \comma \; \sigma _{v}^{2} \rpar $
.
Similar to the elastic correction proposed by Golfarelli et al. (Reference Golfarelli, Maio and Rizzi1998), making reference to the calculation model of a spring, an error correction method was investigated that regards connections between two adjacent points as springs with different degrees of stiffness, and thus the system energy model can be described as
$$\eqalign{\displaystyle{{\prod\limits_{i=1}^n {\theta _i}}\over{\sum\limits_{i=1}^n \displaystyle\prod^{n}_{\matrix{j=1 \cr j\ne i}} {\theta _j}}}\cdot \varepsilon ^2 &=\sum\limits_{i=1}^n {\theta _i ({\hat{\rm x}}_i -{\rm x}_i -({\hat{\rm x}}_{i-1} -{\rm x}_{i-1} ))^2} \cr &=\sum\limits_{i=1}^n {\theta _i \cdot (v_i -v_{i-1} )^2}}$$
where,
${\hat{\rm x}}_{0} ={\rm x}_{0} $
, v
0 = 0, θ
i
is the stiffness of spring i (at point i), and x
i
is the real position at point i.
Transforming the energy model into the force model according to the function of displacement under force
$$\eqalign{\displaystyle{{\prod\limits_{i=1}^{\rm n} {\theta _i } }\over{\sum\limits_{i=1}^n \displaystyle\prod^{n}_{\matrix{j=1 \cr j\ne i}} {\theta _j}}}\cdot \varepsilon &=\theta _1 ({\hat{\rm x}}_1 -{\rm x}_1 -({\hat{\rm x}}_0 -{\rm x}_0 ))^2=\ldots=\theta _n ({\hat{\rm x}}_n -{\rm x}_n -({\hat{\rm x}}_{n-1} -{\rm x}_{n-1} ))^2 \cr &=\theta _1 (v_1 -v_0 )^2=\ldots=\theta _n (v_n -v_{n-1} )^2}$$
A recursion is built to calculate
$v_{i} \lpar i=1\comma \; 2\comma \; \ldots\comma \; n\rpar $
.
The system model can be described as shown in Figure 2.
Schematic diagram of system error.
Here, 
indicates INS position
$\hat x_i$
; 
represents
${\hat{\rm x}}_{i-1} +u_{i-1} $
, which is the ‘real position’ relative to the previous INS point (not the actual real position); and θ
i
is the stiffness of spring i (at point i).
After the system model has been built, θ is discussed at every point. As we can see, error ε is distributed to every point based on the value of θ.
Considering the measurement information of a bathymetry system, a state model at point i + 1 is established:
$$\left\{ {\matrix{{{\rm x}_{i + 1} = {\rm x}_i + u_i + \upsilon _i} \cr {z_{i + 1} = H({\rm x}_{i + 1}) + e_{i + 1}}}}\right.$$
where,
$H=\lsqb \displaystyle{{\partial f}\over{\partial x}}\comma \; \displaystyle{{\partial f}\over{\partial y}}\comma \; -1\rsqb $
is a state transition matrix, z
i+1 is the measurement information (terrain elevation) of bathymetry system at point i + 1, and H(x
i+1) is the estimation of z
i+1 based on TEMEE, which means that the change in the terrain is continuous within a very small area. In addition, e
i
is the measurement noise of the wide-swath bathymetry system, and has a distribution of
$p_{e_{i} } \lpar x\rpar ={N}\lpar 0\comma \; R\rpar $
; thus
$\sum\limits_{i=0}^{n} {e_{i} } =0$
is used for simplicity. For the entire system, an error between the real measurement z
i
and estimation H(x
i
) is caused by v
i
.
Owing to
$p_{{\rm v}_{i} } \lpar x\rpar ={N}\lpar \mu \comma \; \sigma _{v}^{2} \rpar $
,
$v=\lsqb v_{x} \cos \varphi \sin \phi \comma \; v_{y} \cos \varphi \cos \phi \comma \; v_{z} \sin \varphi \rsqb ^{T}$
, and thus
${v}'_{i} =H\lpar v_{i} \rpar =g_{i} \cdot v_{i} $
,
$$g=\displaystyle{{\partial f}\over{\partial x}}\cos \varphi \sin \phi +\displaystyle{{\partial f}\over{\partial y}}\cos \varphi \cos \phi -\sin \varphi .$$
The Probability Density Function (PDF) of v′ is
$$p({v}')=\displaystyle{{1}\over{g\sigma _v \sqrt {2\pi }}}\exp \left\{ {\displaystyle{{-[{v}'-(g\cdot v)]^2}\over{2g^2\sigma_v^2 }}} \right\}$$
Thus,
$p_{{v}'_{i} } \lpar x\rpar ={N}\lpar g\mu \comma \; g^{2}\sigma _{v}^{2} \rpar ={N}\lpar {\mu }'\comma \; \sigma_{{v}'}^{2} \rpar $
fits the normal distribution.
The measurement model can be simplified as
$${z}_i =H({\hat{\rm x}}_i -v_i )=H({\hat{\rm x}}_i )-H(v_i)=H({\hat{\rm x}}_i )+{v}'_i$$
Therefore, the error between real measurement z i and estimation H(x i ) can be seen as being caused by the transmission of INS error v i in the measurement equation.
Owing to
$p_{{v}'_{i} } \lpar x\rpar ={N}\lpar {\mu }'\comma \; \sigma _{e}^{2} \rpar $
,
$$p({z}_i \vert {\rm x}_i )=p({v}'_i )=p({z}_i -H({\hat{\rm x}}_i ))=\displaystyle{{1}\over{(2\pi \sigma _e^2 )^{N/2}}}\cdot {\rm exp}[-\displaystyle{{1}\over{2\sigma _e^2 }}\sum\limits_{k=1}^N {({z}_{i,k} -h({\hat{\rm x}}_{i,k} ))^2} ]$$
Therefore,
$${\rm In}\,p(z_i\mid{\rm x}_i) = - N\,{\rm In}\,\sqrt {2\pi} \cdot \sigma _e - \displaystyle{1 \over {2\sigma _e^2 }}\sum\limits_{k = 1}^N {{(z_{i,k} - h({\hat{\rm x}}_{i ,k}))}^2}$$
where, because the measurements of all sampling points are believed to be independent of the others,
$\sigma _{e} =diag\lpar \sigma _{1} \comma \; \sigma _{2} \comma \; \ldots\comma \; \sigma _{k} \rpar $
is a diagonal matrix consisting of the variance components of each sampling point, N represents the number of sampling points at each point on the path, z
i, k
is the measurement of measuring line k at point i of the bathymetry system, and
$h\lpar {\hat{\rm x}}_{i\comma k} \rpar $
is the estimation of z
i+1, which can be obtained based on the previous measurement by TEMEE. In addition,
$p\lpar {z}_{i} \vert {\rm x}_{i} \rpar $
is the prior probability density function, and θ
i
increases with an increase in
$p\lpar {z}_{i} \vert {\rm x}_{i} \rpar $
. Considering that the value of
$p\lpar {z}_{i} \vert {\rm x}_{i} \rpar $
is within the range of [0,1],
$\theta _{i} =-\ln p\lpar {z}_{i} {\rm \vert x}_{i} \rpar $
is used to extend the variance of the correction value.
2.2. Terrain Elevation Measurement Extrapolation Estimation (TEMEE)
Two independent assumptions are made
1) The terrain elevation meets the extrapolation estimation in the two-dimensional one-way local area, i.e.,
$$\tilde {h}=\sum\limits_{i=1}^N {\lambda _i h_i }$$
where
$\tilde h$
is the value that needs to be estimated, h
i
is the known elevation, and λ
i
is the weight of the corresponding point. Owing to the particularity of extrapolation estimation, every known terrain elevation h
i
is on one side of
$\tilde h$
, and is thus called a one-way local area. Inverse Distance Weighted (IDW), Gaussian Weighted Fitting (GWF) and Kriging can be used in Assumption 1.
2) The terrain elevation meets the mth order polynomial distribution,
i.e.,
$$\tilde h = \sum\limits_{i = 0}^m {\eta _i \cdot x^i}$$
where
$\tilde h$
is the value needed to be estimated, η
i
is a coefficient, and x
i
indicates the estimated point coordinates of the ith power.
A simulation experiment was carried out, and the UTDM for the experiment was measured using GS+ wide-swath bathymetry system in the Zhongsha reef in Qingdao. A total of 400 samples in four areas with different features were selected to test the two assumptions, as shown in Figure 3. The values of the colour scale show the distance between the sea level and seabed.
Underwater digital terrain map of Zhongsha reef in Qingdao.
In the simulation experiment, for both Assumptions 1 and 2,
$d_{1} =d_{2} =\cdots =d_{8} =1\, {m}$
, as shown in Figures 4(a) and 4(b), respectively. The simulation experiments were conducted using a seabed terrain-aided navigation simulation system based on Vega and MFC and running on a computer with an Intel I5 3210M CPU and 4 GB of memory Chen et al. (Reference Chen, Li, Su, Chen and Jiang2015). The bathymetry system was simulated using the intersecting rays of Vega. The simulation system is shown in Figure 5.
Schematic diagram of the chosen points.
Seabed terrain-aided navigation simulation system.
The estimated errors are shown in Table 1.
Estimated Errors.
As shown in Table 1, Assumption 1 produced inaccurate results owing to its ‘one-way’ application, which indicates a lack of known points on the other side, whereas the results of Assumption 2 are closer to the true values. When the order of polynomial fitting is equal to the number of known points, the average absolute error is
$7{\cdot}71\times 10^{-6}\, {m}$
, the absolute error variance is
$4{\cdot}73\times 10^{-10}$
, all relative errors are less than 0·01, and there are no changes in the amount of time consumed. Therefore, the results of Assumption 2 can be considered the true values when the order of the polynomial fitting is equal to the number of known points.
2.3. Correction algorithm
The correction algorithm process is as follows.
Require: multi-beam sonar data
$\sum\limits_{i=1}^{n} {\sum\limits_{k=1}^{N} {h_{k} } } \lpar {\rm x}_{i} \rpar $
Require: INS points
${\hat {\rm x}}={\lcub \hat{\rm x}}_{\rm 1}\comma \; {\hat{\rm x}}_{\rm 2} {\comma \; \ldots\comma \; \hat{\rm x}}_{\rm n} {\rm \rcub }$
Require: INS error ε
For i = 1 to n do
$$\theta _i =\displaystyle{{1}\over{(2\pi \sigma _e^2 )^{N/2}}}\cdot {\rm exp}\left[-\displaystyle{{1}\over{2\sigma_e^2 }}\sum\limits_{k=1}^N {({z}_{i,k} -h_k ({\hat{\rm x}}_i ))^2} \right]$$
End for
Use Equation (3) to calculate
$\sum\limits_{i=1}^{n} {\theta _{i} \cdot v_{i}^{2}}$
Use Equation (4) to calculate v i in each point
For i = 1 to n do
Get the corrected point
${\rm x}_{i} ={\hat{\rm x}}_{i} +v_{i} $
End for
Get the corrected map\medskip
The following is supplementary information for the applied process:
-
(1) The error at all points is the same as that of the final INS error, i.e.,
(13)which means the absolute value of the final INS error
$$\left\{\matrix{\sum\limits_{i=1}^n {\vert {v_{ix} } \vert =\vert {\varepsilon_x } \vert } \cr \sum\limits_{i=1}^n {\vert {v_{iy} } \vert =\vert {\varepsilon_y } \vert } \cr }\right.$$
$\vert \varepsilon \vert $
is equal to the sum of the absolute values of the errors at each point
$\vert {v_{i} } \vert $
.
-
(2) TCCM can only calculate the lateral deviation of the direction of vessel motion, and cannot calculate the deviation of the vessel movement direction.
2.4. Playback experiment and results analysis
To obtain GPS data for a comparison, the sea trial data of the GeoSwath plus (GS+) wide-swath bathymetry system installed beneath the ship were used to conduct a playback experiment. These data include GPS data, Fibre Optic Gyrocompass (FOG) data, and bathymetry system measurement and peripheral equipment data, such as depth gauge data and acoustic velocity data. The depth gauge data were acquired using a sonar altimeter, and the acoustic velocity data were acquired using Mini-SVS (Sound Velocity Sensor) from Valeport Co. Some of the devices used are shown in Figure 6.
GeoSwath plus system.
The path was a rectangular shape with a length of about 160 m and a width of about 55 m, and the final INS error was 5·12 m.
The INS error was simulated by using GPS plus white noise, i.e.,
$$\left\{ {\matrix{ {{\rm x}_{i + 1}^{\rm INS} = {\rm x}_i^{\rm INS} + u_i + e_i} \cr {u_i = {\rm x}_{i + 1}^{\rm GPS} - {\rm x}_i^{\rm GPS} }}} \right.$$
where e
t
is random drift with a distribution of
$p\lpar e_{t} \rpar ={N}\lpar 0\comma \; 2\rpar $
.
The results are as shown in Figures 7 and 8. As shown in Figure 7, the traditional method is to distribute the final INS error evenly to each moment according to the time.
Track correction results.
Track correction error results.
As shown in Figure 8, the average value of the INS error is 4·7075 m, with a variance of 2·2652; the average value of the error using the traditional method is 2·6055 m, with a variance of 1·1887; and the average value of the error using TCCM is 2·3276 m, with a variance of 0·9901. TCCM reduces the error by 50·56% compared to the INS track, and 10·67% compared to the traditional method.
As shown in Figures 7 and 8, the results of the TCCM track are similar to those of the traditional method owing to the algorithm limit in Equation (13), which ignores the fact that error v i is a vector with direction and that such errors will cancel each other out to a certain extent. Thus, the sum of the absolute value of the errors at each point is larger than the absolute value of the final INS error.
By analysing the data, m points
$\tilde {\rm x}=\lcub \tilde {\rm x}_{1} \comma \; \tilde {\rm x}_{2} \comma \; \ldots\comma \; \tilde {\rm x}_{m} \rcub $
, which have a value of θ far greater than the mean value, were found. Suppose that
$\theta \lpar \tilde {\rm x}_{i} \rpar $
is far bigger than the mean value because of a significant deviation, which we call a ‘partial offset’ in Section 3.1, between the estimated position 
and the ‘real’ position estimated based on the previous position, which is 
star at the point
$\tilde x$
in Figure 2. The ITCCM method was proposed based on this assumption.
3. IMPROVED TERRAIN CORRELATION CORRECTION METHOD (ITCCM)
3.1 Partial offset
A marine environment is very complex, and errors can occasionally be approximated as Gaussian white noise; at other times, however, the INS system of the AUV may be additionally disturbed by sudden waves. Partial offset Lθ, the biggest difference between TCCM and ITCCM, is proposed to deal with sudden waves.
A partial offset Lθ can be decomposed into Lθ x and Lθ y in the x- and y-axes of the Geodetic Coordinate System as shown in Figure 9(a), and β is heading angle of AUV.
Schematic diagram of partial offset.
As shown in Figure 9(b), there is a big difference between the real depth observation and the forecast depth observation at 
point, and it makes a big value of θ at 
point. Based on the demand of correction frequency of partial offset, the appropriate threshold is selected, and the threshold value is negatively correlated with the correction frequency. As shown in Figure 9(b), θ is much bigger than threshold at 
point, so a certain distance has been moved along the x
AUV
-axis to calculate the corresponding θ value and the position of the minimum value of θ is the correction position 
point as shown in Figure 9(b). So the distance between 
and 
along the x
AUV
-axis is a partial offset Lθ.
The value of Lθ is related to swath width, swath update rate and speed. The first two items are determined by the parameter settings of wide-swath bathymetry system, which is GS+ wide-swath bathymetry system in our experiment.
3.2 System model and algorithm process
In system
${\rm x}=\lcub {{\rm x}_{1} \comma \; {\rm x}_{2} \comma \; \ldots \comma \; {\rm x}_{n} } \rcub $
, there are m known partial offsets whose value is
${w}={\rm \lcub }w_{\rm 1}\comma \; w_{\rm 2} \comma \; \ldots\comma \; w_{m} {\rm \rcub }$
with PDF
$p\lpar {z\vert \tilde{\rm x}}+{w}\rpar = {\rm \lcub }p\lpar z_{1} {\vert \tilde{\rm x}}_{\rm 1} +w_{\rm 1}\rpar \comma \; p\lpar z_{2} {\vert \tilde{\rm x}}_{\rm 2} +w_{\rm 2} \rpar \comma \; \ldots\comma \; p\lpar z_{\rm m} {\vert \tilde {\rm x}}_{\rm m} +w_{\rm m} \rpar {\rm \rcub }$
, and one global offset ε with PDF
$p\lpar {z}_{n} {\vert x}_{n} +\varepsilon \rpar =1\comma \; \sum\limits_{j=1}^{m} {w_{j} } >\varepsilon $
. A system model was built, i.e.,
$$\left\{ {\matrix{ {{\rm x}_{i + 1} = {\rm x}_i + u_i + \upsilon _i} \cr {\upsilon _i = \upsilon _i,_\varepsilon + {{\upsilon }^{\prime}}_i} \cr {{\rm z}_{i + 1} = h({\rm x}_{i + 1}) + e_i}}} \right.$$
where v
i
is the total offset at point i;
$v_{i\comma \varepsilon } $
is the component of the final INS error acting at point i, which we call a global offset; and v′
i
is the component of the known partial offset value at point i, which is called a partial offset.
When the course is stable and the course angle does not change, the change trend of the INS error can be regarded as approximately stable. The path of the AUV is divided into k stages, including one start stage, k − 2 intermediate stages, and one end stage.
$${\rm x}=\{{\rm x}_1^1 ,\ldots,{\rm x}_i^1 ,\ldots,{\rm x}_1^{\,j},\ldots,{\rm x}_i^{\,j} ,\ldots,{\rm x}_{n_k }^k \},$$
where
${\rm x}_{i}^{\, j} $
is point i in stagej.
After the system model is built, the following restrictions should be carried out for each point offset:
1) Because of the instability of the INS error in the different stages, partial offsets v of each stage are considered independent of each other, which means the final partial offset
${v^{'j}}_{n_{j}}$
in stage j will not influence the total offset in stage j + 1.
2) In stage j, suppose that two adjacent known partial offsets
$w_{i}^{\, j}$
and
$w_{i+p}^{\, j} $
are obtained at the points
${\rm x}_{i}^{\, j}$
and
${\rm x}_{i+p}^{\, j} $
, respectively. In the same stage, all points need to satisfy the motion continuity condition, that is, the motion between adjacent points is continuous and can be guided.
At point
${\rm x}_{i}^{\, j} $
,
$$\eqalign{ p({z}_i^{\,j} {\rm \vert x}_i^{\,j} +w_i^{\,j} )&=p({z}_i^{\,j} -h({\rm x}_i^{\,j} +w_i^{\,j} )) \cr &=\displaystyle{{1}\over{\sqrt {2\pi } \sigma _i^{\,j} }}\exp \left[{{{-[{z}_i^{\,j} -h({\rm x}_i^{\,j} +w_i^{\,j} )]^2}/{2\sigma_i^{\,j2}}}} \right]}$$
where
$\sigma _{i}^{\, j2}$
is the diagonal matrix of the sampling points of the terrain that describe the dispersion degree of
$p\lpar {z}_{i}^{\, j} {\rm \vert x}_{i}^{\, j} +w_{i}^{\, j} \rpar $
. When
${z}_{i}^{\, j} -h\lpar {\rm x}_{i}^{\, j} +w_{i}^{\, j} \rpar \to 0$
,
$$p({z}_i^{\,j} {\rm \vert x}_i^{\,j} +w_i^{\,j}) \to \displaystyle{{1}\over{\sqrt {2\pi } \sigma _i^{\,j} }}\ {and}$$
$$\sigma _i^{\,j} \to \displaystyle{{1}\over{\sqrt {2\pi } p({z}_i^{\,j} {\rm \vert x}_i^{\,j} +w_i^{\,j})}}$$
Suppose that the influence of the offset at point
${\rm x}_{i}^{\, j}$
on the later points such as
${\rm x}_{i+1}^{\, j} \comma \; \ldots\comma \; {\rm x}_{i+p-1}^{\, j}$
is satisfied using the Radial Basis Function (RBF). Taking
${\rm x}_{i+1}^{\, j} $
as an example, the influence should be satisfied with
$${k}\left\| {{\rm x}_{i+1}^{\,j} -{\rm x}_i^{\,j} } \right\|^2=\exp \left[ {{{-\lpar {{\rm x}_{i+1}^{\,j} -{\rm x}_i^{\,j} } \rpar^2}/{2\sigma_1^2 }}} \right] $$
where σ1 represents the width parameter of the function, which controls the radial function range. When
$\sigma _{i}^{\, j} $
is smaller, which means
$\sqrt {2\pi } p\lpar {z}_{i}^{\, j} {\rm \vert x}_{i}^{\, j} +w_{i}^{\, j} \rpar $
is much larger,
$w_{i}^{\, j} $
is more reliable, and thus its scope of action can be increased, which means that σ1 can be taken as a larger value. Making
$\sigma _{1} =p\lpar {z}_{i}^{\, j} {\rm \vert x}_{i}^{\, j} +w_{i}^{\, j} \rpar $
, the partial offset at point
${\rm x}_{i+1}^{\, j} $
is
$${\rm {v}'}_{i+1}^{\,j} ={\rm {v}'}_i^{\,j} +w_{i+p}^{\,j} \cdot \exp \left[ {\displaystyle{{-({\rm x}_{i+1}^{\,j} -{\rm x}_{i+p}^{\,j} )^2}\over{2\cdot p({z}_{i+p}^{\,j} {\rm \vert x}_{i+p}^{\,j} +w_{i+p}^{\,j} )^2}}} \right]$$
Equation (21) is brought into Equation (14) to calculate the total offsets of each of the points and to correct the INS error on the path.
These restrictions correct all of the ‘overshoot’
$\sum\limits_{i=1}^{n_{j}} {\lpar {v}_{i}^{'j} -v_{i\comma \varepsilon }^{\, j}\rpar } $
in stage j behind the end point in the current stage, which will lead to a discontinuity of the map. However, this does not influence the discrete points such as current point and later points, and increases the consistency of the map at every stage, while decreasing the mean error.
The algorithm process of ITCCM is as follows.
Require: multi-beam sonar data
$\sum\limits_{i=1}^{n} {\sum\limits_{k=1}^{N} {h_{k} } } \lpar {\rm x}_{i} \rpar $
Require: INS track points
${\hat {\rm X}}={\lcub \hat{\rm x}}_{\rm 1}\comma \; {\hat{\rm x}}_{\rm 2} {\comma \; \ldots\comma \; \hat{\rm x}}_{\rm n} {\rm \rcub }$
Require: INS error ε
For i = 1 to n do
$$ \theta _i =\displaystyle{{1}\over{(2\pi \sigma _e^2 )^{N/2}}}\cdot {\rm exp}[-\displaystyle{{1}\over{2\sigma _e^2 }}\sum\limits_{k=1}^N {({z}_{i,k} -h_k ({\hat{\rm x}}_i ))^2} ] $$
End for
Use Equation (3) to calculate
$\sum\limits_{i=1}^{n} {\theta _{i} \cdot v_{i}^{2} } $
Use Equation (4) to calculate v i at each point
For i = 1 to n do
$$ {\rm x}_i ={\hat{\rm x}}_i +v_i $$
End for
Lθ = find(θ > threshold value)
For i = 1 to length(Lθ)
Calculate offset of Lθ i
End for
Divide Lθ into k stages
For j = 1 to k
For i = 1 to n j do
Use Equation (21) to calculate
$v_{i}^{\, j} $
Get the corrected points
${\rm x}_{i}^{\, j} ={\hat{\rm x}}_{i}^{\, j} +v_{i}^{\, j} $
End for
End for
Get the corrected map
3.3. Playback experiment and result analysis
In Figure 10, Lθ is indicated with the black dotted circles. Let
${max}\lpar L\theta \rpar =0{\cdot}75\, {m}$
, owing to the swath width and swath update rate were 21 m and 1 s respectively and the average speed was 3 kn. As shown in Figure 11, the average value of the ITCCM error is 1·6295 m, and its variance is 0·6535. ITCCM reduced the error by 38·46% compared to the traditional method, and by 29·99% compared to TCCM. In the start stage shown in Figure 10, the ITCCM track was closer to the GPS track, and the mean ITCCM error was only 1·12 m as compared to 3·03 m of TCCM, as indicated in Figure 11. In the intermediate and end stages, the ITCCM and TCCM tracks were are almost identical, as shown in both Figures 10 and 11.
Track correction results.
Track correction error results.
Therefore, ITCCM worked well in the start stage, and its correction effect was not obvious in the intermediate stage owing to the instability of the INS error in the different stages. In the end stage, both TCCM and ITCCM worked well. Overall, ITCCM achieved a good map correction.
As shown in Figure 12, the interpolated terrain map was created using the bathymetry system data, where the blue dotted indicates the GPS track, and where the black line in Figures 12(b), 12(c), and 12(d) indicates the INS track, TCCM track, and ITCCM track, respectively. The effect of the path correction on the map building is also shown in the figure. As Figure 12 illustrates, the sub-map in the start stage, shown in Figure 12(d), is more similar to the map in Figure 12(a) than the others, and both Figures 12(c) and 12(d) show better sub-map corrections in the end stage compared to that shown in Figure 12(b).
Terrain created by bathymetry system data.
4. TERRAIN-AIDED NAVIGATION SIMULATION EXPERIMENT
To explain the significance of map correction, a TAN simulation experiment was conducted. A GPS map with an added bathymetric error was used as a real-time map of the AUV. In addition, the maps in Figure 12(b), 12(c), and 12(d) were used as prior maps. The simulation system used is shown in Figure 5, and the experiment parameters are shown in Table 2.
Experiment parameters.
The search area of each point was set to a circle with a radius of 10 m, and the results of the experiment are as shown in Table 3.
TAN error.
As shown in Table 3, the mean TAN error of ITCCM is 4·1277, 7·1771, and 4·1149 m for the start, intermediate, and end stages, respectively. In the start stage, ITCCM reduced the error by 55·32% compared to the INS map, and by 52·42% compared to TCCM. In the intermediate stage, ITCCM reduced the error by 10·28% compared to the INS map, and by 7·6% compared to TCCM. Finally, in the end stage, ITCCM reduced the error by 52·63% compared to the INS map, and by 11·53% compared to TCCM.
5. CONCLUSION
An AUV path was divided into three stages: a start stage, an intermediate stage, and an end stage. In the start stage, the ITCCM correction effect was shown to be the best, which means ITCCM is the most effective method when the INS error is relatively stable. In the intermediate stage, ITCCM decreased the error at every point, but the effect was not obvious compared to the raw INS data or TCCM. In the end stage, both TCCM and ITCCM corrected the path and map clearly, although ITCCM did not significantly decrease the error compared to TCCM.
On the whole, the variation trend of the TAN error was consistent with the trend of the error at every point, as shown in Figure 10 and 12, whereas the TAN error decreased when the error at the corresponding point decreased. However, the TAN error was often larger than the error at the corresponding point owing to the accumulation of various errors during the TAN process. Therefore, map correction, using ITCCM, or occasionally TCCM, is an important necessity.
ACKNOWLEDGEMENT
This work is supported by National Middle-aged top-notch Talents Support Program of China, the National Natural Science Foundation of China (Projects: 51279221 and 51309066/E091002).
