[發明專利]一種基于SINS/LBL緊組合的AUV水下導航定位方法

有效

• 申請號: 201410796735.X
• 申請日: 2014-12-19
• 公開（公告）號: CN104457754A
• 公開（公告）日: 2015-03-25
• 發明（設計）人: 張濤;石宏飛;徐曉蘇;陳立平
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/18
• 分類號: G01C21/18;G06F19/00
• 代理公司: 江蘇永衡昭輝律師事務所 32250
• 代理人: 王斌

1.一種基于SINS/LBL緊組合的AUV水下導航定位方法，其特征在于：所用導航定位系統由安裝在AUV上的捷聯慣性導航系統SINS(1)、布放在海底的長基線水聲定位系統LBL(2)和數據處理單元組成，其中，所述的捷聯慣性導航系統SINS(1)包括捷聯解算模塊，所述的長基線水聲定位系統LBL(2)由布放在海底的四個位置已知的水聽器基陣組成，所述的數據處理單元包括SINS兩兩基元與AUV斜距差推算模塊(3)、SINS/LBL緊組合模塊(4)和校正模塊(5)，采用SINS/LBL緊組合方法完成組合導航，所述方法通過下列步驟實現：

(1)捷聯慣性導航系統SINS(1)通過捷聯解算得到相應的包括AUV的位置信息的導航信息，解算的位置信息用地球大地坐標P_SINS(L_S,λ_S,h_S)表示，并將P_SINS(L_S,λ_S,h_S)轉化為用地球直角坐標P_SINS(x_S,y_S,z_S)表示；

(2)SINS兩兩基元與目標斜距差推算模塊(3)根據SINS提供的AUV位置信息P_SINS(x_S,y_S,z_S)和水聽器基陣位置P_i(x_i,y_i,z_i)推算SINS斜距差ρ_SINS；

(3)SINS/LBL緊組合模塊(4)根據長基線水聲定位系統LBL(2)的定位特點建立LBL斜距差模型，將SINS斜距差ρ_SINS和LBL水聽器i(i＝1,2,3)與AUV之間的斜距與水聽器0與AUV之間的斜距之差ρ_LBL的差值作為外部觀測信息輸入到卡爾曼濾波器進行濾波；

(4)校正模塊(5)根據SINS/LBL緊組合模塊(4)的卡爾曼濾波結果對SINS(1)進行校正，最終得到精確的AUV位置信息P_AUV。

2.根據權利要求1所述的一種基于SINS/LBL緊組合的AUV水下導航定位方法，其特征在于：SINS兩兩基元與目標AUV斜距差推算模塊(3)計算SINS斜距差的方法如下：

(1)根據SINS解算的AUV位置P_SINS(x_s,y_s,z_s)和長基線水聲定位系統LBL中水聽器基元位置P_i(x_i,y_i,z_i)計算得到水聽器i(i＝1,2,3)與AUV之間的斜距與水聽器0與AUV之間的斜距之差

(2)將ρ_SINSi利用泰勒級數線性化。設AUV真實位置為P_AUV(x,y,z),(δx,δy,δz)為SINS解算AUV位置的誤差，則x_S＝x+δx,y_S＝y+δy,z_S＝z+δz。將ρ_SINSi泰勒級數展開取前兩項得：

設

同理

其中，G_ij(i＝0,1,2,3；j＝x,y,z)為已知量，可由SINS解算的概略位置P_SINS(x_S,y_S,z_S)和水底應答器陣基元的位置P_i(x_i,y_i,z_i)計算得到，由于SINS解算的概略位置P_SINS(x_S,y_S,z_S)可能有較大誤差，這樣在進行方程線性化時略去高階項會引起線性誤差，可以利用迭代法解算，即在第一次解之后，用它作為近似值再重新計算；

設：e_ix＝G_ix-G_0x，e_iy＝G_iy-G_0y，e_iz＝G_iz-G_0z，i＝1,2,3

于是：

ρ_SINSi＝R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

＝R_i-R₀+e_ixδx+e_iyδy+e_izδz??????????，i＝1,2,3。