Abstract
Due to the fact that attitude error of vehicles has an intense trend of divergence when vehicles undergo worsening coning environment, in this paper, the model of dynamic coning environment is derived firstly. Then, through investigation of the effect on Euler attitude algorithm for the equivalency of traditional attitude algorithm, it is found that attitude error is actually the roll angle error including drifting error and oscillating error, which is induced directly by dynamic coning environment and further affects the pitch angle and yaw angle through transferring. Based on definition of the cone frame and cone attitude, a cone algorithm is proposed by rotation relationship to calculate cone attitude, and the relationship between cone attitude and Euler attitude of spinning vehicle is established. Through numerical simulations with different conditions of dynamic coning environment, it is shown that the induced error of Euler attitude fluctuates by the variation of precession and nutation, especially by that of nutation, and the oscillating frequency of roll angle error is twice that of pitch angle error and yaw angle error. In addition, the rotation angle is more competent to describe the spinning process of vehicles under coning environment than Euler angle gamma, and the real pitch angle and yaw angle are calculated finally.
1. Introduction
Attitude algorithm is a key part of navigation technology and guarantees directly control system accuracy and navigation accuracy of aircrafts, ships, and vehicles. Through decades of persistent efforts by researchers, to expand applicability of attitude algorithm, an outstanding two-stage structure of attitude algorithm is refined and outlined [1], which consists of attitude matrix update cycle by Jordan [2] and rotation vector update cycle by Bortz [3]. Particularly, the crucial coning correction of noncommutativity, calculated from gyro data in a separate algorithm, is the critical process executed in rotation vector update cycle to improve the attitude accuracy, and it has attracted more attentions from demanding researchers in navigation technology. This is because the coning error induced by coning environment can affect navigation accuracy and control precision of vehicles [4, 5]. Miller used a quaternion algorithm with three intervals from gyro to calculate attitude and approximate coning correction error for the coning correction coefficients design under a pure coning condition [6]. Lee et al. improved the quaternion algorithm with four intervals and compared drift error with other fewer-interval algorithms [7]. Based on the Miller achievement, Ignagni proposed a two-speed structure for coning correction and introduced nine algorithms to realize optimization of coning correction coefficients [8]. Jiang and Lin proposed an improved strapdown coning algorithm and disclosed the essential relationship between rotation vector and quaternion [9]. Li et al. divided minor coning correction into a number of subminor intervals and delivered a generalized coning compensation algorithm that is independent of the number of incremental angles [10]. Tang and Chen proposed a coning correction structure containing cross-product of angular rates, cross-product of angular increments, and cross-product of angular rate and increment, and it can analyze effect on attitude by basing on time Taylor series and frequency Taylor series [11]. Meanwhile, the gyro frequency response is a primary cause for pseudo-coning error that is another form of coning error, and the traditional coning correction algorithms cannot work to high-order accuracy. Mark and Tazartes proposed a tuning high-order coning algorithm to match the frequency response characteristics of the gyro and avoided overcompensation for pseudo-coning correction [12]. Kim and Lee added self-vibration of gyro to the causes of pseudo-coning. They showed that vibration had a form generated by different frequency inputted to two orthogonal axes, and coning motion was just a specific case [13]. Savage thought that stochastic dynamic environment sensed by high precision gyro could also cause pseudo-coning error, so he used explicit frequency shaping by minimum least-squares estimation for stochastic data to design coning correction coefficients [14]. Song et al. combined with Miller’s and Savage’s methods and delivered a supplementary coning error equation to achieve superior maneuver accuracy [15]. Considering measurement error of gyro, Fu et al. developed a two-time scale model by singular perturbation technique to realize coning and pseudo-coning correction [16]. Wang et al. used a new coning motion model that contains pure constant coning environment and spinning motion of vehicles to examine the ignored triple-cross-product term of noncommutativity error in attitude rotation vector and test the performance of previous algorithms [17]. Patera disclosed that no attitude error was propagated by using a slewing frame under oscillating coning environment [18] and extended the slewing frame to improve attitude propagation attitude for cases of time varying coning motion [19]. Unfortunately, the method based on the slewing frame cannot provide the common attitude of spinning vehicles under complex coning environment because the relationship of the slewing frame and the common attitude of vehicles is unclear.
Synthesizing and analyzing research achievements above, we find that the recognized coning error and the pseudo-coning error are generated directly while calculating rotation vector increment from gyro in rotation vector update cycle, and the optimization of attitude algorithm is mainly achieved by using the total model and the simplified model of the classical coning environment to restrain drifting component of coning error. However, the oscillating error of attitude exists when the optimization of the two-stage structure of attitude algorithm is used for vehicles under classical coning environment. Although attitude error is not large enough for nonspinning vehicles to attract more attentions, it has an intense trend of divergence over time when coning environment is becoming worse. However, up to now, there is no literature to exploit this issue, and the in-depth study of attitude algorithm of vehicles under high dynamic environment is hardly carried on. Therefore, it is necessary for us to investigate the real attitude of vehicles and disclose the hidden relationship between vehicles’ attitude and dynamic coning environment.
The scheme of this paper is as follows. In Section 2, we derive the model of dynamic coning environment by rotation vector. In Section 3, we provide the traditional two-stage structure of attitude algorithm and investigate the effect of dynamic coning environment on attitude algorithm. In Section 4, based on the definition of the cone frame and cone attitude, we propose a cone algorithm for cone attitude by using the rotation relationship and build the relationship between cone attitude and Euler attitude. In Section 5, the simulations are made to explain the effect of dynamic coning environment on tradition attitude algorithm and verify the validity of the cone algorithm. In Section 6, we conclude our research in this paper and provide our future work.
2. Dynamic Coning Environment
Before studying the effect of dynamic coning environment on attitude algorithm, the derivation of modelling dynamic coning environment is made in this section.
Generally, the classical coning environment is defined as a condition where two orthogonal axes in a vehicle simultaneously experience sinusoidal oscillations that are mutually phase-shifted by 90 degrees [14]. The third axis, which is orthogonal with the other two oscillating axes, will precess around an axis and generate a conical surface in inertial space. Under the classical coning environment, cone half-angle and oscillating frequency are all constant, and the precession frequency of the third axis is equal to oscillating frequency. Obviously, this process has constant cone half-angle and constant precession frequency, so the classical coning environment is static. In contrast, the dynamic coning environment is a complex process with varying cone half-angle, so rotation vector can be defined aswhere is the changing rate of cone half-angle and is the precession frequency. Assuming that and are constant during small time interval, the derivation of (1) can be obtained:
Referring to [3], the angular velocity can be described as
Substituting (1) and (2) into (3), the angular velocity of dynamic coning environment in vehicles can be obtained:
From (4), we see that if the changing rate of cone half-angle is zero but the cone half-angle is nonzero, (4) can be simplified aswhere is the constant cone half-angle. It is clear that (5) is the model of the classical coning environment, and it is just a special case that has no nutation rate and no rotation [3, 6, 9].
3. Effect of Dynamic Coning Environment on Attitude Algorithm
Since the common optimization algorithm is developed under static coning environment, it is reasonable to consider how attitude algorithm is affected by dynamic coning environment. In this section, we will investigate this question.
3.1. Definition of Relative Frames
To describe the movement of vehicles with respect to the reference frame, the common frames are introduced firstly.
The Body Frame . The origin is located at the center of mass of a vehicle, axis coincides with longitudinal axis of a vehicle, axis is pointing up and perpendicular to axis in symmetry plane of a vehicle, and axis is obtained by the right-hand rule.
The Earth Frame . The origin is located at the point of launch site, axis is pointing to a target, axis is pointing up and perpendicular to axis in vertical plane, and axis is obtained by the right-hand rule.
The rotation order of and is as follows:
As is shown in Figure 1, the relationship of and can be described by a transformation matrix:where represents transformation from the body frame to the earth frame, stands for the earth frame, stands for the body frame and Euler attitude angles , , and are the pitch angle, yaw angle, and roll angle, respectively. The range of is , the range of is , and the range of is .
3.2. Two-Stage Structure of Attitude Algorithm
The optimization of attitude algorithm is developed by using two-stage structure of attitude algorithm under the classical coning environment, and the two-stage structure algorithm in modern strapdown inertial navigation systems is given by [1]where represents the reference frame, represents the body frame, is a transformation matrix at attitude updating cycle , is a transformation matrix that transforms vectors from the body frame at cycle into the body frame at cycle , is a rotation vector used to update , and is cross-product antisymmetric matrix composed of components. The updating structure of rotation vector is given bywhere is measuring data from a triaxial gyro. Equation (15) is referred to as the coning error or coning correction.
According to the common optimizing methods of attitude algorithm by using angular increment, some coefficients are designed to restrain the direct component of -axial angular increment under static coning environment [8, 14, 15]:where is the correction coefficient depending on the coning correction structure and is an angular increment sample over a fixed time interval. Due to the sampling number of a triaxial gyro, single-sample algorithm, two-sample algorithm, three-sample algorithm, and so on have been developed, and the validity of algorithm is described by drifting error.
One stage is updating the attitude in an attitude updating cycle, and it is referred to in (8) to (12). The other stage is calculating the rotation vector including an integrated gyro sensed angular rate and a coning correction calculated from gyroscopes data, and it is referred to in (13) to (16).
By analyzing the optimizing process, the so-called coning error is independent directly to vehicles’ movement due to (13) to (16), and and are both scalar by substituting the modular of . So the coning correction under coning environment cannot change the structure of and the effect on Euler attitude in by optimizing. Although drifting error is dealt with through optimization, oscillating error remains.
3.3. Effect of Dynamic Coning Environment on Attitude Algorithm
It is convenient to use the two-stage structure algorithm to design coefficients and restrain drifting error of attitude under static coning environment, but it is difficult to investigate the effect of dynamic coning environment clearly by using two-stage structure algorithm because it is quite difficult to derive simple and clear descriptive equations for oscillating error and drifting error. Nevertheless, two-stage structure algorithm and the Euler attitude algorithm both use Euler angles to define the relationship between the body frame and the earth frame [18], so they are equivalent without considering singular problem [20]. For derivation and analysis of the effect on attitude algorithm clearly, we can use the Euler attitude algorithm to make the investigation.
According to the rotation relationship in Figure 1, the relationship between angular velocity in the body frame and Euler angular velocity can be described as [21] where is angular velocity in the body frame and , , and are Euler angular velocities, respectively. Projecting Euler angular velocity to each axis of the body frame and expanding (17), angular velocity in the body frame can be obtained:where , , and are Euler angular speeds, respectively, and they are all scalar; , , and are components of angular velocity in the body frame to describe the rotation of a vehicle with respect to the earth frame, and they can be obtained by a triaxial gyroscope fixed in the body frame. Transforming (18), the differential equation of Euler attitude is obtained:
In (19), there is singular problem when the pitch angle is equal to 90 degrees. Under pure coning environment, the pitch angle will never attain to 90 degrees, so the singular problem can be ignored reasonably.
Substituting (4) into (19), Euler attitude equations can be obtained:
Under dynamic coning environment, there is no rolling motivation for nonspinning vehicles, so the real roll angle should be zero. However, we note that the roll angle located on the right of the third element of (20) is not zero at all, although the initial value of the roll angle is chosen as zero. This means that the roll angle error is generated. So the third element of (20) actually represents the roll angle error equation. Therefore, the roll angle error rate induced by dynamic coning environment can be determined as
In (21), the first term is direct component and implies drifting error, which has been attracting many researchers’ interests. The second term is alternating component and implies oscillating error. Apparently, drifting error and oscillating error fluctuate by the cone half-angle and the precession frequency. When the cone half-angle is increasing, drifting error and amplitude of oscillating error are both increasing.
From the derivation above by using Euler attitude algorithm, we confirm that attitude of vehicles is affected to generate drifting error and oscillating error under dynamic coning environment.
From the first and second elements of (20), it is seen that the roll angle error affects accuracy of the pitch angle and yaw angle. The accurate pitch angle and yaw angle can be determined without roll angle error:
With the small angle approximation, the pitch angle error and yaw angle error can be obtained by comparing (20) and (24):
In fact, the pitch angle error and yaw angle error are all induced by the transferred roll angle error under dynamic coning environment, so attitude error is actually a sort of induced error by dynamic coning environment. This is the reason why so many researchers pay more attentions to attitude error by coning environment, in, specially, the roll angle error.
4. Coning Algorithm of Spinning Vehicles under Dynamic Coning Environment
From the analysis above, attitude algorithm is affected by dynamic coning environment badly. To calculate attitude accurately, in this section, we propose a coning algorithm of spinning vehicles based on a cone frame and cone attitude and build the relationship between cone attitude and Euler attitude.
4.1. Definition of Cone Frame
Compared with common angular movement, coning motion of spinning vehicles is a sort of angular movement of longitudinal axis of vehicles under coning environment. Actually, this special movement directly shows precession and nutation of longitudinal axis, and self-rotation is accompanying. Since attitude error is induced by coning motion of periodicity, a reasonable method is describing coning motion accurately and calculating attitude without error. Therefore, cone frame and cone angles should be defined specially to describe angular movement.
Referring to gyrodynamics [22], a frame is defined by rotation relationship to describe movement of a rotor in a gyroscope that has typical nutation and precession. To avoid confusion, in this paper, we call this frame the cone frame . In the cone frame, the origin is located in the center of mass of a vehicle, axis coincides with cone axis, axis is pointing up and perpendicular to axis in the vertical surface, and axis is obtained by the right-hand rule. Meanwhile, cone attitude angles including precession angle , nutation angle , and rotation angle is defined relatively. The initial position of is horizontal on the right of cone, and the range is . The initial position of is coincident with cone axis, and the range is . The initial position of is located in symmetry plane of a vehicle, and the range is .
The rotation order of and is as follows:
In the rotation order, , , and are corresponding angular speeds. However, through in-depth study of the rotation order, we find that the rotation angle is obtained by rotating discontinuously along -axis twice, and this rotational process allows the rotation angle to contain precession process. It means that the rotation angle is equal to the negative precession angle if a nonspinning vehicle is under coning environment. Therefore, an isolation of the influence by precession is needed. An indispensable rotation along -axis is added before the second rotation along -axis, and the improved rotation order is shown as
As is shown in Figure 2, the relationship of and can be described by a transformation matrix: where represents transformation from the body frame to the cone frame.
4.2. Cone Algorithm Based on Rotation Relationship
According to the rotation relationship in Figure 2, the angular motion model can be derived aswhere , , and are measuring data of a triaxial gyro in the body frame to describe the motion of a vehicle with respect to the cone frame. It is clear that (29) can describe the angular movement of spinning vehicles under dynamic coning environment, so it is modeling for this condition. Assuming the initial values of and are both zero, the dynamic coning model can be obtained by substituting and into (29):
From (30), we see that if is zero, it can be simplified as
Equation (31) is the same as (4), and this means that and . In addition, (30) in fact provides measuring data from a triaxial gyro in the body frame for vehicles under dynamic coning environment, and, especially, (30) includes rotation information. When vehicles make nutation and rotation without precession, the measuring data represents pitching by providing projected on -axis and -axis of the gyro. When vehicles only make rotation without coning environment, the measuring data provides . It means that (4) can decouple the rotation of the vehicle from pitching and yawing that are represented equivalently by the precession and nutation. Thus, we are sure that (29) or (30) can describe the whole coning motion for vehicles.
By transforming (29), cone attitude equation for , , and can be derived as
We noted that (32) has singular problem when is equal to 0 or . But, in fact, this model is specially derived for coning environment, and it is unnecessary to use it when is equal to 0. Moreover, vehicles are swinging in a plane when is equal to , and the attitude does not make sense. Therefore, the singular problem can be neglected reasonably.
Because the dynamic coning model is derived by using the cone frame, cone attitude is not affected by coning environment. In other words, there is no error in the third axis although the other two axes are oscillating periodically:where is cone attitude error .
The coning environment can be described in the earth frame or the cone frame, so the relationship of Euler attitude and cone attitude is built according to the geometrical relationship in Figure 3:
Therefore, the pitch angle and yaw angle can be calculated by
According to Figure 3, the maximum of is equal to when is () (). The maximum of is equal to when is . This means that and are both less than , so the square root of (9) is not complex-valued. The symbol of and can be determined referring to Tables 1 and 2. It is apparent that the pitch angle and yaw angle are dependent on and . Therefore, the accuracy of the pitch angle and yaw angle is dependent on the precession angle and nutation angle as well.
5. Simulation and Analysis
To disclose the effect of coning environment on traditional attitude algorithm and verify the validity of the cone attitude algorithm, numerical simulations are designed on purpose in this section.
We choose a nonspinning vehicle for simulations because it is convenient to check whether the roll angle or the rotation angle is zero. We assume that the initial Euler attitude of the nonspinning vehicle is , , and , the total simulation time is 600 s, and the update time is 0.0001 s. The optimized coefficients of the two-stage structure of attitude algorithm are chosen as , [8], which is enough for investigating. The conditions of dynamic coning environment are shown in Tables 3 and 4. Under each condition, we will investigate how attitude error is affected.
Attitude error by Table 3 is represented in Figure 4. From Figure 4, we see that as the precession frequency of coning environment is enlarging, attitude error is oscillating and diverging, and the amplitude of both pitch angle error and yaw angle error becomes larger. Euler attitude error is drifting slightly, and this is because the cone half-angle is too small to make attitude error drift obviously. In addition, the oscillating frequency of pitch angle error and yaw angle error is twice that of roll angle error. In Figure 5, the roll angle error is enlarged up to 4° by small during 600 s, which means the increasing process of amplitude of attitude error is accelerated rapidly by nutation. We confirm that the changing rate of cone half-angle affects the roll angle badly more than the precession frequency. It is quite necessary for researchers to attach importance to the effect of dynamic coning environment on attitude algorithms, especially variant nutation condition for vehicles.
(a) The whole process of attitude error
(b) Amplification of attitude error
(a) The whole process of attitude error
(b) Amplification of attitude error
Combining Tables 3 and 4, we choose the precession angular speed 1440°/s and the changing rate of cone half-angle 0.046°/s as the wicked condition of dynamic coning environment, and triaxial angular velocity for this condition is shown in Figure 6. The -axial component of angular velocity is nonzero due to the precession angular speed. The magnitude of each axial component is increasing as the nutation angle is enlarging. The cone attitude is calculated by the cone algorithm. As is shown in Figure 7, we see that the precession angle and nutation angle are changing due to the precession frequency and the changing rate of cone half-angle. Particularly, we note that, during 600 s under dynamic coning environment, the magnitude of rotation angle of the vehicle is below . In fact this error results from the calculating error of computer, and ideally the rotation angle has no error. This is because the rotation order of the cone frame shows the precession, nutation, and rotation of vehicles under arbitrary coning motion, and the angular motion model is established according to the rotation order. When resolving the angular equation, cone attitude can be obtained without error. Therefore, we confirm that dynamic coning environment can be described by the cone frame and the cone attitude of spinning vehicles can be calculated by the cone algorithm. In addition, the rotation angle is more competent to describe the spinning process of vehicles under coning environment than Euler angle gamma. The pitch angle and yaw angle of the vehicle are calculated due to the geometrical relationship of cone attitude and are shown in Figure 8, which can be regarded as the real pitch angle and yaw angle of vehicles.
(a) The whole process of pitch angle and yaw angle
(b) Amplification of pitch angle and yaw angle
6. Conclusion
Attitude error of vehicles has an intense trend of divergence when vehicles undergo worsening coning environment. In this paper, we firstly derive the model of dynamic coning environment. Then through investigation of the effect on Euler attitude algorithm for the equivalency of traditional attitude algorithm, we find that attitude error is actually the roll angle error including drifting error and oscillating error, which is induced directly by dynamic coning environment and further affects the pitch angle and yaw angle through transferring. Based on the definition of the cone frame and cone attitude, we propose a cone algorithm by rotation relationship to calculate cone attitude and establish the relationship between cone attitude and Euler attitude of spinning vehicle. Finally, numerical simulations are made with variant precession frequency and changing rate of cone half-angle to verify the effect of dynamic coning environment on attitude algorithm and validity of cone algorithm. The results show that the induced error of Euler attitude fluctuates by the variation of precession and nutation, especially by that of nutation, and the oscillating frequency of roll angle error is twice that of pitch angle error and yaw angle error. In addition, the rotation angle is more competent to describe the spinning process of vehicles under coning environment than Euler angle gamma, and the real pitch angle and yaw angle are calculated finally.
This paper is beneficial for calculating real attitude of spinning bodies, understanding mechanism of attitude error by coning environment, and developing attitude algorithm, and it also provides a new view on the effect of coning motion. Our future work will focus on the optimization of attitude algorithm under dynamic coning environment and real-time attitude algorithm of spinning vehicles under high dynamic movement.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgment
This study is financially supported by the National Natural Science Foundation of China (nos. 61471046, 61473039, and 11202023).