Novel self-alignment algorithm with unknown latitude for SINS on swing base

LYU Wei-wei, CHENG Xiang-hong

(1. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China;2. Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology of Ministry of Education,Southeast University, Nanjing 210096, China)

Novel self-alignment algorithm with unknown latitude for SINS on swing base

LYU Wei-wei1,2, CHENG Xiang-hong1,2

(1. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China;2. Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology of Ministry of Education,Southeast University, Nanjing 210096, China)

Traditional initial alignment method for strapdown inertial navigation system on swing base requires accurate local geographic latitude information. In view of the alignment on swing base with unknown latitude, an alignment method using gravitational apparent motion vectors at three different moments is proposed to implement the vector calculations. The basic principle of the self-alignment method using three vectors of gravitational apparent motion is deduced. Since the random noise in the accelerometer measurements would affect the accuracy of the constructed gravitational apparent motion vectors and generate collinear vectors in the process of vector operations, an adaptive Kalman filter based on average weighting coefficient is designed, which can effectively reduce the accelerometer random noises in dynamic uncertain environment and improve the alignment accuracy. Simulation and experimental results show that the proposed method can realize the self-alignment of the SINS on swing base when the latitude information is unknown, and the alignment accuracy can reach the extreme accuracy determined by the errors of inertial devices.

unknown latitude; gravitational apparent motion; average weighting coefficient; adaptive Kalman filter; self-alignment

Self-alignment in strapdown inertial navigation system (SINS) is to determine the initial value of attitude matrix by taking inertial device measurement values of the earth rotation angular rate and gravitational accelera-tion as natural reference quantity. Compared with other alignment methods such as transfer alignment and combination alignment, the self-alignment does not need external reference navigation information, so it has been widely studied and used in inertial navigation system. In 2000, IxSea Company claimed that a new self-alignment algorithm could fulfill the initial alignment within 5 min under any swing conditions in their newest product-the Octans[1]. After that, inspired by the alignment idea of tracing apparent motion, many implementation methods have been put forward, which can be divided into two types: the methods based on the dual-vector attitude determination[2,5]and the methods based on vector operation[3,9].

In the dual-vector attitude determination, two gravity vectors at different moments are selected to solve the matrix between navigation frame and body frame by matrix multiplication, and latitude information is necessary to obtain the theoretical gravity and its projection into initial navigation frame. Since only two gravity vectors are used, the alignment accuracy is easily affected by the measurement noise and is always used in coarse alignment stage. In some environments, such as tunnels,mountains, forests, or underwater areas, obtaining the accurate location is not easy. Some applications, such as communication satellite tracking or weapon platform stabilization, have higher directional requirement, but do not necessarily require accurate positioning. Under the conditions that latitude information is unknown, how to achieve the initial alignment, and what factors is the initial alignment accuracy related to? In the vector operation method, the attitude matrix between navigation frame and body frame is determined by geometric relationship among apparent motion vectors at different moments[7].Because no external information is needed, the vector operation method is a complete self-alignment method.

As gravity vectors directly participate in the vector calculation, the vector operation method also suffers from the measurement noise. In Ref. [7], the authors introduced a low-pass filter to reduce the noise data and have achieved some good results in certain environments.In Ref. [4,9], parameter recognition algorithms were designed based on the recursive least square (RLS) algorithm, in which the theoretical apparent motion from the calculated apparent motion was reconstructed without random noise. Although the above methods are useful for the particular studied objects, how to remove the measurement noise effectively in complex and dynamic environment is still a problem to be solved.

In this paper, a novel self-alignment algorithm with unknown latitude for SINS on swing base is studied. The alignment algorithm which is based on apparent motion and vector operation can accomplish the self- alignment on swing base. In order to obtain the accurate vector of gravitational apparent motion from the IMU measurements that contain random noise, an adaptive Kalman filter based on average weighting coefficient is designed,which can effectively remove the random noise and improve the alignment accuracy in dynamic uncertain environments. Finally, the proposed alignment method based on three gravitational vectors and adaptive denoising method is verified by simulation and experiment,and the results show that the alignment accuracy can reach the extreme accuracy determined by the errors of inertial devices.

