1.一种基于角速度的欧拉角傅里埃指数近似输出方法,其特征在于包括以下步骤:
步骤1、(a)根据欧拉方程:
式中:ψ分别指滚转、俯仰、偏航角;p,q,r分别为滚转、俯仰、偏航角速度;全文参数定义相同;这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行;滚转、俯仰、偏航角速度p,q,r的n阶展开式分别为
p(t)=pA[1 cos(ωt) L cos[(n-1)ωt] cos(nωt)]T
+pB[sin(ωt) sin(2ωt) L sin[(n-1)ωt] sin(nωt)]T
q(t)=qA[1 cos(ωt) L cos[(n-1)ωt] cos(nωt)]T
+qB[sin(ωt) sin(2ωt) L sin[(n-1)ωt] sin(nωt)]T
r(t)=rA[cos(ωt) cos(2ωt) L cos[(n-1)ωt] cos(nωt)]T
+rB[sin(ωt) sin(2ωt) L sin[(n-1)ωt] sin(nωt)]T
其中,ω为角频率,
pA=[pa0 pa1 L pa(n-1) pan],pB=[pb1 pb2 L pb(n-1) pbn]
qA=[qa0 qa1 L qa(n-1) qan],qB=[qb1 qb2 L qb(n-1) qbn]
rA=[ra0 ra1 L ra(n-1) ran],rB=[rb1 rb2 L rb(n-1) rbn]
(b)俯仰角的时间更新求解式为:
式中:T为采样周期,
a1=[qAHAξAI(t)|kT(k+1)T+qBHBξBI(t)|kT(k+1)T]2]]>
+[rAHAξAI(t)|kT(k+1)T+rBHBξBI(t)|kT(k+1)T]2]]>
-[pAHAξAI(t)|kT(k+1)T+pBHBξBI(t)|kT(k+1)T]2]]>
a2=pAΩAAI|kT(k+1)THATrAT+pBΩBAI|kT(k+1)THATrAT]]>
+pAΩABI|kT(k+1)THBTrBT+pBΩBBI|kT(k+1)THBTrBT]]>
-[pAHAξAI(t)+pBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATrAT+ξBIT(t)HBTrBT]|kT]]>
a3=pAΩAAI|kT(k+1)THATqAT+pBΩBAI|kT(k+1)THATqAT]]>
+pAΩABI|kT(k+1)THBTqBT+pBΩBBI|kT(k+1)THBTqBT]]>
-[pAHAξAI(t)+pBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATqAT+ξBIT(t)HBTqBT]|kT]]>
|λ|={pAΩAAI|kT(k+1)THATpAT+pBΩBAI|kT(k+1)THATpAT]]>
+pAΩABI|kT(k+1)THBTpBT+pBΩBBI|kT(k+1)THBTpBT]]>
-[pAHAξAI(t)+pBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATpAT+ξBIT(t)HBTpBT]|kT]]>
+qAΩAAI|kT(k+1)THATqAT+qBΩBAI|kT(k+1)THATqAT]]>
+qAΩABI|kT(k+1)THBTqBT+qBΩBBI|kT(k+1)THBTqBT]]>
-[qAHAξAI(t)+qBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATqAT+ξBIT(t)HBTqBT]|kT]]>
+rAΩAAI|kT(k+1)THATrAT+rBΩBAI|kT(k+1)THATrAT]]>
+rAΩABI|kT(k+1)THBTrBT+rBΩBBI|kT(k+1)THBTrBT]]>
-[rAHAξAI(t)+rBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATrAT+ξBIT(t)HBTrBT]|kT}12]]>
ξAI(t)=[t sin(ωt) L sin[(n-1)ωt] sin(nωt)]T
ξBI(t)=[cos(ωt) cos(2ωt) L cos[(n-1)ωt] cos(nωt)]T
HA=H0TH1TLHnTT=diag{1,1ω,12ω,L,1nω},]]>HB=-HA
Hi为HA阵的行向量,
ΩAAI=--0.5t2cos(ωt)ωLsin[(n-1)ωt](n-1)ωsin(nωt)nω-tsin(ωt)ω-cos(ωt)ω2cos(2ωt)2ωLcos[(n-2)ωt]2(n-2)ω+cos(nωt)2nωcos[(n-1)ωt]2(n-1)ω+cos[(n+1)ωt]2(n+1)ωMMOMM-tsin[(n-1)ωt](n-1)ω-cos[(n-1)ωt](n-1)2ω2cos(nωt)2nω-cos[(n-2)ωt]2(n-2)ωLcos[2(n-1)ωt]2(n-1)ωcos[(2n-1)ωt]2(2n-1)ω+cos(ωt)2ω-tsin(nωt)nω-cos(nωt)n2ω2cos[(n+1)ωt]2(n+1)ω-cos[(n-1)ωt]2(n-1)ωLcos[(2n-1)ωt]2(2n-1)ω-cos(ωt)2ωcos(2nωt)2nω]]>
ΩABI=sin(ωt)ωsin(2ωt)2ωLsin[(n-1)ωt](n-1)ωsin(nωt)nω0.5t+sin(2ωt)4ωsin(3ωt)6ω+sin(ωt)2ωLsin(nωt)2nω+sin[(n-2)ωt]2(n-2)ωsin[(n+1)ωt]2(n+1)ω+sin[(n-1)ωt]2(n-1)ωMMOMMsin(nωt)2nω+sin[(n-2)ωt]2(n-2)ωsin[(n+1)ωt]2(n+1)ω+sin[(n-3)ωt]2(n-3)ωL0.5t+sin[2(n-1)ωt]4(n-1)ωsin(nωt)2nω+sin[(2n-1)ωt]2(2n-1)ωsin[(n+1)ωt]2(n+1)ω+sin[(n-1)ωt]2(n-1)ωsin[(n+2)ωt]2(n+2)ω+sin[(n-2)ωt]2(n-2)ωLsin(nωt)2nω+sin[(2n-1)ωt]2(2n-1)ω0.5t+sin(2nωt)4nω]]>
ΩBBI=-cos(2ωt)4ωcos(3ωt)6ω-cos(ωt)2ωLcos(nωt)2nω-cos[(n-2)ωt]2(n-2)ωcos[(n+1)ωt]2(n+1)ω-cos[(n-1)ωt]2(n-1)ωcos(3ωt)6ω+cos(ωt)2ωcos(4ωt)8ωLcos[(n+1)ωt]2(n+1)ω-cos[(n-3)ωt]2(n-3)ωcos[(n+2)ωt]2(n+2)ω-cos[(n-2)ωt]2(n-2)ωMMOMMcos(nωt)2nω+cos[(n-2)ωt]2(n-2)ωcos[(n+1)ωt]2(n+1)ω+cos[(n-3)ωt]2(n-3)ωLcos[2(n-1)ωt]4(n-1)ωcos[(2n+1)ωt]2(2n+1)ω-cos(ωt)2ωcos[(n+1)ωt]2(n+1)ω+cos[(n-1)ωt2(n-1)ωcos[(n+2)ωt]2(n+2)ω+cos[(n-2)ωt]2(n-2)ωLcos[(2n+1)ωt]2(2n+1)ω+cos(ωt)2ωcos(2nωt)4nω]]>
步骤2、在已知俯仰角的情况下,滚转角的时间更新求解式为:
其中
a4=a1+2{[pAHAξAI(t)|kT(k+1)T+pBHBξBI(t)|kT(k+1)T]2]]>
-[qAHAξAI(t)|kT(k+1)T+qBHBξBI(t)|kT(k+1)T]2}]]>
a5=qAΩAAI|kT(k+1)THATpAT+qBΩBAI|kT(k+1)THATpAT]]>
+qAΩABI|kT(k+1)THBTpBT+qBΩBBI|kT(k+1)THBTpBT]]>
-[qAHAξAI(t)+qBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATpAT+ξBIT(t)HBTpBT]|kT]]>
a6=qAΩAAI|kT(k+1)THATrAT+qBΩBAI|kT(k+1)THATrAT]]>
+qAΩABI|kT(k+1)THBTrBT+qBΩBBI|kT(k+1)THBTrBT]]>
-[qAHAξAI(t)+qBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATrAT+ξBIT(t)HBTrBT]|kT]]>
步骤3、在俯仰角、滚转角已知情况下,偏航角的求解为:
ψ(t)=ψ(kT)+∫kTt[b1(t)+b2(t)]dt]]>
式中: