Optimization-based INS in-motion alignment approach for underwater vehicles

Optik 124 (2013) 4581–4585

Contents lists available at ScienceDirect

Optik journal homepage: www.elsevier.de/ijleo

Optimization-based INS in-motion alignment approach for underwater vehicles Wanli Li a,∗ , Kanghua Tang a , Liangqing Lu a , Youlong Wu b a Laboratory of Inertial Technology, Department of Automatic Control, College of Mechatronics and Automation, National University of Defense Technology, Changsha, Hunan 410073, China b School of Mechanical Engineering, Nanjing University of Science and Technology, Nanjing, Jiangsu 210094, China

a r t i c l e

i n f o

Article history: Received 3 September 2012 Accepted 19 January 2013

Keywords: In-motion alignment Doppler Optimization-based

a b s t r a c t Inertial navigation system (INS) in-motion alignment without any geodetic frame observations is still one of the toughest challenges for underwater vehicles. The traditional Kalman-based method which requires a stage of coarse alignment is not suitable for fast in-motion alignment for its slow convergence. In this paper, a novel optimization-based alignment (OBA) scheme is proposed by using the body frame velocity from Doppler Velocity Log (DVL) only when the initial position of the vehicle is available. The INS alignment is transformed into an attitude determination problem which is known as Wahba’s problem. A real time implementation using singular value decomposition (SVD) is given. In addition, a backtracking strategy for updating the final position is proposed. Experimental results show that the accuracy of the alignments is within 0.1◦ in the 600 s alignments. © 2013 Elsevier GmbH. All rights reserved.

1. Introduction The initial alignment is of great importance for any Inertial Navigation Systems (INS). As a dead-reckoning navigation method, the performance of the subsequent navigation computation is largely determined by the accuracy of the alignment process [1]. Traditionally, the alignment process consists of two steps: coarse alignment and fine alignment. In the stage of the coarse alignment, the attitude is estimated to the accuracy of a few degrees by using the accelerometer and gyro measurements [2]. The subsequent fine alignment is usually implemented with gyro-compassing or Kalman filtering methods. Usually, the external adding sensors which provide geodetic frame observations such as GPS are required [3]. Unfortunately, GPS measurements are unavailable for underwater applications. Doppler Velocity Log which measures the speed of the vehicle relative to the sea floor is a good option to assist the INS alignment [4,5]. However, as the velocity measurements provided by DVL is in the Doppler instrumental frame, slow convergence is an inevitable problem when using the traditional Kalman-based scheme. Recently, a novel alignment method using the solutions of the Wahba’s problem is proposed [6–9,12,13]. But in [9,12] related with Wahba’s problem, quite rough formulas are presented that only two pairs of vectors are chosen to provide a least squares fit.

∗ Corresponding author at: Laboratory 313, Department of Automatic Control, College of Mechatronics and Automation, National University of Defense Technology, Changsha, Hunan 410073, China. Tel.: +86 13574835140; fax: +86 073184576305x8216. E-mail addresses: [email protected] (W. Li), tt [email protected] (K. Tang), [email protected] (L. Lu), [email protected] (Y. Wu). 0030-4026/$ – see front matter © 2013 Elsevier GmbH. All rights reserved. http://dx.doi.org/10.1016/j.ijleo.2013.01.069

In [6], this technique is further concluded as an optimization-based alignment (OBA) method by using infinite vector observations and the INS alignment can be equivalently transformed into a “continuous” attitude determination problem. This method is successfully applied in GPS-aided In-flight alignment [8,13]. It is effective with any initial attitude and shows faster convergence than the Kalmanbased method [8]. In [7], an interleaved sampling strategy which improves the accuracy of the alignment is proposed. In this paper, the optimization-based alignment method is adjusted for underwater vehicles applications only when the initial position and Doppler velocity measurements are available. Compared with the traditional Kalman-based methods, the advantage of this technique is obvious. It is effective with any initial attitude and no initial information is required. Experimental results show that this technique is more robust and suitable for fast INS initial alignment. The remainder of this paper is organized as follows. Section 2 presents the mathematical model of the optimization-based alignment method. Section 3 is devoted to the implementation of the proposed algorithm. Section 4 presents the experimental results. Conclusions are drawn in Section 5. 2. Mathematical modeling 2.1. Decomposition of the attitude matrix The attitude matrix, which relates the INS body frame b to the navigation frame n (local-level frame and its orientation is northup-east (NUE)), can be decomposed as follows [9]: C nb = C nn0 C nin0 C iin0 C ibb0 0

