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 systems
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
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:
||Intervals associated with coning and sculling compensation,
ΔT: Data sample interval
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:
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:
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:
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:
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:
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:
Taken over by different sub-minor sample intervals, the cross product of two incremental angular vectors Δθ(i)xΔθ(j) becomes:
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:
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:
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:
where, N is the number of total gyro incremental angular samples in order to
compute ki, namely N = n+p. So the coefficients vector
is obtained as follows:
REAL-TIME IMPLEMENTATION OF ATTITUDE/VELOCITY UPDATING ALGORITHM
After the coning compensation approximation term
computed by Eq. 2, the rotation vector Φm
over the mth minor interval is computed as follows (Park
et al., 1999):
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
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:
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:
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:
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:
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:
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:
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.
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.
This study is supported in part by the Aviation Science Foundation (20110776001, National Key Laboratory of Science and Technology on Aircraft Control, XiAn 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.