1 Alignment method based on gravitational apparent motion

1.1 Gravitational apparent motion in inertial frame

The coordinate frames used in this paper are defined as follows:

① The e0frame is the Earth-fixed coordinate frame.The zeaxis is parallel to the Earth-fixed coordinate frame and the xeaxis is in the equatorial plane and points to the meridian of the ship initial position. The yeaxis completes the right-handed coordinate system;

② The i0frame is the non-rotating inertial coordinate frame, which is formed by fixing the e0frame at the beginning of alignment in the inertial space;

③ The n frame is the navigation coordinate frame which is the local level coordinate frame. The xnaxis points to East, the ynaxis points to North, and the znaxis points to the zenith;

④ The b frame is the ship body coordinate frame.The xbaxis points to the right, the ybaxis points to the forward, and the zbaxis points upward;

⑤ The ib0is defined as inertial frame, which is formed by fixing initial body frame in the inertial space.

The gravitational apparent motion in inertial frame refers to the track of the gravity vector, which is stable in the Earth and rotating in the inertial frame. The gravitational apparent motion in inertial frame can be described as Fig.1. In inertial frame, the gravitational apparent motion is illustrated as a cone with the vertex at the Earth center and the cone axis coinciding with Earth’s rotating axis, and the conical bottom radius is determined by the latitude where the IMU is located.

Fig.1 Trajectory of the gravitational apparent motion in inertial space

1.2 Alignment method based on two vectors of gravitational apparent motion

In the alignment method based on two vectors of gravitational apparent motion, the attitude matrix between body frame and navigation frame can be obtained as follows[2]:

where L is latitude, ωieis the Earth rotation rate, and t0is the start time of the alignment.is the rotation matrix of the body frame b relative to the reference frame ib0and can be calculated using the gyro output as follows:

where the superscripts “˄” and “～” denote the calculated value and the measurement value respectively, andis the gyro measurement value.

Therefore, the attitude matrixcan be determined by multiplication of the above four matrices.However, the alignment accuracy is easily affected by the measurement noise. Moreover, this alignment method needs precise latitude information to participate in attitude solution.

1.3 Alignment method based on three vectors of gravitational apparent motion

In this paper, we propose a self-alignment method based on three vectors of gravitational apparent motion.The attitude matrixcan be divided into two parts as follows:

According to Equations (1) and (4), the problem of solving matrixin self-alignment can be attributed to solving matrix.

1.3.1 Alignment method based on gravitational apparent motion

The geographical relationship between navigation frame and the cone of apparent motion is described as Fig.2.

Fig.2 Alignment mechanism based on gravitational apparent motion

The points of O and Ocdenote the Earth’s center and conical bottom center, respectively. At moment t, the origin of frame n is located at, which is at the circle of conical bottom. The vectorconnecting the point O andis consistent with the Up-axis of frame. The vectorconnecting the point O and Ocis consistent with the Earth rotating axis. The product ofis consistent with the east-axis of frame n, while that of U×E is consistent with the north-axis of frame n. When the projections of all axes in navigation frame are determined in inertial frame, the matrix can be constructed as follows:

Where E, N, and U are the projections of the axes of frame n in frame.

The gravitational apparent motion in inertial framecan be expressed as follows:

Fig.3 Calculation method for cone axis

As shown in Fig.3, the apparent motion vectorsat three moments are used to construct the procedure vectors. Therefore, the vectorcan be constructed as follows:

From Fig.2, the vector U can be directly calculated as follows:

Therefore, the vector N can be constructed as follows:

Based on Equations (10) - (12),in Equation(7) can be constructed as follows:

According to Equations (6), (4), and (13), the initial alignment for SINS can be completed, and the theoretical alignment accuracy of this method can be expressed as follows:

