[發明專利]水下機器人移動式對接目標運動狀態估計方法及系統

在審

• 申請號: 202111192316.1
• 申請日: 2021-10-13
• 公開（公告）號: CN113984045A
• 公開（公告）日: 2022-01-28
• 發明（設計）人: 李柯垚
• 申請（專利權）人: 湖南航天機電設備與特種材料研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S5/18;G06F17/16;H03H17/02
• 代理公司: 長沙朕揚知識產權代理事務所(普通合伙) 43213
• 代理人: 鄧宇

1.一種水下機器人移動式對接目標運動狀態估計方法，應用于自帶慣性導航系統和超短基線定位系統的歸航AUV上，其特征在于，包括以下步驟：

利用擴展卡爾曼濾波器對所述慣性導航系統測量到的歸航AUV的運動狀態進行濾波，得到歸航AUV的運動狀態變化量；

以歸航AUV的運動狀態變化量為控制向量，利用離散卡爾曼濾波器對超短基線定位系統測量到的運動母艇與歸航AUV的相對運動狀態進行濾波，得到修正后的運動母艇與歸航AUV的相對運動狀態；

根據所述慣性導航系統測量到的歸航AUV的運動狀態與所述修正后的運動母艇與歸航AUV的相對運動狀態計算運動母艇的實際運動狀態。

2.根據權利要求1所述的水下機器人移動式對接目標運動狀態估計方法，其特征在于，所述相對運動狀態包括運動母艇相對于歸航AUV的位置以及運動母艇相對于歸航AUV的速度，歸航AUV的運動變化量包括速度變化量以及位置變化量；所述離散卡爾曼濾波器的第一狀態轉移方程為：

其中，m表示超短基線系統對相對運動系統的測量時刻序號，所述相對運動系統由運動母艇與歸航AUV構成，ΔL表示運動母艇相對于歸航AUV的位置，ΔL_m表示第m時刻運動母艇相對于歸航AUV的位置，ΔL_m-1表示第m-1時刻運動母艇相對于歸航AUV的位置，ΔP_auv和ΔV_auv分別代表歸航AUV自身位置和速度變化量，ΔP_auv|m,m-1代表第m-1時刻至第m時刻內歸航AUV的位置變化量，ΔV_auv|m,m-1代表第m-1時刻至第m時刻內歸航AUV的速度變化量，V_relative表示運動母艇相對于歸航AUV的相對速度，V_relative,m-1表示第m-1時刻運動母艇相對于歸航AUV的速度，V_auv，m-1表示第m-1時刻歸航AUV的速度，ΔT表示超短基線系統的測量周期的時長。

3.根據權利要求2所述的水下機器人移動式對接目標運動狀態估計方法，其特征在于，利用離散卡爾曼濾波器對超短基線定位系統測量到的運動母艇與歸航AUV的相對運動狀態進行濾波，包括以下步驟：

初始化所述第一狀態轉移方程的狀態變量、控制向量、狀態轉移矩陣、控制矩陣以及協方差矩陣；

根據m-1時刻預測的狀態變量、m-1時刻的控制向量、m-1至m的狀態轉移矩陣以及控制矩陣預測第一狀態轉移方程m時刻的一步狀態變量；

基于m-1時刻預測的狀態轉移矩陣、m-1時刻的控制矩陣以及m-1時刻的協方差矩陣計算m-1至m時刻的一步協方差矩陣；

獲取超短基線定位系統測量到的運動母艇與歸航AUV在m時刻的相對運動狀態以及對應的測量噪音矩陣，根據預測的m時刻的一步狀態變量、m-1至m時刻的一步協方差矩陣、超短基線定位系統測量到的運動母艇與歸航AUV在m時刻的相對運動狀態以及對應的測量噪音矩陣計算m時刻的增益；基于m時刻的增益更新m時刻的協方差矩陣，并修正m時刻的一步狀態變量，得到修正后的運動母艇在m時刻與歸航AUV的相對運動狀態。