多分辨率去動(dòng)點(diǎn)的IMU激光雷達(dá)定位方法*

楊智宇，陸佳紅，杜 力，鞠 浩

(重慶工商大學(xué) 制造裝備機(jī)構(gòu)設(shè)計(jì)與控制重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400067)

0 引 言

多模態(tài)傳感器融合測(cè)量在各個(gè)領(lǐng)域都起著至關(guān)重要的作用。在自動(dòng)駕駛領(lǐng)域，利用復(fù)雜三維立體空間環(huán)境信息精準(zhǔn)構(gòu)建三維地圖是自動(dòng)駕駛汽車感知系統(tǒng)的重要基礎(chǔ)。同步定位與建圖(simultaneous localization and mapping，SLAM)[1]是三維建圖中的主流算法。環(huán)境感知主要依賴于激光雷達(dá)(light detection and ranging，LiDAR)、相機(jī)和慣性測(cè)量單元(inertial measurement unit，IMU)等傳感器實(shí)現(xiàn)。雖然基于視覺(jué)的SLAM可運(yùn)用于特征信息缺失的場(chǎng)景識(shí)別，但由于初始化、光照和對(duì)范圍的敏感性使得其在單獨(dú)用于自主導(dǎo)航系統(tǒng)時(shí)并不可靠[2]。相反地，利用LiDAR的方法可以在很大程度上避免光照的影響，并且LiDAR地圖相比于通用的視覺(jué)特征點(diǎn)加描述子地圖有更好的穩(wěn)定性。

LOAM(LiDAR odometry and mapping in real-time)[3]是激光SLAM領(lǐng)域最經(jīng)典、應(yīng)用最廣泛的方法。然而,LOAM將點(diǎn)云數(shù)據(jù)直接存儲(chǔ)于全局體素地圖，無(wú)法執(zhí)行回環(huán)檢測(cè)以修正漂移。LeGO-LOAM(lightweight and ground-optimized LiDAR odometry and mapping on variable terrain)[4]利用邊緣點(diǎn)和平面點(diǎn)建立相鄰2次掃描關(guān)系，優(yōu)化后得到位姿變換矩陣，但幀間匹配算法中未能考慮空間移動(dòng)物體特征點(diǎn)對(duì)特征提取和匹配的影響，導(dǎo)致位姿估計(jì)精度降低。

事實(shí)上，LiDAR在空曠環(huán)境等會(huì)出現(xiàn)退化現(xiàn)象。通常利用提高精度構(gòu)建的復(fù)雜算法避免環(huán)境信息缺失，但卻犧牲系統(tǒng)實(shí)時(shí)性能，最終導(dǎo)致建圖累積誤差過(guò)大的問(wèn)題[5]。融合IMU能夠彌補(bǔ)單LiDAR長(zhǎng)時(shí)間定位精度和魯棒性能差的缺陷。然而，兩者數(shù)據(jù)類型不同和復(fù)雜的環(huán)境信息成為融合SLAM算法的難點(diǎn)[6]。如何降低動(dòng)態(tài)環(huán)境中由關(guān)鍵幀匹配誤差導(dǎo)致的定位和建圖累積誤差成為當(dāng)前LiDAR SLAM的研究熱點(diǎn)[7,8]。

針對(duì)上述問(wèn)題，本文創(chuàng)新性地引入了地面平面擬合算法分割地面和利用多分辨率的區(qū)域圖像去除移動(dòng)物體的方法，提出了一種高精度自適應(yīng)多分辨率IMU三維LiDAR SLAM算法。利用地面平面擬合方法分割地面點(diǎn)云，并用自適應(yīng)多分辨率區(qū)域圖像的方法去除動(dòng)態(tài)物體特征點(diǎn)，同時(shí)構(gòu)建靜態(tài)空間約束，用于提高幀間匹配精度，從而提高位姿估計(jì)精度。為驗(yàn)證該方法的可行性，在自主搭建的車載平臺(tái)上，進(jìn)行了模擬小車和實(shí)車測(cè)試。

1 算法概述

算法整體分為4大部分：

1)數(shù)據(jù)預(yù)處理

預(yù)處理包括：計(jì)算IMU預(yù)積分量及畸變矯正、計(jì)算點(diǎn)云去畸變和聚類、實(shí)現(xiàn)地面分割。提取IMU前后幀加速度和角速度數(shù)據(jù)，作為IMU預(yù)積分的數(shù)據(jù)輸入計(jì)算IMU預(yù)積分。在融合算法中，利用LiDAR和IMU之間的外參標(biāo)定進(jìn)行迭代優(yōu)化，并確定旋轉(zhuǎn)變換矩陣。

