[發明專利]一種基于5階SSRCKF的SINS/DVL組合導航濾波方法

有效

• 申請號: 201710256315.6
• 申請日: 2017-04-19
• 公開（公告）號: CN107063245B
• 公開（公告）日: 2020-12-25
• 發明（設計）人: 徐曉蘇;董亞;代維
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 陳靜

本發明公開了一種基于5階SSRCKF的SINS/DVL組合導航濾波方法，具體包括：以水下航行器為研究對象，建立了捷聯慣性導航非線性誤差模型，并以多普勒測速儀作為輔助建立了組合導航模型；采用SSR規則選取CKF的容積點，并建立5階SSRCKF的非線性濾波模型；本發明采用SSR規則對標準CKF濾波方法進行改進，在一定程度上減少了CKF濾波方法的容積采樣點，在高階系統中效果顯著，解決了傳統CKF算法在高階系統中容積采樣點的數目太大導致算法的實時性不好和效率不高的問題。

技術領域

本發明屬于導航領域，特別是涉及一種基于5階SSRCKF的SINS/DVL組合導航濾波方法。

背景技術

捷聯慣性導航系統能實時測量系統所處的位置、當前速度和姿態等導航信息，并能在短時間內保持高精度，具有多輸出信號、強隱蔽性、強自主式等優點，其系統噪聲較低，但作為一種導航信息具有連續性的系統，系統誤差會隨時間積累，若不采取外界導航信息校正，其精度將會快速下降而導致無法使用。

發明內容

發明目的：為了提高AUV組合導航的精度及穩定性，并且考慮到實時的計算量，本發明提出了一種基于5階SSRCKF的SINS/DVL組合導航濾波方法。

技術方案：一種基于5階SSRCKF的SINS/DVL組合導航濾波方法，包括如下步驟：

1)建立SINS的非線性誤差模型：在SINS中選取東北天地理坐標系作為導航坐標系，以水下航行器自身建立載體坐標系，并給定實際計算導航坐標系，建立慣性器件陀螺儀和速度計的誤差模型，并求解系統的姿態誤差方程、位置誤差方程；

2)建立DVL的非線性誤差模型：針對DVL誤差中的刻度誤差以及隨機測量誤差，以一階馬爾科夫過程來擬合誤差方程；

3)根據步驟1和步驟2分別得到的SINS非線性誤差模型和DVL非線性誤差模型建立組合導航濾波模型：選取SINS的三維姿態角誤差、速度誤差的前兩維、位置誤差的前兩維，三維陀螺常值漂移和加速度計的水平方向的兩維常值誤差的12維狀態變量，選取DVL的刻度系數誤差和隨機常值測量誤差作為狀態量；然后根據步驟1和步驟2分別得到的SINS非線性誤差模型和DVL非線性誤差模型共同建立組合導航的狀態方程；以DVL和SINS的東向、北向速度分量的差值作為觀測量,建立SINS/DVL組合導航系統的觀測方程；

4)建立5階SSRCKF濾波模型，輸出導航系統參數：利用5階SSRCKF對SINS和DVL輸出的信息進行濾波、融合，得到導航參數的最優估計并修正速度、位置和姿態矩陣，得到精確的導航信息。

進一步的，所述步驟1)中建立SINS非線性誤差模型的具體步驟包括：

選取東-北-天坐標系為SINS的導航坐標系并記為n系，載體系選用右-前-上記為b系，SINS解算的導航坐標系記為n′系；系統真實姿態角為真實速度Vⁿ＝[V_E,V_N,V_U]^T，真實地理坐標P＝[L,λ,H]^T；記陀螺儀和加速度計的隨機常值誤差分別為ε_b和陀螺儀和加速度計對應的擾動白噪聲分別為w_g和w_f，則陀螺儀誤差ε^b＝ε_b+w_g，加速度計誤差陀螺儀和加速度計的測量值為和和分別為地球自轉角速度和導航坐標系相對地球坐標系的旋轉角速度；V_N和V_E分別表示載體的運動速度的北向分量和東向分量；R_N和R_E分別為卯酉圈半徑和子午圈半徑；

建立SINS的速度、姿態和位置誤差方程，具體如下：

速度方程為：

姿態誤差方程為：