[發明專利]水下定位方法、水下載體航行器及計算機可讀存儲介質

有效

• 申請號: 202110316751.4
• 申請日: 2021-03-24
• 公開（公告）號: CN113075665B
• 公開（公告）日: 2023-06-20
• 發明（設計）人: 楊巍;侯西龍;王超;楊健;陳明;羅明成
• 申請（專利權）人: 鵬城實驗室
• 主分類號: G01S15/88
• 分類號: G01S15/88;G01S15/89;G01S15/86;G01C21/00;G01C21/18;G01C21/16;G01C21/20;G01C25/00
• 代理公司: 深圳市世紀恒程知識產權代理事務所 44287
• 代理人: 張志江

1.一種水下定位方法，其特征在于，所述水下定位方法應用于水下載體航行器，所述水下載體航行器包括慣性測量單元以及超短基線傳感器，所述水下定位方法包括：

確定所述慣性測量單元在第一預設坐標系下的第一位姿數據；

根據所述第一位姿數據以及所述慣性測量單元檢測到的慣性數據，確定目標線加速度值以及目標角速度值；

根據所述目標線加速度值以及所述目標角速度值，確定所述慣性測量單元在第二預設坐標系下的第二位姿數據；

根據所述第一位姿數據、所述第二位姿數據以及預設誤差狀態代價函數確定水下定位信息，所述預設誤差狀態代價函數的參數包括所述慣性測量單元對應的第一殘差參數以及超短基線傳感器對應的第二殘差參數，所述超短基線傳感器用于檢測位姿修正數據，所述位姿修正數據用于確定所述第二殘差參數，所述第一位姿數據包括所述慣性測量單元的線速度，所述第二位姿數據包括第二位置以及第二方向，所述根據所述第一位姿數據以及所述第二位姿數據；

所述根據所述第一位姿數據、所述第二位姿數據以及預設誤差狀態代價函數確定水下定位信息的步驟包括：

根據所述線速度、第一預設角速度以及第二預設角速度，確定第一變量，所述第一預設角速度為第一自由度的角速度，所述第二預設角速度為第二自由度的角速度，所述第一自由度以及所述第二自由度為參考重力的自由度分量，所述第一自由度與所述第二自由度不同，且所述第一自由度與所述第二自由度相互垂直，所述第一預設角速度以及所述第二預設角速度為所述慣性測量單元的角速度的分量；

根據所述第一自由度、所述第二自由度以及所述第二位置，確定矩陣系數；

根據所述第一變量以及所述矩陣系數，確定第二變量；

根據所述第一變量以及所述第二變量，確定所述慣性測量單元的第一殘差值，所述第一殘差值與所述第一殘差參數對應；

獲取超短基線傳感器檢測到的第二殘差值，所述第二殘差值與所述第二殘差參數對應；

根據所述第一殘差值以及所述第二殘差值，確定所述預設誤差狀態代價函數的最小值；

根據所述最小值確定所述水下定位信息。

2.如權利要求1所述的水下定位方法，其特征在于，所述慣性測量單元包括陀螺儀，所述慣性數據包括所述陀螺儀檢測的角速度數據以及加速度計檢測到的物體加速度，所述第一位姿數據包括第一方向，所述根據所述第一位姿數據以及所述慣性測量單元檢測到的慣性數據，確定目標線加速度值以及目標角速度值的步驟包括：

根據所述第一方向以及第一旋轉矩陣值，確定陀螺儀偏置值，所述第一旋轉矩陣值為所述陀螺儀在超短基線傳感器的相鄰幀之間的旋轉矩陣值；

根據所述角速度數據以及所述陀螺儀偏置值，確定所述慣性測量單元在第二預設坐標系下的目標角速度值；

根據所述物體加速度、加速度偏置值、目標重力以及第二旋轉矩陣，確定所述目標線加速度值。

3.如權利要求2所述的水下定位方法，其特征在于，所述根據所述第一方向以及第一旋轉矩陣值，確定陀螺儀偏置值的步驟包括：

根據所述第一旋轉矩陣值、所述第一旋轉矩陣值的預設修正系數以及所述第一方向，確定預設損失函數的最小值，將所述預設損失函數的最小值作為所述陀螺儀偏置值。

4.如權利要求2所述的水下定位方法，其特征在于，所述根據所述物體加速度、加速度偏置值、目標重力以及第二旋轉矩陣值，確定所述慣性測量單元在第二預設坐標系下的線加速度的步驟之前，還包括：

確定所述參考重力的所述第一自由度以及所述第二自由度；

確定所述第一自由度對應的第一角速度以及所述第二自由度對應的第二角速度；

根據所述第一自由度、所述第二自由度、所述第一角速度以及所述第二角速度，確定所述目標重力。

5.如權利要求2所述的水下定位方法，其特征在于，所述根據所述角速度數據以及所述陀螺儀偏置值，確定所述慣性測量單元在第二預設坐標系下的目標角速度值的步驟以及所述根據所述物體加速度、加速度偏置值、目標重力以及第二旋轉矩陣，確定所述目標線加速度值的步驟之間，還包括：

對所述目標角速度值進行積分，將積分后的所述目標角速度值作為所述第二旋轉矩陣。