Research Article Cone Algorithm of Spinning Vehicles under ...tional coning correction algorithms...
Transcript of Research Article Cone Algorithm of Spinning Vehicles under ...tional coning correction algorithms...
-
Research ArticleCone Algorithm of Spinning Vehicles underDynamic Coning Environment
Shuang-biao Zhang,1,2 Xing-cheng Li,1,2 and Zhong Su3
1School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China2Key Laboratory of Dynamics and Control of Flight Vehicle, Ministry of Education, Beijing 100081, China3Beijing Key Laboratory of High Dynamic Navigation Technology, Beijing Information Science & Technology University,Beijing 100101, China
Correspondence should be addressed to Xing-cheng Li; [email protected]
Received 20 August 2015; Revised 30 October 2015; Accepted 10 November 2015
Academic Editor: Hikmat Asadov
Copyright Β© 2015 Shuang-biao Zhang et al. This is an open access article distributed under the Creative Commons AttributionLicense, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properlycited.
Due to the fact that attitude error of vehicles has an intense trend of divergence when vehicles undergo worsening coningenvironment, in this paper, themodel of dynamic coning environment is derived firstly.Then, through investigation of the effect onEuler attitude algorithm for the equivalency of traditional attitude algorithm, it is found that attitude error is actually the roll angleerror including drifting error and oscillating error, which is induced directly by dynamic coning environment and further affectsthe pitch angle and yaw angle through transferring. Based on definition of the cone frame and cone attitude, a cone algorithmis proposed by rotation relationship to calculate cone attitude, and the relationship between cone attitude and Euler attitude ofspinning vehicle is established. Through numerical simulations with different conditions of dynamic coning environment, it isshown 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 angleis more competent to describe the spinning process of vehicles under coning environment than Euler angle gamma, and the realpitch angle and yaw angle are calculated finally.
1. Introduction
Attitude algorithm is a key part of navigation technology andguarantees directly control system accuracy and navigationaccuracy of aircrafts, ships, and vehicles. Through decadesof persistent efforts by researchers, to expand applicabilityof attitude algorithm, an outstanding two-stage structureof attitude algorithm is refined and outlined [1], whichconsists of attitude matrix update cycle by Jordan [2] androtation vector update cycle by Bortz [3]. Particularly, thecrucial coning correction of noncommutativity, calculatedfrom gyro data in a separate algorithm, is the critical processexecuted in rotation vector update cycle to improve theattitude accuracy, and it has attracted more attentions fromdemanding researchers in navigation technology. This isbecause the coning error induced by coning environment canaffect navigation accuracy and control precision of vehicles
[4, 5].Miller used a quaternion algorithmwith three intervalsfrom gyro to calculate attitude and approximate coningcorrection error for the coning correction coefficients designunder a pure coning condition [6]. Lee et al. improved thequaternion algorithm with four intervals and compared drifterror with other fewer-interval algorithms [7]. Based on theMiller achievement, Ignagni proposed a two-speed structurefor coning correction and introduced nine algorithms torealize optimization of coning correction coefficients [8].Jiang and Lin proposed an improved strapdown coningalgorithm and disclosed the essential relationship betweenrotation vector and quaternion [9]. Li et al. divided minorconing correction into a number of subminor intervals anddelivered a generalized coning compensation algorithm thatis independent of the number of incremental angles [10]. Tangand Chen proposed a coning correction structure containingcross-product of angular rates, cross-product of angular
Hindawi Publishing CorporationInternational Journal of Aerospace EngineeringVolume 2015, Article ID 904913, 11 pageshttp://dx.doi.org/10.1155/2015/904913
-
2 International Journal of Aerospace Engineering
increments, and cross-product of angular rate and increment,and it can analyze effect on attitude by basing on time Taylorseries and frequency Taylor series [11]. Meanwhile, the gyrofrequency response is a primary cause for pseudo-coningerror that is another form of coning error, and the tradi-tional coning correction algorithms cannot work to high-order accuracy. Mark and Tazartes proposed a tuning high-order coning algorithm to match the frequency responsecharacteristics of the gyro and avoided overcompensationfor pseudo-coning correction [12]. Kim and Lee added self-vibration of gyro to the causes of pseudo-coning. Theyshowed that vibration had a form generated by different fre-quency inputted to two orthogonal axes, and coning motionwas just a specific case [13]. Savage thought that stochasticdynamic environment sensed by high precision gyro couldalso cause pseudo-coning error, so he used explicit frequencyshaping by minimum least-squares estimation for stochasticdata to design coning correction coefficients [14]. Song et al.combined with Millerβs and Savageβs methods and delivereda supplementary coning error equation to achieve superiormaneuver accuracy [15]. Considering measurement error ofgyro, Fu et al. developed a two-time scale model by singularperturbation technique to realize coning and pseudo-coningcorrection [16]. Wang et al. used a new coning motion modelthat contains pure constant coning environment and spin-ning motion of vehicles to examine the ignored triple-cross-product term of noncommutativity error in attitude rotationvector and test the performance of previous algorithms [17].Patera disclosed that no attitude error was propagated byusing a slewing frame under oscillating coning environment[18] and extended the slewing frame to improve attitude prop-agation attitude for cases of time varying coning motion [19].Unfortunately, themethod based on the slewing frame cannotprovide the common attitude of spinning vehicles undercomplex coning environment because the relationship of theslewing 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 rotationvector increment from gyro in rotation vector update cycle,and the optimization of attitude algorithm is mainly achievedby using the total model and the simplified model of theclassical coning environment to restrain drifting componentof coning error. However, the oscillating error of attitudeexists when the optimization of the two-stage structureof attitude algorithm is used for vehicles under classicalconing environment. Although attitude error is not largeenough for nonspinning vehicles to attract more attentions,it has an intense trend of divergence over time when coningenvironment is becoming worse. However, up to now, thereis no literature to exploit this issue, and the in-depth study ofattitude algorithm of vehicles under high dynamic environ-ment is hardly carried on. Therefore, it is necessary for us toinvestigate the real attitude of vehicles anddisclose the hiddenrelationship between vehiclesβ attitude and dynamic coningenvironment.
The scheme of this paper is as follows. In Section 2, wederive themodel of dynamic coning environment by rotationvector. In Section 3, we provide the traditional two-stage
structure of attitude algorithm and investigate the effectof dynamic coning environment on attitude algorithm. InSection 4, based on the definition of the cone frame andcone attitude, we propose a cone algorithm for cone attitudeby using the rotation relationship and build the relationshipbetween cone attitude and Euler attitude. In Section 5, thesimulations are made to explain the effect of dynamic coningenvironment on tradition attitude algorithm and verify thevalidity of the cone algorithm. In Section 6, we conclude ourresearch in this paper and provide our future work.
2. Dynamic Coning Environment
Before studying the effect of dynamic coning environmenton attitude algorithm, the derivation of modelling dynamicconing environment is made in this section.
Generally, the classical coning environment is defined asa condition where two orthogonal axes in a vehicle simul-taneously experience sinusoidal oscillations that aremutuallyphase-shifted by 90 degrees [14].The third axis, which is orth-ogonal with the other two oscillating axes, will precessaround an axis and generate a conical surface in inertialspace. Under the classical coning environment, cone half-angle and oscillating frequency are all constant, and theprecession frequency of the third axis is equal to oscillatingfrequency. Obviously, this process has constant cone half-angle and constant precession frequency, so the classical con-ing environment is static. In contrast, the dynamic coningenvironment is a complex process with varying cone half-angle, so rotation vector can be defined as
π =[
[
[
0
Ξπ‘ cosΞ©π‘Ξπ‘ sinΞ©π‘
]
]
]
, (1)
where Ξ is the changing rate of cone half-angle and Ξ©is the precession frequency. Assuming that Ξ and Ξ© areconstant during small time interval, the derivation of (1) canbe obtained:
Μπ =[
[
[
0
Ξ cosΞ©π‘ β Ξ©Ξπ‘ sinΞ©π‘Ξ sinΞ©π‘ + Ξ©Ξπ‘ cosΞ©π‘
]
]
]
. (2)
Referring to [3], the angular velocity can be described as
π = Μπ β1 β cos
π
π
2π Γ Μπ +
1
π
2(1 β
sin
π
π
)π
Γ (π Γ Μπ) .
(3)
Substituting (1) and (2) into (3), the angular velocity ofdynamic coning environment in vehicles can be obtained:
π =[
[
[
(cosΞπ‘ β 1)ΩΠcosΞ©π‘ β Ξ© sinΞπ‘ sinΞ©π‘Ξ sinΞ©π‘ + Ξ© sinΞπ‘ cosΞ©π‘
]
]
]
. (4)
-
International Journal of Aerospace Engineering 3
From (4), we see that if the changing rate of cone half-angle is zero but the cone half-angle is nonzero, (4) can besimplified as
π =[
[
[
(cosπΌ β 1)Ξ©βΞ© sinπΌ sinΞ©π‘Ξ© sinπΌ cosΞ©π‘
]
]
]
, (5)
where πΌ is the constant cone half-angle. It is clear that (5) isthe model of the classical coning environment, and it is just aspecial case that has no nutation rate and no rotation [3, 6, 9].
3. Effect of Dynamic Coning Environment onAttitude Algorithm
Since the common optimization algorithm is developedunder static coning environment, it is reasonable to con-sider how attitude algorithm is affected by dynamic coningenvironment. In this section, we will investigate this ques-tion.
3.1. Definition of Relative Frames. To describe the movementof vehicles with respect to the reference frame, the commonframes are introduced firstly.
The Body Frame ππ₯ππ¦ππ§π. The origin is located at the center
ofmass 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:
ππ₯ππ¦ππ§π
οΏ½ΜοΏ½
β
π¦π
ππ₯
π¦ππ§
Μπ
β
π§
ππ₯ππ¦
π§
ΜπΎ
β
π₯
π
ππ₯ππ¦ππ§π. (6)
As is shown in Figure 1, the relationship of ππ₯ππ¦ππ§πand
ππ₯ππ¦ππ§πcan be described by a transformation matrix:
Cππ=
[
[
[
cos π cosπ β sin π cosπ cos πΎ + sinπ sin πΎ sin π cosπ sin πΎ + sinπ cos πΎsin π cos π cos πΎ β cos π sin πΎ
β cos π sinπ sin π sinπ cos πΎ + cosπ sin πΎ β sin π sinπ sin πΎ + cosπ cos πΎ
]
]
]
, (7)
where Cππrepresents transformation from the body frame to
the earth frame, π stands for the earth frame, π stands for thebody frame and Euler attitude angles π, π, and πΎ are the pitchangle, yaw angle, and roll angle, respectively. The range of πis [β90β 90β], the range of π is [β90β 90β], and the range of πΎis [0β 360β].
3.2. Two-Stage Structure of Attitude Algorithm. Theoptimiza-tion of attitude algorithm is developed by using two-stagestructure of attitude algorithm under the classical coningenvironment, and the two-stage structure algorithm in mod-ern strapdown inertial navigation systems is given by [1]
Cπ(π)π(π)
= Cπ(π)π(πβ1)
β Cπ(πβ1)π(π)
, (8)
Cπ(πβ1)π(π)
= I + π1(ππ) (ππΓ) + π
2(ππ) (ππΓ)
2
, (9)
π1(ππ) =
sin
ππ
ππ
= β
π=1
(β1)
πβ1
ππ
2(πβ1)
(2π β 1)!
, (10)
π2(ππ) =
1 β cos
ππ
ππ
2= β
π=1
(β1)
πβ1
ππ
2(πβ1)
(2π)!
, (11)
ππΓ =
[
[
[
[
0 βππ§
ππ¦
ππ§
0 βππ₯
βππ¦
ππ₯
0
]
]
]
]
, (12)
where π represents the reference frame, π represents the bodyframe, Cπ(π)
π(π)is a transformation matrix at attitude updating
cycle π,Cπ(πβ1)π(π)
is a transformationmatrix that transforms vec-tors from the body frame at cycle π into the body frame at cycleπ β 1, π
πis a rotation vector used to update Cπ(πβ1)
π(π), and π
πΓ is
cross-product antisymmetric matrix composed of ππcompo-
nents. The updating structure of rotation vector is given by
ππ= πΌπ+ πΏππ, (13)
πΌπ= β«
π‘
π‘πβ1
π ππ‘, (14)
πΏππβ β«
π‘π
π‘πβ1
(
1
2
ππΓ π +
1
12
ππΓ (ππΓ π)) ππ‘, (15)
where π 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 atti-tude algorithm by using angular increment, some coefficientsare designed to restrain the direct component ofπ₯-axial angu-lar increment under static coning environment [8, 14, 15]:
πΏππ=
πβ1
β
π=1
π
β
π=π+1
πππΞΜππ(π) Γ Ξ
Μππ(π) , (16)
where πππ
is the correction coefficient depending on theconing correction structure and ΞΜπ
πis an angular increment
-
4 International Journal of Aerospace Engineering
y
πΎ
yb
ze
z
O
ye
πΎ
π
π
xb
xeπ
x
zb
π.
.
.
Figure 1: Rotational relationship of ππ₯ππ¦ππ§πand ππ₯
ππ¦ππ§π.
sample over a fixed time interval. Due to the sampling num-ber of a triaxial gyro, single-sample algorithm, two-samplealgorithm, three-sample algorithm, and so on have beendeveloped, and the validity of algorithm is described bydrifting error.
One stage is updating the attitude in an attitude updatingcycle, and it is referred to in (8) to (12). The other stageis calculating the rotation vector including an integratedgyro sensed angular rate and a coning correction calcu-lated from gyroscopes data, and it is referred to in (13) to(16).
By analyzing the optimizing process, the so-called coningerror is independent directly to vehiclesβ movement dueto (13) to (16), and π
1(ππ) and π
2(ππ) are both scalar by
substituting the modular of ππ. So the coning correction πΏπ
π
under coning environment cannot change the structure ofCππ
and the effect on Euler attitude inCππby optimizing. Although
drifting error is dealt with through optimization, oscillatingerror remains.
3.3. Effect of Dynamic Coning Environment on AttitudeAlgorithm. It is convenient to use the two-stage structurealgorithm to design coefficients and restrain drifting error ofattitude under static coning environment, but it is difficult toinvestigate the effect of dynamic coning environment clearlyby using two-stage structure algorithm because it is quitedifficult to derive simple and clear descriptive equationsfor oscillating error and drifting error. Nevertheless, two-stage structure algorithm and the Euler attitude algorithmboth use Euler angles to define the relationship between thebody frame and the earth frame [18], so they are equivalentwithout considering singular problem [20]. For derivationand analysis of the effect on attitude algorithm clearly, wecan use the Euler attitude algorithm to make the investiga-tion.
According to the rotation relationship in Figure 1, therelationship between angular velocity in the body frame andEuler angular velocity can be described as [21]
ππ = οΏ½ΜοΏ½ + Μπ + οΏ½ΜοΏ½, (17)
where ππ is angular velocity in the body frame and οΏ½ΜοΏ½, Μπ, andοΏ½ΜοΏ½ are Euler angular velocities, respectively. Projecting Eulerangular velocity to each axis of the body frame and expanding(17), angular velocity in the body frame can be obtained:
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
=
[
[
[
[
[
0 sin π 1
sin πΎ cos π cos πΎ 0
cos πΎ β cos π sin πΎ 0
]
]
]
]
]
[
[
[
[
[
Μπ
οΏ½ΜοΏ½
ΜπΎ
]
]
]
]
]
, (18)
where Μπ, οΏ½ΜοΏ½, and ΜπΎ are Euler angular speeds, respectively, andthey are all scalar; ππ
πππ₯, πππππ¦
, and πππππ§
are components ofangular velocity in the body frame to describe the rotationof a vehicle with respect to the earth frame, and they canbe obtained by a triaxial gyroscope fixed in the body frame.Transforming (18), the differential equation of Euler attitudeis obtained:
[
[
[
[
[
Μπ
οΏ½ΜοΏ½
ΜπΎ
]
]
]
]
]
=
[
[
[
[
[
[
0 sin πΎ cos πΎ
0
cos πΎcos π
β
sin πΎcos π
1 β tan π cos πΎ tan π sin πΎ
]
]
]
]
]
]
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
. (19)
In (19), there is singular problem when the pitch angleis equal to 90 degrees. Under pure coning environment, thepitch angle will never attain to 90 degrees, so the singularproblem can be ignored reasonably.
Substituting (4) into (19), Euler attitude equations can beobtained:
[
[
[
[
[
Μπ
οΏ½ΜοΏ½
ΜπΎ
]
]
]
]
]
=
[
[
[
[
[
[
[
[
Ξ© sinΞπ‘ cos (Ξ©π‘ + πΎ) + Ξ sin (Ξ©π‘ + πΎ)
βΞ© sinΞπ‘ sin (Ξ©π‘ + πΎ) + Ξ cos (Ξ©π‘ + πΎ)cos π
β2Ξ©sin2Ξπ‘2
+ Ξ© sinΞπ‘ tan π sin (Ξ©π‘ + πΎ) β Ξ tan π cos (Ξ©π‘ + πΎ)
]
]
]
]
]
]
]
]
. (20)
Under dynamic coning environment, there is no rollingmotivation for nonspinning vehicles, so the real roll angleshould be zero. However, we note that the roll angle locatedon the right of the third element of (20) is not zero atall, although the initial value of the roll angle is chosen aszero. This means that the roll angle error is generated. Sothe third element of (20) actually represents the roll angleerror equation.Therefore, the roll angle error rate induced bydynamic coning environment can be determined as
Ξ ΜπΎ = β2Ξ© sin2Ξπ‘2
+ π΄ sin (Ξ©π‘ + ΞπΎ + π) sinΞ©π‘ sinΞπ‘1 β sin2Ξ©π‘ sin2Ξπ‘
,
(21)
π΄ =β
(Ξ© sinΞπ‘)2 + (Ξ)2, (22)
π = βarctanΞ© sinΞπ‘Ξ
. (23)
In (21), the first term is direct component and impliesdrifting error, which has been attracting many researchersβ
-
International Journal of Aerospace Engineering 5
interests. The second term is alternating component andimplies oscillating error. Apparently, drifting error and oscil-lating error fluctuate by the cone half-angle and the pre-cession frequency. When the cone half-angle is increasing,drifting error and amplitude of oscillating error are bothincreasing.
From the derivation above by using Euler attitude algo-rithm, we confirm that attitude of vehicles is affected to gen-erate drifting error and oscillating error under dynamic con-ing environment.
From the first and second elements of (20), it is seen thatthe roll angle error affects accuracy of the pitch angle and yawangle. The accurate pitch angle and yaw angle can be deter-mined without roll angle error:
[
[
Μπ
οΏ½ΜοΏ½
]
]
=
[
[
[
Ξ© sinΞπ‘ cosΞ©π‘ + Ξ sinΞ©π‘
βΞ© sinΞπ‘ sinΞ©π‘ + Ξ cosΞ©π‘cos π
]
]
]
. (24)
With the small angle approximation, the pitch angle errorand yaw angle error can be obtained by comparing (20) and(24):
ΞΜ
π = ΞπΎ (Ξ© sinΞπ‘ sinΞ©π‘ + Ξ cosΞ©π‘) ,
ΞοΏ½ΜοΏ½ =
ΞπΎ (Ξ© sinΞπ‘ cosΞ©π‘ + Ξ sinΞ©π‘)cos π
.
(25)
In fact, the pitch angle error and yaw angle error are allinduced by the transferred roll angle error under dynamicconing environment, so attitude error is actually a sort ofinduced error by dynamic coning environment. This is thereason why so many researchers pay more attentions toattitude error by coning environment, in, specially, the rollangle error.
4. Coning Algorithm of Spinning Vehiclesunder Dynamic Coning Environment
From the analysis above, attitude algorithm is affected bydynamic coning environment badly. To calculate attitudeaccurately, in this section, we propose a coning algorithm ofspinning vehicles based on a cone frame and cone attitudeand build the relationship between cone attitude and Eulerattitude.
4.1. Definition of Cone Frame. Compared with commonangular movement, coning motion of spinning vehicles is
a sort of angular movement of longitudinal axis of vehiclesunder coning environment. Actually, this special movementdirectly shows precession and nutation of longitudinal axis,and self-rotation is accompanying. Since attitude error isinduced by coning motion of periodicity, a reasonablemethod is describing coning motion accurately and calcu-lating attitude without error. Therefore, cone frame and coneangles should be defined specially to describe angular move-ment.
Referring to gyrodynamics [22], a frame is defined byrotation relationship to describe movement of a rotor in agyroscope that has typical nutation and precession. To avoidconfusion, 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 precessionangle πΏ
1, nutation angle πΏ
2, and rotation angle πΏ
3is defined
relatively. The initial position of πΏ1is horizontal on the right
of cone, and the range is [0β 360β].The initial position of πΏ2is
coincident with cone axis, and the range is [0β 90β].The initialposition of πΏ
3is located in symmetry plane of a vehicle, and
the range is [0β 360β].The rotation order of ππ₯
ππ¦ππ§πand ππ₯
ππ¦ππ§πis as follows:
ππ₯ππ¦ππ§π
ΜπΏ1
β
π₯π
ππ₯
ππ¦
ππ§
π
ΜπΏ2
β
π¦
π
ππ₯
ππ¦
ππ§
π
ΜπΏ3
β
π₯
π
ππ₯ππ¦ππ§π. (26)
In the rotation order, ΜπΏ1, ΜπΏ2, and ΜπΏ
3are corresponding
angular speeds. However, through in-depth study of therotation order, we find that the rotation angle is obtained byrotating discontinuously along π₯-axis twice, and this rota-tional process allows the rotation angle to contain precessionprocess. It means that the rotation angle is equal to thenegative precession angle if a nonspinning vehicle is underconing environment. Therefore, an isolation of the influenceby precession is needed. An indispensable rotation ΜπΏ
1along
π₯-axis is added before the second rotation along π₯-axis, andthe improved rotation order is shown as
ππ₯ππ¦ππ§π
ΜπΏ1
β
π₯π
ππ₯
ππ¦
ππ§
π
ΜπΏ2
β
π¦
π
ππ₯
ππ¦
ππ§
π
ΜπΏ3β ΜπΏ1
β
π₯
π
ππ₯ππ¦ππ§π. (27)
As is shown in Figure 2, the relationship of ππ₯ππ¦ππ§πand
ππ₯ππ¦ππ§πcan be described by a transformation matrix:
Cππ
=
[
[
[
[
[
[
[
cos πΏ2
β sin (πΏ1β πΏ3) sin πΏ
2cos (πΏ
1β πΏ3) sin πΏ
2
sin πΏ2sin πΏ1
cos (πΏ1β πΏ3) cos πΏ
1+ sin (πΏ
1β πΏ3) cos πΏ
2sin πΏ1sin (πΏ1β πΏ3) cos πΏ
1β cos (πΏ
1β πΏ3) cos πΏ
2sin πΏ1
β sin πΏ2cos πΏ1cos (πΏ
1β πΏ3) sin πΏ
1β sin (πΏ
1β πΏ3) cos πΏ
2cos πΏ1sin (πΏ1β πΏ3) sin πΏ
1+ cos (πΏ
1β πΏ3) cos πΏ
2cos πΏ1
]
]
]
]
]
]
]
,
(28)
-
6 International Journal of Aerospace Engineering
O
yc(yb)
yc
yb
πΏ1
πΏ1
πΏ
οΏ½ΜοΏ½1
zc
zc zbzb
xb(xb)
xc(xc)
πΏ2
πΏ2
πΏ3 β πΏ1
πΏ3 β πΏ1
3 β 1.
πΏ.
οΏ½ΜοΏ½2
Figure 2: Rotational relationship of ππ₯ππ¦ππ§πand ππ₯
ππ¦ππ§π.
where Cππrepresents transformation from the body frame to
the cone frame.
4.2. Cone Algorithm Based on Rotation Relationship. Accord-ing to the rotation relationship in Figure 2, the angularmotion model can be derived as
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
=
[
[
[
[
[
cos πΏ2β 1 0 1
β sin (πΏ1β πΏ3) sin πΏ
2cos (πΏ
1β πΏ3) 0
cos (πΏ1β πΏ3) sin πΏ
2sin (πΏ1β πΏ3) 0
]
]
]
]
]
[
[
[
[
[
[
ΜπΏ1
ΜπΏ2
ΜπΏ3
]
]
]
]
]
]
,
(29)
where πππππ₯
, πππππ¦
, and πππππ§
are measuring data of a triaxialgyro in the body frame to describe the motion of a vehiclewith respect to the cone frame. It is clear that (29) can describethe angular movement of spinning vehicles under dynamicconing environment, so it is modeling for this condition.Assuming the initial values of πΏ
1and πΏ
3are both zero, the
dynamic coning model can be obtained by substituting πΏ1=
ΜπΏ1π‘ and πΏ
3=
ΜπΏ3π‘ into (29):
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
=
[
[
[
[
[
[
ΜπΏ1cos πΏ2β (
ΜπΏ1β
ΜπΏ3)
βΜ
πΏ1sin {( ΜπΏ
1β
ΜπΏ3) π‘} sin πΏ
2+
ΜπΏ2cos {( ΜπΏ
1β
ΜπΏ3) π‘}
ΜπΏ1cos {( ΜπΏ
1β
ΜπΏ3) π‘} sin πΏ
2+
ΜπΏ2sin {( ΜπΏ
1β
ΜπΏ3) π‘}
]
]
]
]
]
]
.
(30)
From (30), we see that if ΜπΏ3is zero, it can be simplified as
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
=
[
[
[
[
[
[
ΜπΏ1cos πΏ2β
ΜπΏ1
βΜ
πΏ1sin ΜπΏ1π‘ sin πΏ
2+
ΜπΏ2cos ΜπΏ1π‘
ΜπΏ1cos ΜπΏ1π‘ sin πΏ
2+
ΜπΏ2sin ΜπΏ1π‘
]
]
]
]
]
]
. (31)
Equation (31) is the same as (4), and this means that ΜπΏ1=
Ξ© and ΜπΏ2
= Ξ. In addition, (30) in fact provides measuringdata from a triaxial gyro in the body frame for vehicles underdynamic coning environment, and, especially, (30) includesrotation information. When vehicles make nutation androtation without precession, the measuring data representspitching by providing ΜπΏ
2projected on π¦-axis and π§-axis of
the gyro. When vehicles only make rotation without coningenvironment, the measuring data provides ΜπΏ
3. It means that
(4) can decouple the rotation of the vehicle from pitching andyawing that are represented equivalently by the precessionand nutation.Thus, we are sure that (29) or (30) can describethe whole coning motion for vehicles.
By transforming (29), cone attitude equation for πΏ1, πΏ2,
and πΏ3can be derived as
[
[
[
[
[
[
ΜπΏ1
ΜπΏ2
ΜπΏ3
]
]
]
]
]
]
=
[
[
[
[
[
[
[
[
0 β
sin (πΏ1β πΏ3)
sin πΏ2
cos (πΏ1β πΏ3)
sin πΏ2
0 cos (πΏ1β πΏ3) sin (πΏ
1β πΏ3)
1 β sin (πΏ1β πΏ3) tanπΏ2
2
cos (πΏ1β πΏ3) tanπΏ2
2
]
]
]
]
]
]
]
]
[
[
[
[
[
[
πππππ₯
πππππ¦
πππππ§
]
]
]
]
]
]
.
(32)
We noted that (32) has singular problem when πΏ2is equal
to 0 or π/2. But, in fact, this model is specially derived forconing environment, and it is unnecessary to use it whenπΏ2is equal to 0. Moreover, vehicles are swinging in a plane
when πΏ2is equal to π/2, and the attitude does notmake 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 coningenvironment. In other words, there is no error in the thirdaxis although the other two axes are oscillating periodically:
ΞA = 0, (33)
where ΞA is cone attitude error [ΞπΏ1, ΞπΏ2, ΞπΏ3]
π.The coning environment can be described in the earth
frame or the cone frame, so the relationship of Euler attitudeand cone attitude is built according to the geometricalrelationship in Figure 3:
sin2π = sin2πΏ1sin2πΏ2,
sin2πΏ2= cos2π sin2π + sin2π.
(34)
-
International Journal of Aerospace Engineering 7
π
xb
ππΏ1
πΏ2
ye,c
xe,cO
ze,c
Figure 3: Geometrical relationship of Euler attitude and coneattitude.
Table 1: Symbol of π.
πΏ1
π
[0β 180β] +[180β 360β] β
Table 2: Symbol of π.
πΏ1
π
[90β 270β] +[0β 90β] βͺ [270β 360β] β
Therefore, the pitch angle and yaw angle can be calculatedby
π = Β± arcsin
sin πΏ1sin πΏ2
,
π = Β± arcsinβ sin2πΏ2β sin2π
cos2π.
(35)
According to Figure 3, the maximum of π is equal to πΏ2
when πΏ1is (2π + 1) π/2 (π = 0, 1, 2, 3, . . .). The maximum of π
is equal to πΏ2when πΏ
1is ππ. This means that π and π are both
less than πΏ2, so the square root of (9) is not complex-valued.
The symbol of π and π can be determined referring to Tables1 and 2. It is apparent that the pitch angle and yaw angle aredependent on πΏ
1and πΏ2. Therefore, the accuracy of the pitch
angle and yaw angle is dependent on the precession angle andnutation angle as well.
5. Simulation and Analysis
To disclose the effect of coning environment on traditionalattitude algorithm and verify the validity of the cone attitudealgorithm, numerical simulations are designed on purpose inthis section.
We choose a nonspinning vehicle for simulations becauseit is convenient to check whether the roll angle or the rotationangle is zero. We assume that the initial Euler attitude ofthe nonspinning vehicle is π = 0β, π = 1β, and πΎ = 0β,the total simulation time is 600 s, and the update time is0.0001 s.The optimized coefficients of the two-stage structure
Table 3: Varying Ξ© while Ξ = 0.011β/s.
Simulation 1 2 3 4Ξ© (β/s) 360 720 1080 1440
Table 4: Varying Ξ while Ξ© = 360β/s.
Simulation 1 2 3 4Ξ (β/s) 0.011 0.023 0.034 0.046
of attitude algorithm are chosen as π1= 0.45, π
2= 0.675 [8],
which is enough for investigating.The conditions of dynamicconing environment are shown in Tables 3 and 4. Under eachcondition, we will investigate how attitude error is affected.
Attitude error by Table 3 is represented in Figure 4. FromFigure 4, we see that as the precession frequency of coningenvironment is enlarging, attitude error is oscillating anddiverging, and the amplitude of both pitch angle error andyaw angle error becomes larger. Euler attitude error is driftingslightly, and this is because the cone half-angle is too small tomake attitude error drift obviously. In addition, the oscillatingfrequency of pitch angle error and yaw angle error is twicethat of roll angle error. In Figure 5, the roll angle error isenlarged up to 4β by small Ξ during 600 s, which means theincreasing process of amplitude of attitude error is acceleratedrapidly by nutation.We confirm that the changing rate of conehalf-angle affects the roll angle badly more than the preces-sion frequency. It is quite necessary for researchers to attachimportance to the effect of dynamic coning environment onattitude algorithms, especially variant nutation condition forvehicles.
Combining Tables 3 and 4, we choose the precessionangular speed 1440β/s and the changing rate of cone half-angle 0.046β/s as the wicked condition of dynamic coningenvironment, and triaxial angular velocity for this conditionis shown in Figure 6.Theπ₯-axial component of angular veloc-ity is nonzero due to the precession angular speed. The mag-nitude of each axial component is increasing as the nutationangle is enlarging. The cone attitude is calculated by the conealgorithm. As is shown in Figure 7, we see that the precessionangle and nutation angle are changing due to the precessionfrequency and the changing rate of cone half-angle. Par-ticularly, we note that, during 600 s under dynamic coningenvironment, the magnitude of rotation angle of the vehicleis below 10π β 5. In fact this error results from the calculatingerror of computer, and ideally the rotation angle has no error.This is because the rotation order of the cone frame shows theprecession, nutation, and rotation of vehicles under arbitraryconing motion, and the angular motion model is establishedaccording to the rotation order. When resolving the angularequation, cone attitude can be obtained without error.There-fore, we confirm that dynamic coning environment can bedescribed by the cone frame and the cone attitude of spinningvehicles can be calculated by the cone algorithm. In addition,the rotation angle is more competent to describe the spinningprocess of vehicles under coning environment than Eulerangle gamma.The pitch angle and yaw angle of the vehicle arecalculated due to the geometrical relationship of cone attitude
-
8 International Journal of Aerospace Engineering
0 100 200 300 400 500 600
β0.010
0.01
β0.02
0
0.02
β0.5
0
0.5
t (s)
0 100 200 300 400 500 600t (s)
0 100 200 300 400 500 600t (s)
= 360
= 720
= 1080
= 1440
= 360
= 720
= 1080
= 1440
1 = 360
1 = 720
= 1080
= 1440
Ξπ(β)
Ξπ(β)
ΞπΎ(β)
πΏ.
πΏ. 1
1
πΏ.
πΏ.
1
1
πΏ.
πΏ. 1
1
πΏ.
πΏ.
1
1
πΏ.
πΏ. 1
1
πΏ.
πΏ.
(a) The whole process of attitude error
440 441 442 443 444 445
β0.010
0.01
β0.010
0.01
β0.2
0
0.2
t (s)
440 441 442 443 444 445t (s)
440 441 442 443 444 445t (s)
= 360
= 720
= 1080
= 1440
= 360
= 720
= 1080
= 1440
= 360
= 720
= 1080
= 1440
Ξπ(β)
Ξπ(β)
ΞπΎ(β)
1πΏ.
1πΏ. 1
πΏ.
1πΏ.
1πΏ.
1πΏ. 1
πΏ.
1πΏ.
1πΏ.
1πΏ. 1
πΏ.
1πΏ.
(b) Amplification of attitude error
Figure 4: Attitude error by varying Ξ©.
β0.010
0.01
β0.010
0.01
β5
0
5
= 0.011
= 0.023
= 0.034
= 0.046
0 100 200 300 400 500 600t (s)
= 0.011
= 0.023
= 0.034
= 0.046
0 100 200 300 400 500 600t (s)
2 = 0.011
= 0.023
= 0.034
= 0.046
0 100 200 300 400 500 600t (s)
Ξπ(β)
Ξπ(β)
ΞπΎ(β)
πΏ.
2πΏ. 2πΏ
.
2πΏ.
2πΏ.
2πΏ. 2πΏ
.
2πΏ.
2πΏ.
2πΏ. 2πΏ
.
2πΏ.
(a) The whole process of attitude error
445 446 447 448 449 450
β0.01
0
0.01
β0.01
0
0.01
β202
t (s)
= 0.011
= 0.023
= 0.034
= 0.046
445 446 447 448 449 450t (s)
= 0.011
= 0.023
= 0.034
= 0.046
445 446 447 448 449 450t (s)
= 0.011
= 0.023
= 0.034
= 0.046
Ξπ(β)
Ξπ(β)
ΞπΎ(β)
2πΏ.
2πΏ. 2
πΏ.
2πΏ.
2πΏ.
2πΏ. 2πΏ
.
2πΏ.
2πΏ.
2πΏ. 2πΏ
.
2πΏ.
(b) Amplification of attitude error
Figure 5: Attitude error by varying Ξ.
-
International Journal of Aerospace Engineering 9
0 100 200 300 400 500 600
0 100 200 300 400 500 600
0 100 200 300 400 500 600
β200
β100
0
β1000
0
1000
β1000
0
1000
t (s)
t (s)
t (s)
bπebx(β Β·sβ
1)
bπeby(β Β·sβ
1)
bπebz(β Β·sβ
1)
Figure 6: Triaxial angular velocity.
400 405 410 415 420
0 100 200 300 400 500 600
0 100 200 300 400 500 600
0
200
400
0
20
40
β5
0
5
t (s)
t (s)
t (s)
Γ10β5
πΏ1(β)
πΏ2(β)
πΏ3(β)
Figure 7: Cone attitude angles πΏ1, πΏ2, and πΏ
3.
and are shown in Figure 8, which can be regarded as the realpitch angle and yaw angle of vehicles.
6. Conclusion
Attitude error of vehicles has an intense trend of divergencewhen vehicles undergo worsening coning environment. Inthis paper, we firstly derive the model of dynamic coningenvironment. Then through investigation of the effect onEuler attitude algorithm for the equivalency of traditionalattitude algorithm, we find that attitude error is actually theroll angle error including drifting error and oscillating error,which is induced directly by dynamic coning environmentand further affects the pitch angle and yaw angle throughtransferring. Based on the definition of the cone frame andcone attitude, we propose a cone algorithm by rotationrelationship to calculate cone attitude and establish the rela-tionship between cone attitude and Euler attitude of spinningvehicle. Finally, numerical simulations are made with variantprecession frequency and changing rate of cone half-angle toverify the effect of dynamic coning environment on attitude
algorithm and validity of cone algorithm. The results showthat the induced error of Euler attitude fluctuates by the varia-tion of precession and nutation, especially by that of nutation,and the oscillating frequency of roll angle error is twice that ofpitch angle error and yaw angle error. In addition, the rotationangle is more competent to describe the spinning process ofvehicles 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 ofspinning bodies, understanding mechanism of attitude errorby coning environment, and developing attitude algorithm,and it also provides a new view on the effect of coningmotion.Our future work will focus on the optimization of attitudealgorithm under dynamic coning environment and real-timeattitude algorithm of spinning vehicles under high dynamicmovement.
Conflict of Interests
The authors declare that there is no conflict of interestsregarding the publication of this paper.
-
10 International Journal of Aerospace Engineering
0 100 200 300 400 500 600β30
β20
β10
0
10
20
30
ππ
t (s)
π,π
(β)
(a) The whole process of pitch angle and yaw angle
405 405.5 406 406.5 407β20
β15
β10
β5
0
5
10
15
20
ππ
t (s)
π,π
(β)
(b) Amplification of pitch angle and yaw angle
Figure 8: Pitch angle and yaw angle of the vehicle.
Acknowledgment
This study is financially supported by the National NaturalScience Foundation of China (nos. 61471046, 61473039, and11202023).
References
[1] P. G. Savage, βStrapdown inertial navigation integration algo-rithm design part 1: attitude algorithms,β Journal of Guidance,Control, and Dynamics, vol. 21, no. 1, pp. 19β28, 1998.
[2] J. W. Jordan, βAn accurate strapdown direction cosine algo-rithm,β NASA Technical Note NASA TN D-5384, NASA, 1969.
[3] J. E. Bortz, βA new mathematical formulation for strapdowninertial navigation,β IEEE Transactions on Aerospace and Elec-tronic Systems, vol. 7, no. 1, pp. 61β66, 1971.
[4] C. W. Kang, N. I. Cho, and C. G. Park, βApproach to directconing/sculling error compensation based on the sinusoidalmodelling of IMU signal,β IETRadar, Sonar andNavigation, vol.7, no. 5, pp. 527β534, 2013.
[5] M. U. Salman and B. Chang, βActive coning compensation forcontrol of spinning flying vehicles,β in Proceedings of the IEEEInternational Conference on Control Applications Part of IEEEMulti-Conference on Systems and Control (CCA β10), pp. 1832β1837, IEEE, Yokohama, Japan, September 2010.
[6] R. B. Miller, βA new strapdown attitude algorithm,β Journal ofGuidance, Control, and Dynamics, vol. 6, no. 4, pp. 287β291,1983.
[7] J. G. Lee, Y. J. Yoon, J. G.Mark, andD. A. Tazartes, βExtension ofstrapdown attitude algorithm for high-frequency base motion,βJournal of Guidance, Control, and Dynamics, vol. 13, no. 4, pp.738β743, 1990.
[8] M. B. Ignagni, βOptimal strapdown attitude integration algo-rithms,β Journal of Guidance, Control, and Dynamics, vol. 13, no.2, pp. 363β369, 1990.
[9] Y. F. Jiang and Y. P. Lin, βImproved strapdown coning algo-rithms,β IEEE Transactions on Aerospace and Electronic Systems,vol. 28, no. 2, pp. 484β490, 1992.
[10] Z.-T. Li, T.-J. Wu, and L.-H. Ma, βA coning compensation algo-rithm for SINS in high dynamic motion,β Control Engineeringand Applied Informatics, vol. 13, no. 3, pp. 32β40, 2011.
[11] C.-Y. Tang and X.-Y. Chen, βA generalized coning correctionstructure for attitude algorithms,β Mathematical Problems inEngineering, vol. 2014, Article ID 614378, 15 pages, 2014.
[12] J. G. Mark and D. A. Tazartes, βTuning of coning algorithms togyro data frequency response characteristics,β Journal of Guid-ance, Control, and Dynamics, vol. 24, no. 4, pp. 641β647, 2001.
[13] K. Kim and T. G. Lee, βAnalysis of the two-frequency coningmotion with SDINS,β in Proceedings of the AIAA Guidance,Navigation, and Control Conference and Exhibit, Montreal,Canada, August 2001.
[14] P. G. Savage, βExplicit frequency-shaped coning algorithms forpseudoconing environments,β Journal of Guidance, Control, andDynamics, vol. 34, no. 3, pp. 774β782, 2011.
[15] M. Song, W.-Q. Wu, and X.-F. Pan, βApproach to recoveringmaneuver accuracy in classical coning algorithms,β Journal ofGuidance, Control, and Dynamics, vol. 36, no. 6, pp. 1872β1880,2013.
[16] L. Fu, L. L. Wang, and J. H. Hu, βConing algorithm based onsingular perturbation,βAircraft Engineering andAerospace Tech-nology, vol. 85, no. 3, Article ID 17086924, pp. 178β185, 2013.
[17] M.-S. Wang, W.-Q. Wu, J.-L. Wang, and X. Pan, βHigh-order attitude compensation in coning and rotation coexistingenvironment,β IEEE Transactions on Aerospace and ElectronicSystems, vol. 51, no. 2, pp. 1178β1190, 2015.
[18] R. P. Patera, βAttitude propagation for a slewing angular ratevector,β Journal of Guidance, Control, and Dynamics, vol. 33, no.6, pp. 1847β1855, 2010.
[19] R. P. Patera, βAttitude propagation for a slewing angular rate vec-tor with time varying slew rate,β in Proceedings of the AAS/AIAAAstrodynamics Specialist Conference (ASTRODYNAMICS β11),pp. 2529β2546, Girdwood, Alaska, USA, August 2011.
-
International Journal of Aerospace Engineering 11
[20] A. Janota, V. SΜimaΜk, D. Nemec, and J. HrbcΜek, βImproving theprecision and speed of Euler angles computation from low-costrotation sensor data,β Sensors, vol. 15, no. 3, pp. 7016β7039, 2015.
[21] X.-F. Qian, R.-X. Lin, and Y.-N. Zhao, Missile Flight Dynamics,Beijing Institute of Technology Press, Beijing, China, 2000.
[22] Y.-Z. Liu, Gyrodynamics, China Science Publishing & Media,Beijing, China, 2009.
-
International Journal of
AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
RoboticsJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Active and Passive Electronic Components
Control Scienceand Engineering
Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
International Journal of
RotatingMachinery
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporation http://www.hindawi.com
Journal ofEngineeringVolume 2014
Submit your manuscripts athttp://www.hindawi.com
VLSI Design
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Shock and Vibration
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Civil EngineeringAdvances in
Acoustics and VibrationAdvances in
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Electrical and Computer Engineering
Journal of
Advances inOptoElectronics
Hindawi Publishing Corporation http://www.hindawi.com
Volume 2014
The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014
SensorsJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Chemical EngineeringInternational Journal of Antennas and
Propagation
International Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Navigation and Observation
International Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
DistributedSensor Networks
International Journal of