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

                    •  收稿日期?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
                    • 基金項目:  地理信息工程國家重點實驗室基金(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
                      360彩票
                    • [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]. 自動化學報
                    • 加載中
                    計量
                    • 文章訪問數:  4431
                    • HTML全文瀏覽量:  2621
                    • 被引次數: 0
                    出版歷程
                    • 收稿日期:  2019-06-02
                    • 錄用日期:  2019-12-15
                    • 網絡出版日期:  2020-01-16

                    Lidar/IMU緊耦合的實時定位方法

                    doi: 10.16383/j.aas.c190424
                      基金項目:  地理信息工程國家重點實驗室基金(SKLGIE2018-M-3-1), 國家重點研發計劃(2017YFF0206001), 國家自然科學基金(41501491)資助
                      作者簡介:

                      戰略支援部隊信息工程大學地理空間信息學院博士研究生. 2015年獲中南大學測繪工程學士學位, 2018年獲戰略支援部隊信息工程大學控制科學與工程碩士學位. 主要研究方向為多傳感器融合的SLAM, 移動測量. 本文通信作者. E-mail: lsx_navigation@sina.com

                      戰略支援部隊信息工程大學地理空間信息學院教授、博導. 1987年獲解放軍測繪學院測繪科學與技術碩士學位. 主要研究方向為精密工程與工業測量, 導航應用及導航定位與位置服務. E-mail: guangyun_li@163.com

                    •  收稿日期?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

                    摘要: 本文以實現移動小型智能化系統的實時自主定位為目標, 針對激光里程計誤差累計大, 旋轉估計不穩定, 以及觀測信息利用不充分等問題, 提出一種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

                    李帥鑫, 李廣云, 王力, 楊嘯天. 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
                    • 載體利用所搭載傳感器的觀測信息, 在未知環境中進行實時自主定位并增量式構建環境地圖的能力是無人系統的關鍵技術之一[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},{{}_i}} \right]. $$ (1)

                        其中, 位姿變換構成特殊歐式群[26]$ \left[ {{{{R}}_i},{{{t}}_i}} \right] \in SE\left( 3 \right) $; 速度為歐式空間下的三維向量$ {{{v}}_i} \in {{\mathbb{R}}^3} $; 零偏項由陀螺儀零偏$ {}_i^g \in {{\mathbb{R}}^3} $和加速度計零偏$ {}_i^a \in {{\mathbb{R}}^3} $構成${{}_i} =$$ \left[ {{}_i^g,{}_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) + {{}^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}^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) - {{}^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) - {{}^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) - {{}^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} - {}_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} - {}_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} - {}_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具有更為穩定可靠的表現.

                    WeChat 關注分享

                    返回頂部

                    目錄

                      /

                      返回文章
                      返回