高级搜索

留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

多层ICP闭环检测下的误差状态卡尔曼滤波多模态融合SLAM

陈丹 陈浩 王子晨 张衡 王长青 范林涛

陈丹, 陈浩, 王子晨, 张衡, 王长青, 范林涛. 多层ICP闭环检测下的误差状态卡尔曼滤波多模态融合SLAM[J]. 电子与信息学报, 2025, 47(5): 1517-1528. doi: 10.11999/JEIT240980
引用本文: 陈丹, 陈浩, 王子晨, 张衡, 王长青, 范林涛. 多层ICP闭环检测下的误差状态卡尔曼滤波多模态融合SLAM[J]. 电子与信息学报, 2025, 47(5): 1517-1528. doi: 10.11999/JEIT240980
CHEN Dan, CHEN Hao, WANG Zichen, ZHANG Heng, WANG Changqing, FAN Lintao. Error State Kalman Filter Multimodal Fusion SLAM Based on MICP Closed-loop Detection[J]. Journal of Electronics & Information Technology, 2025, 47(5): 1517-1528. doi: 10.11999/JEIT240980
Citation: CHEN Dan, CHEN Hao, WANG Zichen, ZHANG Heng, WANG Changqing, FAN Lintao. Error State Kalman Filter Multimodal Fusion SLAM Based on MICP Closed-loop Detection[J]. Journal of Electronics & Information Technology, 2025, 47(5): 1517-1528. doi: 10.11999/JEIT240980

多层ICP闭环检测下的误差状态卡尔曼滤波多模态融合SLAM

doi: 10.11999/JEIT240980
基金项目: 西安市科技局秦创原重点产业链核心技术攻关项目(23ZDCYJSGG0021-2023)
详细信息
    作者简介:

    陈丹:女,教授,研究方向为智能信息处理、无线激光通信

    陈浩:男,硕士生,研究方向为机器人SLAM、目标跟踪与避障

    王子晨:男,硕士生,研究方向为SLAM、传感器算法以及智能导航

    张衡:男,硕士生,研究方向为多传感器融合SLAM

    王长青:男,硕士生,研究方向为语义分割SLAM

    范林涛:男,硕士生,研究方向为多传感器融合SLAM

    通讯作者:

    陈丹 chdh@xaut.edu.cn

  • 中图分类号: TN958.98; TP212.9; TP18

Error State Kalman Filter Multimodal Fusion SLAM Based on MICP Closed-loop Detection

