[發明專利]基于角速度的歐拉角傅里埃指數近似輸出方法

有效

• 申請號: 201110379904.6
• 申請日: 2011-11-25
• 公開（公告）號: CN102519465A
• 公開（公告）日: 2012-06-27
• 發明（設計）人: 史忠科
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/24
• 分類號: G01C21/24
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 王鮮凱

1.一種基于角速度的歐拉角傅里埃指數近似輸出方法，其特征在于包括以下步驟：

步驟1、(a)根據歐拉方程：

式中：ψ分別指滾轉、俯仰、偏航角；p，q，r分別為滾轉、俯仰、偏航角速度；全文參數定義相同；這三個歐拉角的計算按照依次求解俯仰角、滾轉角、偏航角的步驟進行；滾轉、俯仰、偏航角速度p，q，r的n階展開式分別為

p(t)＝p_A[1?cos(ωt)?L?cos[(n-1)ωt]?cos(nωt)]^T

?????+p_B[sin(ωt)?sin(2ωt)?L?sin[(n-1)ωt]?sin(nωt)]^T

q(t)＝q_A[1?cos(ωt)?L?cos[(n-1)ωt]?cos(nωt)]^T

?????+q_B[sin(ωt)?sin(2ωt)?L?sin[(n-1)ωt]?sin(nωt)]^T

r(t)＝r_A[cos(ωt)?cos(2ωt)?L?cos[(n-1)ωt]?cos(nωt)]^T

?????+r_B[sin(ωt)?sin(2ωt)?L?sin[(n-1)ωt]?sin(nωt)]^T

其中，ω為角頻率，

p_A＝[p_a0?p_a1?L?p_a(n-1)?p_an]，p_B＝[p_b1?p_b2?L?p_b(n-1)?p_bn]

q_A＝[q_a0?q_a1?L?q_a(n-1)?q_an]，q_B＝[q_b1?q_b2?L?q_b(n-1)?q_bn]

r_A＝[r_a0?r_a1?L?r_a(n-1)?r_an]，r_B＝[r_b1?r_b2?L?r_b(n-1)?r_bn]

(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ω},]]>H_B＝-H_A

H_i為H_A陣的行向量，

Ω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]]>

式中：