[发明专利]基于角速度的欧拉角拉盖尔近似输出方法

权利要求书

1.一种基于角速度的欧拉角拉盖尔近似输出方法，其特征在于包括以下步骤：

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

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

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)=1-tξ2(t)=1-2t+0.5t2M(i+1)ξi+1(t)=(1+2i-t)ξi(t)-iξi-1(t),i=2,3,L,n-1]]>

为拉盖尔正交多项式的递推形式，T为采样周期，全文符号相同；

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

式中：

a1=1+p0p1Lpn-1pn∫kT(k+1)T[ξ(t)ξT(t)]dtHTp0p1Lpn-1pnT]]>

+q0q1Lqn-1qn∫kT(k+1)T[ξ(t)ξT(t)]dtHTq0q1Lqn-1qnT]]>

+r0r1Lrn-1rn∫kT(k+1)T[ξ(t)ξT(t)]dtHTr0r1Lrn-1rnT]]>

-p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTp0p1Lpn-1pnT]]>

-q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTq0q1Lqn-1qnT]]>

-r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTr0r1Lrn-1rnT]]>

+0.25{p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

+0.25{q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

-0.25{r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

a2=q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

-0.5r0r1Lrn-1rn∫kT(k+1)T[ξ(t)ξT(t)]dtHTp0p1Lpn-1pnT]]>

+0.5r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

·ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTp0p1Lpn-1pnT]]>

a3=r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T]]>

+0.5q0q1Lqn-1qn∫kT(k+1)T[ξ(t)ξT(t)]dtHTp0p1Lpn-1pnT]]>

-0.5q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

·ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTp0p1Lpn-1pnT]]>

H=-100L000-1-10L000001L000000L000MMMOMMM000L01-1000L001]]>

当p，q，r的展开式最高次项n为奇数时，m＝4，6，K，n+1，高次项n为偶数时m＝5，7，K，n+1；

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

其中

a4=1+p0p1Lpn-1pn∫kT(k+1)T[ξ(t)ξT(t)]dtHTp0p1Lpn-1pnT]]>

+q0q1Lqn-1qn∫kT(k+1)T[ξ(t)ξT(t)]dtHTq0q1Lqn-1qnT]]>

+r0r1Lrn-1rn∫kT(k+1)T[ξ(t)ξT(t)]dtHTr0r1Lrn-1rnT]]>

-p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTp0p1Lpn-1pnT]]>

-q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTq0q1Lqn-1qnT]]>

-r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T.]]>

ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTr0r1Lrn-1rnT]]>

+0.25{p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

-0.25{r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

-0.25{q0q1Lqn-1qnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T}2]]>

a5=p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

+0.5r0r1Lrn-1rn∫kT(k+1)T[ξ(t)ξT(t)]dtHTq0q1Lqn-1qnT]]>

-0.5r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

·ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTq0q1Lqn-1qnT]]>

a6=r0r1Lrn-1rnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

-0.5p0p1Lpn-1pn∫kT(k+1)T[ξ(t)ξT(t)]dtHTq0q1Lqn-1qnT]]>

+0.5p0p1Lpn-1pnHξ0(t)ξ1(t)Lξn(t)ξn+1(t)T|kT(k+1)T]]>

·ξ0(t)ξ1(t)Lξn(t)ξn+1(t)kTHTq0q1Lqn-1qnT]]>

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

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

式中：