where∇Eand∇Nare the equivalent accelerometer errors of the east and north respectively,εEis the equivalent gyro drift of the east,is the acceleration of the gravity, andφE,φN,φUare the minimized alignment errors of pitch, roll, yaw respectively.

As shown in Fig.2, the calculation formula of latitude can be expressed based on the relationships among the apparent motion vectors:

The theoretical accuracy of latitude is related with three axial accelerometer biases and three axial gyro biases, which have effects on the vector-operation in initial alignment process.

1.3.2 Simulation

To facilitate the analysis, the alignment of static base conditions is considered. The simulation conditions are shown in Table 1, and the sensor errors are shown in Table 2. In Case 1, the ship is assumed to be static, and the IMU instruments only have constant errors, as shown in Table 2. In Case 2, the ship is assumed to be static,and the IMU instruments have constant and random errors which are also shown in Table 2. In both cases, the longitude and latitude of the ship are set at 118°E and 32°N. The strapdown algorithm’s update cycle is 5 ms,and the sampling frequency is 200 Hz.

Tab.1 Alignment under two conditions

Tab.2 Sensor error settings

1.3.3 Simulation results

The simulation lasts for 1100 s and the self-alignment is completed at 500 s point. The alignment error curves of yaw, pitch and roll are denoted in Fig.4 respectively, and the red dashed lines and the blue solid lines represent Case 1 and Case 2 respectively. Fig.4 indicates that the initial alignment in Case 1 can be completed, and the alignment accuracy is equal to the theore-tical values determined by Equation (14). Although small fluctuation with 0.03° exists over time, the alignment errors of pitch and roll in Case 2 are close to the theoretical values. However, the yaw error in Case 2 is so large that the alignment process cannot be completed. Simulation results show that the alignment accuracy is independent of alignment time, but it can be easily disturbed by random noise.

Fig.4 Curves of alignment errors (Red dashed lines: Case 1;Blue solid lines: Case 2)

2 Improved method based on adaptive denoising method

According to the analysis in Ref. [6]-[9], the main reason for alignment failure is attributed to the random noise in accelerometer measurements, which causes the following two problems: ① the projections of gravity vector in inertial frame cannot represent the change of gravity because of the random noise; ② as shown in Equation (9), the vectormay get a wrong or reverse value due to the collinearity problem between the vectors of.

Therefore, the random measurement noise of accelerometers is the major reason for the failure of the alignment method based on three vectors of gravitational apparent motion. So how to remove the random measurement noise of the accelerometers in dynamic uncertain environment is a problem which needs to be considered.In this paper we introduce an adaptive Kalman filter (AKF)based on average weighting estimation for denoising the random noise of inertial sensors to improve the precision of measurements[10-13].

2.1 The theory of adaptive Kalman filtering

Considering a multivariable linear discrete system,the state equation and measurement equation can be written as follows:

where Xkis (n×1) state vector, Φk-1is (n×n) transition matrix, Zkis (r×1) observation vector, Hkis (r×n)observation matrix. VariablesWk-1and Vkare uncorrelated white Gaussian noise sequences with means and covariances:

where E(·) denotes the expectation function. The Qkand Rkare the covariance matrix of process and observation errors, respectively. The recursive formulas of innovation-based standard Kalman filter are depicted as follows:

In order to remove the random noise effectively in dynamic environment, an adaptive Kalman filter based on average weighting estimation is proposed. The innovation in AKF is the difference between the measurement value by the filter and its predicted value. Therefore, the innovation is selected as an evaluation factor to detect the divergence of the filter and adjust the Kalman filter parameter online. From Equation (21), the innovation sequence can be written as:

Taking variances on both sides of Equation (25),and considering the orthogonality between observation error and state estimation error, the theoretical covariance matrix of λkcan be expressed as follows:

In the proposed method of adaptive Kalman filter based on average weighting estimation, the optimal estimation of covariance matrix of innovation sequence using weighted average window method can be calculated as follows:

