Observability Degree-Based AUV Single Beacon Navigation Trajectory Optimization with Range-Only Measurements

Aiming at the problem of autonomous underwater vehicle navigation trajectory optimization using single beacon location under direct route condition, a nonlinear system model for AUV single beacon navigation is established, and the linearized system model with error states is constructed by polar coordinate transformation and simplification. Then, current disturbance is considered. To find out the optimum path to utilize range-only measurements, a novel observability degree-based analysis method is proposed, which gives a quantitative insight into convergence characteristics of the error states by using the eigenvalues of the normalized error covariance matrix. Simulation experiments are done to test convergence characteristics of AUV integrated navigation error states with single beacon range-only measurements under direct route control conditions. The experimental results show that the proposed control method is effective, and it has an important engineering application value and provides us with an optimized path.


Introduction
The autonomous underwater vehicle (AUV) navigation system using the underwater single beacon range measurement can achieve a wide area of deployment, and the mutual independence of each beacon greatly improves the concealment of AUVs, which has a high engineering application value and has become a research hotspot of the underwater acoustic positioning [1][2][3][4].A large number of works on the navigation technology using underwater single beacon positioning have been carried out by researchers of the United States, Britain, Russia, Norway, and other countries [3][4][5][6][7][8][9][10][11][12][13], and commercial products have already been applied to AUV guidance and AUV inertial navigation system (INS) error correction [4,6].
Published works on single beacon navigation is extensive; thus, only some of works most relevant are reviewed here.For early work in the single beacon navigation area, Scherbatyuk [14] initially formulated underwater single beacon navigation.Larsen [15] presented a synthetic long baseline algorithm combined with a dead reckoning system for surface ship to achieve submeter accuracy and studied the observability of the concerned system states with specific trajectories in deterministic frameworks.LaPointe [16] then created the virtual long baseline to determine the vehicle's position by asynchronous multiple range measurements along a dead reckoning track and studied the benefits, limitations, and implications of the proposed methodology in the framework of an analysis of observability.To be short, an extensive review on the previous research works of single beacon navigation before 2010 can be found in Webster [17].Several different estimation techniques have been reported for single beacon navigation, such as least squares algorithm in Scherbatyuk [14] and Pedro et al. [18], posebased decentralized extended Kalman filter (EKF) in Webster [17] and Webster et al. [19][20][21], centralized EKF in Webster et al. [22,23] and F. B. Zhang and Y. Q. Zhang [11], and Sum of Gaussians (SOG) filter in Blanco et al. [24], Fabresse et al. [25], and Vallicrosa et al. [26,27].However, the majority of these studies report numerical simulation results of the filter algorithms proposed therein, only a few report observability of system states with "yes" or "no" answers [9,11,12,28], which is not sufficient for the analyst to construct a navigation scheme.
Navigation and control are closely relevant, and there are many control methods available for an autonomous vehicle at present [29,30].To optimize position estimation, the circular trajectory is often adopted in control when the AUV navigation system uses the underwater single beacon range measurement [1,2,6,31].However, normally, the sailing speed of AUV is usually slow, and a circular maneuver is limited by actual underwater environment and other factors.Besides, an AUV still needs to sail for some time before reaching the optimum circular maneuvering distance after it has come to the effective area with the beacon positioning [6,17,31].Thus, the AUV integrated navigation performance using single beacon range measurement under the direct route condition needs in-depth study.The objective of this paper is to provide a novel observability degree-based path planning method for underwater single beacon navigation with range-only measurements under direct route condition.
The remaining of the paper is arranged as follows.In Section 2, on the basis of the general AUV integrated navigation nonlinear motion model, the system linearization error filtering model is established, then a more complex error model is deduced for unknown current disturbances; Section 3 presents the expression equations of observability degrees of the error states by normalized error covariance matrix eigenvalue; under the direct route condition, the simulation analysis is carried out for the error convergence characteristics of AUV integrated navigation using the single beacon range measurement in Section 4; conclusions are summarized in Section 5.

Establishment of the System Model
As the AUV generally uses a depth sensor to get z-axis position information, only the two-dimensional situations are considered in this study [9,12,16].

