Anti-interference self-alignment algorithm by attitude optimization estimation for SINS on a rocking base

XUE Haijian ,WANG Tao,* ,CAI Xinghui ,WANG Jintao ,and LIU Fei

1.High-Tech Institute of Xi’an,Xi’an 710025,China;2.Military Representative Office of Rocket Army Equipment Department,Qingdao 266071,China

Abstract: The performance of a strapdown inertial navigation system (SINS) largely depends on the accuracy and rapidness of the initial alignment.A novel anti-interference self-alignment algorithm by attitude optimization estimation for SINS on a rocking base is presented in this paper.The algorithm transforms the initial alignment into the initial attitude determination problem by using infinite vector observations to remove the angular motions,the SINS alignment is heuristically established as an optimization problem of finding the minimum eigenvector.In order to further improve the alignment precision,an adaptive recursive weighted least squares (ARWLS) curve fitting algorithm is used to fit the translational motion interference-contaminated reference vectors according to their time domain characteristics.Simulation studies and experimental results favorably demonstrate its rapidness,accuracy and robustness.

Keywords: strapdown inertial navigation system (SINS),initial alignment,anti-interference,rocking base,adaptive recursive weighted least squares (ARWLS).

1.Introduction

Strapdown inertial navigation system (SINS) is a selfcontained system of providing measurements by gyroscopes and accelerometers to track the position and orientation of an object relative to a known starting point,orientation,and velocity [1,2].Therefore,it necessitates an alignment stage to determine the initial conditions prior to navigation operation [3],the precise initial alignment for SINS is very important,especially for the high precision SINS,because the performance of the SINS is largely decided by the accuracy and rapidness of the alignment process.The traditional initial alignment often includes two stages: coarse alignment and fine alignment[4-6].The conventional coarse alignment methods generally use two feature vectors of the earth: the acceleration of gravitygand the angular rate of the earth’s rotation ωie,and it asks for the base in a static case [7,8].However,in many cases,the process of the alignment is affected by various interference factors of the base,such as engine vibration,crew motion,and strong flurry,etc.Because the intensity of the interference signal is obviously larger than ωie,the earth rate that is indispensable for the coarse alignment cannot be separated from the gyroscopes outputs.

In order to solve the problem of initial alignment on a rocking base,many scholars have done a lot of research.El-Sheimy et al.[9] presented using pre-filtering technology in which the gyro and accelerometer data were first processed by a low-pass filter to reduce the disturbance and noise in the data.Considering the projection of the gravity in the inertial frame defines a cone whose main axis is the rotational axis of the earth,a series of methods using the inertial frame as a transitional coordinate to realize the initial alignment are investigated [10-13].All these methods are based on decomposition of the attitude matrix into separate earth motion,inertial rate,and alignment.On this basis,the SINS alignment can be transformed into a problem of determining the initial attitude at the beginning of alignment that is right a constant quantity,and James [14] proposed using triaxial attitude determination (TRIAD) algorithm which determines the attitude by first discarding part of the measurements so that a solution exists.Because the algorithm is very simple,it has become the most popular method for determining three-axis attitude.The greatest drawback of the TRIAD algorithm is that it can only accommodate two observations [15].Bar-itzhack et al.[16] proved the attitude matrix calculated by TRIAD algorithm was not optimal and presented an optimized TRIAD algorithm,but it also only use two observations,some accuracy is lost because part of the measurement is discarded.In 1965,Wahba [17] proposed an optimized algorithm,which computed a best estimate of the spacecraft attitude based on a loss function which took into account all measurements.Based on this,Shuster et al.[18] put forward a quaternion estimator (QUEST) algorithm,which maintained all the computational merits of a fast deterministic algorithm while yielding an optimal result.

This paper is dedicated to a novel anti-interference selfalignment algorithm by attitude optimization estimation for SINS on a rocking base.It is an enlightened product by the optimization-based alignment [2,12,19-22],in which the SINS alignment is heuristically established as an optimization problem of finding the minimum eigenvector.It uses attitude update of inertial coordinate to reflect the gesture changes of the carrier under real-time shaking interference and transforms the initial attitude determination problem into the Wahba problem [17] by using infinite vector observations to remove the angular motions interrupting.In order to further improve the alignment precision,a least square (LS) curve fitting algorithm is used to fit translational motion interferencecontaminated reference vectors according to their time domain characteristics,and then the reference vectors are used to compute the Wahba problem,thus realizes high precision self-alignment quickly even in the presence of angular and translational motion interference.

