[发明专利]基于角速度的欧拉角任意步长正交级数近似输出方法

权利要求书

1.一种基于角速度的欧拉角任意步长正交级数近似输出方法，其特征在于包括以下步骤：

步骤1、(a)根据欧拉方程：

式中：分别指滚转、俯仰、偏航角；p，q，r分别为滚转、俯仰、偏航角速度；全文参数定义相同；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p，q，r的n阶展开式分别为

p(t)＝[p₀ p₁ L p_n-1 p_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

q(t)＝[q₀ q₁ L q_n-1 q_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

r(t)＝[r₀ r₁ L r_n-1 r_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

其中

ξ0(t)=1ξ1(t)=cos[acos-1(1-2t/b)]ξ2(t)=2ξ1(t)·ξ1(t)-1Mξi+1(t)2ξ1(t)ξi(t)-ξi-1(t),i=2,3,L,n-1,0≤t≤NT,b=NT]]>

为Chebyshev(切比雪夫)正交多项式的递推形式，a为任意实数，T为采样周期；

(b)俯仰角的时间更新求解式为：

式中：

a₁＝1+[p₀ p₁ L p_n-1 p_n]Ω[p₀ p₁ L p_n-1 p_n]^T

+[q₀ q₁ L q_n-1 q_n]Ω[q₀ q₁ L q_n-1 q_n]^T

+[r₀ r₁ L r_n-1 r_n]Ω[r₀ r₁ L r_n-1 r_n]^T

+0.25{[p₀ p₁ L p_n-1 p_n]ζ}²+0.25{[q₀ q₁ L q_n-1 q_n]ζ}²

-0.25{[r₀ r₁ L r_n-1 r_n]ζ}²

a₂＝[q₀ q₁ L q_n-1 q_n]ζ

-0.5[r₀ r₁ L r_n-1 r_n]Ω[p₀ p₁ L p_n-1 p_n]^T

a₃＝[r₀ r₁ L r_n-1 r_n]ζ

+0.5[q₀ q₁ L q_n-1 q_n]Ω[p₀ p₁ L p_n-1 p_n]^T

ξ=ξ0ξ1LξnT=∫kT(k+1)Tξ(t)dt]]>

ξi=∫kT(k+1)Tξi(t)dt=∫kT(k+1)Tcos[aicos-1(1-2t/b)]dt]]>

=b4{1ai-1cos[(ai-1)cos-1(1-2t/b)]]]>

-1ai+1cos[(ai+1)cos-1(1-2t/b)]}|kT(k+1)T]]>

Ω={Ωji}j=0,1,L,n;i=1,2,L,n=∫kT(k+1)Tξ(t)∫kTTξ(τ)dτdt]]>

Ωji=∫kT(k+1)Tξj(t)∫kTtξi(τ)dτdt]]>

=b8{1ai-1∫kT(k+1)T{cos[(aj-ai+1)cos-1(1-2t/b)]+cos[(aj+ai-1)cos-1(1-2t/b)]}dt]]>

-1ai+1∫kT(k+1)T{cos[(aj-ai-1)cos-1(1-2t/b)]+cos[(aj+ai+1)cos-1(1-2t/b)]}dt}]]>

-b4{1ai-1cos[(ai-1)cos-1(1-2kT/b)]]]>

-1ai+1cos[(ai+1)cos-1(1-2kT/b)]}∫kT(k+1)Tcos[ajcos-1(1-2t/b)]dt]]>

步骤2、(a)在已知俯仰角的情况下，滚转角的时间更新求解式为：

其中

a₄＝1+[p₀ p₁ L p_n-1 p_n]Ω[p₀ p₁ L p_n-1 p_n]^T

+[q₀ q₁ L q_n-1 q_n]Ω[q₀ q₁ L q_n-1 q_n]^T

+[r₀ r₁ L r_n-1 r_n]Ω[r₀ r₁ L r_n-1 r_n]^T

+0.25{[p₀ p₁ L p_n-1 p_n]ζ(t)}²-0.25{[r₀ r₁ L r_n-1 r_n]ζ(t)}²

-0.25{[q₀ q₁ L q_n-1 q_n]ζ(t)}²

a₅＝[p₀ p₁ L p_n-1 p_n]ζ

+0.5[r₀ r₁ L r_n-1 r_n]Ω[q₀ q₁ L q_n-1 q_n]^T

a₆＝[r₀ r₁ L r_n-1 r_n]ζ

-0.5[p₀ p₁ L p_n-1 p_n]Ω[q₀ q₁ L q_n-1 q_n]^T

(b)在俯仰角、滚转角已知情况下，偏航角的求解式为：

ψ(t)=ψ(kT)+∫kTt[b1(t)+b2(t)]dt]]>

式中：