# Underwater Environment SDAP Method Using Multi Single-Beam Sonars

Authors: Zheping Yan; Dongnan Chi; Shuping Hou; Yalin Zheng
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

A new autopilot system for unmanned underwater vehicle (UUV) using multi-single-beam sonars is proposed for environmental exploration. The proposed autopilot system is known as simultaneous detection and patrolling (SDAP), which addresses two fundamental challenges: autonomous guidance and control. Autonomous guidance, autonomous path planning, and target tracking are based on the desired reference path which is reconstructed from the sonar data collected from the environmental contour with the predefined safety distance. The reference path is first estimated by using a support vector clustering inertia method and then refined by Bézier curves in order to satisfy the inertia property of the UUV. Differential geometry feedback linearization method is used to guide the vehicle entering into the predefined path while finite predictive stable inversion control algorithm is employed for autonomous target approaching. The experimental results from sea trials have demonstrated that the proposed system can provide satisfactory performance implying its great potential for future underwater exploration tasks.


---

## Body

## 1. Introduction

Underwater exploration often encounters environment that is difficult or even impossible for humans to access due to their physical constraints such as deep depth, narrow spaces, and severe working conditions. Unmanned underwater vehicle has a number of advantages for exploring underwater environments, such as autonomous control ability and self-sufficient energy supply. Autopilot of UUV often relies on the information or characteristics (e.g., geometrical information) of the surrounding environment, reflected by data collected from sensors such as sonars.When in operation, sonar sends out an acoustic beam and the returned (usually the fastest) beam from the environment is collected to determine the distance and location of the environment. This means that  it can detect the point on the contour that has the shortest distance from the sonar. Therefore, sonar data can be used to plan the desired path of the UUV and control it by changing thruster forces and rudder angles of the UUV to approach the target. An important issue for designing UUV control systems is the strength of the signal observed from sonar. It is weak primarily due to the random effect caused by complicated marine disturbance; other interferences between received beams can be due to delay and scattering effect.In this paper a new autopilot system, known as simultaneous detection and patrolling (SDAP), is proposed to address this challenge. Autonomous guidance and control are implemented synchronously where the reconstructed environment contour is used as the guidance path for UUV navigation. For the environment contour reconstruction, the major focus of research is on simultaneous localization and mapping (SLAM) [1], where navigation is the key issue to be addressed. With the advances in control theory, UUV control systems have rapidly evolved from classic control theory to modern control models, including PID [2, 3], backstepping [4–7], fuzzy theory [8, 9], neutral network [10–13], sliding model [14, 15], model prediction control [16, 17], and feedback linearization [18–23]. In particular, Zou has proposed an optimal inversion-based output tracking approach for the guidance of a vertical takeoff and landing (VTOL) aircraft problem [21]. Song has further improved this approach with better convergence property such that a second order convergence can be achieved even with aggressive trajectories and strong nonlinearities [20]. Given that stable inversion technique has shown excellent performance for achieving stable control inputs, it is chosen to be implemented in the controllers for accuracy, efficiency, and cost effectiveness.In this paper autopilot of UUV for both closed and open environments is considered, as shown in Figure1. In the closed port (Figure 1(a)), there is only one entrance and the UUV has to be able to navigate to the only ready point from any launch and recovery position (L&R). For the open island (Figure 1(b)), there are theoretically infinite numbers of possible entrance points around the island. With consideration of the disturbances such as current direction, an appropriate ready point has to be chosen to guide the UUV to enter into the manned-unknown area. In the manned-known areas, accurate predefined path is applied for the UUV to follow in order to reduce the cost and risks. The main issue addressed in the paper is the navigation using autonomous guidance and control is implemented in manned-unknown areas or area inaccessible for humans where the contour has to be reconstructed in real time. Although in reality, the environment is semiclosed where there is more than one entrance, it is not discussed explicitly here as it can be addressed by applying the strategy for closed environment in an iterative manner.Illustration of the proposed SDAP system for different environments.
(a)
(b)The main novelties of this paper are as follows.(1)
To address the weak sonar data, a wavelet transform is applied to preprocess the original data so as to eliminate possible outliers in the original sonar data. To reduce the information loss in the preprocessing, the wavelet coefficient values of the current time are estimated using those coefficients estimated in the past times and the original sonar data. All the estimated wavelet coefficients are then regarded as the data resource for the contour reconstruction processes.(2)
A support vector clustering (SVC) inertia algorithm is proposed to cluster the data into different classes so as to determine the property of the original sonar data and obtain the boundaries of the classes (also known as contour). The resulting contour after this step is composed of successively connected lines.(3)
To satisfy the inertia property of the UUV path, the initial contour is smoothed using different order Bézier curves which are automatically determined by the local properties of the structural environments.(4)
With the information of the smoothed contour as the reference path and a predefined safety distance, an improved inversion algorithm, finite predictive stable inversion is proposed to control vehicle navigation.The remainder of this paper is organized as follows: in Section2, weak observable sonar data is preprocessed using the wavelet transform and represented as a collection of wavelet coefficients. In Section 3, the wavelet coefficients together with the data confidence limit and DVL information are used to estimate the contour of the environment structure by a support vector clustering inertia method. Addressing the inertia requirement of the vehicle, in Section 4 different order Bézier curves are introduced to smooth the initial contour and automatic decision strategy is made to generate the reference path according to the local properties of the structural environment. In Section 5 details on the controller design methods are presented, and employed for predictive trading during detection mission. The validation experiment design and results are presented in Section 6. Section 7 concludes this paper with some insightful discussions.


## 2. Preprocessing of Sonar Data Using Wavelet Transform

### 2.1. Collection of Sonar Data

In this paper five single-beam sonars are configured on the vehicle in order to automatically detect the underwater environment: three sonars in the front to detect local environment characteristics and two on the left and the right side for contour reconstruction. The three front sonars are deployed in a way where the middle one is located on the axis of the vehicle surge direction and the other two are installed on the left and right of the middle one with an angle of 7.5 degree, respectively.Data collected from sonars often include useful data describing obstacles and noise from random outliers. To address the “false alarm” problem in vehicle navigation, those outliers have to be detected and eliminated as much as possible. Outliers can be divided into noise patches and objects (e.g., fish swarm) that have less or no threats to the vehicle. Therefore, three different types of objects are defined as follows.(a)
Threaten Obstacle (TO). They are obstacles existing in the environment that can be detected by sonar and have threats to the vehicle, including wreck, reef, and iceberg.(b)
Low Threaten Obstacle(LTO).They are obstacles existing in the environment that can be detected by sonar but have low level of threat to the vehicle, such as suspensions, and fish swarms. LTO can be further fractionized to single LTO (SLTO) distributing as single isolated objects and group LTO (GLTO) with unfixed contour.(c)
Noise.Data is collected from sonar that denotes non-obstacles.With the above definitions, data describing TO is considered to be useful while LTO and noise are regarded as outliers in this paper. LetEt and EΔt be data distribution at instants t and (t+Δt), respectively, and the values of 1 and 0 describe data existence and nonexistence, respectively; Lt express the contour of data class at time t, and the sonar data can be classified as Table 1.Table 1
Metrics to categorize obstacles.
E
t
E
Δ
t
L
t change
Class
1
1
No
TO
1
1
Yes
GLTO
1
0
No contour
Noise or SLTO


### 2.2. Preprocessing of Weak Observable Data Using Wavelet Transform Modulus Maxima

Outliers mixed in the dataset can be described as singularities by estimating local Lipchitz exponent using wavelet transforms. Defined either at a certain time instant or in an interval, Lipschitz exponent can be calculated by numerical methods. A wavelet transform modulus maximum (WTMM) is introduced to preprocess the sonar data.Letx(t) and ψ(t) be the sonar data and a certain function (introduced next) at instant t, respectively; the wavelet transform can be defined as Wx(b,a)=(1/a)∫-∞∞x(t)·ψ¯((t-b)/a)dt, where - is complex conjugation and a and b describe scale element and shift coefficient, respectively.Mathematically the local regularity indicated by Lipschitz exponent is the precondition for data reconstruction using wavelet transform. Owing to the relationship between the WTMM and Lipschitz exponent, the pattern of change in WTMM at different scales is of great importance for the preprocessing of weak data. At a certain scale, if the maximum modulus exists at some time point, search along the scale decrease direction within the cone of influence will find a singular point or a peak point close to the zero scale, which can be determined by the Lipschitz exponent.


#### 2.2.1. Estimation of Wavelet Coefficients and Compensation of Lost Data

In order to remedy the eliminated sonar data and to guarantee the continuity of the reconstructed contour, the wavelet coefficient estimation method proposed by Liu and Mao [24] is used. More specifically, the original sonar data in previous five instants and the wavelet coefficients in the previous six instants are used for the estimation as follows:
(1)Wx(kT,f)=fT{δ1x[(k-1)T,f]+δ2x[(k-2)T,f]+δ3x[(k-3)T,f]+δ4x[(k-4)T,f]+δ5x[(k-5)T,f]-λ1Wx[(k-1)T,f]-λ2Wx[(k-2)T,f]-λ3Wx[(k-3)T,f]-λ4Wx[(k-4)T,f]-λ5Wx[(k-5)T,f]-λ6Wx[(k-6)T,f]},
where
(2.2.1)ε=e-fT(σ-iw0),λ1=-6ε,δ1=[(σfT)33ggg-(σfT)44+(σfT)55]ε,δ2=[2(σfT)33-5(σfT)43ggg+26(σfT)515]ε2,λ2=15ε2,λ3=-20ε3,δ3=[22·(σfT)55-2(σfT)3]ε3,δ4=[2·(σfT)33+5·(σfT)43ggg+26·(σfT)515]ε4,λ4=15ε4,λ5=-6ε5,λ6=ε6,δ5=[(σfT)33+(σfT)46ggg+(σfT)515]ε5.


#### 2.2.2. Confidence Limit of a Single-Beam Sonar Data

Confidence limit is introduced to assess the degree of match between the wavelet coefficients and the model calculated using the data. Hypothesis test is used to estimate this confidence limit.Assume thatM is the calculated model built using Hidden Markov Model [24]; the null hypothesis H0 and alternative hypothesis H1 are as the follows.H
0: WT is valid data and equals the data calculated using the model. This means the original sonar data are the true signal from threatening obstacles.H
1: WT is an outlier and its corresponding original sonar data are from either low threatening obstacles or noise.The tracking evaluation function is introduced to estimate the confidence limit,(3)Ev(WT,H0,H1)=P{WT∣H0}P{WT∣H1},
where P{WT∣·} is the probability to obtain the same model data with WT in corresponding hypothesis.IfTmin is defined as the acceptable minimum threshold, the ratio between the evaluation function value and Tmin is used to determine the class property of the data; see Table 2.Table 2
Determination of class types.
Ratio
Class type
≥1
Outliers
<1
Valid   data


## 2.1. Collection of Sonar Data

In this paper five single-beam sonars are configured on the vehicle in order to automatically detect the underwater environment: three sonars in the front to detect local environment characteristics and two on the left and the right side for contour reconstruction. The three front sonars are deployed in a way where the middle one is located on the axis of the vehicle surge direction and the other two are installed on the left and right of the middle one with an angle of 7.5 degree, respectively.Data collected from sonars often include useful data describing obstacles and noise from random outliers. To address the “false alarm” problem in vehicle navigation, those outliers have to be detected and eliminated as much as possible. Outliers can be divided into noise patches and objects (e.g., fish swarm) that have less or no threats to the vehicle. Therefore, three different types of objects are defined as follows.(a)
Threaten Obstacle (TO). They are obstacles existing in the environment that can be detected by sonar and have threats to the vehicle, including wreck, reef, and iceberg.(b)
Low Threaten Obstacle(LTO).They are obstacles existing in the environment that can be detected by sonar but have low level of threat to the vehicle, such as suspensions, and fish swarms. LTO can be further fractionized to single LTO (SLTO) distributing as single isolated objects and group LTO (GLTO) with unfixed contour.(c)
Noise.Data is collected from sonar that denotes non-obstacles.With the above definitions, data describing TO is considered to be useful while LTO and noise are regarded as outliers in this paper. LetEt and EΔt be data distribution at instants t and (t+Δt), respectively, and the values of 1 and 0 describe data existence and nonexistence, respectively; Lt express the contour of data class at time t, and the sonar data can be classified as Table 1.Table 1
Metrics to categorize obstacles.
E
t
E
Δ
t
L
t change
Class
1
1
No
TO
1
1
Yes
GLTO
1
0
No contour
Noise or SLTO