Ideal System Model in the Rectangular Coordinate
System.As shown in Figure 1, L represents the local latitude, ℓ represents the local longitude, and they can form a twodimensional rectangular coordinate system, denoted as n.The beacon is set on the seabed, and its absolute position can be expressed as L b , ℓ b in the n coordinate; AUV trajectory can be expressed as L v t , ℓ v t in the n coordinate.
From the above, the ideal motion equation of AUV in the n coordinate is defined as follows [1,9,32]: In the above equations, V represents the sailing speed of the AUV; ϕ represents the heading angle of the AUV in the n coordinate; R represents the average radius of the earth; α represents the rudder angle of the AUV; d v represents the distance from the rudder to the buoyant centre of the AUV; h t represents the straight-line distance between the AUV and the beacon; ⋅ represents vector inner product computation.

Model Simplification.
The system model which is composed of (1) and ( 2) is strongly nonlinear.In order to simplify the analysis process, we reconstruct the ideal system model in the polar coordinate to reduce difficulties of the subsequent analysis.
Considering that there exists h t > 0 during the whole AUV motion and the effective acoustic ranging distance limit h t is always within tens of nautical miles, (1) and ( 2) can be, respectively, converted approximately into the polar coordinate system as follows: Figure 1: Schematic diagram of the relative position of the system.

Complexity
In the above equations, r t represents the approximate linear distance between the AUV and the beacon; and the expression of θ t is 2.3.Current Disturbance.As stated above, the system dynamic model is based on a kinematic model of the AUV.Without considering current disturbances, the analysis process can be simplified and computation complexity can be reduced greatly, but there would be a mismatch between the computed and true trajectories of the AUV [33][34][35].
Thus, an unknown current disturbance whose magnitude V C and direction ϕ C is assumed for evaluating its impact on path planning.Consequently, the motion equation of AUV under current disturbances in the n coordinate can be derived: Similarly, we obtain the approximate system model under current disturbances in the polar coordinate:

Analytical Model on the Observability
T , nominal trajectory of the AUV is expressed as L v0 t , ℓ v0 t , the approximate ideal system model and the approximate system model under current disturbances then can be linearized.Considering the strong nonlinearity of the system dynamic model and easy derivation by simple Jacobian-form linearization computation, simplified expressions for later illustration are written in the following form [1,33]: In the above equation, δX t represents the nominal state perturbation error; F t represents the Jacobian matrix obtained by linearizing the nominal trajectory; u δ t represents the nominal input perturbation error; G t represents the u δ t propagation matrix; δh t represents the nominal measurement distance perturbation error.
The analytic expressions between the error states and the main relevant variables can directly reflect the observability degree information of the system error states [1,7,18,33].However, due to the existence of trigonometric functions in the system models, it is very difficult to get analytic results by (10).The system error covariance matrix includes the quantitative information of the convergence rate and precision of the system error states; thus, it can be used to provide observability degree characteristics of the system error states [36,37].
Considering the actual situation of discrete processing of the navigation computer during the process of the integrated navigation system, we can derive the discrete form of the system error covariance matrix: In addition, under the discrete condition, the sampling period is short enough; F k+1,k t is regarded as constant value within The Normalization of the Error Covariance Matrix.It can be seen from ( 12) that the convergence characteristics of the system error states are related to the initial error covariance matrix P 0 .For unified comparison, the system error covariance matrix P t k+1 is needed to be normalized in the process of calculation.The steps are as follows [38].
Step 1. Normalizing the upper bound of the eigenvalue of P t k+1 : In the above equation, Tr ⋅ represents operation of "the trace of the matrix"; N δX represents the system dimension.
Step 2. The observability degree information of the system error states are obtained by calculating the eigenvalues of P t k+1 ′ When the normalized error eigenvalue of certain system error state obtained is smaller, the observability degree of this system error state is higher [36,38].
According to the above proposed observability degree analysis methodology, we can analyze the observability degrees of AUV integrated navigation errors using single beacon range measurement comprehensively.

Experiments and Discussions
4.1.Simulation Parameter Determination.Under the situation that the AUV sails on a straight route, V is a constant value, and the rudder angle α = 0.Meanwhile, it can be seen from the above that the linear trajectory is different when the relative position between AUV and the beacon or its heading angle is different.Thus, in order to fully test the observability degrees of the integrated navigation error states when using different linear navigation trajectories, without loss of generality, five AUV initial positions under the condition of equal distance are set in the second quadrant of the rectangular coordinate system which takes the beacon as the origin, as shown in Figure 2. It should be pointed out that the trajectory of the AUV should be located in the half-plane which is indicated by the black solid line "n L "; the angle between n L and the AUV linear navigation trajectory is denoted as dϕ.
Other simulation parameters are listed in Table 1.4) and (8) that the system models change with the latitude of the AUV.As longitude lines converge rapidly with the elevation of latitude, there would be desperate need to investigate convergence characteristics of system error states under the condition that distance between the AUV and the single beacon is constant.Without loss of generality, in this subsection, we assume that there is no current disturbance, and the distance between the AUV and the single bea-

Experimental
For intuitionistic analysis, according to simulation results with five different initial latitude values, normalized eigenvalues of the linear distance error between the AUV and the beacon (δr) and heading angle error δϕ as initial latitude changes are plotted in Figures 3 and 4.
From Figures 3 and 4, it can be concluded that (1) because the obtained normalized eigenvalue is different, the observability degree of the corresponding  When carrying out the simulating calculation, the integrated navigation system parameters are initialized as follows: For intuition, the normalized eigenvalues of the linear distance error between the AUV and the beacon (δr) and heading angle error δϕ are given in Figures 5 and 6 under the five initial positions according to the simulation results.
According to Figures 5 and 6, it can be seen that ( 1) when the linear distance between the AUV and the beacon (r) or heading angle ϕis different, the normalized eigenvalues of integrated navigation relative errors using single beacon range-only measurements of the AUV are different.Therefore, the observability degrees of the corresponding error states are different.When dϕ = 90 °, that is, when the AUV sails along the path in parallel with the vector of the single beacon, the observability degree of the linear distance error between the AUV and the beacon (δr) is the weakest, which is consistent with the existing conclusions [1,[7][8][9]12] (2) under the condition that the heading angle is determined, the observability degree of the heading angle error is stronger than that of the linear distance error between the AUV and the beacon (δr), that is, the convergence rate of the heading angle error is faster during the operation of the filter.Although the observability degree of heading angle error is the highest when dϕ = 90 °, the system errors are not globally observable.As a result, such trajectory is not adopted generally [10-12, 15, 16] igure 3: Normalized eigenvalues of the linear distance error between the AUV and the beacon as initial latitude changes.5 Complexity (3) under the condition that the linear distance between the AUV and the beacon and the heading angle is determined, the higher the relative latitude between the AUV and the beacon, the stronger the observability degree of the linear distance error between the AUV and the beacon (δr) and the weaker the observability degree of the heading angle error δϕ In addition, it can be seen clearly according to Figure 5 that, when dϕ ≈ 30 °, the observability degree of linear distance error between the AUV and the beacon (δr) is the strongest though relative range between the AUV and the beacon is different.Thus, according to this phenomenon, if the AUV needs to sail for some time to reach the optimal circular maneuver relative range of the beacon after entering into the effective area of the single beacon, it can sail according to the spiral trajectory proposed in Figure 7. Along the spiral trajectory, once the condition that the single beacon is just on the AUV side is satisfied, the AUV will turn 30 4.2.3.The Impact of θ t 0 on Convergence Characteristics of System Error States under Current Disturbance.In this subsection, the observability degrees of the system error states are to be investigated to get sight into their convergence characteristics.During simulation, the distance set dL, dℓ between the AUV and the single beacon in the above subsection is used, and current direction can be chosen from the set as follows: For the first case, normalized eigenvalues of the system error states under different current directions with determined initial position are calculated and plotted in Figures 8-11.
Comparing Figure 8 with Figure 5, there exists consistency that the observability degree of the linear distance error between the AUV and the beacon (δr) is the weakest when the AUV sails along the path in parallel with the vector of the single beacon, although the observability degrees of δr with current directions change irregularly.Comparing Figure 9 with Figure 6, it should be pointed out that, by including unknown current disturbance parameters, characteristics of normalized eigenvalues of the heading angle error of the proposed single beacon navigation motion model are     6 Complexity inverted.Obviously, one can see in Figure 10 that, when dϕ ≈ 30 °, the observability degree of the estimated current speed is the strongest, which is another reason we chose the proposed spiral trajectory to approach the single beacon as in the above subsection.From Figure 11, it is illustrated that forward and reverse current disturbances have the same impact on the estimation characteristics of the current direction.
For the second case, to further investigate the error convergence characteristics under the proposed spiral trajectory, we set the simulation parameter dϕ to be 30 °.Therefore, normalized eigenvalues of the system error states under different    7 Complexity current direction and distance conditions can be computed and plotted in Figures 12-15.The distance set index corresponds to the distance set between the AUV and the single beacon which is presented in (19), and the current direction set index corresponds to the current direction set that is presented in (21).Figure 12 shows that the observability degree of the linear distance error between the AUV and the beacon is more vulnerable to initial distance between the AUV and the single beacon than current direction under the certain heading angle.However, the observability degrees of the heading angle error, the estimated current speed error, and the estimated current direction error are just the opposite.It can be seen obviously from Figures 13-15 that the observability degree characteristics of the heading angle error, the estimated current speed error, and the estimated current direction error are the same when the two currents are in a line regardless of their opposite directions.Meanwhile, for a determined θ t 0 , the observability degrees of the linear distance error between the AUV and the beacon, the heading angle error, the estimated current speed error, and the estimated current direction error are different; compromises are needed at different current directions.It is worth noting that the variation of the observability degrees of the linear distance error between the AUV and the beacon, the heading angle error, the estimated current speed error,    Complexity and the estimated current direction error is not distinct with the distance set index for a determined current direction.