The remainder of this paper is organized as follows.Section 2 introduces the fundamental principles and drawbacks of the self-alignment by attitude optimization estimation (SAAOE).In Section 3,the anti-interference SAAOE (AISAAOE) is given including using the adaptive recursive weighted least squares (ARWLS) curve fitting algorithm to remove the translational motion interference.In Section 4,simulation example and experiment test are preformed on a swaying base.Conclusions are made in Section 5.

2.SAAOE

In order to better understand SINS initial alignment,it is necessary to explain the navigation coordinate system,the coordinate frames used for the SINS initial alignment are defined as follows.

(i)eframe: the earth’s core is the origin,xeaxis points to the intersection of the prime meridian and the equator.zeaxis goes upward along earth polar axis.xe,ye,andzeform a righe-hand coordinate frame.The earth centered earth fixed coordinate frame moves with the earth in angular rate ωiewhich is the self-rotational angular rate of the earth.

(ii)iframe: at the starting time for the initial alignment,the earth centered earth fixed coordinate frame coincides with the earth centered inertial coordinate frame and is fixed in the inertial space.

(iii)nframe: the origin is the centroid of the carrier.znaxis goes upward along the local geodetic vertical,ygaxis north (and horizontal) withxgaxis east (and horizontal).

(iv)bframe: the origin is the centroid of the carrier.xbaxis shifts rightward along the carrier’s transverse axis,ybforward along the longitudinal axis,zbupward along the vertical axis.

2.1 Fundamental principles

According to the relations between the coordinate system,at any given moment,the attitude direction cosine matrix(DCM) can be decomposed into three parts,that is

wheren(0) andb(0) denote the inertial nonrotating frame,which are formed by fixing thenframe andbframe at the start-up in the inertial space,respectively.describe the attitude changes of the navigation frame and the body frame during the time interval [0,t] and can be determined by

In addition,it can be easily observed that the initial conditions for the differential equation in (2) and (3) are both identity matrix.Therefore,can be easily obtained based on some well-known attitude update methods.In this respect,the initial alignment has been transformed into determining the constant matrixbased on the decomposition of (1).

The velocity update equation [23-24] is written in thenframe as

Transpose the terms of (4),we have

Then we have

That is

wheregnis the gravity vector in thenframe andfbdenotes the specific force measured by accelerometers in thebframe.is the velocity relative to the earth.

Quaternion is a simpler parameterization of rotation than attitude matrix [2,19,25].Next w e turn to use a fourelement unit quaternionq=[sηT]T,wheresis the scalar part and ηTis the vector part,to encode the initial body attitude matrix(0).The relationship between these two rotation parameters [19] is

where (η×) denotes the skew-symmetric matrix of η.Thus,(7) can be rewritten as

Equation (11) is a quadratic equation in attitude quaternion,multiplying both sides from right byqyields:

According (12) and (13),the matrix form produces a linear equation inqas

Following the famousq-method algorithm for the optimal attitude determination [15,17,26],we could pose the alignment problem as a minimization procedure,i.e.,

where

subject to

Therefore the alignment problem is equivalent to determining an unit quaternion that minimizes (16) and the current attitude matrix is thus obtained by using (2),(3)and the optimal quaternion.It can be proven that the optimal quaternion is exactly the normalized eigenvector ofKbelonging to the smallest eigenvalue [15].A simple explanation is provided below for easy reference.

Define the following Lagrangian function:

where λ is a real scalar.To achieve minimum,must be satisfied

That is

It indicates that the optimal quaternion must be an normalized eigenvector and λ is the corresponding eigenvalue ofK.Substitute (20) into (18),we have

Therefore,the minimization will be achieved if only is chosen to be the normalized eigenvector corresponding to the smallest eigenvalue ofK.The smallest eigenvalue is unique because of the inherent property of the alignment problem.

2.2 Drawbacks analysis

Through the analysis of the previous section,we have so far assumed known ground velocity of the SINS and it is unpractical in many cases.When the system only exists angular interrupting and there is no translational motion interference,the ground velocity is zero,i.e.,

Substitute (22) into (4),we have

At this point,β(t) can be calculated by (9) according to the output of accelerometers and α(t) can be calculated by

However,when angular interrupting and translational motion interference exist at the same time,that is

Therefore,on a rocking base,the SAAOE becomes in principle invalid,it cannot eliminate the influence of angular interrupting and translational motion interference at the same time.Translational motion interference is one of the important error sources for alignment,if we can minimize its impact,the alignment accuracy is the highest.

3.AISAAOE

From the above analysis we can obtain that removing or weakening translational motion on the influence of β(t) is the key to improve the self-alignment accuracy.Below,we analyze the characteristics of β(t) from the view of time domain.

