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

在審

• 申請號: 202010847109.4
• 申請日: 2020-08-21
• 公開（公告）號: CN111964684A
• 公開（公告）日: 2020-11-20
• 發明（設計）人: 周玲;孫慧霞;竇永梅;朱倚嫻;胡杰
• 申請（專利權）人: 運城學院
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01S5/22
• 代理公司: 江蘇瑞途律師事務所 32346
• 代理人: 金龍;劉琦

1.一種基于SINS/LBL緊組合的水下導航混合定位方法及系統，其特征在于，該方法步驟如下：

步驟1，初始化水下導航定位系統：獲取潛器的初始位置參數，獲取潛器的角速率和比力信息，進行捷聯慣導系統初始對準，獲取潛器的初始姿態矩陣和初始速度信息；

步驟2，判斷聲學基陣工作區水聽器的可用個數，當水聽器的可用個數大于等于兩個時，迭代循環步驟3至步驟5，否則停止迭代，繼續判斷水聽器的可用個數；

步驟3，當水聽器的可用個數等于兩個時，利用聲學基陣和捷聯慣導系統解算的潛器位置信息，計算出潛器與聲學基陣之間的斜距、斜距率、斜距差和斜距率差；

當水聽器的可用個數大于兩個時，利用聲學基陣和捷聯慣導系統解算的潛器位置信息，計算出潛器與聲學基陣之間的斜距差和斜距率差；

步驟4，當水聽器的可用個數等于兩個時，長基線水聲定位系統計算出潛器與聲學基陣之間的斜距、斜距率、斜距差和斜距率差；

當水聽器的可用個數大于兩個時，長基線水聲定位系統計算出潛器與聲學基陣之間的斜距差和斜距率差；

步驟5，將所述步驟3中捷聯慣導系統獲取的斜距和斜距率、斜距差和斜距率差以及捷聯慣導系統解算的潛器深度值分別與所述步驟4中長基線水聲定位系統獲取的斜距和斜距率、斜距差和斜距率差、壓力傳感器獲取的潛器深度值作差后進行卡爾曼濾波，并將濾波后輸出的導航誤差估計值反饋修正捷聯慣導系統的導航參數，并輸出相應的潛器位置、姿態和速度。

2.根據權利要求1所述的一種基于SINS/LBL緊組合的水下導航混合定位方法，其特征在于，所述步驟2中判斷聲學基陣工作區水聽器的可用個數，具體判斷方法如下：

通過潛器上聲源獲取潛器相對于聲學基陣中各水聽器的斜距，與聲波傳播距離相比較，當斜距小于聲波傳播距離時，該水聽器為可用水聽器。

3.根據權利要求1所述的一種基于SINS/LBL緊組合的水下導航混合定位方法，其特征在于，所述步驟3中捷聯慣導系統獲取斜距和斜距率的具體過程如下：

將解算的潛器位置由球面坐標系轉換到直角坐標系為(x_I,y_I,z_I)，計算潛器與聲學基陣中各水聽器的斜距為：

式中，(x_a,y_a,z_a)為組成聲學基陣的水聽器位置坐標，為潛器相對于第j號水聽器的斜距，其中j為水聽器編號；

將斜距在潛器位置真值(x,y,z)處泰勒級數展開，取其一次項得：

式中，為第j號水聽器到潛器的真實距離，分別為水聽器在直角坐標三個軸向的方向余弦，δx＝x_I-x，δy＝y_I-y，δz＝z_I-z分別為直角坐標三個軸向的位置誤差；

對斜距求導，得到潛器相對于聲學基陣中各水聽器的斜距率為：

式中，為潛器相對于第j號水聽器的斜距率，變量上方·表示變量的導數；

捷聯慣導系統獲取斜距差和斜距率差的具體過程如下：

將解算的潛器位置由球面坐標系轉換到直角坐標系為(x_I,y_I,z_I)，計算潛器與聲學基陣中各水聽器的斜距差為：

式中，(x_a,y_a,z_a)為組成聲學基陣的水聽器位置坐標，為潛器相對于第j號水聽器的斜距與其相對于第k號水聽器的斜距之差，其中j、k為水聽器編號，且j≠k；

將斜距差在潛器位置真值(x,y,z)處泰勒級數展開，取其一次項得：

式中，為第j號水聽器到潛器的真實距離，為第k號水聽器到潛器的真實距離，分別為水聽器在直角坐標三個軸向的方向余弦之差，δx＝x_I-x，δy＝y_I-y，δz＝z_I-z分別為直角坐標三個軸向的位置誤差；

對斜距差求導，得到潛器相對于聲學基陣中各水聽器的斜距率差為：

式中，為潛器相對于第j號水聽器的斜距率與其相對于第k號水聽器的斜距率之差，變量上方·表示變量的導數。