[發明專利]一種基于雙模型切換的二次傳遞對準方法

有效

• 申請號: 201410356581.2
• 申請日: 2014-07-24
• 公開（公告）號: CN104330092B
• 公開（公告）日: 2017-08-04
• 發明（設計）人: 陳帥;鐘潤伍;王磊杰;董亮;余威;常耀偉;金磊
• 申請（專利權）人: 南京理工大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 南京理工大學專利中心32203
• 代理人: 馬魯晉

1.一種基于雙模型切換的二次傳遞對準方法，應用于由主慣導系統和子慣導系統組成的慣性導航系統，前述主慣導系統和子慣導系統均為捷聯式慣性導航系統，其特征在于，前述方法包括以下步驟：

步驟1、主慣導系統采用固定頻率向子慣導系統發送導航參數，子慣導系統利用主慣導的導航參數完成粗對準，前述導航參數包括速度，姿態和位置信息；

步驟2、子慣導系統基于主慣導系統導航參數構造觀測量；

步驟3、在四元數誤差模型下使用擴展卡爾曼濾波進行迭代解算，待失準角縮小到一預設角度α時，進行濾波切換，然后在歐拉角誤差模型下使用標準卡爾曼濾波進行迭代解算，直到失準角估計值收斂且穩定；

在四元數誤差模型下使用擴展卡爾曼濾波進行迭代解算包括以下步驟：

11)四元數誤差與速度誤差方程：

式中：δQ為四元數誤差，δv為子慣導系統與主慣導系統在導航坐標系下的速度差；矩陣M，U和Y分別為：

式中，為地理坐標系相對于慣性坐標系的角速度在地理坐標系中的投影，為陀螺儀的輸出在載體坐標系中的投影；ε^b為陀螺的隨機常值漂移，表示陀螺測量白噪聲；為利用速度和位置信息得到的的誤差；為子慣導捷聯矩陣；為子慣導加速度計輸出在載體坐標系的投影；▽^b為加速度計在載體坐標系下的隨機常值漂移；為地球坐標系相對于慣性坐標系的角速度在導航坐標系的投影；為導航坐標系相對于地球坐標系的角速度在導航坐標系的投影；vⁿ＝[v_e v_n v_u]為子慣導在導航坐標系下的東北天速度；為速度誤差方程中的非線性項；

12)慣性器件誤差方程為：

13)系統方程為：

式中x＝[δq δv ε ▽]^T_13×1為狀態變量；δq表示四元數誤差；δv表示子慣導系統與主慣導系統在導航坐標系下的速度差；▽表示為加速度計的隨機常值漂移；ε表示系統軸陀螺常值漂移；

f(x)由線性和非線性兩部分組成，即f(x)＝F·x+p(x)，其中

非線性部分R_M是地球子午圈主曲率半徑，R_N是地球卯酉圈主曲率半徑，h是高度，V_u為主慣導系統的天向速度，l為主慣導系統的緯度，ω_ie是地球自轉角速度14)建立系統觀測方程

選取主慣導系統和子慣導系統的速度差作為觀測量，得到系統觀測量為：

Z_obs＝[δv_E δv_N δv_U]^T＝H·x

式中，H＝[0_4×3 I_3×3 0_6×3]_13×3，δv由子慣導系統與主慣導系統在導航坐標系下的速度做差得到；

15)根據前述建立的系統狀態方程、系統觀測方程以及系統的觀測量，采用擴展卡爾曼濾波器進行迭代解算，完成第一次對準，估測并補償失準角；

歐拉角誤差模型下的標準卡爾曼濾波迭代解算包括以下步驟：

21)姿態誤差與速度誤差方程為：

速度誤差方程為：

式中：φ_E是子慣導系統的東向失準角，φ_N是子慣導系統的北向失準角，φ_U是子慣導系統的天向失準角；δV_E是子慣導系統的東向速度誤差，δV_N是子慣導系統的北向速度誤差，δV_U是子慣導系統的天向速度誤差；V_E為主慣導系統的東向速度,V_N為主慣導系統的北向速度，V_U為主慣導系統的天向速度；f_E為主慣導系統的東向比力,f_N為主慣導系統的北向比力，f_U為主慣導系統的天向比力；L為主慣導系統的緯度，R_M是地球子午圈主曲率半徑，R_N是地球卯酉圈主曲率半徑，h是高度，ω_ie是地球自轉角速度；為子慣導系統x軸陀螺常值漂移，為子慣導系統y軸陀螺常值漂移，為子慣導系統z軸陀螺常值漂移；為子慣導系統x軸加速度計常值偏置,為子慣導系統y軸加速度計常值偏置，為子慣導系統z軸加速度計常值偏置；

22)慣性器件誤差方程：

式中，λ_x為子慣導系統x軸安裝誤差角，λ_y為子慣導系統y軸安裝誤差角，λ_z為子慣導系統z軸安裝誤差角；

可得系統方程為：

式中X＝[φ δV ε ▽ λ]^T15×1為狀態變量；φ表示子慣導系統失準角；δV表示子慣導系統的速度誤差；ε表示系統軸陀螺常值漂移；▽表示系統加速度計常值偏置；λ表示系統安裝誤差角；

23)F為系統狀態轉移矩陣：

其中，矩陣塊F₁與F₂如下：

24)建立系統量測方程，選取經過補償后的主慣導系統與子慣導系統的姿態差和速度差為觀測變量，系統量測方程表達為：

Z＝HX+V

式中：為量測變量，V是量測噪聲并且假定其為均值為零的高斯白噪聲，其協方差為E[VV^T]＝R，δv由主子慣導在導航坐標系下的速度相減得到，為子慣導與經過補償后的主慣導姿態做差得到，由前述步驟1)得到的失準角可得到矩陣將主慣導系統的捷聯矩陣與此矩陣相乘：

由可得到經過補償后的主慣導姿態；

H為量測矩陣：

其中：

式中：T_ij為矩陣的元素；

25)根據建立的系統狀態方程、系統觀測方程以及系統的觀測量，采用標準卡爾曼濾波進行濾波迭代解算，直到失準角估計值收斂且穩定；

以及

步驟4、利用步驟3估計出的失準角來修正子慣導系統的姿態矩陣，得到捷聯初始姿態矩陣，以完成二次傳遞對準。

2.根據權利要求1所述的基于雙模型切換的二次傳遞對準方法，其特征在于，前述預設角度α的取值范圍為：|α|≤5°。