## 2.2. Preprocessing of Weak Observable Data Using Wavelet Transform Modulus Maxima

Outliers mixed in the dataset can be described as singularities by estimating local Lipchitz exponent using wavelet transforms. Defined either at a certain time instant or in an interval, Lipschitz exponent can be calculated by numerical methods. A wavelet transform modulus maximum (WTMM) is introduced to preprocess the sonar data.Letx(t) and ψ(t) be the sonar data and a certain function (introduced next) at instant t, respectively; the wavelet transform can be defined as Wx(b,a)=(1/a)∫-∞∞x(t)·ψ¯((t-b)/a)dt, where - is complex conjugation and a and b describe scale element and shift coefficient, respectively.Mathematically the local regularity indicated by Lipschitz exponent is the precondition for data reconstruction using wavelet transform. Owing to the relationship between the WTMM and Lipschitz exponent, the pattern of change in WTMM at different scales is of great importance for the preprocessing of weak data. At a certain scale, if the maximum modulus exists at some time point, search along the scale decrease direction within the cone of influence will find a singular point or a peak point close to the zero scale, which can be determined by the Lipschitz exponent.


### 2.2.1. Estimation of Wavelet Coefficients and Compensation of Lost Data

In order to remedy the eliminated sonar data and to guarantee the continuity of the reconstructed contour, the wavelet coefficient estimation method proposed by Liu and Mao [24] is used. More specifically, the original sonar data in previous five instants and the wavelet coefficients in the previous six instants are used for the estimation as follows:
(1)Wx(kT,f)=fT{δ1x[(k-1)T,f]+δ2x[(k-2)T,f]+δ3x[(k-3)T,f]+δ4x[(k-4)T,f]+δ5x[(k-5)T,f]-λ1Wx[(k-1)T,f]-λ2Wx[(k-2)T,f]-λ3Wx[(k-3)T,f]-λ4Wx[(k-4)T,f]-λ5Wx[(k-5)T,f]-λ6Wx[(k-6)T,f]},
where
(2.2.1)ε=e-fT(σ-iw0),λ1=-6ε,δ1=[(σfT)33ggg-(σfT)44+(σfT)55]ε,δ2=[2(σfT)33-5(σfT)43ggg+26(σfT)515]ε2,λ2=15ε2,λ3=-20ε3,δ3=[22·(σfT)55-2(σfT)3]ε3,δ4=[2·(σfT)33+5·(σfT)43ggg+26·(σfT)515]ε4,λ4=15ε4,λ5=-6ε5,λ6=ε6,δ5=[(σfT)33+(σfT)46ggg+(σfT)515]ε5.


### 2.2.2. Confidence Limit of a Single-Beam Sonar Data

Confidence limit is introduced to assess the degree of match between the wavelet coefficients and the model calculated using the data. Hypothesis test is used to estimate this confidence limit.Assume thatM is the calculated model built using Hidden Markov Model [24]; the null hypothesis H0 and alternative hypothesis H1 are as the follows.H
0: WT is valid data and equals the data calculated using the model. This means the original sonar data are the true signal from threatening obstacles.H
1: WT is an outlier and its corresponding original sonar data are from either low threatening obstacles or noise.The tracking evaluation function is introduced to estimate the confidence limit,(3)Ev(WT,H0,H1)=P{WT∣H0}P{WT∣H1},
where P{WT∣·} is the probability to obtain the same model data with WT in corresponding hypothesis.IfTmin is defined as the acceptable minimum threshold, the ratio between the evaluation function value and Tmin is used to determine the class property of the data; see Table 2.Table 2
Determination of class types.
Ratio
Class type
≥1
Outliers
<1
Valid   data


## 2.2.1. Estimation of Wavelet Coefficients and Compensation of Lost Data

In order to remedy the eliminated sonar data and to guarantee the continuity of the reconstructed contour, the wavelet coefficient estimation method proposed by Liu and Mao [24] is used. More specifically, the original sonar data in previous five instants and the wavelet coefficients in the previous six instants are used for the estimation as follows:
(1)Wx(kT,f)=fT{δ1x[(k-1)T,f]+δ2x[(k-2)T,f]+δ3x[(k-3)T,f]+δ4x[(k-4)T,f]+δ5x[(k-5)T,f]-λ1Wx[(k-1)T,f]-λ2Wx[(k-2)T,f]-λ3Wx[(k-3)T,f]-λ4Wx[(k-4)T,f]-λ5Wx[(k-5)T,f]-λ6Wx[(k-6)T,f]},
where
(2.2.1)ε=e-fT(σ-iw0),λ1=-6ε,δ1=[(σfT)33ggg-(σfT)44+(σfT)55]ε,δ2=[2(σfT)33-5(σfT)43ggg+26(σfT)515]ε2,λ2=15ε2,λ3=-20ε3,δ3=[22·(σfT)55-2(σfT)3]ε3,δ4=[2·(σfT)33+5·(σfT)43ggg+26·(σfT)515]ε4,λ4=15ε4,λ5=-6ε5,λ6=ε6,δ5=[(σfT)33+(σfT)46ggg+(σfT)515]ε5.


## 2.2.2. Confidence Limit of a Single-Beam Sonar Data

Confidence limit is introduced to assess the degree of match between the wavelet coefficients and the model calculated using the data. Hypothesis test is used to estimate this confidence limit.Assume thatM is the calculated model built using Hidden Markov Model [24]; the null hypothesis H0 and alternative hypothesis H1 are as the follows.H
0: WT is valid data and equals the data calculated using the model. This means the original sonar data are the true signal from threatening obstacles.H
1: WT is an outlier and its corresponding original sonar data are from either low threatening obstacles or noise.The tracking evaluation function is introduced to estimate the confidence limit,(3)Ev(WT,H0,H1)=P{WT∣H0}P{WT∣H1},
where P{WT∣·} is the probability to obtain the same model data with WT in corresponding hypothesis.IfTmin is defined as the acceptable minimum threshold, the ratio between the evaluation function value and Tmin is used to determine the class property of the data; see Table 2.Table 2
Determination of class types.
Ratio
Class type
≥1
Outliers
<1
Valid   data


## 3. Initial Contour Reconstruction

The preprocess sonar data by using the wavelet transform can locally amplify abnormal signals or outliers. This observation helps detect and eliminate potential outliers in order to reconstruct the contour of environmental structure. Support vector clustering (SVC) inertial algorithm was used to achieve the initial contour reconstruction.


### 3.1. Structural Environment Construction Using SVC Algorithm