where N is the window size, and i0=k-N+1 is the first epoch. Generally, a short window size may lead to unstable estimation and the window size is chosen empirically for statistical smoothing. By substituting Equation (27)into Equation (22), the modified gain can be expressed as follows:

2.2 Simulation of adaptive Kalman filtering

In order to test the performance of the proposed adaptive Kalman filter, the IMU outputs in static conditions with constant and random sensor errors are used.The window size used for the innovation covariance calculation in adaptive Kalman is 64. In Fig.5, the blue solid lines and the red dashed lines respectively represent the results before and after filtering the IMU outputs in static conditions. From Fig.5, we can conclude that after filtering the accelerometers’ outputs with AKF, the projections of gravitational apparent motion in inertial frame are smoother, and most of the random noise disturbance is removed.

Fig.5 Projections of gravitational apparent motion in inertial frame

3 Simulation and experiment test

3.1 Simulation settings

Simulations are conducted to verify the self-alignment algorithm of the SINS based on three different vectors of gravitational apparent motion and adaptive Kalman filter denoising method. Taking the ship as an example, the ship is assumed to be without translation motion but with swing condition, and the sensor errors are shown in Table 2. The ship is assumed to swing with the function of A·sin(2πf·t+φ0)+ψ0, where A and f denote the amplitude and the frequency of swinging respectively, while φ0and ψ0denote the initial phase and the swing center respectively. The swing parameters are shown in Table 3.In order to simplify the analysis, φ0and ψ0are set at zero.The longitude and latitude of the ship are set at 118°E and 32°N. The sampling frequency of strapdown algorithm is 200Hz. According to Equation (14), the extreme alignment errors for yaw, pitch and roll are 0.2810°,0.0115° and -0.0115° respectively.

Tab.3 Swing parameters

3.2 Simulation results

Fig.6 shows the calculated attitude angles after alignment in Case 3, which are based on three different vectors of gravitational apparent motion and adaptive Kalman filter denoising method proposed in this paper.During the alignment process, the vectors of gravitational apparent motion at time stamps of 10 s, 400 s, and 800 s are recorded, and the self-alignment is completed at 800 s. It can be concluded from Fig.6 that before 800 s(the end of alignment), the values of yaw, pitch and roll are constant at 5°, 2°, and 2° respectively. After 800 s,IMU begins attitude solution, the calculated amplitudes of yaw, pitch and roll are 6°, 10°, and 12° respectively,and the calculated frequencies of yaw, pitch and roll are 0.2 Hz, 0.08 Hz, and 0.14 Hz respectively. It can be seen that the calculated amplitudes and the frequencies of attitude angles are close to the theoretical values.

Fig.7 shows the curves of alignment errors in Case 3.During the alignment process, the vectors of gravitational apparent motion at time stamps of 10 s, 400 s, and 800 s are recorded, and the self-alignment is completed at 800 s point. It can be seen from Fig.7 that the proposedmethod is able to reach the accuracy requirements of SINS initial alignment in swing condition. Furthermore,the mean values and the variance values of yaw, pitch and roll alignment errors in swing condition obtained by this method are shown in Table 4.

Fig.7 Curves of alignment errors with swing base

Tab.4 Statistics of alignment errors under swing condition with constant and random sensor errors

The curves of latitude errors in static condition and swing condition process are shown in Fig.8. The red dashed line represents latitude error under static condition with constant and random sensor errors, which is stable with hardly any fluctuation. It indicates that the mean of latitude error in static condition is 0.0661°. The blue solid line represents latitude error under swing condition with constant and random sensor errors. It can be seen that the latitude error in swing condition fluctuates between 0.057° and 0.084°. When the alignment time increases, the variance of latitude error will decrease.The mean and standard deviation of latitude error in swing condition are 0.0737° and 0.0426° respectively.

Fig.8 Curves of latitude errors (Red dashed line: Static base; Blue solid line: Swing base)

3.3 Experimental results

