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

在審

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

本發明公開了水下機器人移動式對接目標運動狀態估計方法及系統，利用擴展卡爾曼濾波器對慣性導航系統測量到的歸航AUV的運動狀態進行濾波，得到歸航AUV的運動狀態變化量；以歸航AUV的運動狀態變化量為控制向量，利用離散卡爾曼濾波器對超短基線定位系統測量到的運動母艇與歸航AUV的相對運動狀態進行濾波，得到修正后的所述相對運動狀態；根據所述測量到的歸航AUV的運動狀態與修正后的相對運動狀態計算運動母艇的實際運動狀態。本發明用離散卡爾曼濾波器對運動母艇與歸航AUV的相對運動狀態進行濾波時，將歸航AUV當前的狀態變化量作為控制向量傳遞給離散卡爾曼濾波器，能提高運動母艇與歸航AUV的相對運動狀態的精度，進而提高運動母艇實際運動狀態的計算精度。

技術領域

本發明涉及水下機器人移動式對接技術領域，尤其涉及水下機器人移動式對接目標運動狀態估計方法及系統。

背景技術

著我國深?？瓶际聵I的發展，越來越多的深海探測任務需要水下機器人(Autonomous UnderwaterVehicle，簡稱AUV)的參與，比如水下偵查、大范圍且長航程的工作網絡擴展和組建等工作。由于現有的水下通訊能力有限以及AUV自身能源的有限性，AUV在深海執行任務的過程中，在能源系統警告、作業數據容量飽和或者先期任務完成之后，需要與塢站進行對接動作，以此完成AUV與塢站之間的能量交換、數據上傳、任務下載等任務。水下移動式對接技術是完成AUV與塢站之間對接的關鍵技術，在種種深海探測任務中起著至關重要的作用，而AUV在與運動塢站的對接過程中，歸航AUV需要對運動母艇的運動狀態進行估計，并利用估計出的運動母艇的狀態信息(包括運動母艇的位置、速度、姿態)導引AUV完成歸航以及對接。

目前現有的水下機器人運動目標狀態估計技術是將AUV與母艇視為一個線性運動的相對運動系統，即認為歸航AUV與移動母艇之間的相對運動模型式一個勻加速運動模型。因此，目前主流的水下機器人運動目標狀態估計方法是利用超短基線定位系統測量周期內歸航AUV與運動母艇的相對位置，并通過離散卡爾曼濾波器對上述的線性運動系統進行濾波，估計歸航AUV與運動母艇的相對位置差。然而，上述方法由于對系統真實的運動狀態做了簡化處理，損失了一定的估計精度，無法應用在移動式對接任務中，因為，移動式對接任務不僅需要已知AUV與母艇的相對位置，同時需要已知母艇自身的絕對位置，通過其絕對位置估計運動母艇的姿態。

因此，如何解決設計出能應用在移動式對接任務中水下目標運動狀態估計方法，已成為本領域技術人員亟待解決的技術問題。

發明內容

本發明提供了水下機器人移動式對接目標運動狀態估計方法及系統，用于解決現有的水下目標運動狀態估計方法無法應用在移動式對接任務中的技術問題。

為解決上述技術問題，本發明提出的技術方案為：

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

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

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

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

優選的，所述相對運動狀態包括運動母艇相對于歸航AUV的位置以及運動母艇相對于歸航AUV的速度，歸航AUV的運動變化量包括速度變化量以及位置變化量；所述離散卡爾曼濾波器的第一狀態轉移方程為：