[发明专利]基于角速度的飞行器极限飞行时四元数傅里埃近似输出方法

权利要求书

1.一种基于角速度的飞行器极限飞行时四元数傅里埃近似输出方法，其特征在于包括以下步骤：

根据四元数连续状态方程

e·=Aee]]>

和离散状态方程

e(k+1)＝Φ_e[(k+1)T，kT]e(k)

其中e＝[e₁，e₂，e₃，e₄]^TAe=120-p-q-rp0r-qq-r0prq-p0]]>

Φ_e[(k+1)T，kT]为A_e的状态转移矩阵，T为采样周期，

p，q，r分别为滚转、俯仰、偏航角速度；欧拉角θ，ψ分别指滚转、俯仰、偏航角；

状态转移矩阵按照逼近式

Φe[(k+1)T,kT]≈I+ΠAHAξAI(t)|kT(k+1)T+ΠBHBξBI(t)|kT(k+1)T+ΠAPAAI|kT(k+1(THATΠAI+ΠBPBAI|kT(k+1(THATΠAI]]>

+ΠAPABI|kT(k+1(THBTΠBI+ΠBPBBI|kT(k+1(THBTΠBI]]>

-[ΠAHAξAI(t)+ΠBHBξBI(t)]|kT(k+1)T[ξAIT(t)HATΠAI+ξBIT(t)HBTΠBI]|kT]]>

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；

其中，ω为角频率，I=1000010000100001,]]>

ξ_AI(t)＝[t sin(ωt)…sin[(n-1)ωt]sin(nωt)]^T

ξ_BI(t)＝[cos(ωt)cos(2ωt)…cos[(n-1)ωt]cos(nωt)]^T

滚转、俯仰、偏航角速度p，q，r的n阶展开式分别为

p(t)＝[p_a0 p_a1…p_a(n-1)p_an][1cos(ωt)…cos[(n-1)ωt]cos(nωt)]^T+[p_b1 p_b2…p_b(n-1)p_bn][sin(ωt)sin(2ωt)…sin[(n-1)ωt]sin(nωt)]^T

q(t)＝[q_a0 q_a1…q_a(n-1)q_an][1cos(ωt)…cos[(n-1)ωt]cos(nωt)]^T+[q_b1 q_b2…q_b(n-1)q_bn][sin(ωt)sin(2ωt)…sin[(n-1)ωt]sin(nωt)]^T

r(t)＝[r_a0 r_a1…r_a(n-1)r_an][cos(ωt)cos(2ωt)…cos[(n-1)ωt]cos(nωt)]^T+[r_b1 r_b2…r_b(n-1)r_bn][sin(ωt)sin(2ωt)…sin[(n-1)ωt]sin(nωt)]^T

ΠA=12{0-1001000000100-10pa0pa1···pa(n-1)pan]]>

+00-10000-110000100qa0qa1···qa(n-1)qan+000-100100-1001000ra0ra1···ra(n-1)ran}]]>

ΠB=12{0-1001000000100-10pb1pb2···pb(n-1)pbn]]>

+00-10000-110000100qb1qb2···qb(n-1)qbn+000-100100-1001000rb1rb2···rb(n-1)rbn}]]>

ΠA1=12{pa0pa1···pa(n-1)panT0-1001000000100-10]]>

+qa0qa1···qa(n-1)qanT00-10000-110000100+ra0ra1···ra(n-1)ranT000-100100-1001000}]]>

ΠB1=12{pb1pb2···pb(n-1)pbnT0-1001000000100-10]]>

+qb1qb2···qb(n-1)qbnT00-10000-110000100+rb1rb2···rb(n-1)rbnT000-100100-1001000}]]>

HA=H0TH1T···HnTT=diag{1,1ω,12ω,···,1nω},HB=-HA]]>