2)特征提取與匹配

計(jì)算曲率標(biāo)記點(diǎn)云角點(diǎn)、平面點(diǎn)。利用地面平面擬合方法分割地面點(diǎn)和非地面點(diǎn)，高精度處理地面存在坡度情況。提取靜態(tài)點(diǎn)云線特征構(gòu)建點(diǎn)云特征匹配估計(jì)位姿靜態(tài)點(diǎn)云約束，增強(qiáng)LiDAR位姿估計(jì)精度。

3)位姿估計(jì)

通過(guò)IMU原始數(shù)據(jù)計(jì)算增量數(shù)據(jù)，由IMU預(yù)積分計(jì)算得到的每一時(shí)刻IMU增量位姿，并且利用最近一幀LiDAR里程計(jì)位姿構(gòu)建當(dāng)前時(shí)刻IMU里程計(jì)位姿。將當(dāng)前時(shí)刻IMU里程計(jì)位姿賦給激光里程計(jì)，得到當(dāng)前時(shí)刻LiDAR里程計(jì)的位姿。因此，構(gòu)建匹配關(guān)系和因子圖優(yōu)化后的位姿共同作用得到當(dāng)前幀LiDAR位姿。

4)因子圖優(yōu)化[9]

在優(yōu)化環(huán)節(jié)，利用優(yōu)化因子包含LiDAR里程計(jì)因子、IMU預(yù)積分因子和回環(huán)因子的聯(lián)合優(yōu)化方案來(lái)得到車體的全局一致的位姿。在因子圖中輸入數(shù)據(jù)位姿、速度、偏置等數(shù)據(jù)，根據(jù)理論計(jì)算模型優(yōu)化位姿。

算法整體流程框圖如圖1所示。

圖1 算法概述

2 算法過(guò)程

2.1 點(diǎn)云特征提取

利用前后5個(gè)點(diǎn)，計(jì)算點(diǎn)距離差之和的平方值作為曲率的度量，依據(jù)平滑度區(qū)分角點(diǎn)和平面點(diǎn)。曲率計(jì)算公式如下

(1)

2.2 平面擬合地面分割

采用平面模型來(lái)擬合當(dāng)前地面。通常，現(xiàn)實(shí)生活中地面并非完全平整，而且在測(cè)量距離較遠(yuǎn)時(shí),LiDAR會(huì)存在一定的測(cè)量噪聲，單一的平面模型并不能完整擬合現(xiàn)實(shí)復(fù)雜的非結(jié)構(gòu)路況[10]。采用平面模型擬合當(dāng)前地面可處理存在一定坡度的地面情況，實(shí)現(xiàn)高精度的地面分割。將空間平面沿車頭分為若干個(gè)子平面，然后對(duì)子平面進(jìn)行地面平面擬合算法,得到處理陡坡的地面分割方法。構(gòu)建初始平面模型，如下

ax+by+cz+d=0

(2)

即

nTx=-d

(3)

式中x=[x,y,z]T，代表空間平面坐標(biāo)系；n=[a,b,c]T，通過(guò)初始點(diǎn)集協(xié)方差C∈R3×3矩陣來(lái)求解；d為平面模型常數(shù)項(xiàng)。采用種子點(diǎn)集S∈R3作為初始點(diǎn)集，其協(xié)方差矩陣為

(4)

計(jì)算點(diǎn)云中每個(gè)點(diǎn)到平面的正交投影的距離，將該值與設(shè)定的閾值比較，當(dāng)高度小于設(shè)定的閾值，則該點(diǎn)屬于地面點(diǎn)；反之，則為非地面點(diǎn)。經(jīng)分類后將地面點(diǎn)作為下一次迭代的種子點(diǎn)集，迭代優(yōu)化。

2.3 IMU預(yù)積分因子

IMU預(yù)積分是基于相鄰兩幀點(diǎn)云之間的IMU積分測(cè)量值計(jì)算及其動(dòng)態(tài)更新。通過(guò)IMU原始數(shù)據(jù)計(jì)算增量數(shù)據(jù)，由IMU預(yù)積分計(jì)算得到每一時(shí)刻IMU增量位姿。具體通過(guò)獲得每周期位置、速度、姿態(tài)(position,velocity,quaternion, PVQ)增量的測(cè)量值，對(duì)照其他通過(guò)非IMU方式獲得的PVQ增量估計(jì)值，進(jìn)而獲得PVQ增量的殘差[11]。

根據(jù)IMU角速度和加速度測(cè)量值，可以算出車體姿態(tài)、速度和位置增量

(5)

(6)

(7)

