[發明專利]一種GNSS輔助的DVL誤差標定方法

在審

• 申請號: 202011338896.6
• 申請日: 2020-11-25
• 公開（公告）號: CN112504298A
• 公開（公告）日: 2021-03-16
• 發明（設計）人: 徐曉蘇;仲靈通
• 申請（專利權）人: 東南大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 許小莉

1.一種GNSS輔助的DVL誤差標定方法，其特征在于，具體包括以下步驟：

S1：采集實驗數據，包括SINS/GNSS組合導航系統輸出的載體速度向量在導航坐標系n下的投影和載體姿態角，以及DVL輸出的載體速度向量在DVL設備坐標系d下的投影并利用載體姿態將轉換至載體坐標系b下，得

S2：選取安裝偏角誤差和刻度因子誤差ζ為狀態量，建立Kalman濾波狀態方程并離散化；

S3：利用k-1時刻反饋修正得到的安裝偏角轉換矩陣將k時刻的轉換至載體坐標系b下，得DVL輸出的載體速度向量在載體坐標系b下的投影

S4：將k時刻與的差作為Kalman濾波的觀測量，建立k時刻Kalman濾波量測方程；

S5：進行Kalman濾波；

S6：利用安裝偏角誤差估計反饋修正安裝偏角轉換矩陣并置零安裝偏角誤差估計；

S7：重復S3～S6，直至安裝偏角轉換矩陣和刻度因子誤差都收斂到一個準確的值。

2.根據權利要求1所述的GNSS輔助的DVL誤差標定方法，其特征在于，所述步驟S1具體包括以下過程：

S1.1采集實驗數據，包括SINS/GNSS組合導航系統輸出的載體速度向量在導航坐標系n下的投影和載體姿態角，以及DVL輸出的載體速度向量在DVL設備坐標系d下的投影

S1.2利用載體姿態將SINS/GNSS組合導航系統輸出的載體速度向量在導航坐標系n下的投影轉換至載體坐標系b下，得SINS/GNSS組合導航系統輸出的載體速度向量在載體坐標系b下的投影

其中，為k時刻導航坐標系n到載體坐標系b的坐標轉換矩陣，由k時刻SINS/GNSS組合導航系統輸出的載體姿態角轉換獲得；為k時刻SINS/GNSS組合導航系統輸出的載體速度向量在導航坐標系n下的投影；為k時刻SINS/GNSS組合導航系統輸出的載體速度向量在載體坐標系b下的投影。

3.根據權利要求1所述的GNSS輔助的DVL誤差標定方法，其特征在于，所述步驟S2具體包括以下過程：

S2.1選取常量安裝偏角誤差和刻度因子誤差ζ為狀態量，建立系統Kalman濾波狀態方程：

其中，X為狀態向量，為安裝偏角誤差向量，ζ為刻度因子誤差標量；F為系統矩陣，F＝0_4×4；W為系統噪聲向量；

S2.2將狀態方程離散化：

X(k)＝Φ(k,k-1)X(k-1)+G(k,k-1)W(k-1)

其中，X(k)為k時刻的狀態向量；Φ(k,k-1)為k-1時刻到k時刻的狀態一步轉移矩陣，Φ(k,k-1)＝I_4×4；W(k-1)為k-1時刻的系統噪聲向量，G(k,k-1)為k-1時刻到k時刻的系統噪聲輸入矩陣。

4.根據權利要求1所述的GNSS輔助的DVL誤差標定方法，其特征在于，所述步驟S3具體包括以下過程：

利用k-1時刻反饋修正得到的安裝偏角轉換矩陣將k時刻DVL輸出的載體速度向量在DVL設備坐標系d下的投影轉換至載體坐標系b下，得DVL輸出的載體速度向量在載體坐標系b下的投影

其中，為k時刻DVL輸出的載體速度向量在DVL設備坐標系d下的投影；為k-1時刻反饋修正得到的安裝偏角轉換矩陣，表示載體坐標系b與k-1時刻估計DVL設備坐標系的轉換關系；為k時刻DVL輸出的載體速度向量在載體坐標系b下的投影。