In order to verify the self-alignment algorithm of the SINS based on three different vectors of gravitational apparent motion and adaptive Kalman filter denoising method, the turntable test of SINS is conducted. The three-axis gyroscopes’ constant biases are 0.02 (°)/h and the random walks are 0.006 (°)/h1/2. The three-axis acelerometers’ constant biases are 0.5 mg and the random walks are 0.1 mg/Hz1/2. The SINS’s swing amplitudes of yaw,pitch and roll are 3°, 21°, and 5° respectively, and the SINS’s swing cycles of yaw, pitch and roll are 5 s, 12 s,and 7 s respectively. The location of this turntable is determined as east longitude 106.527126° and north latitude 29.590129°. The scale coefficients, coupling coefficients and installation errors of IMU can be determined and compensated through exact calibration. Moreover,the data from IMU and turntable are updated at a rate of 200 Hz. During the whole process of alignment, the SINS do not use the latitude and longitude information.

The vectors of gravitational apparent motion at time stamps of 10 s, 400 s, and 800 s are recorded and the selfalignment is completed at 800 s in the alignment experiment. Fig.9 shows the curves of alignment errors in swing base by the three different vectors of gravitational apparent motion and adaptive Kalman filter denoising method. It can be concluded from Fig.9 that the mean alignment errors of yaw, pitch and roll are 6.981′, -1.779′, and 1.146′ respectively, which are close to the extreme accuracy determined by the errors of inertial devices. The alignment error’s standard deviation of yaw, pitch and roll are 1.37′, 1.108′, and 1.343′ respectively. After 800 s self-alignment, the latitude information can be obtained, and the final calculated latitude value is 29.644 499°. Compared with the theoretical latitude 29.590 129°, the latitude error is only 0.05437°, which is at a high level of accuracy.

Fig.9 Curves of alignment errors with swing base

4 Conclusion

In this paper, a self-alignment algorithm with unknown latitude for SINS on swing base is studied. The vectors of gravitational apparent motion at three different moments are used to construct the vector of cone axis through vector operation. The east axis of current navigation frame in the inertial frame can be acquired by the crossproduct of cone axis vector and current apparent motion vector, and the attitude matrix between body frame and navigation frame can be solved. What’s more, the latitude information can be obtained by calculating the angle between the cone axis vector and the apparent motion vector with no external reference information.

With the purpose of removing the random noise contained in the accelerometer measurements which generates collinear vectors in the process of vector operations,an adaptive Kalman filter based on average weighting estimation is designed. Experimental results indicate that the filtering method can effectively remove the random noise and improve the alignment accuracy in dynamic uncertain environments. Moreover, the simulation and experimental results based on three gravitational apparent motion vectors and adaptive Kalman filter denoising method show that the alignment accuracy can reach the extreme accuracy determined by the errors of inertial devices under swing conditions. The calculated latitude value based on the proposed method in practical experiment is close to the theoretical latitude value, which can provide precise reference information for the SINS in integrated navigation.

[1] Gaiffe T, Cottreau Y, Faussot N, et al. Highly compact fiber optic gyrocompass for applications at depths up to 3000m[C]//International Symposium on Underwater Technology. 2000: 155-160.

[2] 崔鹏程, 邹志勤, 王翌, 等. 杆臂效应误差对晃动基座粗对准的影响[J]. 中国惯性技术学报, 2013, 21(4):462-466.Cui Peng-cheng, Zou Zhi-qin, Wang Yi, et al. Influence of lever-arm effect error on coarse alignment on shaking base[J]. Journal of Chinese Inertial Technology, 2013,21(4): 462-466.

[3] 王跃刚, 杨家胜, 杨波. 纬度未知条件下捷联惯导系统晃动基座的初始对准[J].航空学报, 2012, 33(12): 2322-2329.Wang Yue-gang, Yang Jia-sheng, Yang Bo. SINS initial alignment of swaying base under geographic latitude uncertainty[J]. Acta Aeronaut. Astronaut. Sin, 2012, 33(12):2322-2329.

