[發明專利]適用于極地冰架下環境的自主水下機器人的組合導航方法

在審

• 申請號: 202010198740.6
• 申請日: 2020-03-20
• 公開（公告）號: CN111928850A
• 公開（公告）日: 2020-11-13
• 發明（設計）人: 王軼群;李碩;曾俊寶;趙宏宇;王澤眾;徐會希
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 王倩

1.適用于極地冰架下環境的自主水下機器人的組合導航方法，其特征在于，包括以下步驟：

1)自主水下機器人按照規劃軌跡的約束條件對信標進行機動測距；

2)計算信標的初始位置和方差估計；

3)計算自主水下機器人在當前時刻的量測雅各比矩陣；

4)根據雅各比矩陣計算信標在當前時刻的位置估計和方差估計；

5)根據當前時刻信標的位置估計和方差估計，判斷是否滿足結束條件，若滿足，記錄當前時刻信標位置；否則，返回步驟3)；

6)水下機器人根據記錄的信標位置作為參考點進行導航。

2.根據權利要求1所述的適用于極地冰架下環境的自主水下機器人的組合導航方法，其特征在于，所述規劃軌跡的約束條件為：

其中，ρ_i,i+1表示點X_i到點X_i+1的位置矢量，ρ_i+1,i+2表示點X_i+1到點X_i+2的位置矢量，ρ_c是線性化判定常數，fabs為取絕對值。

3.根據權利要求1所述的適用于極地冰架下環境的自主水下機器人的組合導航方法，其特征在于，步驟2)包括：

求解信標的初始位置估計具體為：

其中第1次收到測距信號的時刻為t₁，自主水下機器人在時刻t₁的位置為X₁＝(x₁,y₁,z₁)^T,同理定義第2、3、4、5次數測距時刻的自主水下機器人位置為X₂、X₃、X₄、X₅，它們均由自主水下機器人搭載的多普勒計程儀、深水羅經、深度計直接測量獲得，R₁、R₂、R₃、R₄、R₅分別表示自主水下機器人在時刻t₁、t₂、t₃、t₄、t₅與信標的測距，它們由測距儀直接測量獲得，H₀和Y是中間變量，；

P₀表示信標的初始位置估計的方差估計，計算方法為：

其中，X_L表示自主水下機器人布放信標時自身的三維位置，E(.)表示數學期望的計算符號。

4.根據權利要求1所述的適用于極地冰架下環境的自主水下機器人的組合導航方法，其特征在于，所述量測雅各比矩陣為：

H_k＝[ρ_i-4,i-3 ρ_i-3,i-2 ρ_i-2,i-1 ρ_i-1,i]^T

其中，H_k為自主水下機器人在時刻t_k的量測雅各比矩陣，ρ_i-4,i-3表示點X_i-4到點X_i-3的移動矢量徑；ρ_i-3,i-2表示點X_i-3到點X_i-2的移動矢量徑；ρ_i-2,i-1表示點X_i-2到點X_i-1的移動矢量徑；ρ_i-1,i表示點X_i-1到點X_i的位移動矢量徑。