2.793

2018影响因子

(CJCR)

  • 中文核心
  • EI
  • 中国科技核心
  • Scopus
  • CSCD
  • 英国科学文摘

留言板

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

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

Lidar/IMU紧耦合的实时定位方法

李帅鑫 李广云 王力 杨啸天

李帅鑫, 李广云, 王力, 杨啸天. Lidar/IMU紧耦合的实时定位方法. 自动化学报, 2020, 46(x): 1−13. doi: 10.16383/j.aas.c190424
引用本文: 李帅鑫, 李广云, 王力, 杨啸天. Lidar/IMU紧耦合的实时定位方法. 自动化学报, 2020, 46(x): 1−13. doi: 10.16383/j.aas.c190424
Li Shuai-Xin, Li Guang-Yun, WANG Li, YANG Xiao-Tian. Lidar/IMU tightly coupled real-time localization method. Acta Automatica Sinica, 2020, 46(x): 1−13. doi: 10.16383/j.aas.c190424
Citation: Li Shuai-Xin, Li Guang-Yun, WANG Li, YANG Xiao-Tian. Lidar/IMU tightly coupled real-time localization method. Acta Automatica Sinica, 2020, 46(x): 1−13. doi: 10.16383/j.aas.c190424

Lidar/IMU紧耦合的实时定位方法


DOI: 10.16383/j.aas.c190424
详细信息
    作者简介:

    战略支援部队信息工程大学地理空间信息学院博士研究生. 2015年获中南大学测绘工程学士学位, 2018年获战略支援部队信息工程大学控制科学与工程硕士学位. 主要研究方向为多传感器融合的SLAM, 移动测量. 本文通信作者. E-mail: lsx_navigation@sina.com

    战略支援部队信息工程大学地理空间信息学院教授、博导. 1987年获解放军测绘学院测绘科学与技术硕士学位. 主要研究方向为精密工程与工业测量, 导航应用及导航定位与位置服务. E-mail: guangyun_li@163.com

  • 基金项目:  地理信息工程国家重点实验室基金(SKLGIE2018-M-3-1), 国家重点研发计划(2017YFF0206001), 国家自然科学基金(41501491)资助

Lidar/IMU Tightly Coupled Real-time Localization Method

More Information
  • Fund Project:  Supported by State Key Laboratory of Geo-Information Engineering(SKLGIE2018-M-3-1), National Key Research and Development Project(2017YFF0206001), National Natural Science Foundation of China(41501491)
  • 摘要: 本文以实现移动小型智能化系统的实时自主定位为目标, 针对激光里程计误差累计大, 旋转估计不稳定, 以及观测信息利用不充分等问题, 提出一种Lidar/IMU紧耦合的实时定位方法—Inertial-LOAM. 数据预处理部分, 对IMU数据预积分, 降低优化变量维度, 并为点云畸变校正提供参考. 提出一种基于角度图像的快速点云分割方法, 筛选结构性显著的点作为特征点, 降低点云规模, 保证激光里程计的效率; 针对地图构建部分存在的地图匹配点搜索效率低和离散点云地图的不完整性问题, 提出传感器中心的多尺度地图模型, 利用环形容器保持地图点恒定, 并结合多尺度格网保证地图模型中点的均匀分布. 数据融合部分, 提出Lidar/IMU紧耦合的优化方法, 将IMU和Lidar构成的预积分因子、配准因子、闭环因子插入全局因子图中, 采用基于贝叶斯树的因子图优化算法对变量节点进行增量式优化估计, 实现数据融合. 最后, 采用实测数据评估Inertial-LOAM的性能并与LeGO-LOAM, LOAM和Cartographer对比. 结果表明, Inertial-LOAM在不明显增加运算负担的前提下大幅降低连续配准误差造成的误差累计, 具有良好的实时性; 在结构性特征明显的室内环境, 定位精度达厘米级, 与对比方法持平; 在开阔的室外环境, 定位精度达分米级, 而对比方法均存在不同程度的漂移.
     收稿日期 2019-06-02    录用日期 2019-12-15 Manuscript received June 2, 2019; accepted December 15, 2019 地理信息工程国家重点实验室基金 (SKLGIE2018-M-3-1), 国家重点研发计划 (2017YFF0206001), 国家自然科学基金(41501491) 资助 Supported by State Key Laboratory of Geo-Information Engineering(SKLGIE2018-M-3-1), National Key Research and Development Project£2017YFF0206001), National Natural Science Foundation of China(41501491)
     本文责任编委 吴毅红 Recommended by Associate Editor WU Yi-Hong 1. 地理信息工程国家重点实验室 西安 710054    2. 战略支援部队信息工程大学地理空间信息学院 郑州 450001 1. State Key Laboratory of Geo-Information Engineering, Xi’an 710054    2. College of Geospatial Information, PLA Information Engineering University, Zhengzhou 450001
    1http://www.cvlibs.net/datasets/kitti/eval_odometry.php
  • 图  1  系统框架图

    Fig.  1  The overview of the system

    图  2  点云分割示例

    Fig.  2  Example of point cloud segmentation

    图  3  IMU与Lidar的频率关系

    Fig.  3  Frequencies of IMU and Lidar

    图  4  局部地图示意图

    Fig.  4  Demonstration for the local map

    图  5  因子图结构

    Fig.  5  Structure of the factor graph

    图  6  数据采集平台

    Fig.  6  Data collection platform

    图  10  Inertial-LOAM轨迹及建图结果

    Fig.  10  Trajectory and mapping result of Inertial-LOAM

    图  7  系统运行时间对比

    Fig.  7  Comparison of time cost of two systems

    图  8  闭环优化效果

    Fig.  8  Performance of loop optimization

    图  9  室外开阔环境IL/LL/L/C轨迹结果对比

    Fig.  9  Comparison of pose estimation of IL/LL/L/C in outdoor environment

    表  1  累计误差结果

    Table  1  Error accumulation result

    场景方法横滚(°)俯仰(°)航向(°)角度偏差(°)X方向(m)Y方向(m)Z方向(m)位置偏差(m)
    2#数据[11]IMU0.7481.0180.5981.39835.09584.652−665.782672.059
    Cartographer0.113−0.7090.9891.2220.4051.3170.6701.532
    LOAM0.0160.1410.9250.9360.3160.3490.0250.471
    LeGO-LOAM0.0610.0810.9160.9210.0680.3380.1150.364
    Inertial-LOAM0.0130.0260.9170.9180.0610.2580.0230.266
    室内环境Cartographer0.003−0.0010.0170.0170.0230.0370.0280.052
    LOAM0.0010.0040.0680.0680.0320.0830.0320.095
    LeGO-LOAM−0.006−0.002−0.0210.0220.0160.047−0.0320.059
    Inertial-LOAM−0.0080.001−0.0200.0210.0210.0430.0270.055
    室外环境Cartographer0.075−0.0240.0810.1131.7472.592−0.4493.158
    LOAM−0.0310.0060.0960.1010.04672.368−0.0652.353
    LeGO-LOAM−0.024−0.5430.0410.545−19.857−14.914−0.35524.836
    Inertial-LOAM0.006−0.0800.0030.080−0.310−0.100−0.0300.328
    下载: 导出CSV
  • [1] 李帅鑫. 激光雷达/相机组合的3D SLAM技术研究. 硕士学位论文, 战略支援部队信息工程大学, 2018.

    LI Shuai-Xin. Research on lidar/camera coupled 3d slam. Master’s thesis, PLA Information Engineering University, 2018.
    [2] Paul J Besl and Neil D McKay. Method for registration of 3-d shapes. In Sensor fusion IV: control paradigms and data structures, volume 1611, pages 586-606. International Society for Optics and Photonics, 1992.
    [3] 3 François Pomerleau, Francis Colas, Roland Siegwart, and Stéphane Magnenat. Comparing icp variants on real-world data sets. Autonomous Robots, 2013, 34(3): 133−148 doi:  10.1007/s10514-013-9327-2
    [4] 4 Hartmut Surmann, Andreas Nüchter, Kai Lingemann, and Joachim Hertzberg. 6d slampreliminary report on closing the loop in six dimensions. IFAC Proceedings Volumes, 2004, 37(8): 197−202 doi:  10.1016/S1474-6670(17)31975-4
    [5] Frank Moosmann and Christoph Stiller. Velodyne slam. In 2011 ieee intelligent vehicles symposium (iv), pages 393−398. IEEE, 2011.
    [6] 6 David Droeschel, Max Schwarz, and Sven Behnke. Continuous mapping and localization for autonomous navigation in rough terrain using a 3d laser scanner. Robotics and Autonomous Systems, 2017, 88: 104−115 doi:  10.1016/j.robot.2016.10.017
    [7] David Droeschel and Sven Behnke. Efficient continuous-time slam for 3d lidar-based online mapping. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pages 1−9. IEEE, 2018.
    [8] Ji Zhang and Sanjiv Singh. Loam: Lidar odometry and mapping in real-time. In Robotics: Science and Systems, volume 2, page 9, 2014.
    [9] 9 Ji Zhang and Sanjiv Singh. Low-drift and real-time lidar odometry and mapping. Autonomous Robots, 2017, 41(2): 401−416 doi:  10.1007/s10514-016-9548-2
    [10] Andreas Geiger, Philip Lenz, and Raquel Urtasun. Are we ready for autonomous driving? the kitti vision benchmark suite. In 2012 IEEE Conference on Computer Vision and Pattern Recognition, pages 3354−3361. IEEE, 2012.
    [11] Tixiao Shan and Brendan Englot. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 4758−4765. IEEE, 2018.
    [12] Wolfgang Hess, Damon Kohler, Holger Rapp, and Daniel Andor. Real-time loop closure in 2d lidar slam. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pages 1271−1278. IEEE, 2016.
    [13] 13 Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. On-manifold preintegration for real-time visual-inertial odometry. IEEE Transactions on Robotics, 2016, 33(1): 1−21
    [14] 14 Yashar Balazadegan Sarvrood, Siavash Hosseinyalamdary, and Yang Gao. Visual-lidar odometry aided by reduced imu. ISPRS International Journal of Geo-Information, 2016, 5(1): 3 doi:  10.3390/ijgi5010003
    [15] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.
    [16] Sebastian Hening, Corey A Ippolito, Kalmanje S Krishnakumar, Vahram Stepanyan, and Mircea Teodorescu. 3d lidar slam integration with gps/ins for uavs in urban gps-degraded environments. In AIAA Information SystemsAIAA Infotech@ Aerospace, page 0448. 2017.
    [17] 17 Frank Dellaert, Michael Kaess, et al. Factor graphs for robot perception. Foundations and Trends® in Robotics, 2017, 6(1-2): 1−139 doi:  10.1561/2300000043
    [18] 18 Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, and Paul Furgale. Keyframe-based visual-inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 2015, 34(3): 314−334 doi:  10.1177/0278364914554813
    [19] Kurt Konolige, Giorgio Grisetti, Rainer Kümmerle, Wolfram Burgard, Benson Limketkai, and Regis Vincent. Efficient sparse pose adjustment for 2d mapping. In 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 22−29. IEEE, 2010.
    [20] 20 Michael Kaess, Ananth Ranganathan, and Frank Dellaert. isam: Incremental smoothing and mapping. IEEE Transactions on Robotics, 2008, 24(6): 1365−1378 doi:  10.1109/TRO.2008.2006706
    [21] Vadim Indelman, Stephen Williams, Michael Kaess, and Frank Dellaert. Factor graph based incremental smoothing in inertial navigation systems. In 2012 15th International Conference on Information Fusion, pages 2154−2161. IEEE, 2012.
    [22] 22 Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John J Leonard, and Frank Dellaert. isam2: Incremental smoothing and mapping using the bayes tree. The International Journal of Robotics Research, 2012, 31(2): 216−235 doi:  10.1177/0278364911430419
    [23] 23 Tong Qin, Peiliang Li, and Shaojie Shen. Vins-mono: A robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics, 2018, 34(4): 1004−1020 doi:  10.1109/TRO.2018.2853729
    [24] Tong Qin and Shaojie Shen. Online temporal calibration for monocular visual-inertial systems. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 3662−3669. IEEE, 2018.
    [25] Shuaixin Li, Guangyun Li, Yanglin Zhou, Li Wang, and Jingyang Fu. Real-time dead reckoning and mapping approach based on threedimensional point cloud. In China Satellite Navigation Conference, pages 643−662. Springer, 2018.
    [26] Timothy D Barfoot. State Estimation for Robotics. Cambridge University Press, 2017.
    [27] 27 Ji Zhang and Sanjiv Singh. Laser-visual-inertial odometry and mapping with high robustness and low drift. Journal of Field Robotics, 2018, 35(8): 1242−1264 doi:  10.1002/rob.21809
    [28] Jens Behley and Cyrill Stachniss. Efficient surfel-based slam using 3d laser range data in urban environments. In Robotics: Science and Systems, 2018.
  • [1] 张豪, 何百岳, 杨旭升, 张文安. 基于可穿戴式惯性传感器的人体运动跟踪方法综述[J]. 自动化学报, doi: 10.16383/j.aas.c180367
    [2] 丁文东, 徐德, 刘希龙, 张大朋, 陈天. 移动机器人视觉里程计综述[J]. 自动化学报, doi: 10.16383/j.aas.2018.c170107
    [3] 伦淑娴, 胡海峰. 基于罚函数内点法的泄露积分型回声状态网的参数优化[J]. 自动化学报, doi: 10.16383/j.aas.2017.c160541
    [4] 王楠, 马书根, 李斌, 王明辉, 赵明扬. 震后建筑内部层次化SLAM的地图模型转换方法[J]. 自动化学报, doi: 10.16383/j.aas.2015.c150125
    [5] 宋宇, 李庆玲, 康轶非, 闫德立. 平方根容积Rao-Blackwillised粒子滤波SLAM算法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2014.00357
    [6] 岳元龙, 左信, 罗雄麟. 提高测量可靠性的多传感器数据融合有偏估计方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2014.01843
    [7] 王坤峰, 李镇江, 汤淑明. 基于多特征融合的视频交通数据采集方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2011.00322
    [8] 李鹏, 刘思峰. 基于灰色关联分析和D-S证据理论的区间直觉模糊决策方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2011.00993
    [9] 董晓坤, 方勇纯, 张玉东. 一种基于临近点集数据融合的AFM动态成像方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2011.00214
    [10] 祝继华, 郑南宁, 袁泽剑, 张强. 基于中心差分粒子滤波的SLAM算法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2010.00249
    [11] 李慧平, 徐德民, 张福斌, 姚尧. 基于量测噪声和观测次数的EKF-SLAM一致性分析[J]. 自动化学报, doi: 10.3724/SP.J.1004.2009.01177
    [12] 杨晶东, 杨敬辉, 洪炳熔. 一种有效的移动机器人里程计误差建模方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2009.00168
    [13] 季秀才, 郑志强, 张辉. SLAM 问题中机器人定位误差分析与控制[J]. 自动化学报, doi: 10.3724/SP.J.1004.2008.00323
    [14] 吴健康, 董梁, 包晓明. 数据流分隔——传感网络中的一种数据融合方法[J]. 自动化学报
    [15] 韩锐, 李文锋. 一种基于线特征的SLAM算法研究[J]. 自动化学报
    [16] 唐琎, 张闻捷, 高琰, 付明玉, 蔡自兴. 不同精度冗余数据的融合[J]. 自动化学报
    [17] 刘红毅, 王蕴红, 谭铁牛. 基于改进ENN算法的多生物特征融合的身份验证[J]. 自动化学报
    [18] 阮小娥, 李换琴, 万百五. 滞后工业过程稳态优化进程中的局部对称双积分型迭代学习控制[J]. 自动化学报
    [19] J.Renno, P.Remagnino, G.A.Jones. 针对自校准地面的学习监控跟踪模型[J]. 自动化学报
    [20] 文成林, 周东华, 潘泉, 张洪才. 多尺度动态模型单传感器动态系统分布式信息融合[J]. 自动化学报
  • 加载中
图(10) / 表(1)
计量
  • 文章访问数:  144
  • HTML全文浏览量:  145
  • PDF下载量:  17
  • 被引次数: 0
出版历程
  • 收稿日期:  2019-06-02
  • 录用日期:  2019-12-15
  • 网络出版日期:  2020-01-16

Lidar/IMU紧耦合的实时定位方法

doi: 10.16383/j.aas.c190424
    作者简介:

    战略支援部队信息工程大学地理空间信息学院博士研究生. 2015年获中南大学测绘工程学士学位, 2018年获战略支援部队信息工程大学控制科学与工程硕士学位. 主要研究方向为多传感器融合的SLAM, 移动测量. 本文通信作者. E-mail: lsx_navigation@sina.com

    战略支援部队信息工程大学地理空间信息学院教授、博导. 1987年获解放军测绘学院测绘科学与技术硕士学位. 主要研究方向为精密工程与工业测量, 导航应用及导航定位与位置服务. E-mail: guangyun_li@163.com

基金项目:  地理信息工程国家重点实验室基金(SKLGIE2018-M-3-1), 国家重点研发计划(2017YFF0206001), 国家自然科学基金(41501491)资助

摘要: 本文以实现移动小型智能化系统的实时自主定位为目标, 针对激光里程计误差累计大, 旋转估计不稳定, 以及观测信息利用不充分等问题, 提出一种Lidar/IMU紧耦合的实时定位方法—Inertial-LOAM. 数据预处理部分, 对IMU数据预积分, 降低优化变量维度, 并为点云畸变校正提供参考. 提出一种基于角度图像的快速点云分割方法, 筛选结构性显著的点作为特征点, 降低点云规模, 保证激光里程计的效率; 针对地图构建部分存在的地图匹配点搜索效率低和离散点云地图的不完整性问题, 提出传感器中心的多尺度地图模型, 利用环形容器保持地图点恒定, 并结合多尺度格网保证地图模型中点的均匀分布. 数据融合部分, 提出Lidar/IMU紧耦合的优化方法, 将IMU和Lidar构成的预积分因子、配准因子、闭环因子插入全局因子图中, 采用基于贝叶斯树的因子图优化算法对变量节点进行增量式优化估计, 实现数据融合. 最后, 采用实测数据评估Inertial-LOAM的性能并与LeGO-LOAM, LOAM和Cartographer对比. 结果表明, Inertial-LOAM在不明显增加运算负担的前提下大幅降低连续配准误差造成的误差累计, 具有良好的实时性; 在结构性特征明显的室内环境, 定位精度达厘米级, 与对比方法持平; 在开阔的室外环境, 定位精度达分米级, 而对比方法均存在不同程度的漂移.

 收稿日期 2019-06-02    录用日期 2019-12-15 Manuscript received June 2, 2019; accepted December 15, 2019 地理信息工程国家重点实验室基金 (SKLGIE2018-M-3-1), 国家重点研发计划 (2017YFF0206001), 国家自然科学基金(41501491) 资助 Supported by State Key Laboratory of Geo-Information Engineering(SKLGIE2018-M-3-1), National Key Research and Development Project£2017YFF0206001), National Natural Science Foundation of China(41501491)
 本文责任编委 吴毅红 Recommended by Associate Editor WU Yi-Hong 1. 地理信息工程国家重点实验室 西安 710054    2. 战略支援部队信息工程大学地理空间信息学院 郑州 450001 1. State Key Laboratory of Geo-Information Engineering, Xi’an 710054    2. College of Geospatial Information, PLA Information Engineering University, Zhengzhou 450001
1http://www.cvlibs.net/datasets/kitti/eval_odometry.php

