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算法有更好的估计性能。
-
关键词:
- 同步定位与建图 /
- 容积卡尔曼滤波 /
- 变分贝叶斯 /
- 一步预测误差协方差矩阵 /
- 观测噪声协方差矩阵
Abstract: 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$ {{\boldsymbol{P}}_{k|k - 1}} $ and the observed noise covariance matrix${{\boldsymbol{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${{\boldsymbol{X}}_k}$ ,$ {{\boldsymbol{P}}_{k|k - 1}} $ and${{\boldsymbol{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. -
算法1 DSACKF SLAM算法 滤波输入：${ {\boldsymbol{X} }_{k - 1|k - 1} },{ {\boldsymbol{P} }_{k - 1|k - 1} },{{\boldsymbol{U}}_{k - 1|k - 1} },{{\boldsymbol{Q}}_k}, { {\boldsymbol{Z} }_k},f( \cdot ),h( \cdot ),\tau ,\rho ,{u_{k - 1|k - 1} }$ (1)预测：依据式(15)—式(17)求取${\boldsymbol{X} }_{k|k - 1}$和$ {{\boldsymbol{P}}_{k|k - 1}} $ (2)参数赋值：${\boldsymbol{T} }_{k|k - 1}^1 = \tau { {\boldsymbol{P} }_{k|k - 1} },\; {\boldsymbol{T} }_{k|k - 1}^1 = m + \tau + 1,\;{\boldsymbol{B} } = \sqrt \rho { {\boldsymbol{E} }_n},\;{\boldsymbol{U}}_{k|k - 1}^1 = {\boldsymbol{B} }{{\boldsymbol{U}}_{k - 1|k - 1} }{ {\boldsymbol{B} }^{\rm{T} } },$
$ u_{k|k - 1}^1 = \rho ({u_{k - 1|k - 1} } - n - 1) + n + 1 $for $ j = 1:N $ (3)更新$ {{\boldsymbol{R}}_k} $和$ {{\boldsymbol{P}}_{k|k - 1}} $ ${u_{k|k} }{\text{ = } }u_{k|k - 1}^1 + 1,\; { {\boldsymbol{T} }_{k|k} } = {\boldsymbol{T} }_{k|k - 1}^1 + 1,\;{\boldsymbol{R}}_k^j = U_{k|k - 1}^j{({u_{k|k} } - n - 1)^{ - 1} },\;{\boldsymbol{P}}_{k|k - 1}^j = {\boldsymbol{T} }_{k|k - 1}^j({ { {\boldsymbol{T} }_{k|k} } - m - 1)^{ - 1} }$ (4)重新计算容积点$ \chi _{k|k - 1,i}^j $并更新观测估计量${\boldsymbol{Z} }_{k|k - 1}^j$
$\begin{aligned}& \chi _{k|k - 1,i}^j = { {\boldsymbol{X} }_{k|k - 1} } + { {\boldsymbol{S} }_{k|k - 1,j} } {\boldsymbol{E} }_m^i\sqrt m , i = 1,2, \cdots, m;\quad \chi _{k|k - 1,i}^j = { {\boldsymbol{X} }_{k|k - 1} } - { {\boldsymbol{S} }_{k|k - 1,j} } {\boldsymbol{E} }_m^i\sqrt m ,i = m + 1,m + 2, \cdots, 2m \\& { {\boldsymbol{S} }_{k|k - 1,j} } {\boldsymbol{S} }_{k|k - 1,j}^{\rm{T} } = {\boldsymbol{P} }_{k|k - 1}^j \\& {\boldsymbol{Z} }_{k|k - 1,i}^j = h(\chi _{k|k - 1,i}^j),i = 1,2, \cdots, 2m;\quad {\boldsymbol{Z} }_{k|k - 1}^j = \frac{1}{ {2m} }\sum\limits_{i = 1}^{2m} { {\boldsymbol{Z} }_{k|k - 1,i}^j} \end{aligned}$(5)更新${ {\boldsymbol{X} }_{k|k} }$和$ {{\boldsymbol{P}}_{k|k}} $
$\begin{array}{l}{ {\boldsymbol{K} } }_{k}^{j}=\left[\dfrac{1}{2m}{\displaystyle \sum _{i=1}^{2m}({\chi }_{k|k-1,i}^{j}-{ {\boldsymbol{X} } }_{k|k-1}){({ {\boldsymbol{z} } }_{k|k-1,i}^{j}-{ {\boldsymbol{Z} } }_{k|k-1}^{j})}^{ {\rm{T} } } }\right]{\left[{{\boldsymbol{R}}}_{k}^{j}+\dfrac{1}{2m}{\displaystyle \sum _{i=1}^{2m}({ {\boldsymbol{z} } }_{k|k-1,i}^{j}-{ {\boldsymbol{Z} } }_{k|k-1}^{j}){({ {\boldsymbol{z} } }_{k|k-1,i}^{j}-{ {\boldsymbol{Z} } }_{k|k-1}^{j})}^{ {\rm{T} } } }\right]}^{-1}\\ { {\boldsymbol{X} } }_{k|k}^{j}={ {\boldsymbol{X} } }_{k|k-1}+{ {\boldsymbol{K} } }_{k}^{j}({ {\boldsymbol{Z} } }_{k}-{ {\boldsymbol{Z} } }_{k|k-1}^{j})，{ {\boldsymbol{P} } }_{k|k}^{j}={ {\boldsymbol{P} } }_{k|k-1}^{j}-{ {\boldsymbol{K} } }_{k}^{j}{ {\boldsymbol{P} } }_{zz,k|k-1}^{j}({ { {\boldsymbol{K} } }_{k}^{j} })^{\rm{T} }\end{array}$(6)更新参数${\boldsymbol{T} }_{k|k - 1}^{j + 1}$和${\boldsymbol{U}}_{k|k - 1}^{j + 1}$
$\begin{gathered} \chi _{k|k,i}^j = {\boldsymbol{X} }_{k|k}^j + { {\boldsymbol{S} }_{k|k,j} } {\boldsymbol{E} }_m^i\sqrt m ,i = 1,2, \cdots, m;\quad \chi _{k|k,i}^j = {\boldsymbol{X} }_{k|k}^j - { {\boldsymbol{S} }_{k|k,j} } {\boldsymbol{E} }_m^i\sqrt m ,i = m + 1,m + 2, \cdots, 2m \\ { {\boldsymbol{S} }_{k|k,j} } {\boldsymbol{S} }_{k|k,j}^{\rm{T} } = {\boldsymbol{P} }_{k|k}^j \\ {\boldsymbol{Z} }_{k|k,i}^j = h(\chi _{k|k,i}^j),i = 1,2, \cdots, 2m \\ {\boldsymbol{T} }_{k|k - 1}^{j + 1} = {\boldsymbol{T} }_{k|k - 1}^1 + \frac{1}{ {2m} }\sum\limits_{i = 1}^{2m} (\chi _{k|k,i}^j - { {\boldsymbol{X} }_{k|k - 1} }){ {(\chi _{k|k,i}^j - { { {\boldsymbol{X} }_{k|k - 1} })}^{\rm{T}}} } ,\quad {\boldsymbol{U} }_{k|k - 1}^{j + 1} = {\boldsymbol{U} }_{k|k - 1}^1 + \frac{1}{ {2m} }\sum\limits_{i = 1}^{2m} ( { {\boldsymbol{Z} }_k} - {\boldsymbol{Z} }_{k|k,i}^j) {({ { {\boldsymbol{Z} }_k} - { {\boldsymbol{Z} }_{k|k,i}^j)}^{\rm{T} } } } \\ \end{gathered}$end for ${ {\boldsymbol{X} }_{k|k} } = {\boldsymbol{X} }_{k|k}^N,{ {\boldsymbol{P} }_{k|k} } = {\boldsymbol{P} }_{k|k}^N,{{\boldsymbol{U}}_{k|k} } = {\boldsymbol{U}}_{k|k - 1}^{N + 1},{u_{k|k} } = u_{k|k - 1}^1 + 1$ 滤波输出：${ {\boldsymbol{X} }_{k|k} },{ {\boldsymbol{P} }_{k|k} },{{\boldsymbol{U}}_{k|k} },{u_{k|k} }$ 
下载: 导出CSV
表 1 不同算法实验误差数据统计比较(噪声时不变)(m)
算法 位置误差 X方向误差 Y方向误差 DSACKF SLAM 1.9453 0.9418 1.0035 VB-ACKF SLAM 2.5629 1.3977 1.1651 UKF SLAM 3.3561 1.7246 1.6316
下载: 导出CSV
表 2 不同算法实验数据统计比较(噪声时变)(m)
SLAM算法 位置误差 X方向误差 Y方向误差 DSACKF SLAM 3.6133 1.8797 1.7336 VB-ACKF SLAM 5.1528 2.9840 2.1688 UKF SLAM 7.0894 3.9145 3.1749
下载: 导出CSV
表 3 不同参数
$ \rho $ 实验数据统计比较(噪声时变)(m)误差 $\rho $=0.80 $\rho $=0.85 $\rho $=0.90 $\rho $=0.95 $\rho $=1.00 位置误差 4.1226 3.9263 3.6133 3.7927 3.8025 X方向误差 1.9917 1.8522 1.6031 1.6533 1.6021 Y方向误差 2.1309 2.0741 2.0102 2.1394 2.2004
下载: 导出CSV
表 4 不同参数
$ \tau $ 实验数据统计比较(噪声时变)(m)误差 $ \tau = 1 $ $ \tau = 2 $ $ \tau = 3 $ $ \tau = 4 $ $ \tau = 5 $ 位置误差 3.6133 3.7169 3.7928 3.8816 3.9325 X方向误差 1.6031 1.6722 1.7141 1.7563 1.8121 Y方向误差 2.0102 2.0447 2.0787 2.1253 2.1204
下载: 导出CSV
-
[1] DURRANT-WHYTE H and BAILEY T. Simultaneous localization and mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99–110. doi: 10.1109/MRA.2006.1638022 [2] CHEN Chaoyang, HE Qi, YE Qiubo, et al. Simultaneous localization and mapping method based on improved cubature Kalman filter[J]. Sensors and Materials, 2021, 33(8): 2591–2606. doi: 10.18494/SAM.2021.3387 [3] ECKENHOFF K, GENEVA P, and HUANG Guoquan. MIMC-VINS: A versatile and resilient multi-IMU multi-camera visual-inertial navigation system[J]. IEEE Transactions on Robotics, 2021, 37(5): 1360–1380. doi: 10.1109/TRO.2021.3049445 [4] 陈孟元, 徐明辉. 基于自组织可增长映射的移动机器人仿生定位算法研究[J]. 电子与信息学报, 2021, 43(4): 1003–1013. doi: 10.11999/JEIT200025CHEN Mengyuan and XU Minghui. Research on mobile robot bionic location algorithm based on growing self-organizing map[J]. Journal of Electronics &Information Technology, 2021, 43(4): 1003–1013. doi: 10.11999/JEIT200025 [5] GONG Zheng, LI J, LUO Zhipeng, et al. Mapping and semantic modeling of underground parking lots using a backpack LiDAR system[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 22(2): 734–746. doi: 10.1109/TITS.2019.2955734 [6] 胡钊政, 刘佳蕙, 黄刚, 等. 融合WiFi、激光雷达与地图的机器人室内定位[J]. 电子与信息学报, 2021, 43(8): 2308–2316. doi: 10.11999/JEIT200671HU Zhaozheng, LIU Jiahui, HUANG Gang, et al. Integration of WiFi, laser, and map for robot indoor localization[J]. Journal of Electronics &Information Technology, 2021, 43(8): 2308–2316. doi: 10.11999/JEIT200671 [7] KIA S S, ROUNDS S, and MARTINEZ S. Cooperative localization for mobile agents: A recursive decentralized algorithm based on Kalman-filter decoupling[J]. IEEE Control Systems Magazine, 2016, 36(2): 86–101. doi: 10.1109/MCS.2015.2512033 [8] BLOESCH M, BURRI M, OMARI S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. The International Journal of Robotics Research, 2017, 36(10): 1053–1072. doi: 10.1177/0278364917728574 [9] LUO Jingwen and QIN Shiyin. A fast algorithm of SLAM based on combinatorial interval filters[J]. IEEE Access, 2018, 6: 28174–28192. doi: 10.1109/ACCESS.2018.2838112 [10] QIN Tong, LI Peiliang, and SHEN Shaojie. VINS-Mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004–1020. doi: 10.1109/TRO.2018.2853729 [11] CAO Yanjun and BELTRAME G. VIR-SLAM: Visual, inertial, and ranging SLAM for single and multi-robot systems[J]. Autonomous Robots, 2021, 45(6): 905–917. doi: 10.1007/s10514-021-09992-7 [12] CAMPOS C, ELVIRA R, RODRÍGUEZ J J G, et al. ORB-SLAM3: An accurate open-source library for visual, visual–inertial, and multimap SLAM[J]. IEEE Transactions on Robotics, 2021, 37(6): 1874–1890. doi: 10.1109/TRO.2021.3075644 [13] 董星亮, 苑晶, 张雪波, 等. 室内环境下基于图像序列拓扑关系的移动机器人全局定位[J]. 机器人, 2019, 41(1): 83–89,103. doi: 10.13973/j.cnki.robot.180100DONG Xingliang, YUAN Jing, ZHANG Xuebo, et al. Mobile robot global localization based on topological relationship between image sequences in indoor environments[J]. Robot, 2019, 41(1): 83–89,103. doi: 10.13973/j.cnki.robot.180100 [14] WILLIAMS B, CUMMINS M, NEIRA J, et al. A comparison of loop closing techniques in monocular SLAM[J]. Robotics and Autonomous Systems, 2009, 57(12): 1188–1197. doi: 10.1016/j.robot.2009.06.010 [15] CHOI K S and LEE S G. An enhanced CSLAM for multi-robot based on unscented Kalman filter[J]. International Journal of Control, Automation and Systems, 2012, 10(1): 102–108. doi: 10.1007/s12555-012-0111-4 [16] CHANDRA K P B, GU DAWEI, and POSTLETHWAITE I. Cubature Kalman filter based localization and mapping[J]. IFAC Proceedings Volumes, 2011, 44(1): 2121–2125. doi: 10.3182/20110828-6-IT-1002.03104 [17] JWO D J, YANG Chifan, CHUANG C H, et al. Performance enhancement for ultra-tight GPS/INS integration using a fuzzy adaptive strong tracking unscented Kalman filter[J]. Nonlinear Dynamics, 2013, 73(1/2): 377–395. doi: 10.1007/s11071-013-0793-z [18] 张文玲, 朱明清, 陈宗海. 基于强跟踪UKF的自适应SLAM算法[J]. 机器人, 2010, 32(2): 190–195. doi: 10.13973/j.cnki.robot.2010.02.013ZHANG Wenling, ZHU Mingqing, and CHEN Zonghai. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2): 190–195. doi: 10.13973/j.cnki.robot.2010.02.013 [19] 张抒扬, 董鹏, 敬忠良. 变分贝叶斯自适应容积卡尔曼的SLAM算法[J]. 哈尔滨工业大学学报, 2019, 51(4): 12–18. doi: 10.11918/j.issn.0367-6234.201801013ZHANG Shuyang, DONG Peng, and JING Zhongliang. Adaptive cubature Kalman filtering SLAM algorithm based on variational Bayes[J]. Journal of Harbin Institute of Technology, 2019, 51(4): 12–18. doi: 10.11918/j.issn.0367-6234.201801013 -
图(5) / 表(5)
计量
- 文章访问数: 569
- HTML全文浏览量: 341
- PDF下载量: 128
- 被引次数: 0
下载:
