[发明专利]基于角速率输入的光纤陀螺惯导系统姿态优化方法

权利要求书

1.一种基于角速率输入的光纤陀螺惯导系统姿态优化方法，其中所述光纤陀螺惯导系统姿态优化过程包括旋转矢量计算模块和四元数更新模块，所述旋转矢量表示载体姿态的变化，所述四元数表示载体相对于参考坐标系的实时方位，

所述的旋转矢量计算模块执行以下步骤：

（11）利用DSP和FPGA实现数据采集功能，在一个姿态更新周期内，FPGA把光纤陀螺串行数据转化为并行数据，对陀螺仪的输出采样(N+1)次，分别得到载体运动角速率ω₀、ω₁、...、ω_N，其中N表示旋转矢量子样数；

（12）利用数值积分方法对陀螺仪采样值进行积分，一个姿态更新周期内陀螺仪角增量输出θ大小为：

θ=hΣk=0NCN,kωk]]>

式中，CN,k=(-1)N-kNk!(N-k)!∫0NΠj=0,j≠kN(t-j)dt,k=0,1,...,N,]]>h为姿态更新周期；

（13）利用前一姿态更新周期内的陀螺仪角增量以及当前陀螺仪采样信息，计算姿态更新周期内的旋转矢量：

Δφ^=θ+h2Σi=0N-1KN-i(ωi×ωN)+G(θ′×θ)]]>

其中，为[t_k,t_k+1]时间段内的旋转矢量，h=t_k+1-t_k，θ′为前一姿态更新周期内的陀螺仪角增量，K_i与G为误差补偿系数；

所述的四元数更新模块执行以下步骤：

（21）由可以计算得到[t_k,t_k+1]时间段内的姿态变化四元数q(h)，计算公式为：

q(h)=cosΔφ^02+Δφ^Δθ^0sinΔφ^02]]>

其中，为旋转矢量的模，q(h)为[t_k,t_k+1]时间段内载体姿态变化四元数；

（22）利用姿态变化四数可以求得[t_k,t_k+1]时间段内的姿态四元数，计算公式为：

Q(tk+1)=Q(tk)⊗q(h)]]>

（23）设Q(t)＝[q₀ q₁ q₂ q₃]^T则姿态四元数与姿态矩阵间存在如下关系式，根据计算得到的可以提取载体俯仰角P、横摇角R以及航向角H，

Cbn(t)=C11C12C13C21C22C23C31C32C33q12+q02-q32-q222(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q22-q32+q02-q122(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q32-q22-q12+q02]]>

对上式实时提取姿态角：

PRH=arcsin(C32C122+C222)arctan(-C31C33)arctan(C12C22).]]>