[發明專利]精確空投導航裝置快速初始定姿裝置及方法

在審

• 申請號: 202010943511.2
• 申請日: 2020-09-09
• 公開（公告）號: CN112099071A
• 公開（公告）日: 2020-12-18
• 發明（設計）人: 尹劍;鄧宏林;趙創社;黃皓;張琬琳;楊萌;陳穎;劉延虎;陰紅衛;袁小瑩;王本國;鞏全成
• 申請（專利權）人: 西安應用光學研究所
• 主分類號: G01S19/53
• 分類號: G01S19/53;G01S19/47;G01C21/18;G01C21/16
• 代理公司: 中國兵器工業集團公司專利中心 11011
• 代理人: 劉二格

1.一種精確空投導航裝置快速初始定姿裝置，其特征在于，包括：初始姿態計算模塊、三個陀螺儀、三個加速度計、衛星導航模塊；陀螺儀和加速度計向初始姿態計算模塊提供實時測得的載體的角加速度以及線加速度信息，作為進行初始姿態計算的輸入量；衛星導航模塊向初始姿態計算模塊提供實時測得的載體位置及速度信息，作為初始姿態計算的參考數據；初始姿態計算模塊根據接收的輸入量和參考數據，完成初始姿態計算。

2.如權利要求1所述的精確空投導航裝置快速初始定姿裝置，其特征在于，所述陀螺儀選用光纖陀螺儀，對載體的右、前、上三個正交軸向的角度增量信息進行采集，并以設定的頻率提供給所述初始姿態計算模塊。

3.如權利要求2所述的精確空投導航裝置快速初始定姿裝置，其特征在于，三個所述加速度計對載體在右、前、上三個正交軸向的線速度增量進行采集，并以設定的頻率提供給所述初始姿態計算模塊。

4.如權利要求3所述的精確空投導航裝置快速初始定姿裝置，其特征在于，所述衛星導航模塊在預熱后進行初始化及鎖星工作，并在導航裝置加電后設定時間內，開始向所述初始姿態計算模塊以固定頻率提供載體的經度、緯度、高度的位置信息，以及東向速度、北向速度、天向速度的速度信息。

5.如權利要求4所述的精確空投導航裝置快速初始定姿裝置，其特征在于，在所述導航裝置加電預熱后，初始姿態計算模塊采集三個陀螺儀傳遞來的載體三個正交軸向的角度增量信息、三個加速度計測得的載體在三個正交軸向的線速度增量、衛星導航模塊送來的載體的經度、緯度、高度的位置信息，以及東向速度、北向速度、天向速度的速度信息，計算出載體初始航向、俯仰、橫滾信息，完成初始姿態信息的計算。

6.基于權利要求5所述精確空投導航裝置快速初始定姿裝置的精確空投導航裝置快速初始定姿方法，其特征在于，包括以下步驟：

步驟1：導航裝置上電后，初始姿態計算模塊向三個陀螺儀、三個加速度計以及衛星導航模塊發送啟動指令，并進行參數初始化；

初始化的參數包括初始姿態計算模塊內的計時器T清零并開始計數，設置以下參數：初始化完成標識F＝0，初始緯度正弦S0＝0，初始緯度余弦Cl0＝0，初始緯度Lat0＝0，初始精度Lon0＝0；

設4乘1矩陣Q1：

其中q1＝1，q2＝0，q3＝0，q4＝0；

設3乘1矩陣矩陣：

設3乘3矩陣C0，C₁；

步驟2：判斷計時器T是否增加了設定的t1秒的時間間隔，如果沒有達到則繼續計數，如果達到了就進入步驟3；

步驟3：采集當前時間間隔內的三個陀螺儀輸出的載體在右、前、上三個方向上的角增量值Δθ_x、Δθ_y、Δθ_z，并將他們組成角增量矩陣Δθ，記為：

三個加速度計輸出的載體在右、前、上三個方向上的速度增量值ΔV_x、ΔV_y、ΔV_z，將他們組成速度增量矩陣ΔV，記為：

步驟4：完成向量V_b的計算：