b0

(1)

4582

W. Li et al. / Optik 124 (2013) 4581–4585

where n0 is the n frame at the start-up; i is Earth-centered initially fixed (ECIF) orthogonal reference frame; ib0 and in0 denote the inertial reference frames, they are formed by fixing the b frame and n frame at the start-up in the inertial space respectively. The rotation matrix of the b frame relative to the ib0 frame C ibb0 is initialized to a 3 × 3 identity matrix and updated by the gyro measurements as follows: i C˙ bb0

b C ibb0 [ωib ×]

=

C nin0 0

(2)

is slowly changing due to the Earth’s rotation. It is given by

C nin0 = C ne 0 C eie C iien0 0

(3)

0

0

where e denotes the Earth-centered Earth-fixed (ECEF) orthogonal reference frame, another newly defined inertial reference frame ie0 is formed by fixing the e frame at the start-up in the inertial space. There exists:

⎡ ⎢

− sin L0 cos 0

− sin L0 sin 0

cos L0

cos L0 sin 0

sin L0

cos 0

0

C ne 0 = ⎣ cos L0 cos 0

⎡ C eie 0

− sin 0 cos(ωie (t − t0 ))

sin(ωie (t − t0 ))

0

⎤

C iien0 0

=

0

⎥ ⎦

0

n0 rE (tk ) tan L0

rN

rN n

0 (tk ) −rE

0

rN

rE

n0 −rE (tk ) tan L0

n0 rE (tk )

rN

rE

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

≈

t+t



=

n0 rN (tk−1 ) n0 (tk−1 ) rE

n0 rN (tk−1 ) n

0 (tk−1 ) rE



+

1 0 0

0



t+t

+

0 1

0

0

0

1

(7)



C inn0 C iin0 b 0 0

tk

tk−1

− C iibn0 C inn00 (t)C nn0 (t)vr (t) − C ibb0 (t)C bd vde (t) 0







tk

tk−1

(8)

C nn0 C nin0 C iin0 C ibb0 C bd vde dt ≈ P n (tk−1 ) b

0

b0

cos L(t)

T

T

C inn00 C nn0 {g nl + ωnie × [ωnie × r]}d}



C ibb0 (t)C bd vde (t) − C ibb0 (t

+ t)C bd vde (t

Define:

V 1 (t) = C inn00 (t + t)C nn0 (t + t) 0

− C inn00 (t)C nn0 (t) 0



0

+ t) + t

0 ωie rE

ωie rE

cos L(t)

t+t

C ibb0 f b d

cos L(t + t)

(13)

T

T

t+t

C inn00 C nn0 {g nl + ωnie × [ωnie × r]}d

t



t+t

+

C ibb0 f b d

(14)

(9)

(15)

Equation (13) can be described as V 2 (t) = C iibn0 V 1 (t)

(16)

0

By choosing the sampling period t, two point sets can be obtained to estimate C iibn0 . Therefore, the INS alignment problem 0

b0

+ C nn0 C nin0 C iin0 C ibb0 C bd vde (tk − tk−1 )

ωie rE

cos L(t + t)

V 2 (t) = C ibb0 (t)C bd vde (t) − C ibb0 (t + t)C bd vde (t + t)

where C bd is a constant rotation matrix from the Doppler sonar’s instrument frame to the b frame [14]; vde is the velocity of Doppler. Furthermore, the current geographic position Pn can be updated by dead reckoning (DR):



0

ωie rE

t+t

−

C ibb0 C bd vde dt

b0

T