上述表達(dá)式即為PVQ增量測(cè)量值(含IMU測(cè)量值和偏差估計(jì)值)與真值之間的關(guān)系。提供一種IMU預(yù)積分約束，利用IMU零偏實(shí)現(xiàn)系統(tǒng)對(duì)于位姿計(jì)算估計(jì)優(yōu)化。

2.4 區(qū)域圖像去動(dòng)態(tài)點(diǎn)

本文通過(guò)構(gòu)建多分辨率的區(qū)域圖像使用投影的區(qū)域圖像的可見(jiàn)性進(jìn)行移動(dòng)點(diǎn)識(shí)別。首先，使用Fk+1表示當(dāng)前關(guān)鍵幀，Mk表示對(duì)應(yīng)的子地圖，該子地圖由Fk+1周圍的滑動(dòng)窗口創(chuàng)建。將此刻LiDAR點(diǎn)云分成2個(gè)互斥的子集，動(dòng)態(tài)點(diǎn)集D和靜態(tài)點(diǎn)集S。因此，當(dāng)前時(shí)刻子地圖和關(guān)鍵幀具體表示為

(·)D∩(·)S=?

(8)

(9)

(10)

τ=γdist(p)

(11)

系統(tǒng)可將自適應(yīng)閾值τ設(shè)為閾值內(nèi)的最大值，可有效避免由于錯(cuò)誤的分辨率預(yù)測(cè)而將靜態(tài)點(diǎn)當(dāng)成動(dòng)態(tài)點(diǎn)。

2.5 LiDAR里程計(jì)因子

如前所述，基于IMU的預(yù)積分因子和激光里程計(jì)因子，可獲得比較準(zhǔn)確的狀態(tài)估計(jì)和建圖。

3 實(shí)驗(yàn)平臺(tái)搭建與測(cè)試分析

本文借助于測(cè)試工具EVO進(jìn)行算法性能測(cè)試實(shí)驗(yàn)，主要通過(guò)精度指標(biāo)RPE以及ATE對(duì)算法性能評(píng)估。實(shí)驗(yàn)室硬件設(shè)備為具有16GBRAM的Intel?CoreTMi7—1070處理器和NVIDIA GeForcen RTK 2060圖形處理器的臺(tái)式機(jī)，系統(tǒng)環(huán)境為基于Ubuntu18.04的機(jī)器人操作系統(tǒng)(robot operation system,ROS)。

使用上述所用設(shè)備對(duì)校區(qū)新北區(qū)域場(chǎng)地展開(kāi)實(shí)地算法性能檢測(cè)實(shí)驗(yàn)。分別使用LOAM算法、A-LOAM算法、LeGO-LOAM算法與本文算法進(jìn)行解析比較。

3.1 硬件平臺(tái)搭建

融合SLAM系統(tǒng)主要圍繞LiDAR和IMU兩大傳感器展開(kāi)理論研究和實(shí)驗(yàn)測(cè)試。LiDAR坐標(biāo)系和IMU坐標(biāo)系都使用左手坐標(biāo)系(右X，前Y，上Z)，其坐標(biāo)軸對(duì)應(yīng)關(guān)系如圖2所示。

圖2 LiDAR和IMU的坐標(biāo)軸指向

IMU通過(guò)串口將6軸信息傳輸?shù)脚_(tái)式電腦主機(jī)。LiDAR通過(guò)統(tǒng)一有線數(shù)據(jù)端口將點(diǎn)云數(shù)據(jù)傳輸給主機(jī)進(jìn)行數(shù)據(jù)傳輸。

3.2 數(shù)據(jù)測(cè)試與分析

在實(shí)車驗(yàn)證時(shí)，首先,需要提前進(jìn)行內(nèi)LiDAR和慣導(dǎo)的外參標(biāo)定。本文使用ROS節(jié)點(diǎn)錄制LiDAR和慣導(dǎo)聯(lián)合標(biāo)定數(shù)據(jù)包，利用瑞士蘇黎世聯(lián)邦理工大學(xué)自動(dòng)駕駛實(shí)驗(yàn)室貢獻(xiàn)的標(biāo)定工具進(jìn)行外參標(biāo)定[14]。

其次，編譯和運(yùn)行相關(guān)文件。錄制相關(guān)節(jié)點(diǎn)的數(shù)據(jù)包，利用可視化工具RVIZ實(shí)現(xiàn)點(diǎn)云數(shù)據(jù)可視化,利用pcl工具保存pcd地圖，便于實(shí)時(shí)應(yīng)用和增強(qiáng)現(xiàn)實(shí)存儲(chǔ)和處理點(diǎn)云數(shù)據(jù)集。

3.2.1 KITTI官方數(shù)據(jù)集驗(yàn)證

