Abstract
The iterative closest contour point (ICCP) matching algorithm has become more and more widely used in the underwater geomagnetic aided inertial navigation system (INS). In practical application, the traditional ICCP algorithm is sensitive to the initial positioning error of the INS and can only do rigid transformation for the INS track of the vehicle. Particularly when there exists scale error, the accuracy and stability of the traditional ICCP algorithm will be affected. To solve this problem, an improved algorithm based on affine transformation is proposed. Firstly, the fundamental of the ICCP is analyzed in detail, and an error analysis of the geomagnetic aided inertial navigation system is carried out, and then the rigid transformation is replaced with affine transformation to improve the performance of the ICCP. In contrast to the conventional approach, the proposed algorithm can solve the rotation, translation, and scaling parameters of the indicated track and the matching track, so it can significantly reduce the interference of the scale error. Experimental results confirm the effectiveness of the proposed algorithm.
1. Introduction
The inertial navigation system (INS) is an autonomous navigation system, which neither relies on any external information nor radiates energy to the outside world. Besides, it has the advantages of a short term, high precision, good concealment, no susceptibility to interference, and so on. Now the INS is the main navigation method for an Autonomous Underwater Vehicle (AUV). However, due to its navigation characteristics, the error of the INS inevitably grows with time. To address this problem, some integrated navigation systems are used to revise the INS error. The terrain aided inertial navigation system is one of the integrated navigation systems. However, the terrain aided inertial navigation system cannot achieve good effect in the area where the terrain is flat or the terrain features are not obvious [1, 2]. In recent years, some other integrated navigation systems such as the INS/Doppler integrated navigation system and INS/geomagnetic integrated navigation system have been studied to provide more accurate and more reliable positioning information [3, 4].
Geomagnetic navigation technology has been used in aircraft control, missile weapon guidance, underwater vehicle positioning, and other fields, showing great military value and application prospects [5, 6]. The key factors affecting the accuracy of geomagnetic matching navigation are the establishment of geomagnetic map, matching algorithm, and the accuracy of geomagnetic sensor measurement [7]. In some practical applications requiring high precision, it is difficult to achieve satisfactory results using the geomagnetic navigation. More often, geomagnetic navigation is mainly used to assist other navigation systems. The geomagnetic aided inertial navigation system is one of the most common methods, and in recent years, several methods have been proposed [8, 9].
In the underwater geomagnetic aided inertial navigation system, the matching algorithm is the key technology. Presently, the iterative closest contour point (ICCP) is one of the most frequently used algorithms, which derives from the iterative closest point (ICP) algorithm in image matching [10]. The ICCP was first proposed by Behzad [11], who used it for gravity-aided navigation; then Bishop systematically analyzed the feasibility and the error effect of the algorithm through a large number of simulation experiments [12]. The traditional ICCP algorithm is easy to implement and has been widely used; however it still has some deficiencies; for example, its robustness is poor and it is prone to diverging and mismatching under large initial error. To resolve these problems, many methods have been put forward. Wang [13] designed a method which is based on the integration of TERCOM and ICCP technology: firstly use the terrain contour matching navigation system (TERCOM) for rough matching and then use the ICCP for accurate matching. Zhang [14] proposed a pretranslation simplified ICCP algorithm, which simplifies the process of matching calculation, eliminates the influence of mismatch in the traditional ICCP effectively, and ensures the credibility of autonomous navigation. Zhang [15] proposed an improved ICCP algorithm based on the particle swarm optimization algorithm, which can achieve better robustness and accuracy. These researches enhanced the traditional ICCP algorithm and achieved certain results, but the algorithm can only do rigid transformation and translation transformation for the indicated track to achieve iterative registration. In the actual situation, there is always scale error between the indicated track and target track, and this may lead to mismatching or lower the matching accuracy [16, 17].
In geometry, an affine transformation is a function between affine spaces, which preserves points, straight lines, and planes [18]. In computer vision, affine transformation is usually used for image registration [19]. Xu [20] proposed an improved ICCP algorithm with affine correction in terrain matching navigation. Xu first used the ICCP algorithm for matching and then corrected the results with affine transformation. However, our work is inspired by the pyramid algorithm in 2D image matching [21] and incorporates the idea of the Iterative Multiscale Matching Method (IMMM). Affine transformation is used multiple times during the iteration process of the initial matching along the contours. Since the initial value of each matching is the optimal value obtained from the previous matching, the iterative process has certain inheritance. The affine correction runs through the whole iteration process. Therefore, from this point of view, our work is different from Xu’s work, and this is also the major contribution of our work.
In this paper, the error characteristics of the INS are analyzed in detail, and the error analysis of the INS is made. Then, the iterative process of matching track based on the fusion algorithm of the ICCP and affine transformation is described. Through affine transformation, the transformation parameters between the indicated track and the matching track are solved and the corresponding relationship is obtained. Finally, the simulation experiment is conducted; the results prove the validity of the proposed algorithm.
2. The Error Analysis of the Geomagnetic Aided Inertial Navigation System
The inertial navigation is a relative navigation method, whose principle is to use inertial components to measure the acceleration of the AUV and then get the current position through an integral operation and initial position information. The error characteristics of the geomagnetic aided inertial navigation system are analyzed as follows:
A purely inertial navigation mode is used before the underwater vehicle enters the matching area. In the inertial navigation system, after ignoring the Coriolis errors and gravity vector errors, the velocity error is a function of specific force , attitude error , and specific force measurement error [22]. The differential equation of the velocity error can be expressed as
In the equation, is the attitude transition matrix from the carrier coordinate to the navigation coordinate.
When the underwater vehicle enters the matching area, due to the integration of each error over time, the system has accumulated a certain amount of position error , velocity error , and attitude error . These errors continue to be transmitted during the accumulation process of matching data. When data accumulation is completed, the velocity equation can be expressed as
In the equation, is the indicating speed of the inertial navigation system at the time ; is the actual speed at the time ; is the variation of velocity error in the process of data accumulation, mainly caused by and .
Since is a velocity error generated before matching, it can be regarded as a constant error and is easy to be disposed of; but is a function of time and , and it also accumulates over time and must be simplified during the modeling. Considering that the accumulation amount of attitude error is very small in a short time, the variation of heading error in the data accumulation phase is ignored; that is, it is assumed that the heading error of the inertial navigation system is kept constant.
In the matching process, the geomagnetic matching positioning mainly determines the position of the vehicle in the horizontal plane, so the attitude has little influence on the positioning results. Under the assumption that the heading error remains unchanged, becomes a linear function of time. If further consideration is given to replace the entire error line with the mean of , the entire matching process is simplified to a constant velocity error process. The velocity error is
In the matching process, the velocity equation is simplified to
If the time entering the matching area is zero and the sampling interval is , then the discretized form of the position updating equation of the inertial navigation system is
Let be the real position of the vehicle; then
As mentioned above, is the accumulated position error before the system enters the matching area. According to (4), (5), and (6),
In (7), is the final positioning error term. It can be seen that the position error of the vehicle is mainly composed of two parts: system position error before matching and position increment error caused by velocity error. The change reflected in the track can be simplified as a transformation between curves, including translation, rotation, and scaling, which, respectively, correspond to the initial position error, heading error, and rate error of the navigation system. Its mathematical description can be expressed aswhere is the track of the inertial navigation system, is a scaling factor, is a rotation factor, and is the actual track.
Through the above analysis, we can see the following: under the influence of velocity error and heading error, there is not only rotation and translation between the INS track and the actual track of the underwater vehicle, but also the scaling. Hence, it is not a rigid transformation relation that involves only rotation and translation. The traditional ICCP algorithm assumes that the actual track is a rigid transformation of the INS indicated track; thus, the matching error may become bigger and even lead to mismatching.
3. Matching Algorithm
3.1. The Principle of the Traditional ICCP Algorithm
The basic principle of the underwater geomagnetic aided navigation system based on the ICCP algorithm is shown in Figure 1. In the underwater navigation, the AUV goes along the actual track , but due to inertial system errors and environmental factors, the inertial navigation system will give an indicated track sequence that deviates from the actual track sequence, and at the same time, the magnetic value of the corresponding points is obtained by the magnetometer. It is generally believed that the position information given by the inertial navigation system is credible in a certain range of error. Therefore, the track sequence of the main inertial navigation system can be treated as the sequence to be matched. According to the geomagnetic information sequence, contour lines are generated in the digital map, and the nearest point sequence is extracted as the matching target then iteratively searches for the rigid transformation , which contains a rotation quantity and a translation quantity . The rigid transformation minimizes the following objective function:where represents the weights of each sequence point, represents the iteration process, and represents the number of track sequence points.
After using the singular value decomposition (SVD) method to obtain the rigid transformation , a rigid transformation is applied to the matching track sequence .where is the track sequence after transformation, and are translations, and is a rotation angle.
The transformed sequence is used to update the sequence , then re-search the nearest point sequence corresponding to the updated matching track on the contour line, and find the new rigid transformation to minimize the objective function. The iteration process will end when ( indicates the set threshold) or the maximum number of iterations is reached. The matching track at this time is the traditional ICCP matching track sequence.
3.2. Improved ICCP Algorithm
3.2.1. A Simplified Affine Transformation Model
An affine transformation is a combination of single transformations such as translation, scaling, similarity transformation, reflection, rotation, shear mapping, and compositions of them in any combination and sequence. Moreover, an affine transformation is one in which parallel lines remain parallel after the transformation. Based on the error analysis of the geomagnetic aided inertial navigation system in Section 2 and in this paper, the affine transformation between the tracks includes only translation, rotation, and scaling. Besides, the transformation in this paper is carried out in two dimensions; a simplified affine transformation model can be established aswhere is the position information before transformation, is the position information after transformation, and is the rotation angle, corresponding to the heading angle error; is the scaling amount, corresponding to the rate error; are are the translation amounts at the and Y directions, respectively, corresponding to the initial position error.
3.2.2. Affine Parameter Solving
Since the affine transformation matrix between the indication track and real track is confirmed, and a simplified transformation model is established by (11), defining rotation matrix , scaling factor , and translation , the indication track sequence , and the real track sequence , then the whole matching process can be performed with (12).
The parameters of each affine transformation are solved as follows [20]. is defined as the difference between sequence and sequence after affine transformation; thenLet , ; thenwhere , is the point's coordinate of ; , is the point’s coordinate of .
Construct the Euclidean Square Distance , which can be expressed asWhen ESD gets the minimum value, the minimum value of is recorded as . In this paper, can be solved by least squares as follows:
From (17) and , the affine parameter values , , , and can be got. When the parameters of each transformation are obtained, the final , , and can be obtained.
3.2.3. Procedure of Improved Algorithm
The procedure of the ICCP algorithm based on affine transformation is as follows:
① The inertial navigation system provides location information for points on the AUV track; the real-time magnetic sensor provides the magnetic values of the corresponding points. That is, the data form is known, and the contour lines are extracted from the known reference digital geomagnetic map
② Select the initial alignment set , which is to set the initial value of the iteration. Then follow the steps from ③ to ⑥, making loop iterations until the final iteration termination condition is reached
③ Find the nearest point for each data point on the contour lines, and name these points as
④ The affine transformation is used to solve the parameters , , and between and , so that the distance between the set and the set is minimized; that is, the following objective function is minimized:
⑤ Transform the set to the set
⑥ Judge whether can meet . If satisfied, it shows that the change of objective function is very small, and it achieves the accuracy to correct the inertial navigation track; otherwise, it is considered a failure
The above process is shown in Figure 2.
4. Experiment and Analysis
In order to test the accuracy and robustness of the improved ICCP algorithm mentioned above, experiments with simulated data and real geomagnetic data were carried out. In the simulations, the background geomagnetic information is provided by an a priori geomagnetic map, acquired in the Bohai Sea, China, as shown in Figure 3. The grid spacing is 200m×200m.
In the simulations, the vehicle is assumed to be equipped with a geomagnetic sensor to collect real-time geomagnetic data during the navigation. The vehicle is supposed to travel in a long track from one point to another point, the INS is assumed to have a drift rate of 1° in the horizontal distance travelled, and the main simulation parameters are shown in Table 1.
4.1. Test without Scale Errors
We first investigated the performance of the improved ICCP algorithm when there were no scale errors between the indicated INS track and real track. Figure 4 shows the matching results of the traditional ICCP algorithm and improved ICCP algorithm. It can be observed that the improved ICCP algorithm gets a similar result to the traditional ICCP.
Figure 5 shows the matching errors of both algorithms in the longitudinal and latitudinal directions. As can be seen, both algorithms can achieve the accuracy within 100 meters. The improved algorithm can get a little better accuracy within 60 meters. Obviously, when there are no scale errors, the difference between the two algorithms is not significant.
(a)
(b)
4.2. Test with Scale Errors
In the second situation, we investigated the performance of the improved ICCP algorithm when there were scale errors between the indicated INS track and real track. The matching results are presented in Figure 6. The result clearly shows that the traditional ICCP algorithm is seriously influenced.
Figure 7 shows the matching errors of both algorithms in the longitudinal and latitudinal directions. As can be seen, the improved algorithm can achieve the accuracy within 180 meters in the longitudinal direction and 140 meters in the latitudinal direction. The matching errors of the traditional algorithm are more than 1500 meters in the longitudinal direction and the maximum error in the latitudinal direction nearly reaches 2500 meters. Apparently, when the traditional ICCP algorithm suffers from large positioning errors due to scale errors, the improved algorithm can still maintain better accuracy and robustness.
(a)
(b)
4.3. Statistic Analysis of ICCP and Improved ICPP Algorithm
The above two tests have limitations. Because the navigation matching results are a little bit uncertain, one test does not reflect the performance of the algorithm very well. In order to test the performance of the ICCP and improved ICCP algorithm objectively and comprehensively, we repeated the above experiment in the same paths but with different matching lengths, and we obtained stable statistics after repeating 100 times. Evaluate the performance of the algorithms from the obtained mean error, variance, standard deviation, matching probability, and other information.
Table 2 shows the statistical results of the repeat tests.
It can be seen from the above experiments that the traditional ICCP algorithm is more susceptible to scale errors, but the improved algorithm can weaken the influence largely. The improved algorithm shows better robustness and accuracy.
5. Conclusions
This paper proposed an ICCP algorithm based on affine transformation to improve the performance of the traditional ICCP algorithm. In this method, the transformation between the indicated track and the matching track includes not only rotation and translation, but also scale transformation. The proposed method has been verified through computer simulations. Experimental results show that the improved ICCP algorithm is more accurate and more robust than the traditional ICCP algorithm; particularly it can effectively reduce the influence of scale error.
In future work, we will try to establish a more accurate geomagnetic field background field and combine the ICCP with evaluation of matching region suitability for the geomagnetic aided inertial navigation system that can be considered.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work was sponsored by the National High Technology Research and Development Program of China (Project No. 2009AA12Z311) and the National Natural Science Foundation of China (Project No. 41376109).