吳明月

激光雷達(dá)和IMU緊耦合SLAM算法

吳明月

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

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

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

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

1 算法框架

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

圖1 算法框架

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

2.1 點(diǎn)云預(yù)處理

2.1.1 點(diǎn)云去畸變

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

因?yàn)镮MU可以高頻率輸出位姿信息，所以采用IMU提供的點(diǎn)云起始時(shí)刻到結(jié)束時(shí)刻的位姿信息對(duì)該幀點(diǎn)云進(jìn)行運(yùn)動(dòng)補(bǔ)償。

2.1.2 特征提取

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

2.2 IMU預(yù)積分

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

2.3 激光里程計(jì)

2.3.1 初值計(jì)算

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

2.3.2 基于iKD-Tree的局部地圖

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

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

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

2.3.3 點(diǎn)云配準(zhǔn)

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

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

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

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

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

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

圖2 位姿融合輸出

3 實(shí)驗(yàn)結(jié)果與分析

Kitti數(shù)據(jù)集測(cè)評(píng)如下：

3.1 實(shí)驗(yàn)環(huán)境搭建

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

3.2 iKD-Tree實(shí)時(shí)性評(píng)定

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

表1 iKD-Tree 與KD-Tree耗時(shí)對(duì)比

3.3 回環(huán)檢測(cè)配準(zhǔn)算法評(píng)定

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

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

表2 Fast-GICP與ICP對(duì)比

4 結(jié)論

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

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

[2] 周治國(guó),曹江微,邸順帆.3D激光雷達(dá)SLAM算法綜述[J].儀器儀表學(xué)報(bào),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－），男，碩士研究生，研究方向?yàn)榧す釹LAM，E-mail:1423513196@qq.com。