Conclusions
Due to the practical underwater environment and other factors, AUV generally sails on a direct route.However, a large number of research works show that when the AUV navigation system uses single beacon range-only measurements, a direct route is not good for the filtering estimation of the system error states.In order to study the AUV single beacon navigation performance utilizing range-only measurements under the direct route condition, a novel observability degree analytical model of the system error states is proposed on the basis of the error covariance matrix eigenvalue by establishing the system linearization system error models, and the normalization processing is carried out in order to strengthen its effectiveness.The simulation results show that different relative position, heading angle between the AUV and the beacon, and current disturbance will affect the observability degrees of the system error states, and an optimized spiral approaching trajectory with a simple turn rule is proposed to improve the system error estimation characteristics.

Figure 2 :
Figure 2: Schematic diagram of the simulation trajectory.

Figure 4 :
Figure 4: Normalized eigenvalues of the heading angle error δϕ as initial latitude changes.

Figure 6 :
Figure 6: Normalized eigenvalues of the heading angle error δϕ under the five initial positions.

Figure 7 :
Figure 7: Spiral trajectory of the optimal circular maneuver range of approaching the single beacon.

Figure 5 :
Figure 5: Normalized eigenvalues of the linear distance error between the AUV and the beacon under the five initial positions.

igure 10 :
Normalized eigenvalues of the estimated current speed error under different current directions.

igure 11 :
Normalized eigenvalues of the estimated current direction error under different current directions.

igure 9 :
Normalized eigenvalues of the heading angle error δϕ under different current directions.

igure 8 :
Normalized eigenvalues of the linear distance error between the AUV and the beacon under different current directions.

Figure 12 :
Figure 12: Normalized eigenvalues of the linear distance error between the AUV and the beacon under different current direction and distance conditions.

Figure 13 :
Figure 13: Normalized eigenvalues of the heading angle error δϕ under different current direction and distance conditions.

Figure 14 :
Figure 14: Normalized eigenvalues of the estimated current speed error under different current direction and distance conditions.

Figure 15 :
Figure 15: Normalized eigenvalues of the estimated current direction error under different current direction and distance conditions.

Table 1 :
Simulation parameters. ) the observability degrees of corresponding error states are of certain regularity as relative angle dϕ changes.The lower the initial latitude, the stronger the observability of the linear distance error between the AUV and the beacon (δr) and the weaker the observability of the heading angle error δϕ 4.2.2.The Impact of θ t 0 on Convergence Characteristics of System Error States without Current Disturbance.For each AUV initial position, simulation is carried out with a different heading angle indicated by the black solid line "n L " in the half-plane, which is dϕ = 10 °× i φ , i φ = 0, 1, … , 18 .
(2) when the heading angle is determined, the observability degree of the heading angle error δϕ is stronger than the linear distance error between the AUV and the beacon (δr)(3