A direct analysis for the time domain features of β(t) is difficult,this article through analysis α(t) indirectly get the time domain characteristics of β(t).According (2),we have

whereLis the latitude.Substituting (26) into (24) yields

The alignment process is usually performed in a few minutes,no more than ten minutes,so ωietis a very small amount.So cos(ωiet) and sin(ωiet) can be rewritten by the third order Taylor series expansion as

To this respect,(27) can be rewritten as

Multiplying both sides of (7) from left byCbn(0) yields

whereekis called innovation vector;β(tk) is the actual measurement;in general,X0=0，Λ0=0.1,P0(0)=Iα and α is a large scalar.We can find that the measurement noise is not used directly in the above algorithm,but merged intoek.The advantage is that we don’t have to know the statistical properties caused by speed disturbance and adaptive weighted to calculate the gain according to the innovation vector.So it will speed up the convergence of the algorithm.

After gainingwhich is the estimate ofBij,substitute it into (34),we can obtain the least square fitting result of β(t),then complete the initial alignment.

4.Simulation study and experimental results

In this section,we carry out simulations and experiments to verify the performance of the AISAAOE.

4.1 Simulations study

In the simulation study,the SINS is assumed to be located at latitude 39.959° and 400 m height.The sampling rate is 100 Hz,and we select medium accuracy inertial measuring component.The specifications of gyroscopes and accelerometers are list in Table 1.

Table 1 Specifications of gyroscopes and accelerometers

We design simulations mimicking typical alignment condition: the first one for a swaying SINS without translations,the other one for a swaying SINS with translational motions.

Firstly,we consider the SINS angularly swaying without translational motions.Due to the periodic angular shaking interference,the pitch angleθ,roll angleγand yaw angleψof carrier are

The Euler angles estimates along with the true value are plotted in Fig.1 and Fig.2 for the first 150 s,through the SAAOE and the AISAAOE,respectively.

Fig.1 Euler angles estimates along with the true value for the first 150 s when the SINS performs angular swaying with no translations by SAAOE

Fig.2 Euler angles estimates along with the true value for the first 150 s when the SINS performs angular swaying with no translations by AISAAOE

They imply that the curves for SAAOE and AISAAOE are too close with the true value to discriminate,they are inherently not influenced by any angular swaying.Fig.3 gives the estimate errors of the three Euler angles for the first 300 s.

Fig.3 Estimate errors of Euler angles for a swaying SINS with no translations by SAAOE and AISAAOE

We can see that the different between SAAOE and AISAAOE is negligible.The two level misalignment angles,roll error and pitch error,converge almost instantaneously with a high-precision (better than 0.007°).In specific,the yaw error takes longer time than the two level misalignment angles to converge,it stabilizes at 0.03° in 100 s.Estimates of the initial constant Euler angles by SAAOE and AISAAOE are plotted in Fig.4.

Fig.4 Initial Euler angles estimates by SAAOE and AISAAOE for the SINS performs angular swaying with no translations

We can find that the estimates of the initial constant Euler angles exist a apparent climbing tends and deviate from the true value because of the existence of gyroscope and accelerometer biases.It indicates that investigating the change of the initial Euler angles can alternatively serve as a function of calibration sensor errors,e.g.,gyroscope and accelerometer biases [19,28].Note that the initial pitch angle and roll angle have a bias of 0.006°,while the initial yaw angle has a bias of 0.01° until the end.

Secondly,the SINS sways with both angular and medium-frequency translational motions interference.Keeping the above angular shaking unchanged,meanwhile,carrier exists translational motions velocity caused by base swaying,they are

where subscriptx,y,zare the three axes of thebframe respectively;Aiare the amplitudes of translational motion andAx=Ay=Az=0.05 m;Tiare the cycles of translational motion andTx=Ty=Tz=2 s;φiis the random phase which obey uniform distribution [0,2π].

The Euler angles estimates along with the true value by SAAOE and AISAAOE for the SINS with both angular and translations motions are given in Fig.5 and the Euler angles estimate errors are plotted in Fig.6.

Fig.5 Euler angles estimates along with the true value by SAAOE and AISAAOE for the SINS with both angular and translations motions

Fig.6 Estimate errors of Euler angles by SAAOE and AISAAOE for the SINS with both angular and translations motions

The translation imposes considerable translations motions effect on the SAAOE method because the zero ground velocity assumption is not valid any more and we cannot measure them.AISAAOE yields a well result because of the aid of the ARWLS algorithm.In specific,the yaw angle error stabilizes at 0.03° in 150 s for AISAAOE,but still has a big shock (about 0.25°) for SAAOE.The above observations show that AISAAOE could cope with both large angular swinging and translational motions,while SAAOE is sensitive to translational motions,especially the yaw angle.The initial Euler angles estimates by SAAOE and AISAAOE for the SINS with both angular and translations motions are plotted in Fig.7.

