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

摘要

本发明公开了一种基于Unscented FastSLAM算法的AUV自主导航方法，包括以下步骤1)AUV在水面通过GPS和导航传感器获得初始位姿信息；2)根据输入AUV的最新控制量和传感器观测量采用无味粒子滤波对AUV位姿和环境路标进行预测；3)采用渐消自适应无味粒子滤波产生参数自适应调节的建议分布函数并从中采样；4)根据每个粒子关联最新观测环境信息，采用无味卡尔曼滤波更新每个特征的估计；5)采用自适应部分系统重采样方法对粒子集进行重采样；6)进行AUV定位和地图构建。本发明通过对Unscented FastSLAM算法的建议分布函数和重采样过程进行改进，能够提高Unscented FastSLAM算法的粒子采样效率，降低粒子的退化程度，使得AUV位姿估计的一致性和自主导航的精准性有很大提高。

主权项

一种基于Unscented FastSLAM算法的AUV自主导航方法，其特征在于，该方法包含下列步骤：1)初始化：AUV在水面通过传感器获得初始位姿信息；2)预测：根据输入AUV的最新控制量和各种传感器观测量采用无味粒子滤波UPF对AUV位姿和环境路标进行预测；3)采样：采用渐消自适应无味卡尔曼滤波AF‑UKF来产生一种参数自适应调节的建议分布函数，并从建议分布函数中采样；4)地图更新：根据每个粒子关联最新观测环境信息，采用无味卡尔曼滤波UKF更新每个特征的估计；5)重采样：计算粒子集的有效粒子数，若有效粒子数小于设定的阈值，则采用自适应部分系统重采样方法对粒子集进行重采样；6)根据重采样生成的新粒子按照以上步骤迭代循环继续进行AUV定位和地图构建。