C iibn0 {C inn00 (t + t)C nn0 (t + t) 0 0

−

C nin0 C iin0 C ibb0 C bd vde (tk − tk−1 )

(12)

where vr (t) = 0 0 ωie rE cos L(t) is caused by the rotation of the Earth. Substituting Eq. (12) into Eq. (11), it yields:

t

P n (tk ) = P n (tk−1 ) +

(11)

v˙ iib0 d = C iibn0 C inn00 (t + t)C nn0 (t + t)vr (t + t)

t

 0

0

0

=

0

{C ibb0 f b + C iibn0 C inn00 C nn0 {g nl + ωnie × [ωnie × r]}}d

t

0

1

t+t

v˙ iib0 d =

− C inn00 (t)C nn0 (t) 0

b0





0

n0 (t ) and r n0 (t ) can attitude matrix can be obtained by Eq. (1). rN k E k be updated by DVL measurements as follows:



T

+ C ibb0 (t + t)C bd vde (t + t)

matrix. Once the initial attitude matrix C iin0 can be estimated, the

n

[ωnie × r] is the centripetal force with r being r = 0 rE 0 . The change of the velocity in the ib0 frame is given by the integral of the Eq. (10):

t

b0

0 (tk ) rE

T [ωie cos L ωie sin L 0 ] is the Earth’s rotation rate in the navigation frame with L being the current geographic latitude; ωnie ×

1

n



where fb is the accelerometer measurement; g nl is the local level gravitational acceleration expressed in the n-frame; ωnie =

(5)

n0 rN (tk )

0 −rN (tk )

(10)

0



where rN and rE are the meridian and transverse radius of the earth n0 (t ) and r n0 (t ) denote the incremental distance respectively; rN k E k of the vehicle in the North and East directions at each time epoch. Since both the in0 frame and the ib0 frame are fixed with respect to the inertial reference frame at the start-up, C iin0 is a constant

n0 rN (tk )

v˙ iib0 = C ibb0 f b + C iibn0 C inn00 C nn0 {g nl + ωnie × [ωnie × r]}

(6)

⎡



In the ib0 frame, the velocity differential equation is described

The equivalent change in velocity is given by the Doppler measurements as follows:

⎤

where L0 and 0 are the geographic latitude and longitude of the initial position respectively; ωie is the turn rate of the Earth. C nn0 is slowly changing with the movement of the vehicle, which can be obtained by

C nn0

as

(4)

(C ne 0 )T

⎢ ⎢ ⎢ ≈I+⎢ ⎢ ⎢ ⎣

0

t

⎢ ⎥ = ⎣ − sin(ωie (t − t0 )) cos(ωie (t − t0 )) 0 ⎦ 0

2.2. Estimation of C iibn0

is transformed into a optimization problem [6,8] which is known as Wahba’s problem [10]: Given two point patterns {xi } and {y i }, i = 1, 2, · · ·, n in m-dimensional space, and we want to fine the

W. Li et al. / Optik 124 (2013) 4581–4585

similarity transformation parameter (R: rotation matrix) giving the minimum value of the mean squared error e2 (R) of these two point patterns: 2 n  1 2   e (R) = y i − Rxi

n

(17)

t = tm+1 . As the update rate of INS is much higher than that of DVL, the sampling time points are chosen according to the DVL. Define:



0

b(t) = 0

V 1 (ti ) V T2 (ti )

Let

(18)

UU = GG = I 3×3

(19)

D is a diagonal matrix of the singular values of M, define:



S=

if det(M) ≥ 0

I 3×3

diag(1, 1, −1) if det(M) < 0

(20)

Then the optimum rotation matrix is uniquely determined by C iibn0 0

= R = USG

T

(24) (25)

(21)

ωie rE

cos L(t)

T

(26)

The alignment algorithm is summarized below: Step 1: Initialization: C nn0 = I 3×3 , C iibn0 = I 3×3 . 0

Step 2: Update the integral of a(t) and b(t); update C nn0 and the current position by Eqs. (7)–(9). Step 3: If the DVL measurement is available, record the values of a(t), b(t), c(t) and d(t). Calculate the defined two point sets V1 (t) and V2 (t) by the recorded data, e.g.:



t2m

tm

a singular value decomposition of M, where

T

T

C inn00 C nn0 {g nl + ωnie × [ωnie × r]}d

t

i=1

UDGT

(23)

d(t) = C inn00 (t) C nn0 (t) 0 0

There are four major techniques reported for solving the socalled Wahba’s problem [11]. Among them, the SVD-based method is thought to be one of the most stable algorithms which provide the best overall accuracy. Therefore, the SVD-based method is employed to provide a least squares fit in this paper. The transformation parameter is given as follows [10]: m 

C ibb0 f b d

c(t) = C ibb0 (t) C bd vde (t)

3.1. SVD-based least squares estimation

M=

t

a(t) =

i=1

3. Algorithm implementation

4583

C ibb0 f b d = a(t2m ) − a(tm )

(27)

Step 4: Update C iibn0 by the SVD-based least squares estimation. 0

Then the current attitude matrix can be obtained by Eq. (1). Step 5: Go to Step 2 until the alignment is finished. Furthermore, when the alignment is finished, the current geodetic position can be obtained if C ibb0 is recorded each DVL update cycle during the process of the alignment. The final current geodetic position can be given by P n ≈ P n (t0 ) +

n 

C nn0 (ti )C nin0 (ti )C iin0 C ibb0 (ti )C bd vde (ti ) 0

b0

(28)

i=1

3.2. Implementation

where P n (t0 ) denotes the initial position; C nn0 is updated by Eqs. (7) and (8). Since the accuracy of C iin0 is high enough when the

The accuracy of the alignment is largely determined by the errors in V1 (t) and V2 (t). The error sources in the alignment include [9]:

4. Experimental results and analysis

b0

(a) The error in C nn0 caused by position update error of the moving vehicle. (b) The error in Doppler velocity measurements. (c) The error in Doppler installation misalignment matrix C bd . (d) The error in C ibb0 caused by gyro bias. (e) The error in acceleration measurements caused by motion disturbance. Therefore, the integral time interval t should be big enough so that the integration process can average these errors over a period of time. Following the idea of the interleaved sampling strategy proposed in [7], the value of t and t + t are chosen as follows: t + t = tm+1 ,

t = t1

t + t = tm+2 ,

t = t2 (22)

.. . t + t = t2m ,

t = tm

where t2m represents the current time epoch. It means that m pairs of vectors can be obtained to provide a least squares fit at t2m . If the current time epoch is odd (e.g. t2m + 1 ), m + 1 pairs of vectors can be obtained and the last pair is selected as: t + t = t2m+1 ,

alignment is finished, an accurate position can be obtained using this backtracking strategy.

4.1. Test arrangement The ship-mounted experimental data were collected to evaluate the performance of the in-motion alignment. The test was carried out in Yangzi River. It was equipped with a navigation degrade IMU, a bottom-lock Doppler and a GPS receiver. In the experiment, the IMU and the GPS receiver were set up on a vessel. The DVL module was put beneath 1 m underwater. The specifications of the sensors are listed below: (a) IMU: consists of three ring laser gyroscopes with a drift rate ◦ of 0.01 /h (1) and three quartz accelerometers with bias of 5 × 10−5 g (1). Its update rate is 200 Hz. (b) Bottom-lock Doppler: provides three-axis velocity measurements in the Doppler instrumental frame with accuracy±5‰ of speed and update rates up to 1 Hz. (c) GPS receiver: provides velocity with precision of about 0.1 m/s, position with precision of about 10 m, and update rates up to 1 Hz. 4.2. Alignment results During the alignment experiments, the vessel sailed at the speed of about 4.5 m/s. Each alignment lasted 600 s. Regarding the

4584

W. Li et al. / Optik 124 (2013) 4581–4585

Fig. 1. Attitude errors relative to the alignment time.

attitude obtained from a high precision INS/GPS integration as reference, Fig. 1 shows the attitude errors during the process of the alignment. As can be seen from the figure, the estimations of the misalignments converged with time. In addition, the horizontal misalignments showed faster convergence than the heading. Fig. 2 illustrates the heading errors of another six experiments during the alignments. It is clearly shown by the partial magnifications in Fig. 2, all of the heading errors converged within 0.1◦ in the 600 s alignments. The accuracy of the heading is about 0.04◦ (1). Fig. 3 shows the position errors during the process of the alignment and that obtained by the backtracking method using Eq. (28). During the alignment, the vessel sailed about 2726 m. Due to the large attitude error at the beginning, the real time position error grew quickly. But with the increase of the alignment time, the

Fig. 4. Heading error comparison with different sampling strategies.

accuracy of the attitude was also increasing. Therefore, the position error reminded around 130 m. However, by using the backtracking method, the final position error is only 15 m and hence makes it possible for practical applications. 4.3. Comparison with other techniques Fig. 4 compares the heading errors by using the interleaved sampling strategy and the sampling method reported in [9,12] referred as the traditional method. It is clearly shown that the heading error corresponding with the interleaved sampling strategy converged much faster than that with the traditional method. It took the interleaved sampling method only 150 s for the heading error to converge within a few tenths of one degree. In addition, with the interleaved sampling strategy, the error curve of heading is of good smoothness; while using the traditional method, the oscillations of the heading error are especially apparent. Fig. 5 compares the heading errors by using the proposed optimization-based and the Kalman-based alignment methods [4,5]. A coarse alignment stage is usually required with the Kalman-based alignment method. But to simplify that analysis, the Kalman-based fine alignment results with initial heading error from 1◦ to 4◦ are presented. It is clearly shown that if the initial heading error is larger than 2◦ , it could not be guaranteed that the heading error is less than 0.1◦ within 600 s by the KF-based method. However, the proposed optimization-based alignment method is effective with any initial attitude and no coarse alignment is required. Therefore, this method is more suitable for fast in-motion alignment for underwater vehicles.

Fig. 2. Heading errors relative to the alignment time.

Fig. 3. Position errors relative to the alignment time.

Fig. 5. Heading error comparison with different alignment methods.

W. Li et al. / Optik 124 (2013) 4581–4585

5. Conclusions In this paper, a novel optimization-based alignment scheme has been developed for underwater vehicles. With the derived DVL-aided alignment model, the INS alignment problem is transformed into a well known attitude determination problem known as Wahba’s problem. Experimental results have demonstrated that this technique is suitable for underwater INS in-motion alignment for its rapidness and robustness. This technique enables the accuracy of the alignments better than 0.1◦ within 600 s. However, as [7] points out, this method takes no account of the sensor biases (accelerator bias, gyro bias) and hence limits the accuracy of the alignment. Further work is still needed to reduce the effects of the sensor biases. Acknowledgments This work is supported by the National Natural Science Foundation of China (No. 61104201). Acknowledgment is also given to Gongmin Yan for his fruitful talks on related topics. References [1] S. Han, J. Wang, A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications, J. Navig. 63 (2010) 663–680.

4585

[2] W. Sun, F. Sun, Novel approach to GPS/INS integration for IMU alignment, J. Syst. Eng. Electron. 22 (3) (2011) 513–518. [3] L. Schimelevich, R. Naor, New approach to coarse alignment, in: Proceedings of Position, Location and Navigation Symposium, 1996, pp. 324–327. [4] W. Gao, X. Zhang, G. Zhao, Y. Ben, A fine alignment method about Dopplerassisted SINS, in: Proceedings of the 2010 IEEE International Conference on Information and Automation, 2010, pp. 2333–2337. [5] Y. Ben, Z. Zhu, Q. Li, X. Wu, DVL aided fine alignment for marine SINS, in: Proceedings of the 2011 IEEE International Conference on Mechatronics and Automation, 2011, pp. 1630–1635. [6] M. Wu, Y. Wu, X. Hu, D. Hu, Optimization-based alignment for inertial navigation systems: theory and algorithm, Aerosp. Sci. Technol. 15 (2011) 1–17. [7] P.M.G. Silson, Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci, IEEE Trans. Instrum. Meas. 60 (6) (2011) 1930–1941, 15. [8] T. Kang, J. Fang, W. Wang, Quaternion optimization based in flight alignment approach for airborne POS, IEEE Trans. Instrum. Mea. 61 (2013) 2916–2923. [9] W. Li, W. Wu, J. Wang, L. Lu, A fast SINS initial alignment scheme for underwater vehicle applications, J. Navig. 66 (2013) 181–198. [10] S. Umeyama, Least-square estimation of transformation parameters between two point patterns, IEEE Trans. Pattern Anal. Mach. Intell. 13 (4) (1991) 376–380. [11] D.W. Eggert, A. Lorusso, R.B. Fisher, Estimating 3-D rigid body transformations: a comparison of four major algorithms, Mach. Vis. Appl. 9 (1997) 272–290. [12] D. Gu, N. El-Sheimy, T. Hassan, Z. Syed, Coarse alignment for marine SINS using gravity in the inertial frame as a reference, in: Proceedings of Position, Location and Navigation Symposium, 2008, pp. 961–965. [13] Y. Wu, X. Pan, Velocity/position integration formula (I): application to inflight coarse alignment, IEEE Trans. Aerosp. Electron. Syst. (2013), Available on http://arxiv.org/abs/1207.1550, in press. [14] J.C. Kinsey, L.L. Whitcomb, In situ alignment calibration of attitude and Doppler sensors for precision underwater vehicle navigation: theory and experiment, IEEE J. Ocean. Eng. 32 (2) (2007) 286–298.