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

李帅永; 谢现乐; 毛文平; 杨雪梅; 聂嘉炜

doi:10.11999/JEIT220031

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

doi: 10.11999/JEIT220031

重庆邮电大学工业物联网与网络化控制教育部重点实验室重庆 400065

基金项目: 国家自然科学基金(61703066)，重庆市基础研究与前沿探索项目(cstc2018jcyjAX0536)，重庆市技术创新与应用发展专项(cstc2018jszx-cyztzxX0028, cstc2019jscx-fxydX0042, cstc2019jscx-zdztzxX0053)

详细信息

作者简介:
李帅永：男，教授，博士生导师，研究方向为自动驾驶与环境感知技术、SLAM与自主导航、压缩感知与超分辨重构、工业无损检测理论等

谢现乐：男，硕士生，研究方向为移动机器人SLAM后端优化

毛文平：男，硕士生，研究方向为移动机器人路径规划

杨雪梅：女，硕士生，研究方向为移动机器人视觉SLAM回环检测算法

聂嘉炜：男，硕士生，研究方向为动态环境下SLAM视觉里程计

通讯作者:
李帅永　lishuaiyong@cqupt.edu.cn

中图分类号: TN713; TP242
计量
- 文章访问数: 730
- HTML全文浏览量: 441
- PDF下载量: 136
- 被引次数: 0
出版历程
- 收稿日期: 2022-01-10
- 修回日期: 2022-06-22
- 网络出版日期: 2022-06-29
- 刊出日期: 2023-03-10

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

Key Laboratory of Industrial Internet of Things & Networked Control, Ministry of Education, Chongqing University of Posts and Telecommunications, Chongqing 400065, China

Funds: The National Natural Science Foundation of China (61703066), Chongqing Basic Research and Frontier Exploration Project (cstc2018jcyjAX0536), Chongqing Technology Innovation and Application Development Special Project (cstc2018jszx-cyztzxX0028, cstc2019jscx-fxydX0042, cstc2019jscx-zdztzxX0053)

摘要

摘要: 为解决移动机器人在同步定位与建图(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.
- Simultaneous Localization And Mapping (SLAM) /
- Cubature Kalman Filter (CKF) /
- Variational Bayes /
- One-step prediction error covariance matrix /
- Observed noise covariance matrix

HTML全文

图 1 仿真实验环境

下载: 全尺寸图片幻灯片

图 2 不同算法仿真实验结果对比(噪声时不变)

下载: 全尺寸图片幻灯片

图 3 不同算法位置均方根误差(噪声时不变)

下载: 全尺寸图片幻灯片

图 4 不同算法仿真实验结果对比(噪声时变)

下载: 全尺寸图片幻灯片

图 5 不同算法位置均方根误差(噪声时变)

下载: 全尺寸图片幻灯片

算法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

参考文献(19)

[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/JEIT200025 CHEN 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/JEIT200671 HU 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.180100 DONG 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.013 ZHANG 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.201801013 ZHANG 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