Abstract
To solve the real-time problem of attitude algorithm for high dynamic bodies, a real-time structure of attitude algorithm is developed by analyzing the conventional structure that has two stages, and a flow diagram of a real-time structure for a Matlab program is provided in detail. During the update of the attitude matrix, the real-time structure saves every element of attitude matrix in minor loop in real time and updates the next attitude matrix based on the previous matrix every subsample time. Thus, the real-time structure avoids lowering updating frequency, though the multisubsample algorithms are used. Simulation and analysis show that the real-time structure of attitude algorithm is better than the conventional structure due to short update time of attitude matrix and small drifting error, and it is more appropriate for high dynamic bodies.
1. Introduction
Spinning bodies are a kind of fight vehicle that has high self-rotating speed, such as guided shells and guided multiple launch rockets, and they are also called the high dynamic bodies. The spinning mode of high self-rotating speed is used to simplify the configuration of control systems and generate stability of the motion and robustness to disturbances [1]. Nevertheless, this mode induces the occurrence of coning motion, which gets worse as the self-rotating speed increases. Researches on aerodynamic design and control system design are made to restrain coning motion and improve flight environment for spinning bodies [2].
In researches on control and navigation of vehicles, attitude algorithm is a key part, and it still has the same position for the high dynamic bodies. The most popular methods to describe the relationship of a body to the reference frame are rotation vector, the direction cosine transformation matrix, Euler angles, and quaternions. Through decades of persistent efforts by researchers, an outstanding two-stage structure of attitude algorithm has been refined and outlined by Savage [3], which consists of attitude matrix update cycle by Jordan [4] and rotation vector update cycle by Bortz [5]. Moreover, an inevitable error named coning error is induced by environment disturbances, and it drifts and diverges in a period of time. So, the drift error indicates the accuracy of attitude algorithm. In addition, the stage of updating the rotation vector has the truncation error for approximation. Up to now, the coning error has attracted many researchers to devote to the development of valid algorithms. Multisubsample optimized algorithms based on bandwidth of gyroscopes are proposed to restrain drifting of coning error, which are also regarded as uncompressed algorithms. It is found that the more the subsamples are chosen, the smaller the drifting rate of coning error is [6–9]. To further improve the accuracy of rotation vectors, compressed and half-compressed algorithms have been presented as well [10, 11]. These algorithms calculate several angular increments at a higher frequency and then compress them as one value in a relative lower frequency. Considering the maneuver situation of flight vehicles, the performance of coning correction of attitude algorithms under pure coning environment is examined to verify the validity [12, 13]. A cone algorithm uses a cone frame and cone attitude to describe the coning movement of high dynamic bodies and restrains the coning error by environment disturbance [14]. In fact, the cone frame and cone attitude are defined according to the characteristic of coning motion of high dynamic bodies. Generally speaking, coning motion includes the nutation motion and the precession motion, which causes attitude coupled. There are also some works focusing on limiting coning motion under an allowable level, such as a nutation control system and nutation damping for spin-stabilized satellites [15, 16]. These works are extremely relevant and provide an important contribution to the improvement of attitude algorithms.
However, the drifting error of attitude algorithms still exists for high speed spinners, and attitude computation becomes even more difficult due to the time constraint. The total update time is dependent on the sum of each subsample time, while the subsample time is determined by the processing ability of hardware. Nevertheless, for high dynamic bodies, the value of the rotational speed is another significant parameter to be considered while solving for the attitude. This is because the conventional structure of attitude algorithm with the compressed mode has the real-time problem, and it could not meet the real-time requirement of bodies with self-rotating speed greater than 20 r/s [17]. In other words, under the condition of high self-rotating speed, it is hard for the conventional structure with the lower updating frequency to provide attitude to control system and navigation system at each right time. In addition, drifting error can not be restrained validly due to the enlarged update time. This issue also exists in information exchange of spacecraft [18]. Besides, it is necessary to notice that MEMS gyroscopes used for high dynamic bodies have intrinsic poorer alignment and lower measurement accuracy. Therefore, in this paper, a real-time structure of the attitude algorithm is proposed and compared to the conventional structure to improve the real-time capabilities for high dynamic bodies.
2. The Conventional Structure of Attitude Algorithm
The conventional structure of attitude algorithm includes two stages. The first stage is to calculate the rotation vector by using the data from gyroscopes in higher frequency, and the second stage is to update the attitude matrix based on the relationship between rotation vector and attitude matrix. The relationship between rotation vector and angular velocity is shown as follows [5]:where represents rotation vector and represents angular velocity. Based on the small angle approximation, (1) can be simplified as
It is seen that (2) is a differential equation, and the rotation vector can be obtained by
Nevertheless, the data sampled from gyroscopes is discrete, and it is difficult to calculate the rotation vector through integration of the second and third terms of (2). Thus, the angular velocity from gyroscopes can be integrated to determine the angular increment:where and represents the update time:where represents the amount of subsamples that determine attitude algorithms and represents subsample time.
Moreover, if the third term of (2) is omitted, the sequential angular increments can be used to calculate the rotation vector bywhere represents the coefficient that can be determined by the optimized algorithm under the classical coning environment [3, 10]. Figure 1 shows coning motion of high dynamic bodies, represents coning axis, represents the coning frequency, and represents the half coning angle.
The classical coning environment can be described by [5]where , , and represent the components of angular velocity.
The second term of (6) can not be eliminated thoroughly by optimization, and it is also regarded as coning error [10]. In general, the performance evaluation of algorithms including the uncompressed and compressed algorithms can be expressed as the drifting error under coning environment. There are several different forms of multisubsample algorithms, but all of them can be summarized as [9, 10]
If the subsample time is fixed, the drifting error is dependent on the update time and the amount of subsamples of attitude algorithm. Moreover, the larger the amount of subsamples is, the smaller the drifting error is. Several drifting errors for different algorithms of conventional structure are provided in Table 1 [12, 19, 20].
The attitude matrix can be updated by using the rotation vector, and the relationship of rotation vector and transformation matrix is shown aswhere is a transformation matrix that transforms vectors from the body frame at cycle into the body frame at cycle and is cross-product antisymmetric matrix composed of components. Thus, the attitude matrix can be determined bywhere represents the earth frame, represents the body frame, and is the attitude matrix at updating cycle .
According to (6) and (10), it is apparent that the calculation accuracy of rotation vector is quite significant. The whole updating process of attitude matrix includes subsamples, and each subsample provides a set of angular increments. Moreover, each set of angular increments is calculated each in Figure 2, and the rotation vector and attitude matrix are updated each .
3. Real-Time Structure of Attitude Algorithm
For the conventional structure of attitude algorithm, several sample points of angular velocity are compressed to one average value by process method to keep the precision of angular velocity, and this average value is related to the subsamples. The amount of subsamples of angular velocity represents the update frequency of attitude matrix. For the low dynamic condition such as low self-rotating speed of bodies or low coning frequency of coning environment, smaller amount of subsamples of attitude algorithm can meet the requirement of accuracy and real-time capability. For high self-rotating speed of bodies or high coning frequency of coning environment, larger amount of subsamples requires processing ability of hardware to guarantee the accuracy. Obviously, it is hard to make trade-off. It can be suggested to maintain the update time to be close to the subsample time of angular velocity and decrease coning error.
In this section, a real-time structure of attitude algorithm is proposed through analyzing the conventional one. The main idea of real-time attitude algorithm is shown briefly in Figure 3, and the angular increment and the attitude matrix can be updated during each processing time. Each attitude matrix is updated based on the previous one, so the more important of the real-time attitude algorithm is to save each element of attitude matrix in advance at each update time. Thus, whatever the amount of subsample is, the update frequency is maintained to be equal to the process frequency of angular velocity.
Considering the extended measuring range of the gyroscopes required during launch phases, MEMS rate gyroscopes are often chosen. The angular velocity sampled by MEMS gyroscopes is composed of real value and random noise signal, so it is necessary to process the original value as a trustworthy value and provide control and navigation system with it. The angular velocity is processed bywhere is the original value of angular velocity, is the processing time, and is the number of sample points of angular velocity during the processing time. Each angular increment is calculated bywhere . It is necessary to note that (12) is different to (4) due to the different integral time. According to Figure 3, the update time of real-time structure of attitude algorithm is equal to the process time:
The rotation vector is calculated by using the sequential angular increments from (12):
The attitude matrix at is updated bywhere is an identity matrix, represents the attitude matrix at , and represents the body matrix from to :
The flow diagram for a Matlab program is shown in Figure 4 to explain the program of real-time structure of attitude algorithm, and the whole update process of attitude matrix has a major loop and deterministic branches in the program. The symbol represents the ordinal number of angular velocity. The variable Count is an index and represents the number of subsamples. In fact, when is less than , the program starts to make the initial alignment of attitude matrix. Then, the subsequent attitude matrixes are calculated directly by choosing the appropriate subsamples of attitude algorithm. Moreover, every element of attitude matrix in the minor loop must be saved in real time according to the corresponding Count. Thus, when is greater than , the attitude matrix can be updated based on the previous matrix.
The drifting errors for different algorithms of real-time structure are shown in Table 2. From the view of drifting error, the single-subsample and two-subsample algorithms are suitable for the low dynamic condition, but the three-, four-, and five-subsample algorithms are suitable for the high dynamic condition. Compared with Table 1, each corresponding drifting error in Table 2 is much smaller if is greater than 1. Moreover, the attitude matrix by real-time structure can be updated in each subsample time other than .
4. Simulation and Analysis
To verify the validity of real-time structure of attitude algorithm, the conventional structure in [10] is chosen to make simulations under different coning conditions in Matlab, and the subsample time is ms. The amount of subsamples is chosen , 2, 3, 4 and 5, respectively, and the optimized coefficients are shown in Table 3.
The update time of attitude matrix is determined by the subsample time, so different algorithms have different update time. The comparison of the update time of conventional structure and real-time structure is shown in Figure 5. It is apparent that the update time of conventional structure is increasing linearly as the amount of subsample increasing, but the update time of real-time attitude structure remains unchanged.
Firstly, self-rotation is considered without coning environment, and the self-rotation speed is assumed to be 1080°/s, 3600°/s, 7200°/s, and 9000°/s, respectively. The angular increment of roll angle in each update cycle by conventional structure and real-time structure is shown in Tables 4 and 5. As the self-rotation speed is increasing, the angular increment by conventional structure becomes large. However, the angular increment by real-time structure remains unchanged under the same self-rotation speed, and it is 0.9° under the condition of self-rotation speed 9000°/s. In fact, the angular increment indicates the attitude error between adjacent sample times. Based on the simulations and analysis, the real-time algorithm has higher accuracy and better real-time capacity than the conventional algorithm.
Then, the coning environment is added, the half coning angle , and the coning frequency = 360°/s.
The single-subsample algorithms of conventional structure and real-time structure have the same update time and accuracy, so the comparison is made for other algorithms. The drifting errors by conventional structure and real-time structure under coning environment are shown in Figures 6 and 7. The drifting error is mainly dependent on the amount of subsamples if the subsample time is fixed. With the same subsamples, the drifting error of real-time structure is much smaller than that of conventional structure. This means the real-time structure has higher accuracy than the conventional structure.
5. Conclusion
For high dynamic bodies with high self-rotating speed and coning environment, the conventional structure of attitude algorithm has the real-time problem, and it can not provide attitude to control system and navigation system at each right time. In this paper, by comparing with the conventional structure, a real-time structure of attitude algorithm with two loops is developed to improve real-time capability for high dynamic bodies, and a flow diagram for a Matlab program is provided in detail. Every element of attitude matrix in the minor loop can be saved in real time, and then the next attitude matrix can be updated based on the previous matrix at every subsample time. Thus, the real-time structure avoids lowering updating frequency, though different multisubsample algorithms are used. Through simulation and analysis, the real-time structure of attitude algorithm is better than the conventional structure because of short update time of attitude matrix and small drifting error, and it is more appropriate for high dynamic bodies.
Our future work will focus on the effect of real-time capacity and synchronism of attitude algorithm on the control system of high dynamic bodies.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This study is financially supported by the National Natural Science Foundation of China (no. 61473039) and the Open Research Project of the Beijing Key Laboratory of High Dynamic Navigation Technology (no. HDN2017002).