通過(guò)在不同的官方數(shù)據(jù)集下，使用本文算法與多種定位和建圖方法，計(jì)算并比較回到初始原點(diǎn)后的相對(duì)平移誤差的大小驗(yàn)證算法的精度，如表1所示。

表1 各種方法回到原點(diǎn)后的相對(duì)平移誤差 m

利用包含LiDAR和慣性里程計(jì)的3個(gè)官方的數(shù)據(jù)集West,Outdoor,Walking，多層次，多場(chǎng)景的全面測(cè)試在同一數(shù)據(jù)集下不同建圖方法在回到原點(diǎn)的相對(duì)平移誤差。由表1得知，利用本文算法實(shí)時(shí)定位建圖，回到原點(diǎn)后的相對(duì)偏移誤差與LeGO-LOAM相比分別減小77.7 %,54.7 %,88.7 %，大幅度提升定位精確度。

3.2.2 實(shí)車數(shù)據(jù)集驗(yàn)證

利用實(shí)驗(yàn)室室內(nèi)和校園場(chǎng)景地圖的建立和可視化證明本文算法的魯棒性和精度;用不同場(chǎng)景和對(duì)比其他算法證明本文算法的魯棒性和精度。

1)自建室內(nèi)數(shù)據(jù)集

在室內(nèi)環(huán)境中，構(gòu)建三維點(diǎn)云地圖，如圖3所示，從地面平整、漂移、墻面貼合度來(lái)看是比較好的。

圖3 室內(nèi)點(diǎn)云建圖

2)自建校園數(shù)據(jù)集

圖4為校園高德衛(wèi)星圖和各種方法和本文方法的點(diǎn)云建圖結(jié)果?？梢钥闯?，后兩者都有很好的建圖效果，且軌跡都與真實(shí)軌跡相同。實(shí)驗(yàn)結(jié)果表明：在大地平整及特征信息豐富的路況下，后兩者均有較好的算法效果能夠滿足實(shí)際應(yīng)用中的精度需求。

圖4 重慶工商大學(xué)新北區(qū)域點(diǎn)云結(jié)果

將點(diǎn)云地圖通過(guò)pcl工具轉(zhuǎn)換為PCD地圖，如圖5所示。利用實(shí)車運(yùn)行本文算法繞南岸校區(qū)新北區(qū)域指定范圍1圈得到運(yùn)行軌跡如圖5(c)所示。

圖5 新北區(qū)域點(diǎn)云PCD地圖及點(diǎn)云完整軌跡

然后，根據(jù)華策組合導(dǎo)航GPS定位得到相應(yīng)路線的真實(shí)軌跡，利用開(kāi)源LeGO-LOAM算法和本文算法得到的軌跡分別進(jìn)行比較，如圖6所示。

圖6 算法軌跡對(duì)比

可從3個(gè)軌跡對(duì)比看出，利用本文算法得到的軌跡圖與真實(shí)軌跡更接近，定位精度效果更好，并通過(guò)表2中與絕對(duì)軌跡誤差對(duì)比得到進(jìn)一步驗(yàn)證。

表2 絕對(duì)軌跡誤差

本文實(shí)驗(yàn)結(jié)果分析表明，使用地面平面擬合方法去地面，使用投影區(qū)域圖的可見(jiàn)性進(jìn)行移動(dòng)點(diǎn)識(shí)別并去除動(dòng)態(tài)物體。與傳統(tǒng)算法LeGO-LOAM相比，誤差的均方根誤差減小51.9 %，標(biāo)準(zhǔn)差減少79.8 %，通過(guò)提高點(diǎn)云匹配精度從而提高定位的精度。

4 結(jié) 論

本文利用地面平面擬合方法分割并標(biāo)記地面點(diǎn)云，并用區(qū)域圖的方法去除動(dòng)態(tài)物體特征點(diǎn)，提高幀間匹配精度。通過(guò)構(gòu)建靜態(tài)空間約束進(jìn)行位姿估計(jì)，提高定位精度。建立幀到局部地圖的匹配代替?zhèn)鹘y(tǒng)的幀到全局地圖匹配，提高了幀圖匹配的效率。利用因子圖聯(lián)合優(yōu)化方案來(lái)得到車體的全局一致位姿，提高位姿估計(jì)精確度。通過(guò)官方數(shù)據(jù)集和校園環(huán)境下實(shí)車驗(yàn)證表明：相比于傳統(tǒng)算法，本文算法計(jì)算絕對(duì)軌跡誤差標(biāo)準(zhǔn)差減小79.8 %，具有更高的精度和魯棒性，能夠有效減少移動(dòng)物體點(diǎn)云對(duì)匹配精度的影響，提高系統(tǒng)定位精度。