IMU進行普通的3D SLAM能得到什么樣的結(jié)果呢？

1

背景

此次的優(yōu)化針對的場景是室內(nèi)小場景、室內(nèi)大場景和室外場景下運行3D SLAM時，z方向的誤差累積、以及十萬平米（≈400*400）建圖時回環(huán)檢測效率低下的問題，機器上使用的主要傳感器3D LiDAR有velodyne16線（或ouster16線），IMU使用的是阿路比-LPMS-IG1。那么在進行優(yōu)化前，進行普通的3D SLAM能得到什么樣“離譜“的結(jié)果呢？有如此一幅點云地圖（見圖1上）在俯瞰視角下彷佛一切”歲月靜好“，但在正視視角下其尾巴都翹上了天（見圖1下）：

圖1：開環(huán)室內(nèi)20x260米走廊場景（俯視圖：上，正視圖：下）

由圖1，我們可以看到在SLAM過程中x、y、yaw的狀態(tài)量估計還是基本準確的，在點云地圖上的邊界沒有出現(xiàn)明顯的結(jié)構(gòu)性問題，但是其在建圖開始（始于地圖最右側(cè)）后，隨著roll、pitch、z的誤差嚴重積累，在跑完這260+米的走廊后終點位置（地圖最左側(cè)）的z坐標與起始點位置的z坐標偏差了8.57米（實際偏差<2m）。再看另一個現(xiàn)象在一個廠區(qū)室外130x240的回環(huán)區(qū)域下進行3D SLAM，得出了如圖2所示的點云地圖：

圖2：閉環(huán)室外130x240米廠區(qū)場景（俯視圖：上，正視圖：下）

由圖2，還是可以得出SLAM過程中狀態(tài)量估計基本準確的狀態(tài)為x、y、yaw，而roll、pitch、z的誤差積累問題嚴重，在上圖中長邊（240米）左、右關(guān)鍵幀的位姿上z坐標間的差值有9.88米（實際偏差<2m）。

針對上面兩個例子中發(fā)現(xiàn)的問題，本人將簡單分析其誤差的產(chǎn)生的原因，并分享根據(jù)誤差產(chǎn)生原因的物理依據(jù)進行優(yōu)化設(shè)計和實現(xiàn)思路。

2

軌跡z方向飄移原因分析

在SLAM建圖過程中軌跡某個狀態(tài)量的不準確、在某個狀態(tài)量上的誤差積累本質(zhì)上都是算法設(shè)計時的假設(shè)與實際情況不符，且在進行SLAM問題建模和解決方案設(shè)計時沒有考慮到假設(shè)與真實情況的不一致性而導(dǎo)致的解算誤差。而在點云配準的階段，經(jīng)典點云配準算法ICP及其變體針對了各種環(huán)境、場景上出現(xiàn)的問題進行了各種各樣的算法設(shè)計以想要高效且魯棒的解決所有使用上的問題，但是z軸方向的飄移其實是由于z軸、roll和pitch這三個機器人狀態(tài)量在一定程度上不可觀測且難以評價和消除，雖然機器上使用了IMU和LiDAR傳感器進行慣性信息與環(huán)境信息的輸入，但軌跡中z方向的誤差來源還是主要集中在LiDAR的數(shù)據(jù)。

2.1、LiDAR靜態(tài)tf標定引入的誤差

一般來說機器人出廠后都需要進行一次詳細地標定工作，但即使再詳盡的標定工作多多少少都還是會留有一些誤差存在，另外隨著機器的使用機器上實際的LiDAR靜態(tài)tf也會因為機器的損耗逐漸與最初標定的數(shù)值發(fā)生偏差。以上圖1中場景點云數(shù)據(jù)為例，在獲取到一幀激光點云數(shù)據(jù)后直接進行靜態(tài)tf的變換，將其從LiDAR坐標系轉(zhuǎn)移到車體坐標系后可獲得俯視圖和正視圖如圖3所示：

圖3：室內(nèi)工廠單幀點云數(shù)據(jù)（俯視圖：上，正視圖：下）

由于該機器是剛出廠且完成了標定，可以由圖3看到當(dāng)機器人靜止水平時LiDAR返回的點云數(shù)據(jù)經(jīng)過tf變換后的roll、pitch誤差還是較小的。但是在另外一臺使用年限較長、經(jīng)過多次維修的機器人上做相同的實驗，roll、pitch的誤差就變得肉眼可見的大了（見圖4）。將上述兩幀點云分別與對應(yīng)的以LiDAR坐標系的點云進行ICP匹配以獲取靜態(tài)tf的真值，可以發(fā)現(xiàn)圖3中roll、pitch的誤差分別是0.011和0.008弧度，而圖4中的roll、pitch誤差分別是0.035和0.017弧度。

