[发明专利]一种刚体空间运动状态的任意阶输出方法

权利要求书

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(t,t0)=I+0∫t0trdt-∫t0tqdt-∫t0trdt0∫t0tpdt∫t0tqdt-∫t0tpdt0+]]>

-∫t0tr∫t0t2rdtdt2-∫t0tq∫t0t2qdtdt2∫t0tq∫t0t2pdtdt2∫t0tr∫t0t2pdtdt2∫t0tp∫t0t2qdtdt2-∫t0tr∫t0t2rdtdt2-∫t0tp∫t0t2pdtdt2∫t0tr∫t0t2qdtdt2∫t0tp∫t0t2rdtdt2∫t0tq∫t0t2rdtdt2-∫t0tq∫t0t2qdtdt2-∫t0tp∫t0t2pdtdt2+···]]>

Φs(t,t0)=I+0-∫t0trdt∫t0tqdt∫t0trdt0∫t0tpdt-∫t0tqdt-∫t0tpdt0+]]>

-∫t0tr∫t0t2rdtdt2-∫t0tq∫t0t2qdtdt2-∫t0tq∫t0t2pdtdt2-∫t0tr∫t0t2pdtdt2-∫t0tp∫t0t2qdtdt2-∫t0tr∫t0t2rdtdt2-∫t0tp∫t0t2pdtdt2∫t0tr∫t0t2qdtdt2-∫t0tp∫t0t2rdtdt2∫t0tq∫t0t2rdtdt2-∫t0tq∫t0t2qdtdt2-∫t0tp∫t0t2pdtdt2+···]]>

I=100010001,]]>以上两式中可取t₀＝kT，t＝(k+1)T；

p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；

高度输出为：

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