[發明專利]基于FMSRUPF算法的AUV對接回收自主導航方法

在審

• 申請號: 201811354176.1
• 申請日: 2018-11-14
• 公開（公告）號: CN109375646A
• 公開（公告）日: 2019-02-22
• 發明（設計）人: 曾慶軍;包靈卉;朱志宇;戴曉強;趙強
• 申請（專利權）人: 江蘇科技大學
• 主分類號: G05D1/10
• 分類號: G05D1/10;G05D1/08;G01S19/49;G06F17/18
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 樓高潮

1.一種基于FMSRUPF算法的AUV對接回收自主導航方法,其特征在于，該方法包含下列步驟：

1)初始化：AUV在水面通過傳感器獲得初始位姿信息；

2)狀態更新：根據輸入AUV的最新控制量和各種傳感器觀測量，及AUV運動狀態模型方程和量測方程采用FMSRUPF算法對每個粒子進行時間更新和量測值更新；

3)權值更新：根據每個粒子關聯最新觀測信息，對每個粒子進行權值更新并歸一化權值；

4)重采樣：若粒子集的有效粒子數小于設定的閾值，則采用系統組合粒子重采樣方法對粒子集進行重采樣；

5)狀態估計更新：根據重采樣生成的新粒子得到AUV的狀態估計，并按照以上步驟迭代循環繼續進行AUV對接回收自主導航。

2.如權利要求1所述的基于FMSRUPF算法的AUV對接回收自主導航方法，其特征在于，步驟1)所述初始位姿信息主要從全球定位系統GPS、捷聯慣性導航系統SINS、多普勒計程儀DVL、超短基線定位系統USBL、測深儀這些傳感器獲得；其中，全球定位系統GPS采集到AUV的經度信息和緯度信息中的任一種或任幾種；捷聯慣性導航系統SINS采集到AUV的三維速度、位置信息和姿態角信息中的任一種或任幾種，位置信息包括緯度、經度和高度信息，姿態角信息包括航向角、俯仰角和橫滾角信息；超短基線定位系統USBL采集到回收裝置在AUV的基陣坐標系內相對于三軸x軸、y軸和z軸的角度信息和分別投影到三軸x軸、y軸和z軸上的距離信息中的任一種或任幾種；多普勒計程儀DVL采集到的AUV速度信息為前進速度信息、橫向速度信息、垂直速度信息中的任一種或任幾種；測深儀采集到AUV的深度信息。

3.如權利要求1所述的基于FMSRUPF算法的AUV對接回收自主導航方法，其特征在于，步驟2)所述AUV運動狀態模型方程和量測方程由下式描述：

x_k＝f(x_k-1,w_k-1) (1)

y_k＝h(x_k,v_k) (2)

式中，x_k、y_k分別為k時刻系統的狀態向量和觀測向量，x_k-1為k-1時刻系統的狀態向量，f(·)、h(·)分別為狀態和觀測的非線性函數，w_k-1、v_k為具有零均值和協方差分別為Q_k-1和R_k的過程噪聲和觀測噪聲，Q_k-1和R_k分別為噪聲w_k-1和v_k的協方差矩陣。