激光雷達和IMU緊耦合SLAM算法

吳明月

激光雷達和IMU緊耦合SLAM算法

吳明月

（天津職業技術師范大學 汽車與交通學院，天津 300222）

近年來，無人駕駛領域成為廣泛關注的熱點方向，同時定位與地圖構建（SLAM）技術是高精地圖創建和無人車輛導航的基礎，當下主流的激光SLAM算法基本能夠滿足應用的需求，但是在大范圍場景建圖的過程中仍然存在漂移的問題，且算法輕量化以及實時性方面依舊有著改進的空間。文章主要進行了激光雷達和慣性測量單元（IMU）緊耦合的同時定位與建圖算法研究，前端部分主要對激光點云數據進行了去除畸變、特征提取，后端使用因子圖融合IMU預積分因子、激光里程計因子和回環檢測因子進行融合位姿輸出。為了提高算法的實時性，文章使用iKD-Tree數據結構維護了一個局部地圖，并使用Fast-GICP算法求解回環檢測位姿變換。在Kitti公開數據集的測試表明，改算法在保證精度的同時提高了算法的實時性和魯棒性。

激光雷達；因子圖優化；IMU；緊耦合；SLAM算法

同時定位與地圖構建（Simultaneous Localiza- tion And Mapping, SLAM）技術是無人車輛實現自主導航的基本前提，主要分為視覺SLAM和激光SLAM。相較于視覺SLAM，激光SLAM不受照明變化的影響，更適合直接捕獲3D空間中環境的細節。但在大場景建圖中僅使用激光雷達會導致較大的漂移，于是添加更多的傳感器進行優化成為廣泛的共識[1-2]。在過去二十年中，人們提出了許多基于激光雷達的SLAM方法。由ZHANG等[3]在2014年提出的實時狀態估計和建圖的激光里程計和建圖（Lidar Odometry And Mapping, LOAM）方法是應用較廣泛的方法之一。LOAM使用激光雷達和慣性測量單元（Inertial Measurement Unit, IMU），在Kitti公開數據集測試榜上一直被列為頂級的基于激光雷達識別方法。但LOAM仍存在一些局限性，因其核心是基于掃描匹配的方法，沒有回環檢測，在特征豐富的環境或者大范圍建圖中性能有所下降。2018年SHAN等[4]提出了一種可以部署到嵌入式平臺，且面向復雜情況的輕量級優化地面雷達里程計和建圖（Lightweight and Groud Optimized-Lidar Odometry And Mapping, Le GO-LOAM）算法，其添加了地面約束，且通過對地面點云的分割降低了運算量。但它是一種松耦合的慣性激光里程計，在大環境中其建圖精度并不理想。2019年YE等[5]提出了一種激光-慣性里程計和建圖（Laser Inertial Odoemtry and Mapping, LIOM）方法，在建圖精度上相較于松耦合方法有明顯提升。由于該算法基于滑動窗口進行優化，隨著窗口參數的增加，影響了算法的實時性且誤差會逐漸累積。2020年SHAN等[6]提出了基于佐治亞理工學院平滑與地圖（Georgia Tech Smooth- ing And Mapping, GTSAM）優化庫的慣導和激光雷達緊耦合激光雷達慣性里程計（Laser Inertial Odoemtry-Smoothing And Mapping, LIO-SAM）。通過定義觀測量作為因子圖因子構建優化模型，并采取了關鍵幀到局部地圖匹配的方式，且支持添加全球定位系統Global Positioning SystemGPS數據作為絕對觀測，因此，該算法建圖精度得到了較大提升。但是隨著圖優化因子的增加會產生算法實時性降低的問題。基于此，本文算法使用了一種新的數據結構iKD-Tree進行局部地圖的管理，同時使用Fast-GICP（Generalized Iterative Closest Point）算法對回環檢測進行了優化。

1 算法框架

本文所研究的算法框架如圖1所示。本文算法采用了激光里程計同IMU里程計緊耦合的方式，在數據預處理階段，IMU數據參與點云去畸變。在激光里程計部分，使用特征點云進行匹配構建優化問題，IMU預積分通過與點云幀最優位姿融合作為迭代優化的初值求解位姿變換。后端基于因子圖優化方法并采用了回環檢測優化全局位姿，在全局最優位姿的基礎上，融合IMU里程計預測位姿，最后實現以IMU頻率輸出車輛位姿信息。

圖1 算法框架

2 關鍵技術

2.1 點云預處理

2.1.1 點云去畸變