設變量n₁、n₂，n₃，t，c，s，Q₂，Q₃，C₂，V_b1，及Δθ^T＝[Δθ_x Δθ_y Δθ_z]，設矩陣順序進行如下運算：

(1)利用矩陣乘法計算n₁＝Δθ^T*Δθ，對n₁進行開方運算得到

(2)計算n₃＝n₂/2；

(3)對n₃進行三角函數運算c＝cos(n₃)*I，s＝sin(n₃)/n₂；

(4)計算出t＝s*d，得到Q₂＝(c+t)*Q₁；

(5)并設Q₂的轉置矩陣為計算出更新后的

(6)設q11＝q1*q1，q12＝q1*q2，q13＝q1*q3，q14＝q1*q4，q22＝q2*q2，q23＝q2*q3，q24＝q2*q4，q33＝q3*q3，q34＝q3*q4，q44＝q4*q4；

計算C₁：

(7)計算C₂＝C1*Δθ，然后計算并更新V_b1，使得V_b＝V_b1；

步驟5：判斷計時器T是否增加了設定的t2秒的時間間隔，如果沒有達到則返回步驟2，如果達到了就進入步驟6；

步驟6：采集衛星導航模塊發來的位置及速度信息：

當前載體所在的經度記為Lon；緯度記為Lat；高度記為Hi；

當前載體的速度信息，東向速度記為Ve，北向速度記為Vn，天向速度記為Vu，組成矩陣

如果F＝0，則令F＝1并計算變量,令Lat0＝lat，Lon0＝Lon，S0＝sin(Lat),Cl0＝cos(Lat)；然后轉入步驟2；如果F＝1則進入步驟7；

步驟7：設置變量S1，S2，S3，gn,SD，CD，Det，矩陣Gn，依次完成如下計算：

(1)計算S1＝sin(Lat)，然后利用S1計算S2＝S1*S1，再計算S3＝S2*S2，以及Det＝Lon-Lon0+7.2921151467e-5*t2；

(2)計算矩陣C0的當前值

(3)計算重力加速度:

gn＝9.7803267714*(1+5.27094e-3*S2+2.32718e-5*S4)-3.086e-6*Hi，

組成矩陣計算：

(4)更新計算：V1＝V1+C0*Wie×v*t2，其中×表示矩陣的叉乘運算；

(5)更新計算：V2＝V2-C0*Gn*t2；

(6)計算出當前矩陣V_n：V_n＝C0*v+V1+V2；

步驟8：判斷計數器T是否到達第第一個時間點T1，如果T不等于T1，則返回步驟2；如果T＝T1，則對當前計算出來的向量V_b和V_n進行記錄，即V0_b＝V_b，V0_n＝V_n，然后進入步驟9；

步驟9：判斷計時器是否到達要求初始姿態計算完成的時間T2，如果T不等于T2，則返回步驟2執行；如果T＝T2則對當前計算出來的向量V_b和V_n的進行記錄，即V1_b＝V_b，V1_n＝V_n，然后進入步驟10；

步驟10：設矩陣Ct1、Ct1r、Ct2、Ct3、Cbn及Cbn中的元素

c11,c12,c13,c21,c22,c23,c31,c32,c33及依次完成如下計算：

(1)計算其中×表示矩陣的叉乘運算，并計算出Ct1的逆矩陣Ct1r，即：

(2)計算Ct2，算法為：

(3)計算Ct3＝Ct1r*Ct2；

(4)計算出Ct4＝Ct3*C₁；

(5)計算矩陣Cbn＝C0^-1*Ct4，其中C0^-1是對矩陣C0的求逆矩陣運算；

(6)利用反三角函數計算最后確定的當前時刻的姿態值，單位為弧度：載體俯仰值Pitch的計算方法為：Pitch＝arcsin(c32)；

載體橫滾值Roll的計算方法為：Roll＝atan2(-c31,c33)；

載體航向值Hdg的計算方法為：Hdg＝atan2(-c12,c22)；

確定上述載體姿態值后，初始姿態的計算過程完成。