基于中心差分卡尔曼滤波器的快速SLAM算法

针对密集路标环境下机器人同时定位与地图创建(SLAM)速度缓慢以及一致性不够收敛,提出了一种使用Stirling多项式插值方法对非线性过程进行近似的卡尔曼滤波器,同时利用路标测量统计信息对SLAM过程中的状态向量和状态协方差进行动态调整的SLAM方法.此方法对预测方程和测量方程使用Stirling多项式插值方法可以近似到二级甚至是更高级的泰勒展开.使用路标统计信息动态计算各个路标对于当前时刻状态向量的权重,利用此权重进行状态向量和状态向量协方差动态调整.实验表明无论是在稀疏路标环境下还是在密集路标环境下此方法在内存占用、SLAM速度以及对机器人定位的一致性上都优于FASTSLAM.

Fast SLAM algorithm based on central difference Kalman filter

A method based on Stirling’s polynomial interpolation to approximate the nonlinear process and using landmark’s statistical information to adjust the state vector and state vector’s covariance is proposed to resolve the simultaneous localization and mapping(SLAM) speed and consistency in dense landmark environment.By using Stirling’s polynomial interpolation,the nonlinearity can be approximated to the second or even higher Taylor series and the state vector and its covariance’s size can be limited to fixed size by the dynamic method based on the weight value of each landmark.It can be seen from experiment that not only in sparse landmark environment but also in dense landmark environment,the SLAM memory,speed and consistency of the proposed method are all superior to those of FASTSLAM.