由于傳統機械式激光雷達的一幀數據是其旋轉一周內形成的所有數據，在運動場景下，采集的原始激光點云數據存在著畸變，不能真實反映環境信息，因此，需要進行點云去畸變。

因為IMU可以高頻率輸出位姿信息，所以采用IMU提供的點云起始時刻到結束時刻的位姿信息對該幀點云進行運動補償。

2.1.2 特征提取

為提高計算效率，需要進行激光點云的特征提取。本文沿用了LOAM[7]方法，通過計算曲率和距離變化得到角點和面點。

2.2 IMU預積分

無人車輛搭載的六軸IMU可以通過加速度計和陀螺儀獲得原始加速度測量值a和角速度測量值w，但這些值中包含了大量的噪聲，考慮IMU的零偏不穩定性噪聲和測量噪聲之后，定義IMU的狀態為

2.3 激光里程計

2.3.1 初值計算

作為基于優化方案的點云匹配，初始值是非常重要的，一個好的初始值會幫助優化問題快速收斂，且避免局部最優解的情況。在系統的初始化階段，使用磁力計提供的位姿信息作為優化的先驗，在接收到IMU預積分提供的位姿增量后，加到上一幀激光里程計最佳位姿上去，作為當前幀迭代優化的先驗位姿估計。

2.3.2 基于iKD-Tree的局部地圖

以關鍵幀位置形成的點云建立iKD-Tree，根據最后一個關鍵幀的位姿，進行最近鄰搜索，提取半徑50 m內的關鍵幀。

根據查詢的結果，把這些點的位置存進一個點云結構中，最近10 s的關鍵幀也保存下來。為避免關鍵幀過多，做一個下采樣，確認每個下采樣后的點索引，根據篩選出來的關鍵幀進行局部地圖構建。

本模塊采用了增量式iKD-Tree進行局部地圖的創建和管理。除了有效的最近鄰搜索外，新的數據結構還支持增量地圖更新，以最小的計算成本進行動態重新平衡和對iKD-Tree進行降采樣操作。iKD-Tree通過維護范圍信息和惰性標簽進行刪除操作，采取并行重建的方式降低延遲，保證了主線程的實時性和準確性，相較于KD-Tree更為高效。由于延遲降低，該方法支持直接將關鍵幀對應的點云加入點云地圖，避免了點云轉換的操作，節約時間的同時，提高了復雜環境中掃描配準的魯棒性。

2.3.3 點云配準

使用當前幀與構建的局部地圖進行點云配準，遍歷當前幀并取出一個角點，將該點從當前幀通過初始的位姿變換到地圖坐標系。在角點局部地圖里面尋找距離當前點比較近的5個點，計算找到的5個點的均值并構建協方差矩陣，進行特征值分解驗證這5個點是否是一條直線。然后根據點的均值往兩邊拓展構成這條線的兩個端點，最大特征值對應的特征向量對應的就是直線的方向向量。求整個點到構建的兩個點形成直線的距離和方向，即殘差與殘差方向，計算殘差關于待求變量的雅可比矩陣，使用高斯牛頓下降法進行迭代優化，得到幀間位姿變換。點面約束基本同理。

2.4 基于Fast-GICP的回環檢測

回環檢測通過周期性匹配當前幀和歷史幀的特征點云，來確認無人車輛是否經過已經走過的位置，從而得到回環相對位姿關系進行全局位姿優化。根據當前關鍵幀與歷史幀間的歐式距離進行檢索，即將關鍵幀列表保存于iKD-Tree中，以半徑搜索當前關鍵幀的相鄰幀，并根據采樣時間判斷是否為相鄰時刻的關鍵幀。形成回環后，使用歷史幀周圍25幀點云構建局部地圖，與當前關鍵幀進行Fast-GICP算法匹配求解位姿變換。

Fast-GICP是一種體素化的廣義迭代最近點算法，用于快速、準確地進行三維點云配準。該方法擴展了GICP方法的體素化，避免了代價昂貴的最近鄰搜索，同時保持了回環檢測算法的精度。因其通過聚集體素中每個點的分布來估計體素分布，使回環檢測算法的實時性得到提升。

2.5 基于因子圖的全局位姿優化

本算法基于因子圖優化進行融合位姿輸出。首先將激光里程計因子和回環檢測因子加入融合框架當中得到點云幀間最佳位姿，再將優化后的位姿信息結合預積分因子糾正IMU零偏，最后使用IMU預積分得到的位姿同點云幀間最佳位姿融合得到IMU頻率的位姿估計。因子圖優化模型位姿融合輸出如圖2所示。

圖2 位姿融合輸出

3 實驗結果與分析

Kitti數據集測評如下：

3.1 實驗環境搭建

硬件配置為CPU-Interi5/16 G、運行內存為250 G SSD的筆記本電腦；操作系統為 Linux ubuntu 20.04；對應的機器人操作系統（Robot Operating System, ROS）為Noetic；測試數據集選用Kitti數據集。

3.2 iKD-Tree實時性評定

在實時性方面，iKD-Tree較KD-Tree降低了26%的耗時，如表1所示。

表1 iKD-Tree 與KD-Tree耗時對比

3.3 回環檢測配準算法評定

Fast-GICP算法較ICP（Iterative Closest Point）算法匹配得分更高，這意味著回環檢測位姿變換求解的魯棒性更好，且計算速度比ICP算法快了一個數量級，如表2所示。

整體來看，本文算法提高了算法的實時性和魯棒性。

表2 Fast-GICP與ICP對比

4 結論

針對單一激光傳感器SLAM算法在大范圍建圖過程中存在的軌跡漂移的問題，本文基于因子圖優化的激光雷達和IMU緊耦合的SLAM算法進行研究，在LIO-SAM算法的基礎上使用iKD-Tree和Fast-GICP方法對算法后端進行了改進。實驗證明，該算法在大場景建圖中相比于LIO-SAM有著更低的耗時，后期還應該進一步進行實車部署，對算法性能進行完善。

[1] 蔡英鳳,陸子恒,李祎承,等.基于多傳感器融合的緊耦合SLAM系統[J].汽車工程,2022,44(3):350-361.

[2] 周治國,曹江微,邸順帆.3D激光雷達SLAM算法綜述[J].儀器儀表學報,2021,42(9):13-27.

[3] ZHANG J,SINGH S.LOAM:Lidar Odometry andMapping in Real-time[J].Robotics:Science and Systems, 2014,2(9):1-9.

[4] SHAN T,ENGLOT B.LeGO-LOAM: Lightweight and Ground-optimized Lidar Odometry and Mapping on Variable Terrain[C]//2018 IEEE/RSJ InternationalConference on IR0S.Piscataway:IEEE,2018:4758-4765.

[5] YE H,CHEN Y,LIU M.Tightly Coupled 3D LidarInertial Odometry and Mapping[C]//2019 International Conference on Robotics and Automation.Piscataway: IEEE,2019:3144-3150.

[6] SHAN T,ENGLOT B,MEYERS D,et al.LIO-SAM: Tightly Coupled Lidar Inertial Odometry Vias Mooth- ing and Mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems.Pisca- taway:IEEE,2020:5135-5142.

[7] ELSEBERG J,MAGNENAT S,SIEGWART R,et al. Comparison of Nearest-neighbor--search Strategies and Implementations for Efficient Shape Registration [J].Journal of Software Engineering for Robotics,2012, 12:1-12.

Tightly Coupled SLAM Algorithm for Lidar and IMU

WU Mingyue

( School of Automobile and Transportation,Tianjin University of Technology and Education, Tianjin 300222, China )

In recent years, the field of unmanned driving has become a hot topic of widespread concern,and the same time,simultaneous localization and mapping(SLAM) technology is the basis of high-precision map creation and unmanned vehicle navigation, and the current mainstream laser SLAM algorithm can basically meet the needs of applications, but there is still a drift problem in the process of large-scale scene mapping, and there is still room for improvement in algorithm lightweight and real-time.In this paper, the simultaneous localization and mapping algorithm of lidar and inertial measurement unit (IMU) tightly coupled is mainly studied, and the front-end part mainly removes distortion and feature extraction of laser point cloud data, and the back-end uses factor map to fuse IMU pre-integration factor, laser odometry factor and loopback detection factor for fusion pose output. In order to improve the real-time performance of the algorithm, this paper uses the iKD-Tree data structure to maintain a local map, and uses the Fast-generalized iterative closest point (GICP) algorithm to solve the loopback detection pose transformation. The test of Kitti's public dataset shows that the proposed algorithm improves the real-time and robustness of the algorithm while ensuring accuracy.

Lidar;Factor graph optimization;IMU;Tight coupled;SLAM algorithm

U469.5

A

1671-7988(2023)21-21-04

10.16638/j.cnki.1671-7988.2023.021.005

吳明月（1995－），男，碩士研究生，研究方向為激光SLAM，E-mail:1423513196@qq.com。