吳明月

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

吳明月

（天津職業(yè)技術(shù)師范大學 汽車與交通學院，天津 300222）

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

激光雷達；因子圖優(yōu)化；IMU；緊耦合；SLAM算法

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

1 算法框架

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

圖1 算法框架

2 關(guān)鍵技術(shù)

2.1 點云預處理

2.1.1 點云去畸變

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

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

2.1.2 特征提取

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

2.2 IMU預積分

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

2.3 激光里程計

2.3.1 初值計算

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

2.3.2 基于iKD-Tree的局部地圖

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

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

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

2.3.3 點云配準

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

2.4 基于Fast-GICP的回環(huán)檢測

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

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

2.5 基于因子圖的全局位姿優(yōu)化

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

圖2 位姿融合輸出

3 實驗結(jié)果與分析

Kitti數(shù)據(jù)集測評如下：

3.1 實驗環(huán)境搭建

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

3.2 iKD-Tree實時性評定

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

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

3.3 回環(huán)檢測配準算法評定

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

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

表2 Fast-GICP與ICP對比

4 結(jié)論

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

[1] 蔡英鳳,陸子恒,李祎承,等.基于多傳感器融合的緊耦合SLAM系統(tǒng)[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。