[4] Liu Xi-xiang, Xu Xiao-su, Zhao Yu, et al. An initial alignment method for strapdown gyrocompass based on gravitational apparent motion in inertial frame[J]. Measurement, 2014, 55: 593-604.

[5] Meng Xiang-fei, Li Xin. Parameter identification for SINS coarse alignment based on apparent velocity[J]. Journal of Chinese Inertial Technology, 2016, 24 (6): 730-735.

[6] 刘锡祥, 杨燕, 黄永江, 等. 未知纬度条件下基于重力视运动与小波去噪的SINS自对准方法[J]. 中国惯性技术学报, 2016, 24(3): 306-313.Liu Xi-xiang, Yang Yan, Huang Yong-jiang, et al. Selfalignment algorithm without latitude for SINS based on gravitational apparent motion and wavelet denoising[J].Journal of Chinese Inertial Technology, 2016, 24(3):306-313.

[7] Li Q, Ben Y Y, Sun F. A novel algorithm for marine strapdown gyrocompass based on digital filter[J]. Measurement, 2013, 46(1): 563-571.

[8] Liu Y T, Xu X X, Liu X X, et al. A self-alignment algorithm for SINS based on gravitational apparent motion and sensor data denoising[J]. Sensors, 2015, 15(5): 9827-9853.

[9] Liu X X, Liu X J, Song Q, et al. A novel self-alignment method for SINS based on three vectors of gravitational apparent motion in inertial frame[J]. Measurement, 2015,62: 47-62.

[10] Gao She-sheng, Zhong Yong-min, Wei Wen-hui, et al.Windowing-based random weighting fitting of systematic model errors for dynamic vehicle navigation[J]. Information Sciences, 2014, 282(40): 350-362.

[11] Peesapati R, Sabat S L, Anumandla K K, et al. Design and implementation of a realtime co-processor for denoising Fiber Optic Gyroscope signal[J]. Digital Signal Processing, 2013, 23: 1813-1825.

[12] Mundla N, Samrat L. An innovation based random weighting estimation mechanism for denoising fiber optic gyro drift signal[J]. Optik, 2014, 125: 1192-1198.

[13] Ding W D, Wang J L, Rizos C, et al. Improving adaptive Kalman estimation in GPS/INS integration[J]. Journal of Navigation, 2007, 60(3): 517-529.

未知纬度的捷联惯导系统晃动基座自对准方法

吕维维1,2，程向红1,2
（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学 仪器科学与工程学院，南京 210096）

传统捷联惯导系统晃动基座对准方法中，一般要求提供当地准确的地理纬度信息，如果给定的纬度存在误差，会对初始对准精度造成影响。针对纬度未知情况下的晃动基座对准问题，提出了利用三个不同时刻的重力视运动向量的运算进行对准的方法，推导了三矢量定姿对准的基本原理。针对含有随机噪声的加速度计测量值构造的重力视运动向量会导致向量运算时出现共线向量的问题，设计了一种基于平均权重系数的自适应卡尔曼滤波器，能在未知动态环境下有效去除加速度计随机噪声，提高对准精度。仿真和试验结果表明，该方法可在纬度未知条件下完成捷联惯导系统晃动基座下的自对准并估计出纬度值，对准精度可达惯性器件误差所决定的对准极限精度。

未知纬度；重力视运动；平均权重系数；自适应卡尔曼；自对准

TP273

：A

1005-6734(2017)03-0281-08

2017-02-11；

：2017-05-22

国家自然科学基金项目（61374215）；上海航天科技创新基金项目（SAST2016024）

吕维维（1989—），男，博士研究生，专业为导航制导与控制。E-mail: lvww0220@seu.edu.cn

联 系 人：程向红（1963—），女，教授，博士生导师。E-mail: xhcheng@seu.edu.cn

10.13695/j.cnki.12-1222/o3.2017.03.001