基于变分贝叶斯双尺度自适应时变噪声容积卡尔曼滤波的同步定位与建图算法

Simultaneous Localization And Mapping Based on Variational Bayses Double-Scale Adaptive time-varying noise Cubature Kalman Filter

为解决移动机器人在同步定位与建图(SLAM)中因系统噪声和观测噪声时变导致状态估计精度降低的问题,该文提出一种基于变分贝叶斯的双尺度自适应时变噪声容积卡尔曼滤波SLAM算法(DSACKF SLAM)。该算法采用逆Wishart分布对一步预测误差协方差矩阵P_(k|k–1)和观测噪声协方差矩阵R_(k)建模,分别用来降低系统噪声和观测噪声的影响,并利用变分贝叶斯滤波实现对移动机器人状态向量X_(k),P_(k|k–1)和R_(k)的联合估计。分别在系统噪声和观测噪声时变和时不变的条件下进行仿真实验,结果表明与基于无迹卡尔曼滤波的SLAM算法(UKF SLAM)、自适应更新观测噪声的容积卡尔曼滤波的SLAM算法(VB-ACKF SLAM)相比,所提DSACKF SLAM算法在噪声时变时,平均位置误差分别减小1.54 m,3.47 m;噪声时不变时,平均位置误差分别减小0.62 m,1.41 m,证明DSACKF SLAM算法有更好的估计性能。

In order to solve the problem that the state estimation accuracy of mobile robot in Simultaneous Localization And Mapping(SLAM)is reduced due to the time-varying system noise and observed noise,a SLAM algorithm is proposed based on variational Bayes Double-Scale Adaptive time-varying noise Cubature Kalman Filter(DSACKF SLAM).The inverse Wishart distribution is used to model the one-step predicted error covariance matrix P_(k|k-1)and the observed noise covariance matrix R_(K)to reduce the influence of system noise and observed noise respectively,and the variational Bayes filter is used to estimate the mobile robot state matrix X_(k),P_(k|k-1)and R_(k).Simulation experiments are carried out under the time-varying and time-invariant conditions of system noise and observed noise respectively.The results show that,compared with the SLAM algorithm based on Unscented Kalman Filter(UKF SLAM)and the SLAM algorithm based on Variational Bayes Adaptive observed noise Cubature Kalman Filter(VB-ACKF SLAM),when the noise is time-varying,the average position error decreases by 1.54 m and 3.47 m respectively.When the noise is time-invariant,the average position error decreases by 0.62 m and 1.41 m respectively.The proposed DSACKF SLAM algorithm has better estimation performance.