在上述分析的基礎(chǔ)上，若是進行純LiDAR的SLAM，一般情況下第一幀關(guān)鍵幀的位姿就是（0，0，0，0，0，0），而由于靜態(tài)tf的誤差存在其真實情況的關(guān)鍵幀并不完全水平，即使在后續(xù)的SLAM軌跡構(gòu)件中沒有其他問題發(fā)生，在開環(huán)情況下z軸的累積誤差就是：

z軸的累積誤差=(實際行走距離)×sin?(初始俯仰角誤差)

圖4：室外單幀點云數(shù)據(jù)（俯視圖：上，正視圖：下）

2.2、LiDAR豎直方向分辨率及入射角引入的誤差

首先從3D激光雷達的豎直方向分辨率開始，以常見的機械式激光雷達為例，即使是128線的3D激光雷達，其豎直方向上的分辨率也是小于水平方向的分辨率的，更何況16線的3D激光雷達。以Velodyne-VLP-16為例，

由于較低的豎直方向分辨率和豎直視場，以至于每一幀激光數(shù)據(jù)能檢測到地面的點云十分有限，且地圖特征遠遠不如水平視場探測到的角點、平面特征，例子可見圖3和圖4中的俯視圖，其中藍色標記的點云即為激光打到地面上的點云。

其次，由于機器人上3D激光雷達普遍的安裝方式（雷達水平安裝在機器人正上方）導(dǎo)致在SLAM構(gòu)建軌跡的過程中激光打到地面上的入射角很大（遠大于水平視場的垂直入射角），雷達入射角越大其產(chǎn)生的光斑也就越大，絕對精度和重復(fù)精度就越差。

上述的結(jié)論，除了可以通過各個雷達技術(shù)論文和技術(shù)參數(shù)手冊獲取，還有一個廣而知之的論證途徑就是LOAM中針對激光點云數(shù)據(jù)的預(yù)處理中定義了兩類異常點并進行了剔除，其中一類就是平行點（定義圖示見圖5），平行點指的就是圖5中的B點，就是激光的射線幾乎和物體的平面平行了，而LOAM中剔除這類點的原因有兩個：

1、 激光的數(shù)據(jù)會不準，射線被拉長

2、這種點被視為特征點后會非常不穩(wěn)定，下一幀可能就沒了，無法進行下一步的點云配準，例如圖5中的LiDAR原點再向左移一點，那么B點就消失了，與之形成對比的就是A點，極短時間內(nèi)不會消失。

圖5：LOAM中平行點定義圖示

由此，若是在SLAM系統(tǒng)中的配準使用的是點云層面的處理，而不是基于特征的匹配，則入射角很大處的地面點云由于絕對精度和重復(fù)精度差，就很有可能影響點云配準時roll、pitch的觀測估算了。

綜上，LiDAR豎直方向分辨率及入射角對SLAM過程中軌跡z軸的影響在于兩個方面，第一個就是由于豎直分辨率低導(dǎo)致地面點云及其特征少，特別是降采樣后可能都沒剩下多少可以用來點云配準了，第二個就是激光雷達打到地面上的點入射角通常較大，其點云精度差且不穩(wěn)定導(dǎo)致在點云配準時出現(xiàn)誤差。

3

SLAM軌跡z方向飄移問題優(yōu)化思路

以上的分析都是常規(guī)過程中可量化且較為統(tǒng)一的誤差來源分析，而其他的原因比如退化場景下配準算法失效引起的誤差、配準算法本身性能導(dǎo)致的誤差還有機器運算資源高壓條件下SLAM運算阻塞引發(fā)的位姿滯后誤差等不在本文中進行優(yōu)化的討論。下文中針對SLAM過程中軌跡z軸飄移的優(yōu)化思路還是針對上文中的靜態(tài)tf標定誤差、LiDAR豎直方向低分辨率及大入射角帶來的誤差。

3.1、靜態(tài)tf標定引入的誤差優(yōu)化

激光雷達坐標系與車體中心坐標系的靜態(tài)tf，首先會在SLAM開始階段引入一個初始位姿誤差，因為機器人在每一次開始SLAM時不一定是完全水平的，那么為了解決這個問題就需要利用IMU的數(shù)據(jù)進行重力方向估計和對每一個關(guān)鍵幀位姿進行重力方向校準，這部分的工作可以參考LIO-SAM（局部因子圖緊耦合的LIO）和VINS-Fusion（滑窗優(yōu)化下的VIO）中針對IMU的高頻慣性數(shù)據(jù)的處理。

