[發明專利]基于角速率輸入的光纖陀螺慣導系統姿態優化方法

有效

• 申請號: 201410023748.3
• 申請日: 2014-01-20
• 公開（公告）號: CN103712623A
• 公開（公告）日: 2014-04-09
• 發明（設計）人: 程向紅;胡杰;馮驥
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 江蘇永衡昭輝律師事務所 32250
• 代理人: 王斌

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