[發明專利]一種基于地球坐標系和右群狀態誤差定義的SINS/USBL組合導航方法

在審

• 申請號: 202211060602.7
• 申請日: 2022-08-31
• 公開（公告）號: CN115307631A
• 公開（公告）日: 2022-11-08
• 發明（設計）人: 徐博;郭瑜;趙玉新;丁文婧;張偉;戴曉強;趙強
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S5/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于地球坐標系和右群狀態誤差定義的SINS/USBL組合導航方法，其特征在于，包括以下步驟：

步驟1：在地球坐標系下對SINS進行純慣性導航解算，得到導航參數和

其中，表示從載體坐標系到地球坐標系的姿態矩陣；表示載體相對于慣性坐標系在地球坐標系下的速度；表示載體在地球坐標系下的位置；

步驟2：獲取當前時刻USBL測量結果，并與上一時刻對比；如果當前時刻USBL測量結果更新，則執行步驟3；如果當前時刻USBL測量結果沒有更新，則執行步驟4；

步驟3：利用SINS/USBL組合導航系統模型估算導航誤差，得到后，執行步驟5；

SINS/USBL組合導航系統模型為：

其中，表示姿態誤差角估計值，表示速度誤差向量估計值，表示位置誤差向量估計值；表示常值漂移，表示常值零偏；“×”表示向量叉乘運算；和是隨機噪聲；表示USBL中應答器的位置；為矩陣的轉置矩陣，為從載體坐標系到USBL基陣坐標系的姿態變換矩陣；V表示USBL的測量噪聲；

步驟4：利用虛擬觀測方程估算導航誤差，得到后，執行步驟5；

虛擬觀測方程為：

X_k＝F_k,k-1X_k-1+G_k-1W_k-1

其中，公式中矩陣向量的下標表示采樣次數，當前時刻為t，t＝kT，T為采樣間隔，k為當前采樣次數；k-i為USBL測量結果上一次更新時對應的采樣次數；F_k,k-1＝I_15×15+A_k-1·T；矩陣向量

步驟5：利用導航誤差的估計值對導航參數進行校正，輸出精確導航參數

2.根據權利要求1所述的一種基于地球坐標系和右群狀態誤差定義的SINS/USBL組合導航方法，其特征在于：所述USBL由應答器和收發器組成，應答器設置在水下或水面母船的底部，它的位置是已知的；所述收發器由一個發射換能器和多個水聽器組成，發射換能器用于將聲信號發送到應答器，水聽器用于接收從應答器返回的聲信號。

3.根據權利要求1所述的一種基于地球坐標系和右群狀態誤差定義的SINS/USBL組合導航方法，其特征在于：所述步驟1具體為：

其中，分別為導航參數的一階導數；表示陀螺儀輸出的角速度；f^b表示加速度計輸出的加速度；ω_ie表示地球自轉角速度；表示當地重力矢量。