圖6：VINS中的滑窗優(yōu)化示意圖

以LIO-SAM為例，可以考慮將SLAM前端模塊進行基于IMU的緊耦合局部因子圖的聯(lián)合優(yōu)化，與此同時完成針對IMU數(shù)據(jù)的初始化（外參及bias的初始估計）、IMU重力估計模塊和IMU預(yù)積分功能，局部因子圖的構(gòu)建主要節(jié)點還是基于點云配準后的關(guān)鍵幀，但此時關(guān)鍵幀之間會再多上一個重力方向校準的約束和IMU預(yù)積分計算出的慣性變換約束，以盡可能地減少靜態(tài)tf和點云配準時帶來的誤差，特別是IMU重力估計模塊，可以避免純LiDAR SLAM在roll、pitch的完全不可觀測問題，在很大程度上優(yōu)化z軸的飄移問題。

圖7：LIO-SAM的緊耦合LIO系統(tǒng)框圖

3.2、豎直方向低分辨率及大入射角帶來的誤差優(yōu)化

豎直方向低分辨率及大入射角帶來的誤差來源根據(jù)上文中的分析，可以歸結(jié)為兩個：第一，由于豎直分辨率低導(dǎo)致地面點云及其特征少，第二，激光雷達打到地面上的點入射角通常較大，其點云精度差且不穩(wěn)定。一言以蔽之，3D激光雷達獲取到的地面信息較少且獲取到的點云信息較差導(dǎo)致的觀測校正不夠理想，誤差由此被引入到SLAM軌跡中且難以在后續(xù)構(gòu)建中被消除。

既然點云的信息又少又差，我們避免直接使用點云信息進行配準操作不就行了嘛。還是先以LIO-SAM為例，在其系統(tǒng)的激光回調(diào)中完成了以下關(guān)鍵步驟：

（1）將當(dāng)前幀位姿初始化并隨后提取當(dāng)前幀的局部角點、平面點云集合，加入到局部map；

（2）將當(dāng)前激光幀角點、平面點集合降采樣后進行scan-to-map匹配，以優(yōu)化當(dāng)前幀的位姿；

（3）若當(dāng)前幀為關(guān)鍵幀則執(zhí)行因子圖優(yōu)化，更新因子圖中所有變量節(jié)點的位姿由此獲得優(yōu)化后的SLAM軌跡?？梢钥吹?，LIO-SAM的系統(tǒng)中沒有進行基于點云的配準，而是通過提取平面點進行基于特征的匹配優(yōu)化計算，而且特征匹配時要求當(dāng)前幀特征點數(shù)量需要足夠多且reference匹配的點數(shù)也足夠多才執(zhí)行優(yōu)化。

我們不妨根據(jù)上文中平面特征的特征匹配過程進一步擴展，平面點云特征不再單單作為用以前端關(guān)鍵幀位姿觀測更新的一部分，而是將其擴展成一個地面的約束（Ground Constrain）以避免z軸的飄移問題，而添加了地面約束后即是嘗試利用地面特征作為假設(shè)條件，在優(yōu)化時考慮地面約束帶來的殘差以影響優(yōu)化結(jié)構(gòu)，為了避免添加地面約束對上下坡場景的不友好，可以對地面約束進行打分計算，只要地面特征足夠“明顯“和地面約束足夠可靠，才在對應(yīng)的關(guān)鍵幀處添加地面約束的殘差塊。

另一個延申就是可以針對每一幀點云數(shù)據(jù)進行地面點的分割和標記，而這個分割后的結(jié)果可以不再像上文一樣單獨使用在前端的配準模塊中，而是進一步地被利用在回環(huán)檢測模塊中以保證地面特征被充分利用。在這里分享兩篇對我啟發(fā)較大的有關(guān)地面點云分割的技術(shù)論文和開源代碼，

3.3、萬能誤差消除術(shù)

萬能誤差消除術(shù)其一：添加和利用好GNSS、Landmarks等外部的絕對位置信息，唯一需要注意的就是當(dāng)GNSS數(shù)據(jù)或者Landmark信息被機器人獲取時，需要自動地判斷出該絕對位置信息是否可靠，避免因為使用垃圾、離群點位置信息而帶來更嚴重的誤差?？蓞⒖嘉恼骆溄樱篽ttps://zhuanlan.zhihu.com/p/603099637。

