[发明专利]基于UnscentedFastSLAM算法的AUV自主导航方法

权利要求书

1.一种基于Unscented FastSLAM算法的AUV自主导航方法，其特征在于，该方法包含下列步骤：

1)初始化：AUV在水面通过传感器获得初始位姿信息；

2)预测：根据输入AUV的最新控制量和各种传感器观测量采用无味粒子滤波UPF对AUV位姿和环境路标进行预测；

3)采样：采用渐消自适应无味卡尔曼滤波AF-UKF来产生一种参数自适应调节的建议分布函数，并从建议分布函数中采样；

4)地图更新：根据每个粒子关联最新观测环境信息，采用无味卡尔曼滤波UKF更新每个特征的估计；

5)重采样：计算粒子集的有效粒子数，若有效粒子数小于设定的阈值，则采用自适应部分系统重采样方法对粒子集进行重采样；

6)根据重采样生成的新粒子按照以上步骤迭代循环继续进行AUV定位和地图构建。

2.如权利要求1所述的基于Unscented FastSLAM算法的AUV自主导航方法，其特征在于，所述步骤1)初始化过程包括通过全球定位系统GPS获得初始位置信息；通过多普勒计程仪DVL、加速度计、陀螺仪获得初始速度和姿态信息；其中，全球定位系统GPS获取的AUV的初始位置信息为经度信息和纬度信息；多普勒计程仪DVL采集到的AUV速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种；陀螺仪采集到的AUV姿态角信息包括航偏角信息、横滚角信息、俯仰角信息中的任一种或任几种。

3.如权利要求1所述的基于Unscented FastSLAM算法的AUV自主导航方法，其特征在于，步骤2)根据输入AUV的最新控制量和各种传感器观测量，及AUV运动预测模型和传感器测量模型对每个粒子中的AUV位姿和对应路标采用无味粒子滤波UPF进行预测，所述AUV运动预测方程和传感器测量方程分别为：

x_t＝f(x_t-1,w_t-1)(1)

z_t＝h(x_t,v_t)(2)

式中，x_t、z_t分别为t时刻系统的状态向量和观测向量，f(·)、h(·)分别为状态和观测的非线性函数，w_t、v_t为具有零均值和协方差分别为Q_t和R_t的过程噪声和观测噪声。