The main idea of SVC is to project sonar data{x1,x2,…,xN} into high-dimension hypersphere space with a minimum radius using nonlinear mapping H={Φ(xi)∣1≤i≤N}. For the mapping data in hypersphere, it holds that ∥Φ(xi)-a∥2≤R2 where a and R denote the center and radius of the sphere. The objective function is described as [25],
(4)max{∑i=1NK(xi,xi)αi-∑i=1,j=1NαiαjK(xi,xj)}.In high dimension space, the distance fromx to the sphere center a, R2=∥Φ(x)-a∥2 is adapted as
(5)R2=K(x,x)-2∑i=1NαiK(xi,x)+∑i=1,j=1NαiαjK(xi,xj),
where hypersphere radius R=∑iR(αi*)/N, a support vector αi*, and the number of support vector N. Class bound of dataset will be collected
(6){x∣R(x)=R}.
Outliers are defined as
(7){x∣R(x)>R}.After the clustering, the sonar data are separated into three classes according to the position with respect to the class bound, shown in Table3. ϑ and P are the relaxation factor and penalty coefficient to balance the performance.Table 3
Criterion of clustering.
Class
Condition
Conclusion
Nonbounded support vector
ϑ
i
=
0, 0<αi<P
Class  contour
Bounded support vector
ϑ
i
>
0, αi=P
Outliers
Data class
α
i
=
0
Class  dataAdjacency matrixA=(Aij)N×N is included to determine type of classes with the distance as its elements:
(8)Aij={1∀t∈seg(xi,xj),R(t)≤R0other,
where seg(xi,xj) expresses the line segment between xi and xj. If the line is either inside or outside the hypersphere, xi and xj are then attributed to the same class. A three-step implementation is as follows.Step  1. k-average interpolation is applied on the line between xi and xj.Step  2. Compute x=∥xi-xj∥·l/k, and let Δx be regarded as the determinant distance and l is the number of lines inside or on the hypersphere.Step  3. If x≥Δx, xi and xj are attributed to the same class; otherwise they are from different classes and will have to be further differentiated in the next subsection.


### 3.2. Inertia Algorithm for Improving Construction

Inertia algorithm is a common action delay method proposed to avoid unnecessary actions of the UUV in order to escape from potentially threatening obstacles [25]. This idea is adopted here to determine the classes of those data that has not been successfully classified. Outliers determined by the SVC algorithm are regarded as candidate outliers. Inertia method is used to determine the class label of those data that have not been explicitly classified. After this step the sonar data are separated into three classes: data class, class bound data, and outliers by using inertia algorithm. The flowchart of inertia algorithm is illustrated in Figure 2.Figure 2
Flow chart of outlier inertia detection for preprocessed data.Letk and ρT, respectively, denote the number of classes and a preset minimum constant threshold. ∀i≥1, if it is satisfied that
(9)∃⋃k{xk∣k>ρT}⊆Ci.Ci is regard as a known class. An alternative set is introduced to temporarily place preoutliers, denoted as AlS.The conditions for classifying the sonar data into the alternative set can be described as follows.(1)
There is not any known class.At the beginning of path planning, there is not enough sonar data to be clustered and to clearly indicate any obstacles. The datax is placed into alternative set AlS. If there is more sonar data in the visible range of x and the density increases to ρT, it means that the dataset constitutes the first class C1. If the density is still lower than ρT with time evolution of Δt, x will be determined as LTO or noise and will not be used to reconstruct the contour of the environment.(2)
If anyCi exists with i≥1, i∈N+, the class may include class data, class bound data, and/or outliers and thus cannot be directly determined. Let X be an arbitrary dataset and described as
(10)X={⋃j=1mxj∣m<ρT,xj∉Cany},where Cany denotes any known class. It implies that once the destiny of sonar data is less than ρT, it will be placed into AlS for further assessment.Assume thatNc is the number of known classes, and Class set C is defined as
(11)C=⋃i∈MCi={x:∃i∈Nc,withx∈Ci}.Outlier set is symbol byO, including the outliers and the data with density less than the preset threshold ρT.Criterion 1. For any sonar data x, if it is satisfied that x∈Li, 1≤i≤M, where Li is the bound of known class Ci, we will have x∈Ci.For arbitrary datax, Criterion 1 is utilized to identify whether it belongs to any known class. If it just falls into bound Li, x is assigned to Ci directly and x∈Ci; otherwise, go to the next criterion. In order to determine the properties of the data, visible space is defined as follows.Visible space is an artificial sphere space centered atx with a radius of r. It is used to estimate distribution density near x. In the visible space, if the density of sonar data is larger than a predefined threshold, we can determine that it is a valid class.Criterion 2. ∀x, x∉Ci, x does not belong to any known class. Assume that V={x∣|x-xa|<r}, with xa as a point in the visible range of x. If the number of data satisfying the condition xa∈Ci is not less than ρT, then Ci is a known class in x’s visible range.Criterion 3. If the number of data in Ci is larger than ρT, and Ci is the unique known class of x, it can be ascertained that x∈Ci directly.To determine the property of anyx that has not been successfully classified by using Criterion 1, Criterion 3 is important to ensure whether it can be assigned to a unique known class. If it fails, reclustering has to be performed.It is observed that the objects for reclusteringCRe only contain the classes including the data point and those in its visible range, and therefore the effectiveness of algorithm can be increased:
(12)CRe={x∣x∉Ci⋂|x-xa|<r}⋃{xa∣xa∈Ci}.Sonar datax with unknown class in its visible range will be located in the alternative set AlS. Similarly, those data that can still not be classified by reclustering are also placed into AlS for further analysis.Criterion 4. ∀1≤i≤M, ∃xi∈AlS, SVC algorithm is applied to cluster and class Cr is obtained. If the density ρR of Cr is larger than ρT, it is considered that Cr⊆C. The total number of M increases by 1. Otherwise, data x (or dataset X) exist which satisfies the following condition:
(13){x∣t>tT}⊆O{X∣TotalnumberofX<ρT,t>tT}⊆O,
where t shows the time related to the data x (or set X).In environmental structure detection, data gradually accumulates. Through preprocessing using wavelet transform and clustering by SVC inertia algorithm, an initial contour can be reconstructed with data class bound as reference. The initial contour consists of several successively connected lines.


## 3.1. Structural Environment Construction Using SVC Algorithm

The main idea of SVC is to project sonar data{x1,x2,…,xN} into high-dimension hypersphere space with a minimum radius using nonlinear mapping H={Φ(xi)∣1≤i≤N}. For the mapping data in hypersphere, it holds that ∥Φ(xi)-a∥2≤R2 where a and R denote the center and radius of the sphere. The objective function is described as [25],
(4)max{∑i=1NK(xi,xi)αi-∑i=1,j=1NαiαjK(xi,xj)}.In high dimension space, the distance fromx to the sphere center a, R2=∥Φ(x)-a∥2 is adapted as
(5)R2=K(x,x)-2∑i=1NαiK(xi,x)+∑i=1,j=1NαiαjK(xi,xj),
where hypersphere radius R=∑iR(αi*)/N, a support vector αi*, and the number of support vector N. Class bound of dataset will be collected
(6){x∣R(x)=R}.
Outliers are defined as
(7){x∣R(x)>R}.After the clustering, the sonar data are separated into three classes according to the position with respect to the class bound, shown in Table3. ϑ and P are the relaxation factor and penalty coefficient to balance the performance.Table 3
Criterion of clustering.
Class
Condition
Conclusion
Nonbounded support vector
ϑ
i
=
0, 0<αi<P
Class  contour
Bounded support vector
ϑ
i
>
0, αi=P
Outliers
Data class
α
i
=
0
Class  dataAdjacency matrixA=(Aij)N×N is included to determine type of classes with the distance as its elements:
(8)Aij={1∀t∈seg(xi,xj),R(t)≤R0other,
where seg(xi,xj) expresses the line segment between xi and xj. If the line is either inside or outside the hypersphere, xi and xj are then attributed to the same class. A three-step implementation is as follows.Step  1. k-average interpolation is applied on the line between xi and xj.Step  2. Compute x=∥xi-xj∥·l/k, and let Δx be regarded as the determinant distance and l is the number of lines inside or on the hypersphere.Step  3. If x≥Δx, xi and xj are attributed to the same class; otherwise they are from different classes and will have to be further differentiated in the next subsection.


## 3.2. Inertia Algorithm for Improving Construction

Inertia algorithm is a common action delay method proposed to avoid unnecessary actions of the UUV in order to escape from potentially threatening obstacles [25]. This idea is adopted here to determine the classes of those data that has not been successfully classified. Outliers determined by the SVC algorithm are regarded as candidate outliers. Inertia method is used to determine the class label of those data that have not been explicitly classified. After this step the sonar data are separated into three classes: data class, class bound data, and outliers by using inertia algorithm. The flowchart of inertia algorithm is illustrated in Figure 2.Figure 2
Flow chart of outlier inertia detection for preprocessed data.Letk and ρT, respectively, denote the number of classes and a preset minimum constant threshold. ∀i≥1, if it is satisfied that
(9)∃⋃k{xk∣k>ρT}⊆Ci.Ci is regard as a known class. An alternative set is introduced to temporarily place preoutliers, denoted as AlS.The conditions for classifying the sonar data into the alternative set can be described as follows.(1)
There is not any known class.At the beginning of path planning, there is not enough sonar data to be clustered and to clearly indicate any obstacles. The datax is placed into alternative set AlS. If there is more sonar data in the visible range of x and the density increases to ρT, it means that the dataset constitutes the first class C1. If the density is still lower than ρT with time evolution of Δt, x will be determined as LTO or noise and will not be used to reconstruct the contour of the environment.(2)
If anyCi exists with i≥1, i∈N+, the class may include class data, class bound data, and/or outliers and thus cannot be directly determined. Let X be an arbitrary dataset and described as
(10)X={⋃j=1mxj∣m<ρT,xj∉Cany},where Cany denotes any known class. It implies that once the destiny of sonar data is less than ρT, it will be placed into AlS for further assessment.Assume thatNc is the number of known classes, and Class set C is defined as
(11)C=⋃i∈MCi={x:∃i∈Nc,withx∈Ci}.Outlier set is symbol byO, including the outliers and the data with density less than the preset threshold ρT.Criterion 1. For any sonar data x, if it is satisfied that x∈Li, 1≤i≤M, where Li is the bound of known class Ci, we will have x∈Ci.For arbitrary datax, Criterion 1 is utilized to identify whether it belongs to any known class. If it just falls into bound Li, x is assigned to Ci directly and x∈Ci; otherwise, go to the next criterion. In order to determine the properties of the data, visible space is defined as follows.Visible space is an artificial sphere space centered atx with a radius of r. It is used to estimate distribution density near x. In the visible space, if the density of sonar data is larger than a predefined threshold, we can determine that it is a valid class.Criterion 2. ∀x, x∉Ci, x does not belong to any known class. Assume that V={x∣|x-xa|<r}, with xa as a point in the visible range of x. If the number of data satisfying the condition xa∈Ci is not less than ρT, then Ci is a known class in x’s visible range.Criterion 3. If the number of data in Ci is larger than ρT, and Ci is the unique known class of x, it can be ascertained that x∈Ci directly.To determine the property of anyx that has not been successfully classified by using Criterion 1, Criterion 3 is important to ensure whether it can be assigned to a unique known class. If it fails, reclustering has to be performed.It is observed that the objects for reclusteringCRe only contain the classes including the data point and those in its visible range, and therefore the effectiveness of algorithm can be increased:
(12)CRe={x∣x∉Ci⋂|x-xa|<r}⋃{xa∣xa∈Ci}.Sonar datax with unknown class in its visible range will be located in the alternative set AlS. Similarly, those data that can still not be classified by reclustering are also placed into AlS for further analysis.Criterion 4. ∀1≤i≤M, ∃xi∈AlS, SVC algorithm is applied to cluster and class Cr is obtained. If the density ρR of Cr is larger than ρT, it is considered that Cr⊆C. The total number of M increases by 1. Otherwise, data x (or dataset X) exist which satisfies the following condition:
(13){x∣t>tT}⊆O{X∣TotalnumberofX<ρT,t>tT}⊆O,
where t shows the time related to the data x (or set X).In environmental structure detection, data gradually accumulates. Through preprocessing using wavelet transform and clustering by SVC inertia algorithm, an initial contour can be reconstructed with data class bound as reference. The initial contour consists of several successively connected lines.


## 4. Smoothing Initial Contour Based on Local Environment Characters

### 4.1. Extraction of Local Environmental Characteristics

Based on the assumption that the data collected from the 5 sonars are accurate, three different local environmental characteristics can be determined, and they are described as follows.


#### 4.1.1. Line Path

Figure3 shows two scenarios where the vehicle is located on the right (Figure 3(a)) and left side (Figure 3(b)) of a local linear environment, respectively. l1, l2, and l3 denote the data collected from the left, middle, and right sonars in the front of the vehicle, respectively, while lland lr are the data collected from the left and right side sonars.Figure 3
Diagram illustrating line path.If the left (resp., right) side sonar data is valid and the right (resp., left) one shows maximum effective distance, then the vehicle is on a line path and on the right (resp., left) side of the contour. Remark: energy carried with an UUV is often limited. When there is no obstacle on one side of the vehicle, the sonar on that side can be turned off in order to conserve energy and extend the working time for the mission.


#### 4.1.2. Narrow Path

If the distance between the environment contours on both sides of the path is not wide enough for the vehicle to turn safely, the vehicle is in a narrow path. Figure4 illustrates this scenario where the distances l12 and l23 are calculated by using the data from the three front sonars. If the sonar data from the left and right side are effective and l12+l23<2dc where dc is the predefined minimum turning radius, the vehicle is in a narrow turning path.Figure 4
Diagram illustrating narrow turn path.Proof.
The triangle inequality theorem states that any one side of a triangle is always shorter than the sum of the other two sides. For the triangle withl12, l23, and l13 as its sides, it holds that
(14)l12+l23>l13.
Letl0 be the line connecting the two sides of the contours; it also holds that
(15)l13≥l0.
Then we have
(16)l0≤l13<l12+l23.
If l12+l23<2dc is satisfied, it holds that l0<2dc. That is, the line is smaller than the turning radius, and the vehicle cannot steer out of this narrow turning environment by normal turning motion. Theorem holds.


#### 4.1.3. Regular Turning Path

The determination of regular turning path is similar to the narrow turning path. If the left side sonar data is effective, the vehicle in regular turning path can be separated into two situations according to the data from the 3 sonars at the front; see Figure5. The left front sonar beam is located on the previous path, with the middle and right ones being located on the rear path in Figure 5(a). The difference between (b) and (a) is the left and middle sonar beam located on the previous path, with the right sonar beam being on the rear path. The stage of UUV turning is reflected. If the sonar data on one side is effective and the other side is ineffective or has the maximum value (this is different to narrow turning path), it can be determined that the vehicle is in a regular turning path.Diagram illustrating normal turn paths.
(a)
(b)


### 4.2. Smoothness of the Local Environmental Contour

The initial contour reconstructed by using the SVC inertia algorithm comprises lines that are successively connected. Due to the inertia property of the underwater vehicle, this cannot be used as the reference path for tracking. In this paper, Bézier curve is introduced to smooth the initial contour in order to extract a reference path that can be used for navigation.Given a set of control pointsP0,P1,…,Pn, ∀t∈[0,1], nth-order Bézier curve can be defined as B(t)=∑i=0nPibi,n(t), with Bernstein Polynomial bi,n(t)=[ni]Tti(1-t)n-i, i=0,1,…,n. Different order Bézier curves are chosen to smooth the initial contour. To determine the curve orders, control points are located first. Considering navigation requirement and the contour characteristics, it is intrinsic that the starting point, end point, and intersection point between turning paths should be the control points. Moreover, the width of the local environment is another significant issue to be considered for the order of Bézier curve.Based on Section4.1, the local environmental characteristics are separated into line and turning paths, and then the orders of different characteristics are classified. It has been proved that 2nd-order Bézier curve is sufficient for turning path restriction [26], and as such Bézier curves used here must satisfy C2 continuity condition.C
k
continuity condition: Bézier curves P(t) and Q(t) in Ck continuous at t0 are,
(17)P(t0)=Q(t0)P˙(t0)=Q˙(t0)⋮P(k)(t0)=Q(k)(t0).Based on the definition ofCk continuity condition, smoothness conditions for n segments can be shown as
(18)Pi-1ni-1=Pi0ni-1(Pi-1ni-1-Pi-1ni-1-1)=ni(Pi1-Pi0)ni-1(ni-1-1)(Pi-1ni-1-2·Pi-1ni-1-1+Pi-1ni-1-2)=ni(ni-1)(Pi2-2·Pi1+Pi0),
where i=2,3,…,2N-3, and ni is the number of control points for the ith Bézier subcurves. The first and last segments are special cases and their orders are discussed first and followed by those ones in the middle.


#### 4.2.1. First and Last Segments

For the first and last segments,B1(t) and B2N-3(t), with the known control points W1=P10 and WN=P2N-34. To find the orders of these curves, the following must hold.Condition 1. The heading ψ0 and ψf at control points W1 and WN must be guaranteed as follows:
(19)p11=W1+c0[cosψ0sinψ0]T,c0∈R+,p2N-33=WN-cf[cosψfsinψf]T,cf∈R+,
where c0 and cf are constant parameters. To satisfy Condition 1, P10 and P11 of the first segment, as well as P2N-33 and P2N-34 of the final segment must be known.Condition 2. C2 continuity condition with contiguous subcurves must be guaranteed. Therefore, the points Pi-1ni-1, Pi-1ni-1-1, and Pi-1ni-1-2 should be known in (18).Given all the above conditions, the total number of the control points required to be known is 5 for the first and last segment, respectively. This implies that 4th-order Bézier curves are needed for both of them.


#### 4.2.2. Middle Segments

To guaranteeC2 continuity condition, for each middle segment it has 6 control points in total: three control points, respectively, from the previous segment (including Pi2, Pi1 and Pi0) and the segment after (including Pi-1ni-1, Pni-1-1, and Pni-1-2) are known (see (30)). Therefore, the order of Bézier subcurves is 5 for the middle segments, iB(t), i=3,5,…,2N-5. Table 4 summarizes the order selection for different local environment characters.Table 4
Orders of Bézier curve.
Local environment character
Order of Bézier curve
Turning
2nd-order
Start and final segments
4th-order
Middle segments
5th-order


## 4.1. Extraction of Local Environmental Characteristics

Based on the assumption that the data collected from the 5 sonars are accurate, three different local environmental characteristics can be determined, and they are described as follows.


### 4.1.1. Line Path

Figure3 shows two scenarios where the vehicle is located on the right (Figure 3(a)) and left side (Figure 3(b)) of a local linear environment, respectively. l1, l2, and l3 denote the data collected from the left, middle, and right sonars in the front of the vehicle, respectively, while lland lr are the data collected from the left and right side sonars.Figure 3
Diagram illustrating line path.If the left (resp., right) side sonar data is valid and the right (resp., left) one shows maximum effective distance, then the vehicle is on a line path and on the right (resp., left) side of the contour. Remark: energy carried with an UUV is often limited. When there is no obstacle on one side of the vehicle, the sonar on that side can be turned off in order to conserve energy and extend the working time for the mission.


### 4.1.2. Narrow Path

If the distance between the environment contours on both sides of the path is not wide enough for the vehicle to turn safely, the vehicle is in a narrow path. Figure4 illustrates this scenario where the distances l12 and l23 are calculated by using the data from the three front sonars. If the sonar data from the left and right side are effective and l12+l23<2dc where dc is the predefined minimum turning radius, the vehicle is in a narrow turning path.Figure 4
Diagram illustrating narrow turn path.Proof.
The triangle inequality theorem states that any one side of a triangle is always shorter than the sum of the other two sides. For the triangle withl12, l23, and l13 as its sides, it holds that
(14)l12+l23>l13.
Letl0 be the line connecting the two sides of the contours; it also holds that
(15)l13≥l0.
Then we have
(16)l0≤l13<l12+l23.
If l12+l23<2dc is satisfied, it holds that l0<2dc. That is, the line is smaller than the turning radius, and the vehicle cannot steer out of this narrow turning environment by normal turning motion. Theorem holds.


### 4.1.3. Regular Turning Path

The determination of regular turning path is similar to the narrow turning path. If the left side sonar data is effective, the vehicle in regular turning path can be separated into two situations according to the data from the 3 sonars at the front; see Figure5. The left front sonar beam is located on the previous path, with the middle and right ones being located on the rear path in Figure 5(a). The difference between (b) and (a) is the left and middle sonar beam located on the previous path, with the right sonar beam being on the rear path. The stage of UUV turning is reflected. If the sonar data on one side is effective and the other side is ineffective or has the maximum value (this is different to narrow turning path), it can be determined that the vehicle is in a regular turning path.Diagram illustrating normal turn paths.
(a)
(b)


## 4.1.1. Line Path

Figure3 shows two scenarios where the vehicle is located on the right (Figure 3(a)) and left side (Figure 3(b)) of a local linear environment, respectively. l1, l2, and l3 denote the data collected from the left, middle, and right sonars in the front of the vehicle, respectively, while lland lr are the data collected from the left and right side sonars.Figure 3
Diagram illustrating line path.If the left (resp., right) side sonar data is valid and the right (resp., left) one shows maximum effective distance, then the vehicle is on a line path and on the right (resp., left) side of the contour. Remark: energy carried with an UUV is often limited. When there is no obstacle on one side of the vehicle, the sonar on that side can be turned off in order to conserve energy and extend the working time for the mission.


## 4.1.2. Narrow Path

If the distance between the environment contours on both sides of the path is not wide enough for the vehicle to turn safely, the vehicle is in a narrow path. Figure4 illustrates this scenario where the distances l12 and l23 are calculated by using the data from the three front sonars. If the sonar data from the left and right side are effective and l12+l23<2dc where dc is the predefined minimum turning radius, the vehicle is in a narrow turning path.Figure 4
Diagram illustrating narrow turn path.Proof.
The triangle inequality theorem states that any one side of a triangle is always shorter than the sum of the other two sides. For the triangle withl12, l23, and l13 as its sides, it holds that
(14)l12+l23>l13.
Letl0 be the line connecting the two sides of the contours; it also holds that
(15)l13≥l0.
Then we have
(16)l0≤l13<l12+l23.
If l12+l23<2dc is satisfied, it holds that l0<2dc. That is, the line is smaller than the turning radius, and the vehicle cannot steer out of this narrow turning environment by normal turning motion. Theorem holds.


## 4.1.3. Regular Turning Path

The determination of regular turning path is similar to the narrow turning path. If the left side sonar data is effective, the vehicle in regular turning path can be separated into two situations according to the data from the 3 sonars at the front; see Figure5. The left front sonar beam is located on the previous path, with the middle and right ones being located on the rear path in Figure 5(a). The difference between (b) and (a) is the left and middle sonar beam located on the previous path, with the right sonar beam being on the rear path. The stage of UUV turning is reflected. If the sonar data on one side is effective and the other side is ineffective or has the maximum value (this is different to narrow turning path), it can be determined that the vehicle is in a regular turning path.Diagram illustrating normal turn paths.
(a)
(b)


## 4.2. Smoothness of the Local Environmental Contour

The initial contour reconstructed by using the SVC inertia algorithm comprises lines that are successively connected. Due to the inertia property of the underwater vehicle, this cannot be used as the reference path for tracking. In this paper, Bézier curve is introduced to smooth the initial contour in order to extract a reference path that can be used for navigation.Given a set of control pointsP0,P1,…,Pn, ∀t∈[0,1], nth-order Bézier curve can be defined as B(t)=∑i=0nPibi,n(t), with Bernstein Polynomial bi,n(t)=[ni]Tti(1-t)n-i, i=0,1,…,n. Different order Bézier curves are chosen to smooth the initial contour. To determine the curve orders, control points are located first. Considering navigation requirement and the contour characteristics, it is intrinsic that the starting point, end point, and intersection point between turning paths should be the control points. Moreover, the width of the local environment is another significant issue to be considered for the order of Bézier curve.Based on Section4.1, the local environmental characteristics are separated into line and turning paths, and then the orders of different characteristics are classified. It has been proved that 2nd-order Bézier curve is sufficient for turning path restriction [26], and as such Bézier curves used here must satisfy C2 continuity condition.C
k
continuity condition: Bézier curves P(t) and Q(t) in Ck continuous at t0 are,
(17)P(t0)=Q(t0)P˙(t0)=Q˙(t0)⋮P(k)(t0)=Q(k)(t0).Based on the definition ofCk continuity condition, smoothness conditions for n segments can be shown as
(18)Pi-1ni-1=Pi0ni-1(Pi-1ni-1-Pi-1ni-1-1)=ni(Pi1-Pi0)ni-1(ni-1-1)(Pi-1ni-1-2·Pi-1ni-1-1+Pi-1ni-1-2)=ni(ni-1)(Pi2-2·Pi1+Pi0),
where i=2,3,…,2N-3, and ni is the number of control points for the ith Bézier subcurves. The first and last segments are special cases and their orders are discussed first and followed by those ones in the middle.


### 4.2.1. First and Last Segments

For the first and last segments,B1(t) and B2N-3(t), with the known control points W1=P10 and WN=P2N-34. To find the orders of these curves, the following must hold.Condition 1. The heading ψ0 and ψf at control points W1 and WN must be guaranteed as follows:
(19)p11=W1+c0[cosψ0sinψ0]T,c0∈R+,p2N-33=WN-cf[cosψfsinψf]T,cf∈R+,
where c0 and cf are constant parameters. To satisfy Condition 1, P10 and P11 of the first segment, as well as P2N-33 and P2N-34 of the final segment must be known.Condition 2. C2 continuity condition with contiguous subcurves must be guaranteed. Therefore, the points Pi-1ni-1, Pi-1ni-1-1, and Pi-1ni-1-2 should be known in (18).Given all the above conditions, the total number of the control points required to be known is 5 for the first and last segment, respectively. This implies that 4th-order Bézier curves are needed for both of them.


### 4.2.2. Middle Segments

To guaranteeC2 continuity condition, for each middle segment it has 6 control points in total: three control points, respectively, from the previous segment (including Pi2, Pi1 and Pi0) and the segment after (including Pi-1ni-1, Pni-1-1, and Pni-1-2) are known (see (30)). Therefore, the order of Bézier subcurves is 5 for the middle segments, iB(t), i=3,5,…,2N-5. Table 4 summarizes the order selection for different local environment characters.Table 4
Orders of Bézier curve.
Local environment character
Order of Bézier curve
Turning
2nd-order
Start and final segments
4th-order
Middle segments
5th-order


## 4.2.1. First and Last Segments

For the first and last segments,B1(t) and B2N-3(t), with the known control points W1=P10 and WN=P2N-34. To find the orders of these curves, the following must hold.Condition 1. The heading ψ0 and ψf at control points W1 and WN must be guaranteed as follows:
(19)p11=W1+c0[cosψ0sinψ0]T,c0∈R+,p2N-33=WN-cf[cosψfsinψf]T,cf∈R+,
where c0 and cf are constant parameters. To satisfy Condition 1, P10 and P11 of the first segment, as well as P2N-33 and P2N-34 of the final segment must be known.Condition 2. C2 continuity condition with contiguous subcurves must be guaranteed. Therefore, the points Pi-1ni-1, Pi-1ni-1-1, and Pi-1ni-1-2 should be known in (18).Given all the above conditions, the total number of the control points required to be known is 5 for the first and last segment, respectively. This implies that 4th-order Bézier curves are needed for both of them.


## 4.2.2. Middle Segments

To guaranteeC2 continuity condition, for each middle segment it has 6 control points in total: three control points, respectively, from the previous segment (including Pi2, Pi1 and Pi0) and the segment after (including Pi-1ni-1, Pni-1-1, and Pni-1-2) are known (see (30)). Therefore, the order of Bézier subcurves is 5 for the middle segments, iB(t), i=3,5,…,2N-5. Table 4 summarizes the order selection for different local environment characters.Table 4
Orders of Bézier curve.
Local environment character
Order of Bézier curve
Turning
2nd-order
Start and final segments
4th-order
Middle segments
5th-order


## 5. Path Tracking Control

Two types of feedback linearization methods are used for path tracking control: differential geometry feedback linearization and stable inversion, which can be used for exactly automatic target approaching in known region and contour reconstruction in unknown region, two stages of the navigating process. This will be detailed in this section.The path tracking control model for a UUV can be described with state vectors as follows:(20)x˙(t)=f(x)+g(x)μ(t),y(t)=[y1y2y3]T=h(x(t)),
where x(t)=[xyψuvr]T∈R6 denotes states, μ(t)=[τuτr]T∈R2 denotes input variable matrix, mapping f(·):R6→R6, g(·):R6→R6×2, and h(·):R6→R3 is smooth enough. f and g are nonlinear items and input coefficient items given in the following:
(21)f=[ucosψ-vsinψ+τduusinψ+vcosψ+τdvr+τdrp1vr+p2up4ur+p5vp7uv+p8r],g=[000000p30000p9],
where p1=m22/m11, p2=(-Xu-Xu|u||u|)/m11, p3=1/m11, p4=m11/m22, p5=(-Yv-Yv|v||v|)/m22, p6=1/m22, p7=(m11-m22)/m33, p8=(-Nr-Nr|r||r|)/m33, p9=1/m33, m11=m-Xu˙, m22=m-Yv˙, and m33=Iz-Nr˙. m is the mass of the UVV; X(·), Y(·), and N(·) are the derivatives of the hydrodynamic coefficients related to the added mass; Iz denotes the initial moment of the UUV under body coordinate system, τu and τr are the input force, and moment τdu, τdv, and τdr are the disturbance at those corresponding directions.


### 5.1. Autonomous Arriving Based on Differential Geometry Feedback Linearization

The UUV is diving underwater with arbitrary states (including heading and position) at any point and has to be able to navigate towards a preset target near the structural environment. This is known asAutonomous Arriving. A differential geometry feedback linearization control algorithm with rolling path guidance is proposed to achieve accurate tracking. Figure 6 illustrates how to find the initial points: circle arc is used to represent rolling path according to the initial heading and orientation of the vehicle, Ω and Ωt represent the desired path and rolling path, respectively, ψ and ψd are the current heading direction and desired heading direction, respectively, and T denotes the tangent point of Ω and Ωt, which is the initial point on Ω for the vehicle. Note, the UUVs in Figure 6 have the same initial positions but different headings (upward and downward in Figures 6(a) and 6(b), resp.). Given the desired orientation and the inertia of the vehicle, the vehicle is unable to turn at T in the scenario as shown in Figure 6(b). Further analysis is required and detailed as follows.Rolling path with different orientations to find initial points on reference path.
(a)
(b)Let model output beη=[η1η2]T, η1 is the distance of the chosen path, and η2=ψ.It has been proved that any nonlinear path can be reconstructed by circular arcs and lines [27]. Therefore, with circle and line as example, rolling paths are generated as the desired path for the guidance of the UUV.(1)
Circle Path. Assume thatpf(xf,yf) is the center of a circle path and the radius of the circle path Rf is a constant; then the output can be given as
(22)η1=(x-xf)2+(y-yf)2-R,where pL(x,y) is the actual initial position of the vehicle. From the equation above, |η1| is the minimum distance between pL and the circle path.According to the position between the desired path and the current position of the vehicle, radius of a rolling path can be derived as follows:(a)
when the initial position of the UUV is outside of the circular path (see Figure7(a)) and ϖ is a constant
(23)Rt={η1+ϖRη1>R2Rothers,(b)
when the initial position of the UUV is inside of the circular path (see Figure7(b))
(24)Rt={η1+2R2-ϖRpL≠pfR2others.(2)
Line Path. With line pathAx+By+C=0, the output η1 is given as follows,
(25)η1=(Ax+By+C)A2+B2,where A2+B2≠0. In this case Rt=ϖη1+R0 and R0 is a constant.Rolling path generation for three different situations. (a) UUV is outside circle path. (b) UUV is outside circle path. (c) UUV is outside line path.
(a)
(b)
(c)Assumption 1.
UUV tracking system satisfiesm11<m22 and the surge and yaw velocity are nonobservable.Figure7 illustrates how the rolling path is generated in three different cases as discussed above. The solid and dashed circles are tangent to the rolling circular path across the vehicle position pL. According to the consistency between the heading direction of the vehicle and the desired path, the solid arc pLp^ will be chosen as the rolling path while the orientation of the vehicle becomes close to the direction of the desired path. Once a rolling path is generated as above, a feedback linearization controller can be designed for automatic arriving.Theorem 1.
Assume (i) the mass, added mass, and damping coefficients are diagonal matrixes; (ii) Assumption1 is satisfied. For the nonlinear tracking model (20), according to the relative position between the vehicle and the originally desired path, if a circular arc rolling path with radius Rt is chosen in real time with suitable parameters (k11,k12) and (k21,k22), the controller can be designed.Proof.
The lines and circular arcs can be combined to form any nonlinear path; therefore, the proof is established from the following two aspects.(1)
Desired path is a circle path.
Obtaining a direct relationship between the outputη and the input vector,
(26)η˙1=1(x-xf)2+(y-yf)2·[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]η˙2=ψ˙η¨1=-0.5ek3×[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]2+ek[(x˙-x˙f)2+(x-xf)(x¨-x¨f)]+ek[(y˙-y˙f)2+(y-yf)(y¨-y¨f)]η¨2=r˙,
where
(27)ek=1(x-xf)2+(y-yf)2.
Define state functionh=[h1h2]T, and
(28)u1=p1vr+p2u+p3τduv1=p4ur+p5v+p6τdvh1=-0.5ek3hh×[(x˙-x˙f)(x-xf)+(y˙-y˙f)(y-yf)]2hh+(x-xf)(x¨fu1cosψ-ursinψhhhhhhhh5hhh-v1sinψ-vrcosψ-x¨f)hh+ek·{(x˙-x˙f)2+(y˙-y˙f)2hhhhhhhh+(y-yf)(y¨fu1sinψ+urcosψhhhhhh5555555555hh+v1cosψ-vrsinψ-y¨f)(x˙-x˙f)2}h2=p7uv+p8r+p9τdrη¨=h+[ek·(p3τu((x-xf)cosψ+(y-yf)sinψ))p9τr].
Choose a new inputμ=[ν1ν2]T and neglect nonlinear portion; let e=(x-xf)2+(x-xf)2-R be the tracking error, and the input vector can be derived as follows:
(29)ν1=-k11e1-k12e˙1=-k11×((x-xf)2+(y-yf)2-R)-k12[1(x-xf)2+(y-yf)2]·[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]ν2=η¨2d-k21e2-k22e˙2=ψ¨d-k21(ψ-ψd)-k22(ψ˙-ψ˙d),
where k11, k12, k21, and k22 are constants, η2d=ψd.
The input vector can be shown as(30)τu=(ν1-h1)p3ek[(x-xf)cosψ+(y-yf)sinψ]τr=(ν2-h2)p9.(2)
Desired path is line path.
The proving process is similar to the above:(31)η˙1=(Ax˙+By˙+C)A2+B2η˙2=ψ˙η¨1=u˙(Acosψ+Bsinψ)+v˙(Bcosψ-Asinψ)A2+B2hhh+A(-ursinψ-vrcosψ)A2+B2hhh+B(urcosψ-vrsinψ)+CA2+B2η¨2=r˙h1=tk[A(-ursinψ-vrcosψ)jjffj+B(urcosψ-vrsinψ)+Cjjffj+u1(Acosψ+Bsinψ)jjffj+v1(Bcosψ-Asinψ)]h2=p7uv+p8r+p9τdrη¨=h+[tkp3(Acosψ+Bsinψ)τup9τr]
with tk=1/A2+B2.
Lete1=tk(Ax+By+C) be the tracking error and e2=η2d-η2 the heading error; the new inputs are described as
(32)ν1=-k11e1-k21e˙1jjj=-k11tk(Ax+By+C)gggg-k21tk(Ax˙+By˙)ν2=η˙d2-k12e2-k22e˙2jjj=ψ¨d-k12(ψ-ψd)-k22(ψ˙-ψ˙d).
The control output can be shown as(33)τu=(ν1-h1)[p3tk(Acosψ+Bsinψ)]τr=(ν2-h2)p9


### 5.2. Contour Reconstruction Based on Finite Predictive Stable Inversion

For∀ε>0, a predictive time instant Tp can be found to obtain stable input μd(tc) using future output yd(t), t∈[tc,tc+Tp]. It satisfies
(34)∥μd(tc)-μp(tc)∥≤ε,
where μp(tc) is the desired input. The inner dynamic condition is given as
(35)L≜[Ls(tc)Lu(tf)]≜[σs(tc)σu(tf)],tf=tc+Tp.A finite predictive path is regarded as known variables in the time window[tc,tc+Tp]. The desired output in this time window is utilized to describe the stable and unstable part of the inner dynamics. Picard iteration method [10] is introduced to solve the bounded solution of both parts.Given the nonlinear tracking model (20), the linearity item of inner dynamics σ(t) is extracted to obtain a bounded solution.LetAσ=∂s(σ,Yd)/∂σ|Yd=0,σ=0, and the linearity part σ-˙ can be resolved from σ(t) as σ-˙=Aσσ. The inner dynamics can be described as
(36)σ˙(t)=Aσσ(t)+[s(σ(t),Yd(t))-Aσσ(t)]≜Aσσ(t)+Ψ(σ(t),Yd(t)).A
σ can be further separated into a stable part As and an unstable part Au, which, respectively, denote the characteristic values in the left and right planes of the imaginary axis of the complex plane. Thus, the inner dynamics equations can be rewritten as
(37)σ˙s=Asσs+IsΨs([σsσu]T,Yd)≜Asσs+Ψs(σs,σu,Yd)σ˙u=Auσu+IuΨu([σuσu]T,Yd)≜Auσu+Ψu(σs,σu,Yd).With Picard iteration method, the bounded solution for both parts of the inner dynamic can be derived as(38)σ˙s,k(t)=Asσs,k(t)+Ψs(σs,k-1(t),σu,k-1(t),Yd(t))σ˙u,k(t)=Auσu,k(t)+Ψu(σs,k-1(t),σu,k-1(t),Yd(t)).For anyt∈[tc,tf], the bounded solution can be described as follows:(1)
Initial solution is shown withk=0,
(39)σ0(t)=[eAs(t-tc)Ls(tc)e-Au(tf-t)Lu(tf)].(2)
Withk≥1,(40)σk(t)=[σs,k(t)σu,k(t)]=[eAs(t-tc)Ls(tc)+∫tcteAs(t-τ)Ψs(σs,k-1(τ),σu,k-1(τ),Yd(τ))dτe-Au(tf-t)Lu(tf)-∫ttfe-Au(τ-t)Ψu(σs,k-1(τ),σu,k-1(τ),Yd(τ))dτ]≜SM(σk-1(·),Yd(·),L)(t).It is clear that the integration operation for the stable and unstable inner dynamics are from current time instanttc to t forward and backward, respectively.Assumption 2.
Inner dynamics characterΨ(·,·) is a nonlinear item, and it is satisfied that local Lipschitz condition at origin with any constants (K1, K2) and any small positive constant σ0 hold that, for any t∈R, a bounded function exists
(41)∥Ψ(σ(t),Y(t))-Ψ(σ-(t),Y-(t))∥∞≤K1∥σ(t)-σ-(t)∥∞+K2∥Y(t)-Y-(t)∥∞,
where
(42)∥Y(·)∥∞<σ0,∥Y-(·)∥∞<σ0,∥σ-(·)∥∞<σ0,∥σ(·)∥∞<σ0.Remark. Nonlinearity for inner dynamics will decrease with the decrease of Lipchitz constants K1 and K2. If Ψ(·,·) is continuous and differentiable, the locally Lipchitz condition is satisfied.Assumption 3.
A positive constantK exists to hold that
(43)∥Φs(t,t0)∥∞≤Ke-α(t-t0)∀t0≤t≤tf∥Φu(tf,t)∥∞≤Ke-β(tf-t)∀t0≤t≤tf,
where α and β are minimum character values of As and Au, respectively,
(44)α<infi|Re(λi(As))|β<infi|Re(λi(Au))|,
where “inf” denotes the lower bound, “Re” means the real part, and λi(As) and λi(Au) are characteristic values of As and Au, respectively.Remark. The large α and β are, the further is the distance from dynamics poles to the imaginary axis implying stronger hyperbolic properties.Theorem 2.
If Assumptions1 and 2 are satisfied and there exist positive constants Ky and Kσ, it holds that
(45)∥μd(σ(t),Y(t))-μd(σ-(t),Y-(t))∥∞≤Ky∥Y(t)-Y-(t)∥∞+Kσ∥σ(t)-σ-(t)∥∞.
If Lipchitz constantsK1 and K2 in (41) satisfy (K1+K2)/2<1, there is only one fixed point σ*(·) to hold that σ*(t)=s[σ*(·),Yd(·)](t) for any ∀t∈[tc,tc+Tp]. The error between the desired input μe,d(tc) and the input μd,m(tc) derived from finite predictive inversion can be quantitated as
(46)ed,m(tc)≜∥μe,d(tc)-μd,m(tc)∥∞≤KσKKαβ[2KKαβK2+e-β^Tp1-δβ]∥Yd(·)∥∞,
where
(47)Kαβ=Kαβ,2(1-Kαβ,1)Kαβ,1=KK1max{(1/α),(1/β)}Kαβ,2=KK2max{(1/α),(1/β)}.The proof is not included here due to space limit. The reader is referred to the original paper for details [21]. According to Theorem 2, the parameter Tp can be optimized and thus the time window.


## 5.1. Autonomous Arriving Based on Differential Geometry Feedback Linearization

The UUV is diving underwater with arbitrary states (including heading and position) at any point and has to be able to navigate towards a preset target near the structural environment. This is known asAutonomous Arriving. A differential geometry feedback linearization control algorithm with rolling path guidance is proposed to achieve accurate tracking. Figure 6 illustrates how to find the initial points: circle arc is used to represent rolling path according to the initial heading and orientation of the vehicle, Ω and Ωt represent the desired path and rolling path, respectively, ψ and ψd are the current heading direction and desired heading direction, respectively, and T denotes the tangent point of Ω and Ωt, which is the initial point on Ω for the vehicle. Note, the UUVs in Figure 6 have the same initial positions but different headings (upward and downward in Figures 6(a) and 6(b), resp.). Given the desired orientation and the inertia of the vehicle, the vehicle is unable to turn at T in the scenario as shown in Figure 6(b). Further analysis is required and detailed as follows.Rolling path with different orientations to find initial points on reference path.
(a)
(b)Let model output beη=[η1η2]T, η1 is the distance of the chosen path, and η2=ψ.It has been proved that any nonlinear path can be reconstructed by circular arcs and lines [27]. Therefore, with circle and line as example, rolling paths are generated as the desired path for the guidance of the UUV.(1)
Circle Path. Assume thatpf(xf,yf) is the center of a circle path and the radius of the circle path Rf is a constant; then the output can be given as
(22)η1=(x-xf)2+(y-yf)2-R,where pL(x,y) is the actual initial position of the vehicle. From the equation above, |η1| is the minimum distance between pL and the circle path.According to the position between the desired path and the current position of the vehicle, radius of a rolling path can be derived as follows:(a)
when the initial position of the UUV is outside of the circular path (see Figure7(a)) and ϖ is a constant
(23)Rt={η1+ϖRη1>R2Rothers,(b)
when the initial position of the UUV is inside of the circular path (see Figure7(b))
(24)Rt={η1+2R2-ϖRpL≠pfR2others.(2)
Line Path. With line pathAx+By+C=0, the output η1 is given as follows,
(25)η1=(Ax+By+C)A2+B2,where A2+B2≠0. In this case Rt=ϖη1+R0 and R0 is a constant.Rolling path generation for three different situations. (a) UUV is outside circle path. (b) UUV is outside circle path. (c) UUV is outside line path.
(a)
(b)
(c)Assumption 1.
UUV tracking system satisfiesm11<m22 and the surge and yaw velocity are nonobservable.Figure7 illustrates how the rolling path is generated in three different cases as discussed above. The solid and dashed circles are tangent to the rolling circular path across the vehicle position pL. According to the consistency between the heading direction of the vehicle and the desired path, the solid arc pLp^ will be chosen as the rolling path while the orientation of the vehicle becomes close to the direction of the desired path. Once a rolling path is generated as above, a feedback linearization controller can be designed for automatic arriving.Theorem 1.
Assume (i) the mass, added mass, and damping coefficients are diagonal matrixes; (ii) Assumption1 is satisfied. For the nonlinear tracking model (20), according to the relative position between the vehicle and the originally desired path, if a circular arc rolling path with radius Rt is chosen in real time with suitable parameters (k11,k12) and (k21,k22), the controller can be designed.Proof.
The lines and circular arcs can be combined to form any nonlinear path; therefore, the proof is established from the following two aspects.(1)
Desired path is a circle path.
Obtaining a direct relationship between the outputη and the input vector,
(26)η˙1=1(x-xf)2+(y-yf)2·[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]η˙2=ψ˙η¨1=-0.5ek3×[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]2+ek[(x˙-x˙f)2+(x-xf)(x¨-x¨f)]+ek[(y˙-y˙f)2+(y-yf)(y¨-y¨f)]η¨2=r˙,
where
(27)ek=1(x-xf)2+(y-yf)2.
Define state functionh=[h1h2]T, and
(28)u1=p1vr+p2u+p3τduv1=p4ur+p5v+p6τdvh1=-0.5ek3hh×[(x˙-x˙f)(x-xf)+(y˙-y˙f)(y-yf)]2hh+(x-xf)(x¨fu1cosψ-ursinψhhhhhhhh5hhh-v1sinψ-vrcosψ-x¨f)hh+ek·{(x˙-x˙f)2+(y˙-y˙f)2hhhhhhhh+(y-yf)(y¨fu1sinψ+urcosψhhhhhh5555555555hh+v1cosψ-vrsinψ-y¨f)(x˙-x˙f)2}h2=p7uv+p8r+p9τdrη¨=h+[ek·(p3τu((x-xf)cosψ+(y-yf)sinψ))p9τr].
Choose a new inputμ=[ν1ν2]T and neglect nonlinear portion; let e=(x-xf)2+(x-xf)2-R be the tracking error, and the input vector can be derived as follows:
(29)ν1=-k11e1-k12e˙1=-k11×((x-xf)2+(y-yf)2-R)-k12[1(x-xf)2+(y-yf)2]·[(x-xf)(x˙-x˙f)+(y-yf)(y˙-y˙f)]ν2=η¨2d-k21e2-k22e˙2=ψ¨d-k21(ψ-ψd)-k22(ψ˙-ψ˙d),
where k11, k12, k21, and k22 are constants, η2d=ψd.
The input vector can be shown as(30)τu=(ν1-h1)p3ek[(x-xf)cosψ+(y-yf)sinψ]τr=(ν2-h2)p9.(2)
Desired path is line path.
The proving process is similar to the above:(31)η˙1=(Ax˙+By˙+C)A2+B2η˙2=ψ˙η¨1=u˙(Acosψ+Bsinψ)+v˙(Bcosψ-Asinψ)A2+B2hhh+A(-ursinψ-vrcosψ)A2+B2hhh+B(urcosψ-vrsinψ)+CA2+B2η¨2=r˙h1=tk[A(-ursinψ-vrcosψ)jjffj+B(urcosψ-vrsinψ)+Cjjffj+u1(Acosψ+Bsinψ)jjffj+v1(Bcosψ-Asinψ)]h2=p7uv+p8r+p9τdrη¨=h+[tkp3(Acosψ+Bsinψ)τup9τr]
with tk=1/A2+B2.
Lete1=tk(Ax+By+C) be the tracking error and e2=η2d-η2 the heading error; the new inputs are described as
(32)ν1=-k11e1-k21e˙1jjj=-k11tk(Ax+By+C)gggg-k21tk(Ax˙+By˙)ν2=η˙d2-k12e2-k22e˙2jjj=ψ¨d-k12(ψ-ψd)-k22(ψ˙-ψ˙d).
The control output can be shown as(33)τu=(ν1-h1)[p3tk(Acosψ+Bsinψ)]τr=(ν2-h2)p9


## 5.2. Contour Reconstruction Based on Finite Predictive Stable Inversion

For∀ε>0, a predictive time instant Tp can be found to obtain stable input μd(tc) using future output yd(t), t∈[tc,tc+Tp]. It satisfies
(34)∥μd(tc)-μp(tc)∥≤ε,
where μp(tc) is the desired input. The inner dynamic condition is given as
(35)L≜[Ls(tc)Lu(tf)]≜[σs(tc)σu(tf)],tf=tc+Tp.A finite predictive path is regarded as known variables in the time window[tc,tc+Tp]. The desired output in this time window is utilized to describe the stable and unstable part of the inner dynamics. Picard iteration method [10] is introduced to solve the bounded solution of both parts.Given the nonlinear tracking model (20), the linearity item of inner dynamics σ(t) is extracted to obtain a bounded solution.LetAσ=∂s(σ,Yd)/∂σ|Yd=0,σ=0, and the linearity part σ-˙ can be resolved from σ(t) as σ-˙=Aσσ. The inner dynamics can be described as
(36)σ˙(t)=Aσσ(t)+[s(σ(t),Yd(t))-Aσσ(t)]≜Aσσ(t)+Ψ(σ(t),Yd(t)).A
σ can be further separated into a stable part As and an unstable part Au, which, respectively, denote the characteristic values in the left and right planes of the imaginary axis of the complex plane. Thus, the inner dynamics equations can be rewritten as
(37)σ˙s=Asσs+IsΨs([σsσu]T,Yd)≜Asσs+Ψs(σs,σu,Yd)σ˙u=Auσu+IuΨu([σuσu]T,Yd)≜Auσu+Ψu(σs,σu,Yd).With Picard iteration method, the bounded solution for both parts of the inner dynamic can be derived as(38)σ˙s,k(t)=Asσs,k(t)+Ψs(σs,k-1(t),σu,k-1(t),Yd(t))σ˙u,k(t)=Auσu,k(t)+Ψu(σs,k-1(t),σu,k-1(t),Yd(t)).For anyt∈[tc,tf], the bounded solution can be described as follows:(1)
Initial solution is shown withk=0,
(39)σ0(t)=[eAs(t-tc)Ls(tc)e-Au(tf-t)Lu(tf)].(2)
Withk≥1,(40)σk(t)=[σs,k(t)σu,k(t)]=[eAs(t-tc)Ls(tc)+∫tcteAs(t-τ)Ψs(σs,k-1(τ),σu,k-1(τ),Yd(τ))dτe-Au(tf-t)Lu(tf)-∫ttfe-Au(τ-t)Ψu(σs,k-1(τ),σu,k-1(τ),Yd(τ))dτ]≜SM(σk-1(·),Yd(·),L)(t).It is clear that the integration operation for the stable and unstable inner dynamics are from current time instanttc to t forward and backward, respectively.Assumption 2.
Inner dynamics characterΨ(·,·) is a nonlinear item, and it is satisfied that local Lipschitz condition at origin with any constants (K1, K2) and any small positive constant σ0 hold that, for any t∈R, a bounded function exists
(41)∥Ψ(σ(t),Y(t))-Ψ(σ-(t),Y-(t))∥∞≤K1∥σ(t)-σ-(t)∥∞+K2∥Y(t)-Y-(t)∥∞,
where
(42)∥Y(·)∥∞<σ0,∥Y-(·)∥∞<σ0,∥σ-(·)∥∞<σ0,∥σ(·)∥∞<σ0.Remark. Nonlinearity for inner dynamics will decrease with the decrease of Lipchitz constants K1 and K2. If Ψ(·,·) is continuous and differentiable, the locally Lipchitz condition is satisfied.Assumption 3.
A positive constantK exists to hold that
(43)∥Φs(t,t0)∥∞≤Ke-α(t-t0)∀t0≤t≤tf∥Φu(tf,t)∥∞≤Ke-β(tf-t)∀t0≤t≤tf,
where α and β are minimum character values of As and Au, respectively,
(44)α<infi|Re(λi(As))|β<infi|Re(λi(Au))|,
where “inf” denotes the lower bound, “Re” means the real part, and λi(As) and λi(Au) are characteristic values of As and Au, respectively.Remark. The large α and β are, the further is the distance from dynamics poles to the imaginary axis implying stronger hyperbolic properties.Theorem 2.
If Assumptions1 and 2 are satisfied and there exist positive constants Ky and Kσ, it holds that
(45)∥μd(σ(t),Y(t))-μd(σ-(t),Y-(t))∥∞≤Ky∥Y(t)-Y-(t)∥∞+Kσ∥σ(t)-σ-(t)∥∞.
If Lipchitz constantsK1 and K2 in (41) satisfy (K1+K2)/2<1, there is only one fixed point σ*(·) to hold that σ*(t)=s[σ*(·),Yd(·)](t) for any ∀t∈[tc,tc+Tp]. The error between the desired input μe,d(tc) and the input μd,m(tc) derived from finite predictive inversion can be quantitated as
(46)ed,m(tc)≜∥μe,d(tc)-μd,m(tc)∥∞≤KσKKαβ[2KKαβK2+e-β^Tp1-δβ]∥Yd(·)∥∞,
where
(47)Kαβ=Kαβ,2(1-Kαβ,1)Kαβ,1=KK1max{(1/α),(1/β)}Kαβ,2=KK2max{(1/α),(1/β)}.The proof is not included here due to space limit. The reader is referred to the original paper for details [21]. According to Theorem 2, the parameter Tp can be optimized and thus the time window.


## 6. Evaluations and Results

### 6.1. Evaluation Criteria

Evaluation criteria are set out for assessing the contour accuracy and predictive controlperformance towards detection mission using the UUV under weak observable conditions, respectively.


#### 6.1.1. Evaluation Criterion for Contour Accuracy

Errors between the reconstructed contour and the environment model are computed in order to evaluate the accuracy of the contour reconstruction (only the contours estimated using the SVC inertia algorithm are evaluated here). Considering the characteristics of the environment, “accumulate error” and “overall error” are proposed. Figure8 illustrates how these errors are computed where the solid lines express the actual contours of the environment model while the dashed lines indicate the contour reconstructed by using the SVC inertia algorithm.Figure 8
Illustration of the accumulate error and overall error.(1)Accumulative ErrorΔda. The error between the contour of the environment model and the reconstructed one can accumulate over time, and the accumulative error is used to evaluate this trend.As shown in Figure8, d1, d3, and d5 are the deflections between the environment contours and the reconstructed smooth path at the starting point of each segment. d2, d4, and d6 are the deflections at the end point of each segment. Assume that ds is the safe distance, and the errors of each segment Δda1, Δda2, and Δda3 are described as follows:
(48)Δda1=(d2-ds)-(d1-ds)=d2-d1Δda2=(d4-ds)-(d3-ds)=d4-d3Δda3=(d6-ds)-(d5-ds)=d6-d5.
The accumulative error can be defined as follows:
(49)Δda=∑i=1NΔdai/Ni=1,2,3.(2)Overall ErrorΔdw. The entire environment contour is reconstructed and the overall error Δdw at the end point is defined as
(50)Δdw=d6-ds.Remark. Both Δda and Δdw have signed values. A positive value means the deflection is larger than the safe distance while a negative value means the deflection is smaller than the safe distance implying a threat to the vehicle navigation.


#### 6.1.2. Evaluation Criteria for Autonomous Tracking Control

To estimate the performance of autonomous tracking control, the error is defined asek=y(k)-y^(k) where y(k) is the navigating path of the vehicle and y^(k) the smoothed contour from reconstruction. More specifically, the error at the k instant is the minimum distance from the vehicle position to the contour, that is, the tangent point on the contour. Three measures are proposed to analyze the errors.(1)Zero Mean Value. Define e~=(1/n)∑k=1nek as the estimated value of error serial {ek}. To guarantee the zero mean value, for any n∈Z+, it is satisfied that e~→0. From the definition of error, it will be affected by the estimated mean values of the output y~; therefore, reestimation of the mean value e-=e~/y~<eT is needed. eT is preset on the basis of accuracy requirement.(2)Validity. It is defined as the degree of deflection between the error serial {ek} and the zero mean value. Standard deviation σ=(1/(n-1))∑k=1n(ek-e-)2 is introduced to estimate the data validity.(
3
)
Independence. The error serial describes the error between the real navigating path and the smoothed contour after subtracting the safe distance. Error variables are regarded as random variables and thus the independence describes the randomness. Autocorrelation coefficient rp is used to describe the independence as follows:
(51)rp=1(n-p-1)∑k=1n-p(ΔekΔep+k),
where Δek=ek-e-k. In this paper the autocorrelation coefficient is rewritten as ρp=rp/r0, and the relation between ρp-k will be plotted for demonstration. If autocorrelation coefficients are in the Independence Confidence Limit Interval (-σ,σ), the error serial is not correlated and vice versa.


### 6.2. Validation Using Sea Trial Data

The performance of the proposed model is verified with the data collected from a sea trial operated at Xiaoping Island in Dalian, China, in August 2009. We choose sonar data collected from two different environments, respectively, a port and two islands. The single-beam sonars were assembled on one side of a fishing boat to simulate UUV sonars. To guarantee the number of sonar data satisfying the clustering requirement, 3 single beam sonars were fixed in the boat side; see Figure9. The sonars used in the sea trial are manufactured by Kongsberg, Norway. The boat was steered along the contour with acoustic beam vertically acting on environment. The sonars were deployed below the bottom of the boat to avoid acoustic beam to project onto the boat. The boat sails at a velocity of 2 m/s.Figure 9
Deployment of the sonars.Sonar data are collected during the boat navigating along islands horizontally in real time. To verify the effectiveness of the proposed control algorithm in disturbance situation, synthetic disturbance noise is added:tdu=40×(1+rand(·)), τdv=35×(1+rand(·)), τdr=40×(1+rand(·)), withrand(·) denoting zero-mean Gaussian random noise. From Figure 10 the sonar assembly in the sea can be seen, with diving in the water as Figure 10(a) and navigating as Figure 10(b).Sonar data collection in sea trial.
(a)
(b)


#### 6.2.1. Path Following Validation Using Data from Islands

In the first experiment, the boat navigates between two small islands in order to move close to them (see Figure11). We record the data from initial point A (−30, −245) with heading of 60 degree. The effective distance of sonars is 200 m. Figure 12 shows the process of data clustering. At the beginning, the number of data is too small to construct accurate data class; therefore, data are placed into alternative set; see Figure 12(a). With the number of sonar data increasing, data with the same property, including data in alternative set, are clustered in corresponding class contours with support vectors distributed on. Sonar data determined by the rules in Section 3 are eliminated as outliers (see Figures 12(b) and 12(c)). The contours close to UUV, determined by normal line and distance, connected with lines successively, are regarded as preliminary contour, waiting for postprocessing.Figure 11
Sea trial in islands environment.Islands contour construction and patrolling.
(a)
(b)
(c)On the basis of initial contour (see solid green line in Figures12(b) and 12(c)), the smoothing method described in Section 4.2 is applied and a reasonable desired reference path (dash black curves) for vehicle navigation can be achieved. When the vehicle is close to the island on the right less than 200 m, sonar data returned is collected to construct the contour in real time which is regarded as the guidance for the vehicle to navigate, keeping a fixed distance (set to 10 m in this paper) as the safe distance. The optimal time window parameter is Tp≥3 s. Considering the control period 0.5 s, Tp=3 s satisfies the requirement (5 original sonar data and 6 wavelet coefficients) to estimate wavelet coefficients at any time. According to Section 6.1, the error series denoting the deflection between the actual navigation path and the reconstructed contour is computed; see Figure 13.Figure 13
Patrolling errors atx- and y-axis in islands contour.The mean (standard deviation) value alongx- and y-axis is 0.15 (0.37) m and 0.98 (0.30) m, respectively. According to evaluation criterion for contour accuracy in Section 6.1.1, the accumulated error for island contour is 1.45 m, and overall error is 1.74 m. The negative deflection values are much smaller than the safe distance; this means that the UUV can safely complete the detection mission as designed. The maximum absolute value of the autocorrelation coefficients at x-axis and y-axis is 0.14 and 0.16, respectively, while the respective standard deviations are 0.37 and 0.30 (see Figure 14). This confirms that the error series is independent without influence from the designed controller. This means that the designed controller using finite predictive stable inversion algorithm can control the vehicle to follow a path predicted in real time.Figure 14
Error autocorrelation coefficients of error serials.


#### 6.2.2. Path Tracking Validation Using Data from Port

With the same deployment and trial methods as above, sonar data was collected in a port as Figure15 with its environment pictures at the right. The initial position of the boat was (−1605, −688). Owing to the port area is wide and safe distance is set to 30 m. Figure 16 demonstrates the autonomous tracking of the boat in the port. From Figure 16, it can be seen that the smoothed path illustrated by black solid is reconstructed using data from 3 sonars and guides the vehicle to approach the target. The noise in the data is detected and eliminated to guarantee the accuracy of the contour.Figure 15
Sea trial in port environment.Figure 16
Port contour construction and tracking.Figure17 illustrates the error of the smoothed path reconstructed and the actual path without consideration of the safe distance. Figure 18 shows the autocorrelation coefficients of the tracking error series to assess whether the errors are from the control method or stochastic disturbance. The maximum absolute value of autocorrelation coefficients at x-axis and y-axis are 0.20 and 0.20, within the range of standard deviations, respectively, (−0.26, 0.26) and (−0.38, 0.38) (see Figure 17). Therefore, it is true that error series are caused stochastically. Moreover, the accumulated error for island contour tracking is 1.50 m, and overall error is 1.20 m, respectively.Figure 17
Path error serials atx- and y-axis for patrolling port contour.Figure 18
Error autocorrelation coefficients of error serials.


## 6.1. Evaluation Criteria

Evaluation criteria are set out for assessing the contour accuracy and predictive controlperformance towards detection mission using the UUV under weak observable conditions, respectively.


### 6.1.1. Evaluation Criterion for Contour Accuracy

Errors between the reconstructed contour and the environment model are computed in order to evaluate the accuracy of the contour reconstruction (only the contours estimated using the SVC inertia algorithm are evaluated here). Considering the characteristics of the environment, “accumulate error” and “overall error” are proposed. Figure8 illustrates how these errors are computed where the solid lines express the actual contours of the environment model while the dashed lines indicate the contour reconstructed by using the SVC inertia algorithm.Figure 8
Illustration of the accumulate error and overall error.(1)Accumulative ErrorΔda. The error between the contour of the environment model and the reconstructed one can accumulate over time, and the accumulative error is used to evaluate this trend.As shown in Figure8, d1, d3, and d5 are the deflections between the environment contours and the reconstructed smooth path at the starting point of each segment. d2, d4, and d6 are the deflections at the end point of each segment. Assume that ds is the safe distance, and the errors of each segment Δda1, Δda2, and Δda3 are described as follows:
(48)Δda1=(d2-ds)-(d1-ds)=d2-d1Δda2=(d4-ds)-(d3-ds)=d4-d3Δda3=(d6-ds)-(d5-ds)=d6-d5.
The accumulative error can be defined as follows:
(49)Δda=∑i=1NΔdai/Ni=1,2,3.(2)Overall ErrorΔdw. The entire environment contour is reconstructed and the overall error Δdw at the end point is defined as
(50)Δdw=d6-ds.Remark. Both Δda and Δdw have signed values. A positive value means the deflection is larger than the safe distance while a negative value means the deflection is smaller than the safe distance implying a threat to the vehicle navigation.


### 6.1.2. Evaluation Criteria for Autonomous Tracking Control

To estimate the performance of autonomous tracking control, the error is defined asek=y(k)-y^(k) where y(k) is the navigating path of the vehicle and y^(k) the smoothed contour from reconstruction. More specifically, the error at the k instant is the minimum distance from the vehicle position to the contour, that is, the tangent point on the contour. Three measures are proposed to analyze the errors.(1)Zero Mean Value. Define e~=(1/n)∑k=1nek as the estimated value of error serial {ek}. To guarantee the zero mean value, for any n∈Z+, it is satisfied that e~→0. From the definition of error, it will be affected by the estimated mean values of the output y~; therefore, reestimation of the mean value e-=e~/y~<eT is needed. eT is preset on the basis of accuracy requirement.(2)Validity. It is defined as the degree of deflection between the error serial {ek} and the zero mean value. Standard deviation σ=(1/(n-1))∑k=1n(ek-e-)2 is introduced to estimate the data validity.(
3
)
Independence. The error serial describes the error between the real navigating path and the smoothed contour after subtracting the safe distance. Error variables are regarded as random variables and thus the independence describes the randomness. Autocorrelation coefficient rp is used to describe the independence as follows:
(51)rp=1(n-p-1)∑k=1n-p(ΔekΔep+k),
where Δek=ek-e-k. In this paper the autocorrelation coefficient is rewritten as ρp=rp/r0, and the relation between ρp-k will be plotted for demonstration. If autocorrelation coefficients are in the Independence Confidence Limit Interval (-σ,σ), the error serial is not correlated and vice versa.


## 6.1.1. Evaluation Criterion for Contour Accuracy

Errors between the reconstructed contour and the environment model are computed in order to evaluate the accuracy of the contour reconstruction (only the contours estimated using the SVC inertia algorithm are evaluated here). Considering the characteristics of the environment, “accumulate error” and “overall error” are proposed. Figure8 illustrates how these errors are computed where the solid lines express the actual contours of the environment model while the dashed lines indicate the contour reconstructed by using the SVC inertia algorithm.Figure 8
Illustration of the accumulate error and overall error.(1)Accumulative ErrorΔda. The error between the contour of the environment model and the reconstructed one can accumulate over time, and the accumulative error is used to evaluate this trend.As shown in Figure8, d1, d3, and d5 are the deflections between the environment contours and the reconstructed smooth path at the starting point of each segment. d2, d4, and d6 are the deflections at the end point of each segment. Assume that ds is the safe distance, and the errors of each segment Δda1, Δda2, and Δda3 are described as follows:
(48)Δda1=(d2-ds)-(d1-ds)=d2-d1Δda2=(d4-ds)-(d3-ds)=d4-d3Δda3=(d6-ds)-(d5-ds)=d6-d5.
The accumulative error can be defined as follows:
(49)Δda=∑i=1NΔdai/Ni=1,2,3.(2)Overall ErrorΔdw. The entire environment contour is reconstructed and the overall error Δdw at the end point is defined as
(50)Δdw=d6-ds.Remark. Both Δda and Δdw have signed values. A positive value means the deflection is larger than the safe distance while a negative value means the deflection is smaller than the safe distance implying a threat to the vehicle navigation.


## 6.1.2. Evaluation Criteria for Autonomous Tracking Control

To estimate the performance of autonomous tracking control, the error is defined asek=y(k)-y^(k) where y(k) is the navigating path of the vehicle and y^(k) the smoothed contour from reconstruction. More specifically, the error at the k instant is the minimum distance from the vehicle position to the contour, that is, the tangent point on the contour. Three measures are proposed to analyze the errors.(1)Zero Mean Value. Define e~=(1/n)∑k=1nek as the estimated value of error serial {ek}. To guarantee the zero mean value, for any n∈Z+, it is satisfied that e~→0. From the definition of error, it will be affected by the estimated mean values of the output y~; therefore, reestimation of the mean value e-=e~/y~<eT is needed. eT is preset on the basis of accuracy requirement.(2)Validity. It is defined as the degree of deflection between the error serial {ek} and the zero mean value. Standard deviation σ=(1/(n-1))∑k=1n(ek-e-)2 is introduced to estimate the data validity.(
3
)
Independence. The error serial describes the error between the real navigating path and the smoothed contour after subtracting the safe distance. Error variables are regarded as random variables and thus the independence describes the randomness. Autocorrelation coefficient rp is used to describe the independence as follows:
(51)rp=1(n-p-1)∑k=1n-p(ΔekΔep+k),
where Δek=ek-e-k. In this paper the autocorrelation coefficient is rewritten as ρp=rp/r0, and the relation between ρp-k will be plotted for demonstration. If autocorrelation coefficients are in the Independence Confidence Limit Interval (-σ,σ), the error serial is not correlated and vice versa.


## 6.2. Validation Using Sea Trial Data

The performance of the proposed model is verified with the data collected from a sea trial operated at Xiaoping Island in Dalian, China, in August 2009. We choose sonar data collected from two different environments, respectively, a port and two islands. The single-beam sonars were assembled on one side of a fishing boat to simulate UUV sonars. To guarantee the number of sonar data satisfying the clustering requirement, 3 single beam sonars were fixed in the boat side; see Figure9. The sonars used in the sea trial are manufactured by Kongsberg, Norway. The boat was steered along the contour with acoustic beam vertically acting on environment. The sonars were deployed below the bottom of the boat to avoid acoustic beam to project onto the boat. The boat sails at a velocity of 2 m/s.Figure 9
Deployment of the sonars.Sonar data are collected during the boat navigating along islands horizontally in real time. To verify the effectiveness of the proposed control algorithm in disturbance situation, synthetic disturbance noise is added:tdu=40×(1+rand(·)), τdv=35×(1+rand(·)), τdr=40×(1+rand(·)), withrand(·) denoting zero-mean Gaussian random noise. From Figure 10 the sonar assembly in the sea can be seen, with diving in the water as Figure 10(a) and navigating as Figure 10(b).Sonar data collection in sea trial.
(a)
(b)


### 6.2.1. Path Following Validation Using Data from Islands

In the first experiment, the boat navigates between two small islands in order to move close to them (see Figure11). We record the data from initial point A (−30, −245) with heading of 60 degree. The effective distance of sonars is 200 m. Figure 12 shows the process of data clustering. At the beginning, the number of data is too small to construct accurate data class; therefore, data are placed into alternative set; see Figure 12(a). With the number of sonar data increasing, data with the same property, including data in alternative set, are clustered in corresponding class contours with support vectors distributed on. Sonar data determined by the rules in Section 3 are eliminated as outliers (see Figures 12(b) and 12(c)). The contours close to UUV, determined by normal line and distance, connected with lines successively, are regarded as preliminary contour, waiting for postprocessing.Figure 11
Sea trial in islands environment.Islands contour construction and patrolling.
(a)
(b)
(c)On the basis of initial contour (see solid green line in Figures12(b) and 12(c)), the smoothing method described in Section 4.2 is applied and a reasonable desired reference path (dash black curves) for vehicle navigation can be achieved. When the vehicle is close to the island on the right less than 200 m, sonar data returned is collected to construct the contour in real time which is regarded as the guidance for the vehicle to navigate, keeping a fixed distance (set to 10 m in this paper) as the safe distance. The optimal time window parameter is Tp≥3 s. Considering the control period 0.5 s, Tp=3 s satisfies the requirement (5 original sonar data and 6 wavelet coefficients) to estimate wavelet coefficients at any time. According to Section 6.1, the error series denoting the deflection between the actual navigation path and the reconstructed contour is computed; see Figure 13.Figure 13
Patrolling errors atx- and y-axis in islands contour.The mean (standard deviation) value alongx- and y-axis is 0.15 (0.37) m and 0.98 (0.30) m, respectively. According to evaluation criterion for contour accuracy in Section 6.1.1, the accumulated error for island contour is 1.45 m, and overall error is 1.74 m. The negative deflection values are much smaller than the safe distance; this means that the UUV can safely complete the detection mission as designed. The maximum absolute value of the autocorrelation coefficients at x-axis and y-axis is 0.14 and 0.16, respectively, while the respective standard deviations are 0.37 and 0.30 (see Figure 14). This confirms that the error series is independent without influence from the designed controller. This means that the designed controller using finite predictive stable inversion algorithm can control the vehicle to follow a path predicted in real time.Figure 14
Error autocorrelation coefficients of error serials.


### 6.2.2. Path Tracking Validation Using Data from Port

With the same deployment and trial methods as above, sonar data was collected in a port as Figure15 with its environment pictures at the right. The initial position of the boat was (−1605, −688). Owing to the port area is wide and safe distance is set to 30 m. Figure 16 demonstrates the autonomous tracking of the boat in the port. From Figure 16, it can be seen that the smoothed path illustrated by black solid is reconstructed using data from 3 sonars and guides the vehicle to approach the target. The noise in the data is detected and eliminated to guarantee the accuracy of the contour.Figure 15
Sea trial in port environment.Figure 16
Port contour construction and tracking.Figure17 illustrates the error of the smoothed path reconstructed and the actual path without consideration of the safe distance. Figure 18 shows the autocorrelation coefficients of the tracking error series to assess whether the errors are from the control method or stochastic disturbance. The maximum absolute value of autocorrelation coefficients at x-axis and y-axis are 0.20 and 0.20, within the range of standard deviations, respectively, (−0.26, 0.26) and (−0.38, 0.38) (see Figure 17). Therefore, it is true that error series are caused stochastically. Moreover, the accumulated error for island contour tracking is 1.50 m, and overall error is 1.20 m, respectively.Figure 17
Path error serials atx- and y-axis for patrolling port contour.Figure 18
Error autocorrelation coefficients of error serials.


## 6.2.1. Path Following Validation Using Data from Islands

In the first experiment, the boat navigates between two small islands in order to move close to them (see Figure11). We record the data from initial point A (−30, −245) with heading of 60 degree. The effective distance of sonars is 200 m. Figure 12 shows the process of data clustering. At the beginning, the number of data is too small to construct accurate data class; therefore, data are placed into alternative set; see Figure 12(a). With the number of sonar data increasing, data with the same property, including data in alternative set, are clustered in corresponding class contours with support vectors distributed on. Sonar data determined by the rules in Section 3 are eliminated as outliers (see Figures 12(b) and 12(c)). The contours close to UUV, determined by normal line and distance, connected with lines successively, are regarded as preliminary contour, waiting for postprocessing.Figure 11
Sea trial in islands environment.Islands contour construction and patrolling.
(a)
(b)
(c)On the basis of initial contour (see solid green line in Figures12(b) and 12(c)), the smoothing method described in Section 4.2 is applied and a reasonable desired reference path (dash black curves) for vehicle navigation can be achieved. When the vehicle is close to the island on the right less than 200 m, sonar data returned is collected to construct the contour in real time which is regarded as the guidance for the vehicle to navigate, keeping a fixed distance (set to 10 m in this paper) as the safe distance. The optimal time window parameter is Tp≥3 s. Considering the control period 0.5 s, Tp=3 s satisfies the requirement (5 original sonar data and 6 wavelet coefficients) to estimate wavelet coefficients at any time. According to Section 6.1, the error series denoting the deflection between the actual navigation path and the reconstructed contour is computed; see Figure 13.Figure 13
Patrolling errors atx- and y-axis in islands contour.The mean (standard deviation) value alongx- and y-axis is 0.15 (0.37) m and 0.98 (0.30) m, respectively. According to evaluation criterion for contour accuracy in Section 6.1.1, the accumulated error for island contour is 1.45 m, and overall error is 1.74 m. The negative deflection values are much smaller than the safe distance; this means that the UUV can safely complete the detection mission as designed. The maximum absolute value of the autocorrelation coefficients at x-axis and y-axis is 0.14 and 0.16, respectively, while the respective standard deviations are 0.37 and 0.30 (see Figure 14). This confirms that the error series is independent without influence from the designed controller. This means that the designed controller using finite predictive stable inversion algorithm can control the vehicle to follow a path predicted in real time.Figure 14
Error autocorrelation coefficients of error serials.


## 6.2.2. Path Tracking Validation Using Data from Port

With the same deployment and trial methods as above, sonar data was collected in a port as Figure15 with its environment pictures at the right. The initial position of the boat was (−1605, −688). Owing to the port area is wide and safe distance is set to 30 m. Figure 16 demonstrates the autonomous tracking of the boat in the port. From Figure 16, it can be seen that the smoothed path illustrated by black solid is reconstructed using data from 3 sonars and guides the vehicle to approach the target. The noise in the data is detected and eliminated to guarantee the accuracy of the contour.Figure 15
Sea trial in port environment.Figure 16
Port contour construction and tracking.Figure17 illustrates the error of the smoothed path reconstructed and the actual path without consideration of the safe distance. Figure 18 shows the autocorrelation coefficients of the tracking error series to assess whether the errors are from the control method or stochastic disturbance. The maximum absolute value of autocorrelation coefficients at x-axis and y-axis are 0.20 and 0.20, within the range of standard deviations, respectively, (−0.26, 0.26) and (−0.38, 0.38) (see Figure 17). Therefore, it is true that error series are caused stochastically. Moreover, the accumulated error for island contour tracking is 1.50 m, and overall error is 1.20 m, respectively.Figure 17
Path error serials atx- and y-axis for patrolling port contour.Figure 18
Error autocorrelation coefficients of error serials.


## 7. Conclusions

A new autopilot system, known as SDAP, is proposed for the exploration of underwater environments using UUV equipped with multiple single-beam sonars. The main issue studied is the control problem in detection process which is separated into two stages according to different requirements of the mission, accurate tracking and autonomous tracking. A rolling path generation method is present to guide the vehicle to follow a preset path accurately. For autonomous tracking stage, wavelet transform is introduced to preprocess weak observable data and the wavelet coefficients obtained are used to reconstruct the contour of the environment using the SVC inertia algorithm. To satisfy the inertia property of the UUV which requires a smooth reference path, different order Bézier curves are included to fit the initial contours for the desired reference path by considering a fixed safety distance. Finite predictive stable inversion method is applied to control the vehicle in order to follow the predictive path in real time. Data collected from a sea trial is used to validate the proposed technique, and the results have demonstrated that the algorithms are able to control vehicle navigating along the desired paths that are either preset or predicted automatically. It has laid a solid foundation for using UUV to perform SDAP mission.During environment detection, the accuracy of environment information obtained is vital to guide UUV steering safely. With the insight, the navigation error will affect the environment outline constructed. In this paper, it is assumed that the navigation error is not considered in the UUV steering. In the future study, it is necessary to include navigation error into the SDAP issue for completeness. Otherwise, the further verification should be implemented through inserting the algorithms into UUV and executing mission in the real environment underwater.



---

*Source: 718012-2013-11-05.xml*