Fig.7 Initial Euler angles estimates by SAAOE and AISAAOE for the SINS with both angular and translations motions.

It appears that the climbing tends of the yaw angle is similar with Fig.6 and the two level angles also have a bias of 0.006° like Fig.4.

Thirdly,we compare our method with the SAAOE with a low-pass filter (SAAOEWLF) described in [19].The low-pass filter is selected for illustration to be a 100-order equiripple finite impulse filter (FIR) with the pass frequency at 0.01 Hz and the stop frequency at 0.1 Hz.The SINS sways with both angular and different frequency translational motions interference,i.e.,

Frequency 1 (Fr1):Tx=Ty=Tz=0.2 s;

Frequency 2 (Fr2):Tx=Ty=Tz=2 s;

Frequency 3 (Fr3):Tx=Ty=Tz=10 s.

The estimate errors of the Euler angles by AISAAOE and SAAOEWLF for different frequency translational motions interference are plotted in Fig.8 and Fig.9,respectively.

Fig.8 Estimate errors of Euler angles by AISAAOE for different frequency translational motions interference.

Fig.9 Estimate errors of Euler angles by SAAOEWLF for different frequency translational motions interference.

We can see that,compared with SAAOEWLF,AISAAOE yields a better result,especially for the heading errors in Fr3.In specific,the heading errors by AISAAOE reach 0.05° within 250 s for all three frequencies.And the heading errors by SAAOEWLF reduce to 0.05° within 250 s for Fr1 and Fr2,but still have a big shock (about 0.5°) for Fr3.This is mainly because that,with the frequency of translational motions interference decreasing,the low-pass filter is not valid any more.In order to eliminate the lower frequency translational motions interference,we need to design a higher-order FIR,this will inevitably lead to delay growth too.The above observations show that AISAAOE could cope with different frequency translational motions interference,while SAAOEWLF is sensitive to low frequency translational motions interference because of its defects.

4.2 Experimental results

This section reports our experimental result by examining a real SINS system consisting of three ring laser gyroscopes with drift rate 0.005°/h (1σ) and three quartz accelerometers with bias 5×10-5m/s2(1σ) at output rate 200 Hz.The SINS is installed on a vehicle as shown in Fig.10.

Fig.10 Setup of the experimental platform

In order to evaluate the accuracy of alignment we needed a criterion for comparison.Thus we design the experiment scheme as shown in Fig.11.

Fig.11 Experiment scheme

As can be seen from Fig.11,the experiment data is collected as such.Firstly,during the period of 0 s to 300 s,this is the static-base alignment segment,the vehicle keep static for high-precision static-base alignment,then is working at navigation mode to produce standard attitude,which is used to provide the reference attitude of high accuracy for the results comparison.Secondly,during the period of 300 s to 600 s,this is the oscillatorybase alignment segment,we impose some interference on the vehicle,such as engine vibration,crew motion,crew get on or off the vehicle,people shake the vehicle,etc.The reference attitude derived by the static-base alignment segment is shown in Fig.12,and the Euler angles estimate errors are plotted in Fig.13.

Fig.12 Reference attitude derived by the static-base alignment segment

Fig.13 Estimate errors of Euler angles for the real SINS system

As can be shown in Fig.13,the superiority of the proposed method can be easily observed,AISAAOE has a much faster convergent speed than SAAOE,especially for the heading error,the estimate errors of heading angle by AISAAOE is less than 0.05° in 150 s,less than 0.03°in 300 s,while the estimate errors of heading angle by SAAOE is less than 2° in 150 s,less than 0.3° in 300 s.This is mainly because that,the added interference exists a variety of translational motions,the AISAAOE can outperform the SAAOE because of the help of the ARWLS algorithm.And it is consistent with the previous simulation results.

5.Conclusions

The SAAOE is an attractive and useful method for the SINS initial alignment.It shows that the alignment problem can be equivalently transformed into an attitude determination problem using infinite vector observations over a continuous time interval and is not influenced by any angular swaying of the carrier.However,while the SINS on a rocking base with both angular and translational motions interference,SAAOE is sensitive to translational motions because the zero ground velocity assumption is not valid any more.By using an ARWLS curve fitting algorithm to fit the translational motion interference-contaminated reference vectors,a modified AISAAOE is introduced.The simulation study and experimental results for the SINS initial alignment on a rocking base with both angular and translational motions indicate that the AISAAOE can outperform the SAAOE because of the help of the ARWLS algorithm.