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

(重慶工商大學(xué) 制造裝備機構(gòu)設(shè)計與控制重慶市重點實驗室，重慶 400067)

0 引 言

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

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

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

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

1 算法概述

算法整體分為4大部分：

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

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

2)特征提取與匹配

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

3)位姿估計

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

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

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

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

圖1 算法概述

2 算法過程

2.1 點云特征提取

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

(1)

2.2 平面擬合地面分割

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

ax+by+cz+d=0

(2)

即

nTx=-d

(3)

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

(4)

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

2.3 IMU預(yù)積分因子

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

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

(5)

(6)

(7)

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

2.4 區(qū)域圖像去動態(tài)點

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

(·)D∩(·)S=?

(8)

(9)

(10)

τ=γdist(p)

(11)

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

2.5 LiDAR里程計因子

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

3 實驗平臺搭建與測試分析

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

使用上述所用設(shè)備對校區(qū)新北區(qū)域場地展開實地算法性能檢測實驗。分別使用LOAM算法、A-LOAM算法、LeGO-LOAM算法與本文算法進行解析比較。

3.1 硬件平臺搭建

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

圖2 LiDAR和IMU的坐標軸指向

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

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

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

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

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

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

表1 各種方法回到原點后的相對平移誤差 m

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

3.2.2 實車數(shù)據(jù)集驗證

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

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

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

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

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

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

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

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

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

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

圖6 算法軌跡對比

可從3個軌跡對比看出，利用本文算法得到的軌跡圖與真實軌跡更接近，定位精度效果更好，并通過表2中與絕對軌跡誤差對比得到進一步驗證。

表2 絕對軌跡誤差

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

4 結(jié) 論

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