萬能誤差消除術(shù)其二：回環(huán)檢測?；丨h(huán)檢測是所有SLAM問題的解決方案里必不可少關(guān)鍵模塊，比如經(jīng)典的Scan-Context，基于多分辨率地圖的分支定界+相關(guān)性匹配，以及利用語義信息的各種回環(huán)檢測算法，只要在SLAM過程中找到正確的回環(huán)，基本上就可以消除90%的誤差了，但是怎么保證回環(huán)檢測的效率、準確度又是一個研究方向了。

4

優(yōu)化結(jié)果展示

使用優(yōu)化后的算法模塊在相同的數(shù)據(jù)集和硬件環(huán)境下獲得了與圖1和圖2例子進行對比的圖10開環(huán)室內(nèi)工廠環(huán)境和圖8閉環(huán)室外廠區(qū)環(huán)境結(jié)果，其中優(yōu)化后的結(jié)果（見圖8）SLAM軌跡圖中長邊（240米）中最左和最右關(guān)鍵幀的位姿上z坐標間的差值從之前的9.88米優(yōu)化成1.82m，圖10的軌跡結(jié)果則從原先260米長度下累積8.57米誤差，到現(xiàn)在300米累積0.89米的誤差，以上兩個對比實例都說明了上文中的優(yōu)化思路是可行的。

圖8：閉環(huán)室外廠區(qū)130x240米（俯視圖：上，正視圖：下）

圖9：閉環(huán)室內(nèi)+室外廠區(qū)205x240米（俯視圖：上，正視圖：下）

圖10：室內(nèi)廠區(qū)120x300米（俯視圖：上，正視圖：下）

除開大場景、長軌跡的SLAM構(gòu)建，還使用優(yōu)化后的SLAM算法模塊在小范圍的地庫及室內(nèi)房間場景進行了魯棒性測試，地庫場景的有效高度區(qū)間相較于工廠與室外環(huán)境會更加小，同時存在更多的動態(tài)障礙物（移動的車輛和行人），室內(nèi)房間場景由于空間更加狹窄，3D激光雷達獲取到的地面特征幾乎沒有，由此也就更加依賴IMU的重力估計和校準功能，同時還需要避免錯誤的地面約束影響SLAM軌跡發(fā)生偏移。圖11和圖12分別對應(yīng)優(yōu)化后的SLAM算法模塊下地庫、室內(nèi)房間場景下的結(jié)果。

圖11：室內(nèi)地庫50x70米（俯視圖：上，正視圖：下）

圖12：室內(nèi)房間22x35米（俯視圖：上，正視圖：下）

5

總結(jié)

在純LiDAR SLAM的算法模塊中添加IMU慣性里程計約束、IMU重力估計及校準，并針對點云數(shù)據(jù)進行地面點云的分割和針對地面點云進行平面特征匹配，可在一定程度上解決LO在z、roll、pitch這幾個機器人狀態(tài)量輕微不可觀的問題。值得說明的一點是，在本人的優(yōu)化工作中，先是嘗試在SLAM算法模塊的前端使用局部因子圖框架，利用LiDAR觀測（地面點云在此階段也被利用到點云配準算法中以進一步提升z、roll、pitch的觀測精度）得到的關(guān)鍵幀及幀間IMU慣性里程計約束，獲取一個優(yōu)化后LIO關(guān)鍵幀并輸入到后端中，這樣的處理在回環(huán)檢測觸發(fā)之前，前端LIO中還是會積累一定程度的z軸飄移（但比沒有添加地面點云優(yōu)化配準和imu約束前要好一些）。而最有效的優(yōu)化處理則是在后端的優(yōu)化問題構(gòu)造時，針對每一個關(guān)鍵幀添加必要的IMU重力校準殘差項和地面約束殘差項，如此一來才能真正意義上的解決z軸飄移的問題。

在后端優(yōu)化的改造工作中，難點在于如何保證添加的IMU重力校準殘差項和地面約束殘差項是精準和可靠的，在改造初期由于經(jīng)常引入較差的IMU重力校準約束和地面約束而出現(xiàn)關(guān)鍵幀位姿出現(xiàn)大跳變和SLAM軌跡失敗的問題，地面約束的約束質(zhì)量確認可以通過地面點云分割質(zhì)量和地面特征匹配得分的高低來進行把控，而要保證IMU重力校準精度，一方面需要加強針對IMU數(shù)據(jù)的使用問題建模和解決方案設(shè)計，添加重力校準約束前再進一步進行完整性分析以最后把控約束質(zhì)量（這部分的內(nèi)容后續(xù)再加更輸出文章）。

審核編輯：劉清