Funds: Xi’an Science and Technology Bureau Qinchuangyuan Key Industrial Chain Technology Program (23ZDCYJSGG0021-2023)
  • 摘要: 同步定位与地图构建(SLAM)技术是移动机器人智能导航的基础。该文针对单一传感器SLAM技术存在的问题,提出一种基于激光雷达多层迭代最近点(MICP)点云匹配闭环检测的误差状态卡尔曼滤波(ESKF)多传感器紧耦合2D-SLAM算法。在完成视觉与激光雷达多模态数据的时空同步后,建立了里程计误差模型以及激光雷达与机器视觉点云匹配误差模型,并将其应用于误差状态卡尔曼滤波进行多模态数据融合,以提高SLAM的准确性和实时性。在公共数据集KITTI下进行的Gazebo环境仿真结果表明,该所提算法能够完整还原单一激光2D-SLAM无法获取到的环境障碍物信息,并能显著提高机器人轨迹估计和相对位姿估计精度。最后,采用Turtlebot2机器人在复杂实际大场景下进行了SLAM实验验证,结果表明所提多模态融合SLAM方法可以完整复原环境信息,实现实时的高精度2D地图构建。
  • 图  1  基于ESKF的多模态紧耦合SLAM框图

    图  2  多层ICP流程图

    图  3  不同算法下的机器人位姿精度评估

    图  4  仿真世界模型

    图  5  SLAM构建的仿真世界模型地图

    图  6  Turtlebot2结构图

    图  7  室内大规模场景示意图

    图  8  Gmapping算法构建地图

    图  9  Cartographer算法构建地图

    图  10  基于MICP闭环检测Gmapping算法构建地图

    图  11  MICP-ESKF多传感器紧耦合SLAM算法构建地图与机器人运行轨迹

    1  基于MICP匹配闭环检测的ESKF多传感器紧耦合SLAM

     输入:$k + 1$时刻的机器人位姿${{\boldsymbol{x}}_{k + 1}}$
     1.将点云数据转换到全局坐标系,使得Sub-ICP进行初始化;
     2.构建点云匹配误差数学模型${\boldsymbol{F}}$,构建里程计误差模型;
     3.利用式(7)、式(17)分别得到非系统误差协方差${{\boldsymbol{Q}}_k}$,${{\boldsymbol{D}}_k}$;
     4.将里程计估算的机器人位姿作为位姿预测值输入ESKF,将激
     光里程计计算的位姿作为观测值输入ESKF迭代;
     5.通过$\delta {\hat {\boldsymbol{x}}_k}$对状态变量${{\boldsymbol{x}}_k}$进行位姿修正;
     6.更新后的位姿进行定位与地图构建。
     输出:修正后的高精度地图
    下载: 导出CSV

    表  1  不同数据样本下SLAM算法闭环检测精度与用时测试

    数量样本 约束数量 检测精度(%) 计算占用时间(s)
    Cartographer MICP Karto Cartographer MICP Karto
    Aces 971±5 98.1 97.9 96.4 1366 1291 1405
    Intel 5786±5 97.2 98.4 96.6 2691 2523 2725
    MIT CSAIL 916±5 93.4 94.2 90.0 7678 7284 7862
    Freiburg bldg 79 1857±5 94.1 93.2 92.9 424 393 419
    Freiburg hospital 412±5 98.8 98.5 96.5 1061 894 1084
    MIT Killian Court 554±5 77.3 80.1 73.1 4820 4627 4839
    下载: 导出CSV

    表  2  不同SLAM算法下的机器人相对位姿误差

    SLAM算法 最大误差 平均误差 中位数误差 最小误差 均方根误差
    MICP-ESKF-SLAM 0.09332 0.08214 0.01721 0.00934 0.01594
    MICP-SLAM 0.15587 0.12773 0.04418 0.02021 0.05672
    Gmapping 1.03219 0.54158 0.49083 0.22892 0.50026
    Cartographer 0.61091 0.30171 0.21823 0.10442 0.29528
    Karto 0.73019 0.44203 0.29575 0.12471 0.32757
    下载: 导出CSV
  • [1] WANG Zhongli, CHEN Yan, MEI Yue, et al. IMU-assisted 2D SLAM method for low-texture and dynamic environments[J]. Applied Sciences, 2018, 8(12): 2534. doi: 10.3390/app8122534.
    [2] CHAN S H, WU P T, and FU Lichen. Robust 2D indoor localization through laser SLAM and visual SLAM fusion[C]. 2018 IEEE International Conference on Systems, Man, and Cybernetics, Miyazaki, Japan, 2018: 1263–1268. doi: 10.1109/SMC.2018.00221.
    [3] CHEN L H and PENG C C. A robust 2D-SLAM technology with environmental variation adaptability[J]. IEEE Sensors Journal, 2019, 19(23): 11475–11491. doi: 10.1109/JSEN.2019.2931368.
    [4] XU Wei and ZHANG Fu. FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 3317–3324. doi: 10.1109/LRA.2021.3064227.
    [5] XU Wei, CAI Yixi, HE Dongjiao, et al. FAST-LIO2: Fast direct LiDAR-inertial odometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053–2073. doi: 10.1109/TRO.2022.3141876.
    [6] LIANG Xiao, CHEN Haoyao, LI Yanjie, et al. Visual laser-SLAM in large-scale indoor environments[C]. 2016 IEEE International Conference on Robotics and Biomimetics, Qingdao, China, 2016: 19–24. doi: 10.1109/ROBIO.2016.7866271.
    [7] GAO Xiang, WANG Rui, DEMMEL N, et al. LDSO: Direct sparse odometry with loop closure[C]. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, 2018: 2198–2204. doi: 10.1109/IROS.2018.8593376.
    [8] ZHU Zulun, YANG Shaowu, and DAI Huadong. Enhanced visual loop closing for laser-based SLAM[C]. The 2018 IEEE 29th International Conference on Application-specific Systems, Architectures and Processors, Milan, Italy, 2018: 1–4. doi: 10.1109/ASAP.2018.8445128.
    [9] WANG Han, WANG Chen, and XIE Lihua. Intensity scan context: Coding intensity and geometry relations for loop closure detection[C]. 2020 IEEE International Conference on Robotics and Automation, Paris, France, 2020: 2095–2101. doi: 10.1109/ICRA40945.2020.9196764.
    [10] FAN Yunfeng, HE Yichang, and TAN U X. Seed: A segmentation-based egocentric 3D point cloud descriptor for loop closure detection[C]. 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, USA, 2020: 5158–5163. doi: 10.1109/IROS45743.2020.9341517.
    [11] CHEN Dan, ZHANG Heng, TANG Linao, et al. Multimodal fusion simultaneous localization and mapping method based on multilayer point cloud matching closed-loop detection[J]. Journal of Electronic Imaging, 2024, 33(3): 033024. doi: 10.1117/1.JEI.33.3.033024.
    [12] CHEN Yang and MEDIONI G. Object modeling by registration of multiple range images[C]. 1991 IEEE International Conference on Robotics and Automation, Sacramento, USA, 1991: 2724–2729. doi: 10.1109/ROBOT.1991.132043.
    [13] 杨晶东, 杨敬辉, 洪炳熔. 一种有效的移动机器人里程计误差建模方法[J]. 自动化学报, 2009, 35(2): 168–173. doi: 10.3724/SP.J.1004.2009.00168.

    YANG Jingdong, YANG Jinghui, and HONG Bingrong. An efficient approach to odometric error modeling for mobile robots[J]. Acta Automatica Sinica, 2009, 35(2): 168–173. doi: 10.3724/SP.J.1004.2009.00168.
    [14] KÜMMERLE R, STEDER B, DORNHEGE C, et al. On measuring the accuracy of SLAM algorithms[J]. Autonomous Robots, 2009, 27(4): 387–407. doi: 10.1007/s10514-009-9155-6.
    [15] JULIER S, UHLMANN J, and DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477–482. doi: 10.1109/9.847726.
    [16] 程效军, 施贵刚, 王峰, 等. 点云配准误差传播规律的研究[J]. 同济大学学报: 自然科学版, 2009, 37(12): 1668–1672. doi: 10.3969/j.issn.0253-374x.2009.12.020.

    CHENG Xiaojun, SHI Guigang, WANG Feng, et al. Research on point cloud registration error propagation[J]. Journal of Tongji University: Natural Science, 2009, 37(12): 1668–1672. doi: 10.3969/j.issn.0253-374x.2009.12.020.
    [17] 武汉大学测绘学院测量平差学科组. 误差理论与测量平差基础[M]. 3版. 武汉: 武汉大学出版社, 2014.

    Surveying Adjustment Group School of Geodesy Wuhan University. Error Theory and Foundation of Surveying Adjustment[M]. 3rd ed. Wuhan: Wuhan University Press, 2014.
    [18] 崔文, 薛棋文, 李庆玲, 等. 基于三维点云地图和ESKF的无人车融合定位方法[J]. 工矿自动化, 2022, 48(9): 116–122. doi: 10.13272/j.issn.1671-251x.17997.

    CUI Wen, XUE Qiwen, LI Qingling, et al. Unmanned vehicle fusion positioning method based on 3D point cloud map and ESKF[J]. Journal of Mine Automation, 2022, 48(9): 116–122. doi: 10.13272/j.issn.1671-251x.17997.
    [19] WAN Guowei, YANG Xiaolong, CAI Renlan, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]. 2018 IEEE International Conference on Robotics and Automation, Brisbane, Australia, 2018: 4670–4677. doi: 10.1109/ICRA.2018.8461224.
    [20] 郭文卓, 李林阳, 程振豪, 等. 先验地图/IMU/LiDAR的图优化和ESKF位姿估计方法对比[J]. 测绘科学, 2023, 48(4): 88–97. doi: 10.16251/j.cnki.1009-2307.2023.04.010.

    GUO Wenzhuo, LI Linyang, CHENG Zhenhao, et al. Comparison of prior map/IMU/LiDAR map optimization and error state Kalman filter pose estimation methods[J]. Science of Surveying and Mapping, 2023, 48(4): 88–97. doi: 10.16251/j.cnki.1009-2307.2023.04.010.
    [21] LIU Shengshu, LEI Yixing, and DONG Xin. Evaluation and comparison of gmapping and karto SLAM systems[C]. The 2022 12th International Conference on CYBER Technology in Automation, Control, and Intelligent Systems, Baishan, China, 2022: 295–300. doi: 10.1109/CYBER55403.2022.9907154.
    [22] HESS W, KOHLER D, RAPP H, et al. Real-time loop closure in 2D LIDAR SLAM[C]. Proceedings of 2016 IEEE International Conference on Robotics and Automation, Stockholm, Sweden, 2016: 1271–1278. doi: 10.1109/ICRA.2016.7487258.
    [23] 王勇, 陈卫东, 王景川, 等. 面向动态高遮挡环境的移动机器人自适应位姿跟踪算法[J]. 机器人, 2015, 37(1): 112–121. doi: 10.13973/j.cnki.robot.2015.0112.

    WANG Yong, CHEN Weidong, WANG Jingchuan, et al. Self-adaptive pose-tracking algorithm for mobile robots in dynamic and highly-occluded environments[J]. Robot, 2015, 37(1): 112–121. doi: 10.13973/j.cnki.robot.2015.0112.
    [24] GRISETTI G, STACHNISS C, and BURGARD W. Improving grid-based SLAM with rao-blackwellized particle filters by adaptive proposals and selective resampling[C]. 2005 IEEE International Conference on Robotics and Automation, Barcelona, Spain, 2005: 2432–2437. doi: 10.1109/ROBOT.2005.1570477.
    [25] 丁林祥, 陶卫军. ROS/Gazebo在SLAM算法评估中的应用[J]. 兵工自动化, 2022, 41(4): 87–92. doi: 10.7690/bgzdh.2022.04.018.

    DING Linxiang and TAO Weijun. Application of ROS/gazebo in the evaluation of SLAM algorithm[J]. Ordnance Industry Automation, 2022, 41(4): 87–92. doi: 10.7690/bgzdh.2022.04.018.
  • 加载中
图(11) / 表(3)
计量
  • 文章访问数:  228
  • HTML全文浏览量:  148
  • PDF下载量:  36
  • 被引次数: 0
出版历程
  • 收稿日期:  2024-11-01
  • 修回日期:  2025-02-26
  • 网络出版日期:  2025-03-05
  • 刊出日期:  2025-05-01

目录

    /

    返回文章
    返回