[发明专利]一种刚体空间运动状态的任意步长正交级数输出方法

摘要

本发明公开了一种刚体空间运动状态的任意步长正交级数输出方法，该方法通过定义三元数，使得机体轴系三个速度分量和三元数构成线性微分方程组，并采用任意步长正交多项式对滚转、俯仰、偏航角速度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+Πvζ+ΠvΩΠvT]]>Φs[(k+1)T,kT]≈I+Πsζ+ΠsΩΠsT]]>p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；I=100010001,]]>ξ(t)＝[ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T，ξ0(t)=1ξ1(t)=cos[acos-1(1-2t/b)]ξ2(t)=2ξ1(t)·ξ1(t)-1···ξi+1(t)=2ξ1(t)ξi(t)-ξi-1(t)i=2,3,···,n-1,0≤t≤NT,b=NT]]>为基于Chebyshev(切比雪夫)正交多项式得到的任意步长正交多项式的递推形式，a为任意实数，滚转、俯仰、偏航角速度p，q，r的展开式分别为p(t)＝[p₀ p₁…p_n-1 p_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^Tq(t)＝[q₀ q₁…q_n-1 q_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^Tr(t)＝[r₀ r₁…r_n-1 r_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^TΠv=0000010-10p0p1···pn-1pn]]>+00-1000100q0q1···qn-1qn+010-100000r0r1···rn-1rn]]>Πs=0000010-10p0p1···pn-1pn]]>+001000-100q0q1···qn-1qn+0-10100000r0r1···rn-1rn]]>ζ=ζ0ζ1···ζ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,···,n,i=1,2,···,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]]>高度输出为：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.]]>