English Abstract

  • 载体利用所搭载传感器的观测信息, 在未知环境中进行实时自主定位并增量式构建环境地图的能力是无人系统的关键技术之一[1]. 激光雷达(Lidar)因其分辨率高、抗干扰强、不受光照条件影响等特点被广泛应用于无人系统中, 被称为“无人车的眼睛”. 惯性导航技术(Inertial Navigation System, INS)是一种不受外界因素干扰的自主导航手段, 可以在卫星导航信号失锁情况下提供载体的位姿信息. 因此将二者结合实现自主定位与地图构建已成为学术界与工业界的共识. 然而, 对于诸如无人机、智能机器人等运算资源有限的小型智能系统, 实现实时的6自由度(Degree of Freedom, DoF)位姿估计并增量式构建环境地图是十分困难的. 一方面, 由于定位与地图构建相互依赖. 位姿估计可以为地图构建提供必要的先验信息, 而对已构建地图的重复观测又可以消除位姿估计的不确定度[1], 二者的交织使得实时定位与地图构建(Simultaneous Localization and Mapping, SLAM)问题具有极高的复杂度. 另一方面, 由于数据流中包含了数据量庞大的点云, 以及高频采样的惯性测量单元(Inertial Measurement Unit, IMU)数据, 如何高效且充分的融合点云与IMU数据也是一个难点.

    点云配准是利用连续点云进行位姿推估的核心方法, 迭代最邻近点算法(Iterative Closest Point, ICP)[2, 3]是应用最为广泛的点云配准算法. Surmann将2D Lidar安装在编码器和转台上获取三维点云, 并采用ICP算法估计相Lidar的运动[4]. Moosmann提出基于3D Lidar和ICP的解决方案, 介绍了一种基于匀速运动模型的点云扭曲的补偿方法并对补偿结果进行了分析[5]. 由于这两种方法均采用ICP算法直接对大规模点云进行处理, 配准效率低, 实时性差; 另外, 这种增量式递推方法缺少对点云配准误差累计的优化后端. Droeschel提出多分辨率格网地图(Multi-ReSolution, MRS)的环境表达方式, 和一种基于点云表面元模型的概率配准算法[6], 并在此基础上提出了一种分层优化的后端图优化策略[7], 实现对连续轨迹和地图的优化. 但由于该方法侧重强调地图构建的一致性, 因此将地图作为变量节点一并纳入后端优化中, 运算量较大, 在小型智能系统上难以达到实时性. Zhang提出的激光雷达里程计与地图构建方法(Lidar Odometry and Mapping, LOAM)[8, 9]代表该领域的最高水准, 该方法在KITTI(Karlsruhe Institute of Technology and Toyota technological Institute)[10]评测中排名第二. LOAM采用点到边和点到面的联合配准方法估计Lidar的相对运动; 为保证实时性, 在两线程上分别运行高频的激光里程计模块和低频的地图构建模块, 低频线程接收高频线程的位姿估计结果. Tian在[11]中指出LOAM在计算资源有限的条件下效果大大下降, 并基于此提出了面向无人车(Unmanned Ground Vehicle, UGV)的轻量级LOAM方法(Lightweight and Ground-Optimized LOAM, LeGO-LOAM). 该方法增加点云分割处理模块降低点云规模, 采用两步法配准点云, 并增加闭环检测线程实现对累计误差的在线修正. 但这两种方法都只是利用IMU提供的姿态信息为点云配准和点云畸变消除提供运动估计的先验信息, 在优化计算时并未将其作为观测约束进行整体优化, 数据利用不够充分. 此外, 仅依靠点云配准进行位姿推估和地图构建在实际应用中误差累计较大, 且极易出现配准错误, 尤其在较为缺乏结构性特征的室外环境下或Lidar快速旋转时. 这对缺乏重定位能力的系统而言是致命的, 将导致定位和地图构建的失败. Google 2016年发布的开源SLAM库Cartographer[12]采用一种分层优化的思路. 前端采用无损卡尔曼滤波(UKF, Unscented Kalman Filter)算法融合多源数据进行位姿推估并构建子地图. 后端以子地图为基本单元构建优化问题, 并提出分支界定算法加速子地图间约束的构建. 该系统仅在前端进行了较为松散的数据耦合, 而在后端优化时仍是仅以点云的匹配为约束, 数据融合不够紧密.

    多传感器数据紧融合算法整体可分为滤波算法和平滑算法两类[13, 14]. 滤波算法[15, 16]数据处理简单, 实时性强, 在工业界得到了广泛应用. 但由于其本身的递推性质, 线性化误差将不断累积, 导致精度下降. 平滑算法包括全平滑(Full Smoothing)和固定滞延平滑(Fixed-lag Smoothing)两种. 全平滑[17]对所有变量整体优化, 精度高, 但在SLAM问题中随着轨迹扩张变量个数激增, 无法满足实时性. 固定滞延平滑[18, 19]是一种滤波和全优化的折中方式, 它在时间轴上设置一个随时间滑动的固定窗口, 每次仅优化窗口内的变量, 并边缘化其余变量. 由于优化迭代时所有变量都将被重新线性化, 因此线性化累计误差较小, 精度得到保证; 同时由于窗口大小固定, 优化变量个数基本不变, 实时性可以满足. 该算法的缺点是变量边缘化时协方差矩阵变稠密, 这一定程度上增加了计算负担, 影响了计算效率. Kaess用因子图(Fator Graph)表示变量间的关系, 提出一种增量式平滑的全优化算法[20, 21], 代表当前最先进的优化方法. 该方法将因子图保存在贝叶斯树中[22], 当有新的因子节点加入时, 识别被影响的变量节点并仅对它们进行优化更新, 从而维持全优化的稀疏特性. 目前基于因子图全优化算法的IMU数据紧融合研究主要集中在相机与IMU的组合中[23, 24], 而在Lidar与IMU的组合中还鲜有涉及.

    综上所述, 针对小型智能系统的实时定位研究具有重要的现实意义, Lidar/IMU的紧耦合方案仍需进一步探讨. 本文在LOAM[9]和LeGO-LOAM[11]的基础上并结合前期工作[25], 提出一种在未知环境下, 基于Lidar/IMU紧耦合的实时定位方法—Inertial-LOAM, 通过在预处理、配准和后端优化等多层次的数据融合, 实现多源数据的紧密耦合, 降低轨迹漂移, 增强系统在室外开阔环境和快速转动时的鲁棒性. 实验结果表明本文方法可以满足小型智能的实时定位需求, 定位性能显著提升.

    • 系统接收3D Lidar的原始点云和IMU输出的角速度及加速度数据, 输出连续位姿估计和环境地图. 需要说明的是, Lidar/IMU的外参数需预先标定, 在后续数据处理中作为已知量. 传感器需时间对齐, 一般可由IMU向Lidar输出PPS(Pulse Per Second)脉冲和推荐定位信息(GPRMC)信息, 实现同步触发; 也可利用IMU的高频输出特性, 对带有时间戳的数据进行滤波和内插实现.

      系统整体分为五个部分: 1) 数据预处理. 将原始点云投影为深度图像, 并进行快速的地面点及目标分割剔除野点, 并从分割后的点云中提取特征点; 同时, 利用IMU预积分得到的相对运动估值对特征点进行畸变校正; 2) 激光里程计. 将连续时刻的特征点配准, 估计Lidar相对运动, 并据此再次校正特征点; 3) 地图构建. 将校正后的特征点与局部地图配准, 优化位姿估计结果, 并更新局部地图; 4) 闭合回环. 检测轨迹是否闭合, 并将闭合处的点云配准结果作为闭环约束关系; 5) 因子图优化. 系统维护一个全局的因子图, 各模块向因子图中插入IMU预积分因子、配准因子和闭环因子, 每当插入新的因子节点, 优化计算一次. 为保证实时性, 系统在三个线程上并行运行, 其中预处理和激光里程计占用主线程, 地图构建和闭合回环各占一个分线程. Inertial-LOAM系统框架图如图1.

      图  1  系统框架图

      Figure 1.  The overview of the system

    • 载体坐标系定义为$ {{\rm{B}}} $, 与IMU坐标系保持一致; 世界坐标系定义为$ {{\rm{W}}} $, 原点为系统初始化时的载体系中心, $ {\rm{z}} $轴指向与世界坐标系下的重力方向对齐; 假设Lidar第$ i $次扫描的起始时刻为$ {t_i} $, 扫描得到的全部点云为$ {{\cal P}_i} $, 其中任意一点记为$ {}_{\mathop{{{\rm{B}}}}\nolimits} {{{p}}_n} \in {{\cal P}_i} $; IMU在$ \left[ {{t_i},{t_{i + 1}}} \right] $内采集的数据为$ {{\cal I}_{\left( {i,i{\rm{ + }}1} \right)}} $, 由于IMU输出频率高于Lidar, $ {{\cal I}_{\left( {i,i{\rm{ + }}1} \right)}} $中包含了$ N $$ {{\rm{B}}} $下的加速度和角速度$ \left[ {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right),{}_{\mathop{{{\rm{B}}}}\nolimits} {{{\tilde{ \omega }}}_{WB}}\left( {t_i^n} \right)} \right],n \in {\bf N} $. 系统在$ {t_i} $时刻的状态包括姿态, 位置, 速度和IMU零偏

      $$ {{x}}_i = \left[ {{{{R}}_i},{{{t}}_i},{{{v}}_i},{{{b}}_i}} \right]. $$ (1)

      其中, 位姿变换构成特殊欧式群[26]$ \left[ {{{{R}}_i},{{{t}}_i}} \right] \in SE\left( 3 \right) $; 速度为欧式空间下的三维向量$ {{{v}}_i} \in {{\mathbb{R}}^3} $; 零偏项由陀螺仪零偏$ {{b}}_i^g \in {{\mathbb{R}}^3} $和加速度计零偏$ {{b}}_i^a \in {{\mathbb{R}}^3} $构成${{{b}}_i} =$$ \left[ {{{b}}_i^g,{{b}}_i^a} \right] \in {{\mathbb{R}}^6} $. 需要说明的是, 本文侧重于研究系统的定位方法, 因此地图点不作为状态量进行优化, 以保证后端优化的运算效率. 此外, 所有Lidar的状态量过于庞大, 且部分状态量十分冗余(如: Lidar静止), 若将其全部作为状态量进行优化, 一方面很快就会超出计算机的运算能力, 另一方面也会造成运算资源浪费. 因此, 在数据处理中仅选取具有代表性的关键帧作为优化估计的状态量. 定义$ {{\cal K}_k} $$ {t_k} $时刻的所有关键帧, 其对应的状态量为${{\cal X}_k} = {\left\{ {{{{{x}}}_i}} \right\}_{i \in {{\cal K}_k}}} $; 其对应的所有观测量为$ {{\cal Z}_k} = {\left\{ {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}} \right\}_{\left( {i,j} \right) \in {{\cal K}_k}}} $.

      在这些假设和定义下, 状态估计问题可描述为: 在给定观测信息$ {\cal Z}_k $和先验信息$ {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right) $的条件下, 估计$ {\cal X}_0 $的后验概率问题, 即

      $$ \begin{split}& {\mathop{p}\nolimits} \left( {{{\cal X}_k}\left| {{{\cal Z}_k}} \right.} \right) \propto {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right){\mathop{p}\nolimits} \left( {{{\cal Z}_k}\left| {{{\cal X}_k}} \right.} \right)= \\ & {\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)\prod\limits_{\left( {i,j} \right) \in {{\cal K}_k}} {{\mathop{p}\nolimits} \left( {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}\left| {{{\cal X}_k}} \right.} \right)} \end{split}. $$ (2)

      由于观测量已知, 在联合概率分布中将其作为参数而非随机变量. 根据马尔可夫性质, $ {{\cal P}_i} $仅与$ {t_i} $时刻的状态$ {{{x}}_i} $有关, 则式(2)可分解为

      $$ \begin{split} &{{\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)} \prod \limits_{\left( {i,j} \right) \in {{\cal K}_k}} {\mathop{p}\nolimits} \left( {{{\cal P}_i},{{\cal I}_{\left( {i,j} \right)}}\left| {{{\cal X}_k}} \right.} \right)= \\ &{\mathop{p}\nolimits} \left( {{{\cal X}_0}} \right)\prod\limits_{\left( {i,j} \right) \in {{\cal K}_k}} {\mathop{p}\nolimits} \left( {{{\cal I}_{\left( {i,j} \right)}}\left| {{{{{x}}}_i},{{{{x}}}_j}} \right.} \right) \prod\limits_{i \in {{\cal K}_k}} {{\mathop{p}\nolimits} \left( {{{\cal P}_i}\left| {{{{{x}}}_i}} \right.} \right)} \end{split}. $$ (3)

      变量因子的最大后验概率(Maximum A Posteriori, MAP)可由式(3)推得

      $$ \begin{aligned} & {{\hat {\cal X}}_k}= \mathop {\arg \min }\limits_{{{\cal X}_k}} \left( \!\!{ - \ln {\mathop{p}\nolimits} \left( {{{\cal X}_k}\left| {{{\cal Z}_k}} \right.} \right)} \right) =\\ & \mathop {\arg \min }\limits_{{{\cal X}_k}} \left( \begin{array}{l} \!\! \left\| {{{{{r}}}_0}} \right\|_{{{{\Sigma }}_0}}^2 \!\!+\!\! \sum\limits_{\left( {i,j} \right) \in {{\cal K}_k}} \!\!{\left\| {{{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}}} \right\|_{{{{\Sigma }}_{{{\cal I}_{\left( {i,j} \right)}}}}}^2} \\ \!\!+\!\! \sum\limits_{i \in {{\cal K}_k}} {\left\| {{{{{r}}}_{{{\cal P}_i}}}} \right\|_{{{{\Sigma }}_{{{\cal P}_i}}}}^2} \!\!+\!\! \sum\limits_{\left( {i,j} \right) \in {{\cal K}_k}} \!\!{\left\| {{{{{r}}}_{{{\cal P}_{\left( {i,j} \right)}}}}} \right\|_{{{{\Sigma }}_{{{\cal P}_{\left( {i,j}\!\!\!\!\!\! \right)}}}}}^2} \end{array} \right) \end{aligned}, $$ (4)

      式中$ {{r}} $表示观测模型与实际观测的残差, 是关于状态量$ {\cal X}_k $的函数, $ {{\Sigma }} $为对应的协方差矩阵. 后文将对式(4)中各项的具体形式做详细说明.

    • (1) 点云数据处理

      本文采用前期工作[25]中的运动模型估计方法校正点云, 即根据预积分得到的相对运动估计将点云统一至同一时刻载体坐标系下. 图2为KITTI数据集[10]的处理实例. 点云的特征提取主要分为以下步骤:

      图  2  点云分割示例

      Figure 2.  Example of point cloud segmentation

      step1 将无序的点云$ {\cal{P}} $转为有序的深度图$ \mathop{{D}}\limits_{r \times c} $, 以提升点云搜索速度. $ \mathop{{D}}\limits_{r \times c} $的行数$ r $为Lidar线阵的行数, 列数$ c $根据深度图水平角分辨率设定. $ \mathop{{D}}\limits_{r \times c} $中每个像素对应一个或几个点云中的点, 像素值取最远点的深度值. 如图2(c), 深度图大小为$ 64 \times 860 $, 每个像素约包含5个点.

      step2 不同于LeGO-LOAM[11]中的地面点分割方法, 本文采用基于角度图像的分割策略. 首先根据垂直方向和水平方向的坐标差 $ \Delta x $$ \Delta z $计算深度图上同一列相邻行的两点$ A $$ B $间的垂直夹角$ \alpha $(如2(d)), 构成夹角图像, 并对其采用Savitsky-Golay滤波算法平滑处理得到如2(e)所示的平滑后夹角图像$ \mathop {{A}}\limits_{\left( {r - 1} \right) \times c} $. 对于车载点云, 一般可以认为满足$ \alpha $接近于0的像素是地面点所对应的区域; 采用广度优先算法(Breadth-First Search, BFS)算法在$ \mathop {{A}}\limits_{\left( {r - 1} \right) \times c} $上搜索小于阈值的点, 将其标记为地面点$ \mathop {{G}}\limits_{r \times c} $.

      step3 对剔除地面点后的深度图进行目标分割. 当某一激光光束$ {{OP}} $与两激光点连线$ {{PQ} }$所构成的夹角较小时, 认为两激光点$ {{P}} $$ {{Q}} $位于不同目标上. 循环搜索生成如2(f)的分割图像$ \mathop {{L}}\limits_{r \times c} $, 并将$ \mathop {{L}}\limits_{r \times c} $中点数较少的目标作为野点剔除.

      step4 将地面和目标分割后的部分作为特征图$ \mathop {{F}}\limits_{r \times c} = \mathop {{G}}\limits_{r \times c} + \mathop {{L}}\limits_{r \times c} $, 并在其上进行特征提取, 提取方法与[8]中相同. 根据各点深度值$ {d_i} $计算某一点在其同行邻域点$ {\cal S} $中的粗糙度$ \theta $:

      $$ \theta = \frac{1}{{\left| {\cal S} \right| \cdot \left\| {{d_i}} \right\|}} \cdot \left\| {\sum\limits_{j \in {\cal S},j \ne i} {\left( {{d_j} - {d_i}} \right)} } \right\|, $$ (5)

      并将$ \theta $较大的非地面点标记为边缘点$ {\cal P}^E $, 将$ \theta $较小的标记为平面点$ {\cal P}^F $. 从$ {\cal P}^E $中选取$ n_E $$ \theta $最大的点构成$ {{\cal P}^{{E_{max}}}} \in {{\cal P}^E} $, 从$ {\cal P}^F $中选取$ n_F $$ \theta $最小且为地面点的点构成$ {{\cal P}^{{F_{min}}}} \in {{\cal P}^F} $.

      (2) IMU数据处理

      IMU的观测模型为:

      $$ {}_{\mathop{{{\rm{B}}}}\nolimits} {{\tilde{ \omega }}_{WB}}\left( {t_i^n} \right) = {}_{\mathop{{{\rm{B}}}}\nolimits} {{{\omega }}_{WB}}\left( {t_i^n} \right) + {{{b}}^g}\left( {t_i^n} \right) + {{{\eta }}^g},\qquad \quad \;\;\; \;$$ (6)
      $$ {}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) = {{R}}_{WB}^{\top} \left( {t_i^n} \right)\left( {{}_W{{a}}\left( {t_i^n} \right) - {}_W{{g}}} \right) + {{\bf{b}}^a}\left( {t_i^n} \right) + {{\bf{\eta }}^a}. $$ (7)

      其中,$ {{\eta }} \sim {\cal N}\left( {{\bf{0}},{{\Sigma }}} \right) $表示观测噪声, $ {{R}}_{WB}^{\top} $$ {{\rm{B}}} $$ {{\rm{W}}} $的旋转矩阵, $ {}_W{{g}} $为重力加速度. 根据IMU的动力学模型对$ {}_{\mathop{{{\rm{B}}}}\nolimits} {{\bf{\omega }}_{WB}}\left( {t_i^n} \right) $$ {}_W{\bf{a}}\left( {t_i^n} \right) $在IMU采样间隔时间$ \delta t $内积分:

      $$ \begin{array}{l} {{{R}}_{WB}}\left( {t_i^{n + 1}} \right) = {{{R}}_{WB}}\left( {t_i^n} \right) \cdot \\ \quad \exp \left[ {{{\left( {\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {{{\tilde{ \omega }}}_{WB}}\left( {t_i^n} \right) - {{{b}}^g}\left( {t_i^n} \right) - {{{\eta }}^g}} \right) \cdot \Delta t} \right)}^ {\wedge} }} \right] \end{array}, \;\;\;$$ (8)
      $$ \begin{array}{l} {}_W{{v}}\left( {t_i^{n + 1}} \right) = {}_W{{v}}\left( {t_i^n} \right) + {}_W{{g}} \cdot \Delta t+ \\ {{{R}}_{WB}}\left( {t_i^n} \right)\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) - {{{b}}^a}\left( {t_i^n} \right) - {{{\eta }}^a}} \right) \cdot \Delta t \end{array}, \qquad \quad $$ (9)
      $$ \begin{array}{l} {}_W{{t}}\left( {t_i^{n + 1}} \right) = {}_W{{t}}\left( {t_i^n} \right) + {}_W{{v}}\left( {t_i^n} \right) \cdot \Delta t +\displaystyle \frac{1}{2}{}_W{{g}} \cdot \Delta {t^2}\\ \displaystyle \quad + \frac{1}{2}{{{R}}_{WB}}\left( {t_i^n} \right)\left( {{}_{\mathop{{{\rm{B}}}}\nolimits} {\tilde{ a}}\left( {t_i^n} \right) - {{{b}}^a}\left( {t_i^n} \right) - {{{\eta }}^a}} \right) \cdot \Delta {t^2} \end{array}. $$ (10)

      由于IMU输出频率很高, 状态估计时若直接将IMU采样时刻对应的全部位姿作为变量节点插入因子图进行优化, 无疑是不现实的[21]. 通常通过预积分处理, 将高频输出的加速度和角速度观测量转化为状态量间的位姿变换, 构成关键帧状态量之间的约束因子[23], 从而将所有IMU观测量转化为一个预积分观测量, 传感器采样频率与IMU因子关系如图3.

      图  3  IMU与Lidar的频率关系

      Figure 3.  Frequencies of IMU and Lidar

      根据式(8)-(10)定义关键帧$ {{{x}}}_i $$ {{{x}}}_j $间的运动增量:

      $$ \begin{aligned} \Delta {{{R}}_{ij}} &= {{R}}_i^{\top} {{{R}}_j}\\ &= \prod\limits_{k = i}^{j - 1} {\exp \left( {{{\left( {\left( {{{{\tilde{ \omega }}}_k} - {{b}}_k^g - {{{\eta }}^g}} \right) \cdot \Delta t} \right)}^ {\wedge} }} \right)} \end{aligned}, $$ (11)
      $$ \begin{split}& \Delta {{{v}}_{ij}} = {{R}}_i^{\top} \left( {{{{v}}_j} - {{{v}}_i} - {{g}} \cdot \Delta {t_{ij}}} \right) = \\ &\sum\limits_{k = i}^{j - 1} {\Delta {{{R}}_{ik}}\left( {{{{\tilde{ a}}}_k} - {{b}}_k^a - {{{\eta }}^a}} \right) \cdot \Delta t} \end{split}, \qquad \quad \;\;\;\;$$ (12)
      $$ \begin{aligned}& \Delta {{{t}}_{ij}} = {{R}}_i^{\top} \left( {{{{t}}_j} - {{{t}}_i} - {{{v}}_i}\Delta {t_{ij}} -\displaystyle \frac{1}{2}\sum\limits_{k = i}^{j - 1} {{{g}}\Delta {t^2}} } \right)= \\ & \sum\limits_{k = i}^{j - 1} {\left( {\Delta {{{v}}_{ik}}\Delta t +\displaystyle \frac{1}{2}\Delta {{{R}}_{ik}}\left( {{{{\tilde{ a}}}_k} - {{b}}_k^a - {{{\eta }}^a}} \right)\Delta {t^2}} \right)} \end{aligned}. $$ (13)

      为方便书写保证公式简洁, 上式省略了左下标的坐标系标注, 并将$ \left( \cdot \right) \left( t_i \right) $简写为$ {\left( \cdot \right)_i} $; 其中$ \Delta {t_{ij}} =$$ \sum\limits_{k = i}^{j - 1} {\Delta t} $. 假设关键帧$ {{{{x}}}_i} $$ {{{{x}}}_j} $间的IMU零偏保持不变, 则可由式(11)-(13)可得预积分观测模型[13]:

      $$ \Delta {{\tilde{ R}}_{ij}} = {{R}}_i^{\top} {{{R}}_j}\exp \left( {\delta \phi _{ij}^ {\wedge} } \right) \qquad \qquad \qquad \quad \;\;\;\;$$ (14)
      $$ \Delta {{\tilde{ v}}_{ij}} = {{R}}_i^{\top} \left( {{{{v}}_j} - {{{v}}_i} - {{g}} \cdot \Delta {t_{ij}}} \right) + \delta {{{v}}_{ij}} \qquad \quad$$ (15)
      $$ \Delta {{\tilde{ t}}_{ij}} = {{R}}_i^{\top} \left( {{{{t}}_j} - {{{t}}_i} - {{{v}}_i}\Delta {t_{ij}} - \frac{1}{2}{{g}}\Delta {t_{ij}}^2} \right) + \delta {{{t}}_{ij}} $$ (16)

      上式中$ {\left[ {\begin{array}{*{20}{c}} {\delta \phi _{ij}^{\top}}&{\delta {{v}}_{ij}^{\top}}&{\delta {{t}}_{ij}^{\top}} \end{array}} \right]^{\top}} $均为随机变量, 其协方差矩阵$ {{\bf{\Sigma }}_{ij}} $可由误差传播定律推导. 由式(14)-(16)即可直接列出式(4)中的残差项${{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} = $$ {\left[ {\begin{array}{*{20}{c}} {{{{r}}}_{\Delta {{{R}}_{ij}}}^{\top}}&{{{{r}}}_{\Delta {{{v}}_{ij}}}^{\top}}&{{{{r}}}_{\Delta {{{t}}_{ij}}}^{\top}} \end{array}} \right]^{\top}} $.

    • 激光里程计模块利用连续的特征点云序列$ \left\{ {{\cal P}_{k + 1}^{{E_{max}}},{\cal P}_{k + 1}^{{F_{min}}}} \right\} $, $ \left\{ {{\cal P}_k^E,{\cal P}_k^F} \right\} $和IMU预积分结果$ \int {{{\cal I}_{\left( {k,k{\rm{ + }}1} \right)}}} $估计载体的相对运动, 输出频率与Lidar的采样频率保持一致.

      首先, 利用$ \int {{{\cal I}_{\left( {k,k{\rm{ + }}1} \right)}}} $$ \left\{ {{\cal P}_{k + 1}^{{E_{max}}},{\cal P}_{k + 1}^{{F_{min}}}} \right\} $进行坐标变换, 使之与$ \left\{ {{\cal P}_k^E,{\cal P}_k^F} \right\} $坐标系统一. 分别在$ {\cal P}_k^E $$ {\cal P}_k^F $中搜索$ {\cal P}_{k + 1}^{{E_{max}}} $$ {\cal P}_{k + 1}^{{F_{min}}} $的匹配点. ${}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i}\in $$ {\cal P}_{k + 1}^{{E_{max}}} $的匹配点由最邻近点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} \in {\cal P}_k^E $和次临近点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} \in {\cal P}_k^E $组成, 可利用KD树搜索得到. 点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} $到直线$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) $的距离$ {{d}}_E $为:

      $$ {{{{d}}}_E} = \frac{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}} \right) \times \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right)} \right\|}}{{\left\| {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right\|}}. $$ (17)

      $ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{\bf{p}}_i} \!\in\! {\cal P}_{k + 1}^{{F_{max}}} $的匹配点由$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}\! \in\! {\cal P}_k^F $, $ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} \!\in\! {\cal P}_k^F $$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m} \in {\cal P}_k^F $组成, 点$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} $到平面$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right) $的距离$ {{{{d}}}_F} $为:

      $$ \begin{array}{l} {{\bf{d}}_F} = \\ \frac{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j}} \right) \cdot \left( {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) \times \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right)} \right)} \right\|}}{{\left\| {\left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) \times \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} - {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right)} \right\|}} \end{array}. $$ (18)

      其中$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j} $为利用KD树搜索到的目标点最邻近点, $ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l} $为相邻线上的最邻近点, 而$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m} $为同一线上的次邻近点. 由于$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_i} \in {{\cal P}_k} $$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} \in {{\cal P}_{k + 1}} $存在三维变换关系:

      $$ {}_{{{\mathop{{{\rm{B}}}}\nolimits} _{k + 1}}}{{{p}}_i} = {{\tilde{ R}}_{\left( {k + 1,k} \right)}} \cdot {}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_i} + {}_{{B_k}}{{\tilde{ t}}_{\left( {k + 1,k} \right)}}, $$ (19)

      因此, 将式(19)分别带入式(17)和式(18), 得:

      $$ f\left( {{{{\tilde{ R}}}_{\left( {k + 1,k} \right)}},{}_{{B_k}}{{{\tilde{ t}}}_{\left( {k + 1,k} \right)}}} \right) = {{{d}}}. $$ (20)

      $ {{d}} $最小, 利用Levenberg-Maquardt算法即可求解旋转量和平移量$ {{\bar{ T}}_{\left( {k + 1,k} \right)}} = \left[ {{{{\bar{ R}}}_{\left( {k + 1,k} \right)}},{}_{{B_k}}{{{\bar{ t}}}_{\left( {k + 1,k} \right)}}} \right] $的估值.

      需要说明的是: 1)为提高配准精度, 搜索匹配点时, 点对$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l}} \right) $$ \left( {{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_j},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_l},{}_{{{\mathop{{{\rm{B}}}}\nolimits} _k}}{{{p}}_m}} \right) $在分割图像$ \mathop {{L}}\limits_{r \times c} $中应属于同一目标, 否则舍弃该匹配. 2) 结合UGV特点和地面点云的几何属性, 采用LeGO-LOAM中的两步法估计载体的6DoF运动. 先利用 $ {\cal P}_{k + 1}^{{F_{min}}} $估计载体的横滚角, 俯仰角及垂直方向的平移量, 将估值带入后再利用$ {\cal P}_{k + 1}^{{E_{max}}} $估计载体的偏航角及水平方向平移量. 地面点云对横滚角, 俯仰角及垂直方向具有很好的约束, 而对偏航角及水平方向平移量没有约束, 非地面点云与之相反. 经测试验证, 采用这种方法可以加快配准迭代的收敛速度, 在达到相同配准精度的前提下, 计算速度提升35%[11].

    • 地图构建模块通过将特征点云$ \left\{ {{\cal P}_{k + 1}^{{E_{max}}},{\cal P}_{k + 1}^{{F_{min}}}} \right\} $与地图$ {{\cal M}_k} $配准, 优化激光雷达里程计模块估计的位姿$ {{\bar{ T}}_{\left( {k + 1,k} \right)}} = \left[ {{{{\bar{ R}}}_{\left( {k + 1,k} \right)}},{}_{{B_k}}{{{\bar{ t}}}_{\left( {k + 1,k} \right)}}} \right] $, 同时更新地图$ {{\cal M}_{k + 1}} $. 由于地图配准需要占用大量运算资源, 为保证实时性, 该模块低频运行. 点到地图的配准方法与上节相同.

      LOAM和LeGO-LOAM中并未定义局部地图, 只是根据垂直角可视范围, 以传感器为中心搜索与索引特征点可能存在匹配的地图点. 这种粗略的搜索方法过于依赖阈值: 搜索得到的地图点过多, 则配准效率低下; 若地图点过少, 则可能造成地图点与特征点对周围环境表达的不一致, 部分可观测的地图点缺失, 导致配准精度下降.

      [27][28]类似, 本文提出采用以传感器为中心的多尺度地图模型建立局部地图$ {{\cal M}_k} $, 并定义$ {{\cal M}_k} $的参考系与当前关键帧一致$ {{{m}}_k} = {{{{x}}}_i} $, 地图构建模块每运行一次, 便插入一个关键帧. 局部地图共分三层, 各层栅格大小不一, 相互嵌套. 栅格中的点存储于环形容器中, 保持局部地图中点个数不变, 从而保持地图配准模块的处理速度相对恒定. 点坐标保存为点在栅格内的相对位置, 以便于局部地图坐标系变化时点坐标更新. 局部地图的示意图如图4, 大小不一的三种栅格分别表示不同分辨率的格网地图; 栅格中的点表示该格网中存储的点云; 当载体运动时局部地图随之移动, 局部地图的中心由$ {{{m}}_{k - 1}} $的原点$ {O_{old}} $$ {{{m}}_{k}} $的原点$ {O_{new}} $移动, 局部地图向前进方向添加栅格, 并剔除远离方向的栅格.

      图  4  局部地图示意图

      Figure 4.  Demonstration for the local map

      这种局部地图表示方法有两个特点: 1) 由于局部地图始终保持以载体为中心, 使得地图中的点大部分位于Lidar的可视范围内, 避免了对不可视地图点的搜索. 2) 多分辨率格网地图更符合Lidar放射状采样的性质, 近处点云密集, 而远处相对稀疏. 随着移动Lidar的移动高分辨率栅格内的点逐渐增加并稳定, 能够更准确的刻画环境的真实面貌, 避免地图点与特征点不一致性的产生. 这也是与其他格网地图表达点云方法[28, 27]的最主要区别.

    • 闭合回环模块检测载体的轨迹是否形成闭合, 即是检测载体是否回到之前的位置. 若构成闭环则利用GICP算法将当前的特征点云与闭环处的特征点云配准, 得到相对位姿关系$ {\bar{ T}} $, 构成闭环约束因子, 并将其插入因子图中. 闭环检测根据当前关键帧位置与其余关键帧间的距离判断: 将关键帧列表$ {{\cal X}_k} = {\left\{ {{{{{x}}}_i}} \right\}_{i \in {{\cal K}_k}}} $保存于KD树中, 以半径$ R $搜索当前关键帧$ {{{{x}}}_{{{\cal K}_{k + 1}}}} $的相邻关键帧, 并根据采样时间判断是否为相邻时刻的关键帧.

      本文提出采用大闭环与小闭环结合的方式, 大闭环表示机器人回到之前的位置, 可以建立闭环约束修正全局误差累计; 而小闭环则表示间隔关键帧之间的共视关系, 可以为转角位置提供更多约束, 从而提升激光里程计对Lidar旋转运动的鲁棒性.

    • 因子图优化模块在系统中维护一个全局因子图, 因子图结构如图5. 图中右侧为系统实际运行时构建的因子图, 左侧是对其结构的抽象化说明. 假设给定初始状态$ {{\cal X}_0} $, 对应式(4)中的$ \left\| {{{{{r}}}_0}} \right\|_{{{{\Sigma }}_0}}^2 $. 地图构建模块向因子图插入配准因子, 对应式(4)中的$ \left\| {{{{{r}}}_{{{\cal P}_i}}}} \right\|_{{{{\Sigma }}_{{{\cal P}_i}}}}^2 $:

      图  5  因子图结构

      Figure 5.  Structure of the factor graph

      $$ {{{{r}}}_{{{\cal P}_i}}} = \left( {{{{{x}}}_i} \ominus {{{m}}_{i - 1}}} \right) \ominus {{\bar{ T}}_{\left( {i,i - 1} \right)}}, $$ (21)

      数据预处理模块计算相邻关键帧之间的IMU预积分, 并向因子图插入IMU预积分因子, 对应式(4)中的$ \left\| {{{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}}} \right\|_{{{{\Sigma }}_{{{\cal I}_{\left( {i,j} \right)}}}}}^2 $:

      $$ {{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} = \left( {{{{{x}}}_i} \ominus {{{{x}}}_j}} \right) \ominus \int {{{\cal I}_{\left( {i,j} \right)}}} , $$ (22)

      其中$ {{{{r}}}_{{{\cal P}_i}}} $$ {{{{r}}}_{{{\cal I}_{\left( {i,j} \right)}}}} $是一一对应的. 闭环检测模块向因子图插入闭环因子, 对应式(4)中的$ \left\| {{{\bf{r}}_{{{\cal P}_{\left( {i,j} \right)}}}}} \right\|_{{{{\Sigma }}_{{{\cal P}_{\left( {i,j} \right)}}}}}^2 $:

      $$ {{{{r}}}_{{{\cal P}_{\left( {i,j} \right)}}}} = \left( {{{\bf{x}}_i} \ominus {{{{x}}}_j}} \right) \ominus {{\bar{ T}}_{\left( {i,j} \right)}} , $$ (23)

      式中$ \ominus $表示$ {\mathfrak{s}}{\mathfrak{e}}\left( 3 \right) $中位姿变换的逆运算. 每当因子图中插入新的节点, 就对整个因子图优化计算一次, 更新当前时刻的位姿估计, 实现点云数据和IMU数据的深度融合. 公式推导参考[13].

    • 为客观全面地评估本文所提方法的性能, 共进行两组实验: 1) 采用[11]公开的两组数据集1#和3#对系统运行速度、闭环优化效果进行评价和对比; 2) 分别采用[11]公开的数据集2#和实测数据, 根据回到起始点的位姿偏差定量评估系统定位精度. 数据采集平台为搭载了Xsens MTi-G-710 IMU和Velodyne VLP-16 Lidar的地面移动机器人(如图6), 数据采集频率分别为200 Hz和10 Hz, 两传感器采用软同步方式进行时间对齐. 数据采集环境为室内地下停车场和室外开阔广场(如图10(c)(d)), 数据采集时间约为1000 $ s $, 平台运动速度约为1 $ m/s $, 总距离约为1 $ km $. 系统采用c++实现, 在机器人操作系统(Robot Operation System, ROS)中运行. 小型智能地面机器人的数据处理器为4G内存并搭载Intel i5 1.8 GHz CPU的终端(如图6).

      图  6  数据采集平台

      Figure 6.  Data collection platform

      图  10  Inertial-LOAM轨迹及建图结果

      Figure 10.  Trajectory and mapping result of Inertial-LOAM

      在数据融合前, 需要对系统进行初始化, 其主要目的是: 1) 获得IMU与Lidar之间的相对位姿$ {{T}}_L^B $, 即传感器外参数; 2) 计算载体系$ {\rm{B}} $下的重力加速度$ {}_B{{g}} $, 从而得到载体坐标系$ {\rm{B}} $到世界坐标系$ {\rm{W}} $的转换关系$ {{T}}_B^W $. 结合本文对坐标系统的定义, 在本实验中, $ {} _B{{g}} $采用[23]中所述方法计算得到, 而后将其旋转至$ {\rm{W}} $系下得到旋转关系$ {{R}}_B^W $; 为保证精度, Lidar/IMU外参数$ {{T}}_L^B $的标定在结构性较好且设置了特殊靶标的实验室预先进行, 数据处理中作为已知量. 基本原理是分别采用激光雷达里程计和IMU预积分得到一段时间内相邻状态间的约束$ {}_L{{C}}_i^{i + 1} $$ {}_B{{C}}_i^{i + 1} $, 从而构建关于$ {{T}}_L^B $的优化方程:

      $$ {\hat{ T}}_L^B = \mathop {\arg \min }\limits_{{{T}}_L^B} \left( {{{\left( {{}_B{{C}}_i^{i + 1}} \right)}^{ - 1}} \cdot {{T}}_L^B \cdot {}_L{{C}}_i^{i + 1}} \right). $$ (24)

      用特殊欧式群[26]$ \left[ {{{{R}}_i},{{{t}}_i}} \right] \in SE\left( 3 \right) $表达位姿变换, 即可在流形上利用L-M算法求解外参的估值$ {\hat{ T}}_L^B $. 一般而言, 传感器外参数在数据采集过程中保持不变, 因此在后续数据处理中可将其作为固定量.

    • 分别统计LeGO-LOAM和本文Inertial-LOAM对1#和3#两组数据集的单帧数据处理时间, 包括数据预处理, 激光里程计, 地图构建, 优化计算等各模块及系统总体的单帧处理时长(如图7), 比较并分析两系统数据处理的速度和效率.

      图  7  系统运行时间对比

      Figure 7.  Comparison of time cost of two systems

      从图中可以看出: 两系统总处理时间的95%以上都小于Lidar的采样间隔0.1$ s $, 说明系统满足实时处理的要求; 由于Inertial-LOAM采用多分辨率局部地图, 地图构建模块避免了对全局地图点的搜索, 因此平均处理速度更快. 而在数据预处理时, 由于增加了IMU数据的预积分计算, 该模块处理时间略有增加; 另外, Inertial-LOAM的优化计算时间并未明显增加, 说明融合IMU数据后并未过多增加系统的计算负担.

      根据闭环优化前后的地图偏差定性评价闭环优化对系统定位和建图精度的提升作用, 1#和3#两组数据闭环优化前后所构建的地图及轨迹如图7中(a)和(b). 从图7(a)可以明显看出, 闭环优化可以有效处理大闭环造成的累计误差, 对轨迹精度和地图一致性的提升具有重要作用; 图7(b)的激光里程计误差累计不明显, 因此闭环优化作用效果不甚显著. 采用Inertial-LOAM方法和[11]中1#和3#数据, 构建的地图和轨迹的结果如图10(a)(b).

    • 在IMU和Lidar数据融合时, 对传感器观测量的信任度由观测量的质量决定. 因此需要预先标定IMU, 统计其阿伦方差量, 以确定对角速度和加速度观测值的信任程度.

      实验时, 在起点附近架设高精度全站仪, 并在机器人上粘贴标志点. 每当测量平台回到起点附近时, 用高精度全站仪观测标志点, 并以其位姿变换作为相对变换的参考值$ {}_G{{T}} $. 估计的位姿$ {}_W{{{x}}} $的精度由累计误差的均方根(Root Mean Square Error, RMSE)表示:

      $$ {{{e}}_n} = \left( {{}_W{{{{x}}}_n} \ominus {}_W{{{{x}}}_0}} \right) \ominus {}_{\mathop{{\rm{G}}}\nolimits} {{T}}, $$ (25)
      $$ {\mathop{{\rm{RMSE}}}\nolimits} \left( {{{{e}}_{1:n}}} \right) = {\left( {\frac{1}{N}\sum\limits_{n = 1}^N {{{\left\| {{{{e}}_i}} \right\|}^2}} } \right)^{\frac{1}{2}}}. $$ (26)

      其中, $ \ominus $表示位姿变换的逆运算. 采用起终点相同的2#数据, 分别对Cartographer[12], LeGO-LOAM[11], LOAM[8], IMU和本文所提出的Inertial-LOAM(以下分别简称为C, LL, L, I, 和IL)进行精度对比, 并说明IMU在数据融合中的作用; 采用实测的室内停车场、室外开阔广场数据对比IL, LL, L和C的精度, 分析本文方法的性能和优势. 误差累计结果如表1. 需要说明的是, 为客观评价系统漂移程度, 实验结果均未进行闭环优化, LL, L和C均为加入IMU观测后的结果, 误差累计均为各系统处理5次的平均值.

      表 1  累计误差结果

      Table 1.  Error accumulation result

      场景方法横滚(°)俯仰(°)航向(°)角度偏差(°)X方向(m)Y方向(m)Z方向(m)位置偏差(m)
      2#数据[11]IMU0.7481.0180.5981.39835.09584.652−665.782672.059
      Cartographer0.113−0.7090.9891.2220.4051.3170.6701.532
      LOAM0.0160.1410.9250.9360.3160.3490.0250.471
      LeGO-LOAM0.0610.0810.9160.9210.0680.3380.1150.364
      Inertial-LOAM0.0130.0260.9170.9180.0610.2580.0230.266
      室内环境Cartographer0.003−0.0010.0170.0170.0230.0370.0280.052
      LOAM0.0010.0040.0680.0680.0320.0830.0320.095
      LeGO-LOAM−0.006−0.002−0.0210.0220.0160.047−0.0320.059
      Inertial-LOAM−0.0080.001−0.0200.0210.0210.0430.0270.055
      室外环境Cartographer0.075−0.0240.0810.1131.7472.592−0.4493.158
      LOAM−0.0310.0060.0960.1010.04672.368−0.0652.353
      LeGO-LOAM−0.024−0.5430.0410.545−19.857−14.914−0.35524.836
      Inertial-LOAM0.006−0.0800.0030.080−0.310−0.100−0.0300.328

      图  8  闭环优化效果

      Figure 8.  Performance of loop optimization

      2#数据的采集环境与3#相同, 特征丰富但机器人运动较快. 由表1可以看出, 仅利用IMU积分的位姿推估误差累计严重, 无法得到正确的结果. 因此, 在后续实验中不再将其作为对比对象. 尽管IMU位置漂移严重, 但姿态偏差相对较小. 说明IMU能提供一个较好的姿态约束, 而这正是Lidar里程计所缺少的, 间接反映了激光里程计与IMU耦合的优势.

      在室外特征丰富但运动较快的情况下, L、LL和IL均能得到分米级的定位结果而C精度较差. LL和IL在数据预处理阶段对特征点云进行了目标分割, 剔除了大量主要特征以外的野点, 避免其对配准精度的影响, 因此较L的精度有所提升, 其中IL最优. 由于C将三维点云投影至二维格网中, 它对类似墙面等结构性较强的环境有较好的适应性, 而室外诸如树木、车辆等特征不能保证较好的配准精度. 因此, 即便在前端融合了IMU数据, 最终仍产生较大误差.

      在相对狭小的室内环境中, C、LL与IL的位姿估计精度基本都在0.05$ m $左右, 其中C方法最优, 较L提升近一倍, 四种方法的定位精度均能达到厘米量级. 由于室内相对狭小, 路面平坦, 且包含大量的墙面及边沿线等明显的结构特征, 点云配准精度高, 在IMU/Lidar数据融合解算时占据更大权重, 因此IMU对定位结果的作用较小. 室内环境中IL地图和轨迹结果如图10(c).

      在室外开阔且运动相对平缓的条件下, LL随运行时间增加, 误差累计愈发严重, 无法得到可靠的位姿估计结果; L和C性能优于LL, 但仍存在一定误差累计. 在不采用闭环优化的前提下, IL通过融合IMU信息, 显著降低定位漂移, 精度仍能达到分米量级. 四者轨迹和地图结果对比如图9. 室外空间较为开阔且结构性特征较少, 路面不够平整造成移动机器人运动时产生剧烈的震动, 因此点云包含较多噪点. 由于未在后端优化中融入IMU观测约束, 且LL为了适应轻量级终端, 提取的特征点云较为稀疏, 导致配准误差较大, 出现明显的误差累计; 尽管L的运行效率不及LL, 但相对稠密的点云保证了其配准的精度, 但轨迹仍不可避免的存在漂移; 虽然C在前端融合了IMU数据, 但本质上只是利用融合的数据构建更为精确的局部地图, 以此保证更准确的前端匹配, 导致数据融合的作用在后端优化中体现不明显, 不足以弥补连续配准引起的轨迹发散问题. 本文提出的IL通过充分融合Lidar和IMU的观测信息对轨迹进行整体优化, 有效降低了轨迹漂移, 提升了位姿估计的精度. 从图9的对比中还可以看出, 融合IMU信息后系统在快速直角转弯处的表现显著提升, 反映了IMU良好的姿态约束的作用. 采用室外数据和IL方法所构建的地图和轨迹结果如图10(d).

      图  9  室外开阔环境IL/LL/L/C轨迹结果对比

      Figure 9.  Comparison of pose estimation of IL/LL/L/C in outdoor environment

    • 本文面向小型智能平台, 针对现有的Lidar/IMU SLAM方法数据融合不充分, 对开阔环境和快速旋转鲁棒性差, 地图构建缺乏一致性的问题, 提出了一种Lidar/IMU紧耦合的实时定位方法—Inertial-LOAM. 首先在数据预处理阶段采用地面分割效果更好的基于夹角图像的地面点云分割方法, 并对IMU数据进行预积分处理. 其次, 在地图构建阶段定义了一个以传感器为中心的MRS局部地图, 提升地图匹配的速度和精度. 最后, 在系统中增加因子图优化模块, 对Lidar和IMU观测数据进行整体优化, 实现多源观测值的紧密耦合.

      采用实测数据对系统进行全面测试和评估, 可以得到以下结论:

      (1) IMU预积分计算略微增加了数据预处理时间, 同时IMU预积分因子的加入使因子图结构更为复杂, 但整体而言对系统的运算负担增加不大, 可以满足实时性要求; 另外由于定义了MRS局部地图, 避免了对全局地图的搜索, 地图构建模块速度更快.

      (2) 闭合回环对系统精度的提升具有重要作用. 通过对整体误差的修正, 能够有效降低误差累计对定位结果带来的影响, 提升构建地图的一致性.

      (3) 在结构特征不明显的开阔室外环境, LeGO-LOAM等方法无法进行可靠的位姿估计, 而Inertial-LOAM仍能得到准确的定位结果; 同时, 在快速直角转弯处, Inertial-LOAM较依赖点云配准的LeGO-LOAM具有更为稳定可靠的表现.

参考文献 (28)

目录

    /

    返回文章
    返回