Abstract
The performance of the inertial navigation system/Doppler velocity log (INS/DVL) is mainly degraded by DVL error. An optimization-based algorithm is widely used and effective on calibrating DVL error. In this algorithm, the loss function established is a quadratic function of DVL attitude error; however, the existing optimization-based calibration method is just of first-order generally, so that it is not able to reach the best calibration efficiency. In order to improve the efficiency, an iterative calibration algorithm based on the second-order Newton optimization is proposed innovatively to calibrate the DVL error. In addition, the Wolfe principle is introduced to limit its loss function’s oscillation around the extreme point to improve the efficiency and accuracy of the Newton optimization. The excellent efficiency and accuracy of the proposed calibration scheme are validated by simulation and field tests.
1. Introduction
High-accuracy navigation is a key technology that ensures the implementation of military and commercial activities for underwater vehicles. According to different application scenarios, there are many different navigation systems, such as acoustic positioning system (APS), Doppler velocity log (DVL), and inertial navigation system (INS). The APS can provide high-accuracy navigation; however, its position sensing relies on the seafloor-mounted transponder beacons. The INS is an autonomous navigation system, but its velocity and position tend to drift with time due to the integrations performed. In contrast, DVL not only can measure velocity without depending on the other equipment but also has good long-term accuracy; therefore, it has been widely used in long-endurance underwater vehicles [1–3].
In the past, the accuracy of DVL navigation was mainly limited by the attitude sensor error and the DVL-related errors such as DVL scale factor error and the attitude misalignment between DVL and attitude sensor. In recent years, a series of new high-accuracy attitude sensors were developed to provide attitude information for DVL with only 0.01° error or even less. Therefore, the attitude sensor error has been largely reduced, while the DVL-related errors have become the remaining challenges. In order to suppress their negative effect on navigation accuracy, many calibration algorithms have been proposed. According to the external information used in the calibration process, these algorithms can be divided into self-calibration algorithm without external information dependence [4, 5], two-point method utilizing the positions of only two points on the trajectory, and the reference-assisted method which requires the high-accuracy position or velocity information on the whole calibration trajectory. Although the self-calibration method and the two-point method have less dependence on external information, their calibration time is generally longer and accuracy is poorer; therefore, the reference-assisted calibration method is the most preferable scheme for vehicles which performs high-accuracy long voyage tasks.
In the early research, the calibration schemes were usually based on the least square estimation (LS) in which the reference position was provided by the long baseline acoustic positioning system (LBL) [6]. In later research, the calibration accuracy was further improved because the regularity of attitude matrix was utilized to constrain the LS result [7]. Limited by the distance of acoustic transmission, LBL can only provide the position information for the vehicles within 10 kilometers from the transponder. In contrast, the global positioning system (GPS) has the ability to provide superior global navigation and has been employed widely by ocean vehicles [6]. Therefore, the calibration algorithm that adopts the reference information provided by GPS instead of LBL was proposed [7, 8]. These calibration methods are all based on the LS; however, its performance is vulnerable to measurement error and stochastic noise. Therefore, the iterative calibration method based on Kalman filter (KF) was proposed where the measurement error’s effect is suppressed by the redundant predicted state. The attitude misalignment can be effectively calibrated using the LS and KF, but there are still two main disadvantages remaining: (1) ignoring the DVL scale factor error: the scale factor error is one of the main error sources for DVL velocity; it cannot be ignored in the high-accuracy navigation applications; (2) considering the relationship between attitude misalignment and measurement as linear simply: this relationship is nonlinear in real application because the effects on the measurement of attitude misalignment and scale factor are coupling. Therefore, the accuracy of these linear estimation methods (KF, LS, etc.) must be suppressed by the nonlinear coupling error. In order to avoid the nonlinear error, calibration scheme comparing the DVL-derived tracks with the reference one is proposed. Because the influences of scale factor error and attitude misalignment on DVL position are different, the geometric relationship between DVL-derived track and the reference can be used to deduce the attitude misalignment and scale factor conversely [9–12]. In this scheme, the vehicle must navigate on the specified track for a long time [13]. To improve the calibration efficiency, an iterative calibration algorithm based on gradient descent optimization was proposed where the scale factor error and attitude misalignment can both be figured out without dependence on a specified track [14]. However, the fact that the loss function established in this algorithm is of second-order while the gradient descent is only first-order optimization method causes low calibration efficiency [15].
A new iterative calibration method based on the Newton optimization, a second-order optimization algorithm, is proposed in this paper to improve the calibration efficiency. In addition, considering that the fixed-step principle adopted in the Newton optimization is prone to cause the oscillation of loss function value near the extreme point, the Wolfe principle is introduced to calculate the best step adaptively to solve this problem. Finally, the advantage of the proposed algorithm on accuracy and efficiency is verified, respectively, in simulation and field tests.
2. Problem Formulation
Figure 1 shows an underwater vehicle installed with GPS receiver, INS, and DVL. These three sensors provide the navigation information, respectively, in the navigation frame , vehicle body frame , and DVL body frame . In order to fuse the information of INS and DVL and establish the high-accuracy integrated navigation system, two problems should be settled in advance: (1) figuring out the attitude misalignment from the frame to the frame and the lever-arm vector caused by the misalignment of installation centers of INS and DVL, so as to eliminate the spatial inconsistency of two subsystems; (2) calculating the DVL scale factor to eliminate its negative effect on the accuracy of DVL velocity. These two problems, DVL error calibration in other words, are exactly what we will solve in this paper.
During the calibration, the GPS velocity is regarded as the reference velocity. And the INS body frame is coincident with the vehicle body frame for simplicity of exposition. The relation between DVL and GPS velocity can be expressed as [16]where is the vehicle velocity in frame provided by DVL; the vehicle velocity in frame provided by GPS; the DVL scale factor; the attitude misalignment from the frame to the frame expressed in direction cosine matrix (DCM) [9]; the attitude misalignment from the frame to the frame; the angular velocity of frame with respect to the frame; the lever-arm vector from the origin of frame to that of . The symbol is the operator of the cross product, and it satisfies for arbitrary two vectors, where is the skew-symmetric matrix [17]. On the right side of (1), , , and are unknown parameters. , , and are provided by GPS or figured out with simple calculation; therefore, they all can be regarded as known where the calculation process of is given as follows:where is body angular rate measured by gyroscopes in the body frame; gyroscopes bias which can be neglected due to its small enough quantitative magnitude; Earth rotation rate which is approximately , a very small value, so it can be also neglected. In addition, is achieved aswhere and are the elements in north and east directions of velocity vector; is the radius of Earth meridian circle; is the radius of Earth prime vertical circle; and , respectively, are the height and latitude at the position of the vehicle staying. In (3), vehicle velocity is far smaller than Earth radius; therefore, the elements of are approximately equal to zero. From the above analysis, we can conclude that , due to the small value of the other variables except , and we rewrite (1) as
3. Calibration Scheme
All the error terms in (4) can be calibrated by the iterative calibration method based on Newton optimization proposed in this paper. Next, we will introduce the calibration scheme in detail.
3.1. Calibration of Scale Factor
Since the DCM does not change the modulus of the vector, the modulus values are still equal after removing DCMs on both sides of (2); that is,
The scale factor can be achieved as
In (6), all the noises of , , and will degrade the precision of calibration. In order to eliminate this negative influence, we will integrate (2) on the time period to gain
In order to obtain the suitable form of (7) for computer processing, the discretization of (7) has been done aswhere represents the value on the time with . On the right side of (8), the variables except can be calculated or measured directly. The effect of on the calibration emerges only when the vehicle performs angular movement; that is, . In addition, its effect will be offset if the vehicle performs an angular movement in the opposite direction. For the reason that the underwater vehicle performs mainly linear movement and few angular movements in practice, the physical meaning of (8) will keep approaching the “scale factor error” while the effect of gets weak. Therefore, it is not necessary to calibrate the accurate lever-arm vector. A coarse lever-arm vector given artificially can meet the requirement of navigation accuracy. Based on the above analysis, it can be seen that the calibration accuracy of derived from (8) is immune to the attitude misalignment ; besides, it will get higher as the time increases.
3.2. Calibration of Attitude Misalignment
Calibration of attitude misalignment, in other words, is solving . In (4), has been solved and is also given a coarse value, while is the only remaining unknown matrix. Therefore, we will construct the observation equation according to (4). Similarly, the integral of velocity will be used to calibrate the attitude misalignment instead of the noise-contained velocity. The discrete form of (4) is given directly as
Substituting and into (9) gives the observation equation of as
Substituting with the equivalent quaternion and reorganizing the terms [18, 19],where is the conjugation of . In theory, if the attitude misalignment is correctly calibrated, the modulus of should be extremely small. For this purpose, we establish the loss function aswhere is the operator of calculating modulus. Our goal is to solve a quaternion to minimize the value of loss function; that is,
Substituting and into (11) achieves the expansion of as [20]
In (14), is the quadratic function of elements of quaternion , and so is its modulus . Therefore, if the first-order optimization algorithm, such as the gradient descent method, is used to search the optimal solution of , the optimization efficiency will not be very high. In order to improve the optimization efficiency, we propose using the Newton method which is a second-order optimization method to search the optimal solution instead of the first-order one. In the following, the calibration process using the Newton method will be introduced in detail.
The second-order Taylor series expansion of around iswhere is the quaternion of iteration; the operator of calculating the gradient; the operator of calculating the Hessian matrix [21]. For simplicity of exposition, and are, respectively, denoted as and ; in addition, and are denoted as and .
The optimal solution of is that one which makes the function value of reach minimum, which is the stationary point of ; that is,
The expression of can be obtained by calculating the gradient of (15):
Setting (17) equal to zero gains
If the initial value is given, the iterative solution of quaternion satisfying (18) can be constructed as
The method of searching the optimal result according to the iterative format of equation (19) is called the Newton method. Although the method can be used to iterate the optimal results, it has two disadvantages: (1) huge computation burden caused by the recalculation of the Hessian matrix at each iteration, which is composed of the second-order partial derivative of the loss function; (2) low efficiency or oscillation near the stationary point due to the fixed-step iteration of the Newton method [22]. To overcome these problems, two targeted measures are carried out in our research: (1) replacing the Hessian matrix with the approximate matrix to avoid solving the second partial derivative, that is, adopting the Newton method; (2) searching the best step adaptively using the Wolfe principle instead of the fixed-step strategy [23]. The so-called Wolfe principle is to search the step that meets the following conditions:where ; ; and .
Before constructing the approximate matrix B, we need to derive the Newton principle from (18):where ; . can be replaced with calculated as in the following iteration formulation:where can be taken as the unit matrix. Therefore, the key is constructing the correction matrix in each step. We can hold it as
Substituting (23) into (22) and combining formula (21),
Supposing , , , and gains
Based on (23)–(25), the correction matrix is obtained as follows:
Substituting and derived, respectively, from (20) and (22) into (19) gains the improved iterative formula as
It must be noted that a threshold should be set as the criterion of stopping iteration. If the is smaller than , is taken as the final calibration result. The quaternion is only one way to express the attitude misalignment. It is completely equivalent to other expressions, such as Euler angle and DCM. These expressions can transform with each other, and the conversion relationship between them is shown in [24].
According to the above derivation, the DVL error iterative calibration method based on Newton optimization is summarized in Table 1.
4. Simulation Test
The simulation test based on INS/DVL/GPS is conducted first to validate the advance of the proposed iterative calibration method based on the Newton optimization. The whole simulation test lasts for 600 s, during which the vehicle accelerates, keeps uniform velocity, and turns left and right as Table 2 shows to simulate the real movements of the vehicle.
In the simulation test, the vehicle moves along the trajectory presented in Figure 2. Reference velocity is provided by GPS, so the GPS velocity error is regarded as 0. The initial alignment between INS and GPS has been completed by default, so the velocities provided by INS and GPS can be compared after a simple conversion between frame and frame . The output frequencies of INS, DVL, and GPS are 200 Hz, 1 Hz, and 1 Hz, respectively. The noise of GPS velocity is and the constant bias of the gyroscopes and accelerometers is and , respectively. The initial parameters of simulation are set properly where the scale factor error of DVL is 0.005; attitude misalignment between frame and frame ; lever-arm vector from DVL to INS ; starting point position ; initial velocity 0.
The EKF with the velocity error measurement, gradient descent optimization, and Newton optimization is used to calibrate DVL scale factor and attitude misalignment. In Figures 3–6, red, black, and blue solid lines, respectively, represent the calibration errors of these three methods. For the convenience of analysis, we calculate the means and root mean square errors of scale factor and attitude misalignment after convergence between 300 s and 600 s and then list them in Tables 3 and 4.
The scale factor and attitude misalignment are estimated simultaneously in EKF, while they are estimated separately in the gradient descent optimization method and the Newton optimization method. Because the same method is adopted to solve the scale factor for these two optimization methods, the estimated scale factors have equal values. It is the reason why the black line and blue line coincide completely.
In Figure 3, it can be seen that the estimated scale factor using EKF method fluctuates greatly because it is affected by the noise of measurement, that is, GPS velocity noise. In addition, its mean is relatively larger than that of the scale factors estimated by the other methods due to the coupling of attitude misalignment.
Figures 4 and 6, respectively, show the estimation errors of pitch angle and yaw angle. It can be seen that the EKF estimation still fluctuates greatly due to the GPS velocity noise, while the estimation accuracy of the gradient descent method and the Newton method is smooth. In addition, the error convergence of the Newton method is significantly faster than that of the gradient descent method. And also, the estimation of the Newton method is steadier after the convergence because of the introduction of the Wolfe principle; the optimal step size is adaptively determined to avoid the oscillation of loss function near the extreme point.
The estimated roll angle error is shown in Figure 5. Obviously, it is not calibrated correctly. This is because the vehicle always travels along with the y-axis of frame ; the velocities in x-axis and z-axis are extremely small; therefore, the attitude misalignment in these two directions is unobservable. Fortunately, the attitude misalignment in y-axis, that is, roll angle error, has no effects on the positioning accuracy of INS/DVL, so it is acceptable not to estimate its value.
5. Field Test
The land vehicle field test is conducted to further evaluate the proposed algorithm to estimate the possibility of applying it to the underwater vehicles. The inertial measurement unit of INS is composed of three fiber optic gyroscopes with constant bias and three quartz accelerometers with constant bias. The reference velocity is provided by a reference navigation system whose sensors are PHINS developed by French firm IXBLUE and FlexPark6 GNSS receiver developed by NovAtel. The DVL data is simulated by reference system, where the velocity under frame is transformed to frame using the true attitude information. And then the scale factor error, attitude misalignment, and the lever-arm vector are artificially added to the simulated DVL date, respectively, as 0.005, [0.8°, −0.2°,0.6°], and [0 5 m 0]. The INS and PHINS are fixed on the same mounting plate shown in Figure 7.
The whole filed test lasts for 600s where the various actions, such as parking, acceleration, uniform velocity, deceleration, and turning, are carried out by the test vehicle. And the trajectory is shown in Figure 8. EKF and the algorithms based on gradient descent and Newton optimization are all applied to DVL error calibration for comparing their accuracies and efficiencies. In Figures 9–12, the blue lines, red lines, and yellow lines represent, respectively, the calibration errors of these three algorithms. During this field test, the vehicle carried out totally four large maneuvers where the EKF calibration errors in Figures 9–12 all show violent fluctuation, while the optimization-based algorithms are almost not influenced. In Table 5, the RMSEs show this result quantitatively. It reconfirms the conclusion that the optimization algorithms are insensitive to noise.
The same method is adopted by the gradient descent algorithm and the Newton algorithm to calculate the scale factor; therefore, the red and yellow lines in Figure 9 are identical. In Figures 10–12, the calibrated attitude misalignment errors are extremely approaching after 200 s. Thus, their mean value in Table 6 is almost equal. However, the Newton algorithm reaches this accuracy earlier than the gradient descent algorithm. This proves that the second-order Newton optimization algorithm has higher efficiency than the first-order gradient descent optimization algorithm. In this field test, the roll angle error is still not solved correctly as it is unobservable. Fortunately, it is not necessary to be calibrated because it has no effect on navigation accuracy.
6. Conclusion
To enhance the performance of the DVL calibration, the iterative calibration algorithm based on Newton optimization is proposed innovatively. According to the analysis in theory and the test validation, three important conclusions can be achieved. (1) The loss function established in optimization algorithms is a quadratic function of the attitude misalignment angles. Therefore, the higher efficiency of convergence can be gained by the second-order Newton optimization algorithm compared with the first-order gradient descent. (2) The introduction of the Wolfe principle enables us to search the optimal step adaptively making the calibration accuracy steadier. (3) The iterative calibration algorithm which solves the scale factor and attitude misalignment separately can get a more accurate estimation result because it avoids the coupling between attitude misalignment and scale factor.
Data Availability
The raw/processed data cannot be shared at this time as the data also form part of an ongoing study.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported in part by The Inertial Technology Key Lab Fund under Grant 614250607011709, the Fundamental Research Funds for the Central Universities under Grants 2242019K40040 and 2242019K40041, Key Laboratory Fund for Underwater Information and Control under Grant 614221805051809, and the Natural Science Foundation of Jiangsu Province, China, under Grant BK20190344.