基于UnscentedFastSLAM算法的AUV自主导航方法

摘要

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