基于迭代扩展Kalman滤波建议分布和线性优化重采样的快速同步定位与构图

Fast Simultaneous Localization and Mapping Based on Iterative Extended Kalman Filter Proposal Distribution and Linear Optimization Resampling

针对标准快速同步定位与构图(FastSLAM)方法中由于样本退化及贫化导致自主水下航行器(Autonomous Underwater Vehicle,AUV)及路标位置估计精度严重下降的问题,该文提出一种基于迭代扩展Kalman滤波(Iterative Extended Kalman Filter,IEKF)建议分布和线性优化重采样的FastSLAM方法,通过IEKF融入最新观测值从而降低样本退化,为了降低样本的贫化,将重采样过程中复制的样本与部分被抛弃的样本通过线性组合产生新样本。建立AUV的运动学模型、特征模型及传感器的测量模型,通过Hough变换提取特征构建全局地图,采用改进的FastSLAM方法基于海试数据进行了AUV同步定位与构图试验,结果表明该文所设计的方法能够有效避免样本的退化及贫化,提高了AUV及路标的位置估计精度;此外,一致性分析结果表明所设计算法具有长期一致性。

The location estimated accuracy of Autonomous Underwater Vehicle （AUV） and landmarks decrease because of the degeneracy and impoverishment of samples in standard Fast Simultaneous Localization And Mapping （FastSLAM） algorithm. A improved FastSLAM algorithm based on Iterative Extended Kalman Filter （IEKF） proposal distribution and linear optimization resampling is presented in order to solve this issue. The latest observation is integrated with IEKF in order to decrease the sample degeneracy while the new samples are produced by the linear combination of copied samples and some abandoned ones in order to reduce the sample impoverishment. The kinematic model of AUV, feature model and the measurement models of sensors are all established. And then features are extracted with Hough transform to build the global map. The experiment of the improved FastSLAM algorithm with trial data shows that it can avoid the degeneracy and impoverishment of samples effectively and enhance the location estimation accuracy of AUV and landmarks. Moreover, the consistency analysis showed that the method possesses the consistency of long term.