Abstract: In this study, a general scheme for implementation of attitude and velocity algorithm in Strapdown Inertial Navigation System (SINS) under real-time environment was presented. This real-time implementation scheme was formulated in a recursive form using two time-rate executions and utilized integer arithmetic only. The coding of the attitude and velocity algorithm was conducted by a navigation computer (such as DSP or FPGA). It is shown that the general real-time implementation of strapdown attitude and velocity algorithm can be used in strapdown inertial navigation system for any combinations of gyro incremental angle sample of mth minor interval and (m-1)th minor interval.
INTRODUCTION
In a Strapdown Inertial Navigation System (SINS) used in super-speed vehicles, inertial instruments (three-gyros and three-accelerometers) are rigidly attached to the vehicle, which suffer from the high dynamics environment of vehicle movement directly. In order to keep tracking of body attitude and velocity accurately in severe vibratory environment, the ability of the strapdown system’s attitude and velocity algorithm may be the critical factor in determining its performance, if accurate navigation is to be achieved (Lee et al., 1990). Therefore, the real-time implementation of attitude and velocity updating algorithm has played a critical role in strapdown inertial navigation systems.
Nowadays, commonly used attitude updating algorithms for most modern-day strapdown inertial navigation systems was the quaternion method based on the rotation vector. The quaternion method was quite popular used in reality due to the advantages of its nonsingularity, simplicity and computational efficiency. It has been proven that the quaternion method based on the rotation vector can effectively suppress and even eliminate the noncommutativity error, which is one of the major error sources in attitude updating algorithms. Ignagni (1996) proposed the concept of distance between the cross products leading to optimal accuracy characteristics and minimum computational throughput required in coning compensation algorithm. The algorithms utilizing the enhancement concept have the simplest form in generating many distinct gyro-sample cross products and optimal coefficients to minimize the coning compensation error (Ignagni, 1996; Lee et al., 1990).
Lee et al. (1990) proposed the generic equivalency between the coning integrals in attitude algorithm and the sculling integrals. The equivalency allows us to convert a coning compensation approximation algorithm in an attitude updating algorithm to its sculling approximation algorithm counterpart in velocity updating algorithm using a simple mathematical formula. In this study, a general scheme for implementation of attitude and velocity SINS algorithm in real-time environment was presented. This scheme of real-time implementation was formulated in a recursive form using two time-rate executions and using only integer arithmetic.
CONING AND SCULLING COMPENSATION APPROXIMATION ALGORITHMS
In the development of the coning and sculling compensation algorithms, it is usual that each major interval is divided into a number of minor intervals, each in turn being divided into a number of sub-minor sample intervals over which the gyro incremental angle and accelerometer incremental velocity are measured, as shown in Fig. 1. This is referred to as two time-rate structure that updates the coning/sculling compensation in every minor interval, whereas the attitude and velocity of vehicle are updated only once in every major interval.
The coning compensation ΔΦm over the minor interval from τm-1 to τm is defined as the integral of the noncommutativity rate vector over the interval:
| (1) |
| Fig. 1: | Intervals associated with coning and sculling compensation, ΔT: Data sample interval |
Where:
and ω is the angular rate vector, the components of which are sensed by a set of three gyros. According to the concept of distance between the cross products, cross products with equal distance behave exactly the same for coning inputs (Lee et al., 1990; Park et al., 1999). Taking advantage of this property, a generalized algorithmic approximation to the coning integral defined by Eq. 1 consists of the sum of all possible cross products. Those products are formed from the gyro incremental angle for the n sub-minor sample intervals that make up the minor computational interval of rotation vector (Ignagni, 1996) such as:
| (2) |
where, n is the number of gyro incremental angle sample for mth minor interval; p is the number of gyro incremental angle sample for (m-1)th minor interval; Δθm(i) is ith gyro incremental angle sample for mth minor interval; Δθm-1(j) is jth gyro incremental angle sample for (m-1)th minor interval and kn-i and k2n-j are the constant coefficients of Δθm(i) and Δθm-1(j) for any combinations of n and p, respectively.
The sculling compensation Δvsculm over the minor interval from τm-1 to τm is defined as follows:
| (3) |
Where:
By comparing the sculling compensation algorithm in Eq. 3 with the coning compensation algorithm in Eq. 1, the equivalency between the two was demonstrated (Ignagni, 1998; Roscoe, 2001). So, using a simple mathematical formula that converts the coning compensation approximation algorithm in Eq. 2 to the sculling compensation approximation algorithm counterpart, a generalized algorithmic approximation to the sculling compensation algorithm defined by Eq. 3 is derived as follows:
| (4) |
where Δvm(i) is ith accelerometer incremental velocity sample for mth minor interval, Δvm-1(j) is jth accelerometer incremental velocity sample for (m-1)th minor interval and the other as what defined in Eq. 5. Especially, the constant coefficients of Δvm(i) and Δvm-1(j) are the same as the constant coefficients of Δθm(i) and Δθm-1(j), respectively.
OPTIMAL COEFFICIENTS OF CONING/SCULLING COMPENSATION ALGORITHM
Because the coefficients of coning compensation approximation algorithm in Eq. 2 are the same as coefficients of sculling compensation algorithm in Eq. 4, so we only need to derive the coefficients of coning compensation approximation algorithm. In order to derive the coefficients of coning compensation approximation algorithm in Eq. 2, a pure coning motion is selected as base motion of vehicle and is defined by the angular rate vector as:
| (5) |
where, I and J are the unit vectors along two orthogonal axes of the vehicle. Applying the coning motion in Eq. 5 to Eq. 1, we obtain:
| (6) |
where, T is the minor interval. For the coning environment defined by Eq. 5, the gyro incremental angular vector over a sub-minor sample interval of duration ΔT from tk-1 to tk is:
| (7) |
Taken over by different sub-minor sample intervals, the cross product of two incremental angular vectors Δθ(i)xΔθ(j) becomes:
| (8) |
where λ = Ω·ΔT. It can be seen from Eq. 8 that the value of the cross product of two incremental angular vectors is independent of the absolute time but depends only on a multiple of the sub-minor sample interval ΔT (Lee et al., 1990; Park et al., 1999).
Substituting Eq. 8 into Eq. 2 and expanding each term by the Taylor series, the algorithmic approximation to coning compensation over the minor interval is obtained as:
| (9) |
where, Aij is the constant coefficient and is defined as:
Expanding each term in Eq. 6 using the Taylor series and substituting ΩT = nΩΔT = nλ into Eq. 6, we obtain:
| (10) |
In order to find the optimal coefficients of coning compensation algorithm ki (i = 1, 2þ, n+p-1) in Eq. 1, let the corresponding items of Eq. 9 and 10 equal and let:
so the simultaneous equation for ki can be defined as follows:
| (11) |
where, N is the number of total gyro incremental angular samples in order to
compute ki, namely N = n+p. So the coefficients vector
| (12) |
REAL-TIME IMPLEMENTATION OF ATTITUDE/VELOCITY UPDATING ALGORITHM
After the coning compensation approximation term
| (13) |
where, θm is the sum of the gyro incremental angular samples over the mth minor interval as follows:
According to the two-speed structure of the algorithm (Lee et al., 1990; Ignagni, 1996), the rotation vector Φm over the major attitude updating interval is computed as follows:
| (14) |
Similar to the rotation vector computation over the major interval, the velocity updating computation over the major interval is as follows. The velocity rotation compensation term Δvrotm of velocity updating algorithm is:
Where:
and
So the velocity compensation term ΔvSFm solving for an integral that represents the change in velocity (in body frame coordinate) over the mth minor interval caused by specific force acceleration is as follows:
| (15) |
And the velocity compensation ΔvG/Corm solving for an integral that represents the change in velocity (in body frame coordinate) over the mth minor interval caused by nocuous acceleration (namely gravity and Coriolis acceleration) is as follows:
Because gn, ωnin and ωnen even vary smoothly in highly dynamic environments (due to the small size of the earth angular rates and the smooth vary of body velocity), they can be approximated by their average values over the mth minor interval, so the approximation for ΔvG/Corm is as follows:
| (16) |
where, m-1/2 is the parameter value midway over the mth minor interval. Because gnm-1/2 is not explicitly available in Eq. 16, so in reality the linear extrapolation is usually used to approximate gnm-1/2 as follows:
So the velocity at τm time is as follows:
| (17) |
where, vm-1 is the body velocity at τm-1 time and Cnb(m-1) is the body attitude matrix at τm-1 time. Similar to the two-speed structure of above attitude updating algorithm, the velocity vector vM over the major interval is computed as follows:
| (18) |
where, v0 and Cnb(0) are the velocity and attitude matrix at last major interval, respectively.
Considering a real-time implementation of the attitude/velocity updating algorithm in a digital computer (such as DSP or FPGA), two problems need to be solved, namely floating-point arithmetic and recursive calculation. The attitude updating algorithm as well as the velocity updating algorithm requires floating-point arithmetic. In addition, for recursive calculation, coning compensation and quaternion update in attitude updating algorithm and sculling compensation and velocity update in velocity updating algorithm have to be carried out in each update interval.
Lee et al. (1990) and Li et al. (2010), a general scheme that could ease implementing the attitude algorithms in a recursive form using high-rate and low-rate executions and using integer arithmetic only. In this study, we generalized this general scheme to be used in the implementation of attitude and velocity updating algorithm.
In the attitude and velocity updating algorithm, it is desirable that the coning/sculling compensation be updated in every minor interval, whereas the quaternion and velocity are updated only once in every major interval. In this way, the time to compute the quaternion and velocity updating can be reduced while the coning/sculling compensation is updated at a high rate. To make it possible, an algorithm has to be obtained to directly update the coning/sculling compensation instead of using the quaternion and velocity.
To obtain a recursive form of attitude and velocity updating algorithm, now we separate Eq. 14 and 18 into two parts: (1) high rate and (2) low rates as shown in Eq. 19 and 20:
| • | High rate: |
| (19a) |
| (19b) |
| (19c) |
| (19d) |
| (19e) |
| (19f) |
| (19g) |
| (19h) |
| (19i) |
| (19j) |
| (19k) |
| (19l) |
| (19m) |
| (19n) |
| • | Low rate: |
| (20a) |
| (20b) |
where, the rotation vector Φ calculated at every major interval is used to obtain the corresponding the updating quaternion over the major interval. And so we can use the updating quaternion to obtain the attitude matrix Cnb of vehicle. Attitude matrix Cnb(M-1) in Eq. 20b is the attitude matrix of vehicle at last major interval calculated by rotation vector Φ. The coefficients ki (i = 1,2,...,N-1) in Eq. 19c and 19d can be calculated by Eq. 12 for any combinations of n and p.
CONCLUSIONS
In this study, a general scheme to implementation of strapdown inertial navigation attitude and velocity algorithm in real-time environment was presented. This scheme of real-time implementation was formulated in a recursive form using two time-rate executions and using integer arithmetic only.
The important aspect of the proposed real-time implementation of strapdown inertial navigation attitude and velocity algorithm was divided into two time-rate computations. The high-rate algorithm that updated the coning/sculling compensation can be performed entirely in integer arithmetic. The time-consuming of attitude and velocity updating was performed at a lower rate, resulting in much savings in the real-time implementation of strapdown inertial navigation system.
Considering the coding of the algorithm in a navigation computer (such as DSP or FPGA), it was showed that the general real-time implementation of strapdown attitude and velocity algorithm can be used in strapdown inertial navigation system for any combinations of n and p.
ACKNOWLEDGMENTS
This study is supported in part by the Aviation Science Foundation (20110776001, National Key Laboratory of Science and Technology on Aircraft Control, Xi’An Flight Automatic Control Research Institute), the Zhejiang Provincial Natural Science Foundation (Y1100696 and R1090052), the Fundamental Research Funds for the Central Universities (2011QNA4021) and the National Nature Science Foundation of China under grant No. 61070003 and 61071128.