[发明专利]基于FMSRUPF算法的AUV对接回收自主导航方法

权利要求书

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的协方差矩阵。