[发明专利]一种刚体空间运动状态的傅里埃输出方法

摘要

本发明公开了一种刚体空间运动状态的傅里埃输出方法，该方法通过定义三元数，使得机体轴系三个速度分量和三元数构成线性微分方程组，并采用傅里埃级数对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，可以按照任意阶保持器的方式求解系统的状态转移矩阵，进而得到刚体运动离散状态方程的表达式，避免了姿态方程奇异问题，从而得到刚体主要运动状态；本发明通过引入三元数使得状态转移矩阵为分块上三角形式，可以降阶求解状态转移矩阵，大大简化了计算复杂度，便于工程使用。

主权项

1.一种刚体空间运动状态的傅里埃输出方法，其特征包括以下步骤：机体轴系三个速度分量输出为：u(t)v(t)w(t)t=(k+1)T=Φv[(k+1)T,kT]u(t)v(t)w(t)t=kT+gΦv[(k+1)T,kT]Φs[(k+1)T,kT]s1(t)s2(t)s3(t)t=kT]]>+g∫kT(k+1)TΦv[(k+1)T,τ]nxnynzdτ]]>其中：u，v，w分别为沿刚体机体轴系x，y，z轴的速度分量，n_x，n_y，n_z分别为沿x，y，z轴的过载，g为重力加速度，s₁、s₂、s₃为定义的三元数，且s1(t)s2(t)s3(t)t=(k+1)T=Φs[(k+1)T,kT]s1(t)s2(t)s3(t)t=kT]]>Φv[(k+1)T,kT]≈I+ΠvAHAξAT(t)|kT(k+1)T+ΠvBHBξBT(t)|kT(k+1)T+ΠvAPAAT|kT(k+1(THATΠvAT+ΠvBPBAT|kT(k+1(THATΠvAT]]>+ΠvAPABT|kT(k+1(THBTΠvBT+ΠvBPBBT|kT(k+1(THBTΠvBT]]>-[ΠvAHAξAT(t)+ΠvBHBξBT(t)]|kT(k+1)T[ξATT(t)HATΠvAT+ξBTT(t)HBTΠvBT]|kT]]>Φs[(k+1)T,kT]≈I+ΠsAHAξAT(t)|kT(k+1)T+ΠsBHBξBT(t)|kT(k+1)T+ΠsAPAAT|kT(k+1(THATΠsAT+ΠsBPBAT|kT(k+1(THATΠsAT]]>+ΠsAPABT|kT(k+1(THBTΠsBT+ΠvBPBBT|kT(k+1(THBTΠsBT]]>-[ΠsAHAξAT(t)+ΠsBHBξBT(t)]|kT(k+1)T[ξATT(t)HATΠsAT+ξBTT(t)HBTΠsBT]|kT]]>p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；I=100010001,]]>ξ_A1(t)＝[t sin(ωt)…sin[(n-1)ωt]sin(nωt)]^Tξ_B1(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][1 cos(ω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)]^Tq(t)＝[q_a0 q_a1…q_a(n-1) q_an][1 cos(ω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)]^Tr(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ΠvA=0000010-10pa0pa1···pa(n-1)pan]]>+00-1000100qa0qa1···qa(n-1)qan+010-100000ra0ra1···ra(n-1)ran]]>ΠsA=0000010-10pa0pa1···pa(n-1)pan]]>+001000-100qa0qa1···qa(n-1)qan+0-10100000ra0ra1···ra(n-1)ran]]>ΠvB=0000010-10pb1pb2···pb(n-1)pbn]]>+00-1000100qb1qb2···qb(n-1)qbn+010-100000rb1rb2···rb(n-1)rbn]]>ΠsB=0000010-10pb1pb2···pb(n-1)pbn]]>+001000-100qb1qb2···qb(n-1)qbn+0-10100000rb1rb2···rb(n-1)rbn]]>HA=H0TH1T···HnTT=diag{1,1ω,12ω,···,1nω},]]>H_B＝-H_A高度输出为：h·=uvws1s2s3]]>其中：h为高度；姿态角的输出为：ψ(t)=ψ(kT)+∫kTtqs2(t)+rs3(t)s22(t)+s32(t)dt]]>其中：θ，ψ分别表示滚转、俯仰、偏航角，s1(t)s2(t)s3(t)=Φs(t,kT)s1(t)s2(t)s3(t)t=kT.]]>