1

背景

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

圖1：開環室內20x260米走廊場景（俯視圖：上，正視圖：下）

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

圖2：閉環室外130x240米廠區場景（俯視圖：上，正視圖：下）

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

針對上面兩個例子中發現的問題，本人將簡單分析其誤差的產生的原因，并分享根據誤差產生原因的物理依據進行優化設計和實現思路。

2

軌跡z方向飄移原因分析

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

2.1、LiDAR靜態tf標定引入的誤差

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

圖3：室內工廠單幀點云數據（俯視圖：上，正視圖：下）

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

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

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

圖4：室外單幀點云數據（俯視圖：上，正視圖：下）

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

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

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

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

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

1、 激光的數據會不準，射線被拉長

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

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

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

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

3

SLAM軌跡z方向飄移問題優化思路

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

3.1、靜態tf標定引入的誤差優化

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

圖6：VINS中的滑窗優化示意圖

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

圖7：LIO-SAM的緊耦合LIO系統框圖

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

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

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

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

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

（3）若當前幀為關鍵幀則執行因子圖優化，更新因子圖中所有變量節點的位姿由此獲得優化后的SLAM軌跡。可以看到，LIO-SAM的系統中沒有進行基于點云的配準，而是通過提取平面點進行基于特征的匹配優化計算，而且特征匹配時要求當前幀特征點數量需要足夠多且reference匹配的點數也足夠多才執行優化。

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

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

3.3、萬能誤差消除術

萬能誤差消除術其一：添加和利用好GNSS、Landmarks等外部的絕對位置信息，唯一需要注意的就是當GNSS數據或者Landmark信息被機器人獲取時，需要自動地判斷出該絕對位置信息是否可靠，避免因為使用垃圾、離群點位置信息而帶來更嚴重的誤差。可參考文章鏈接：https://zhuanlan.zhihu.com/p/603099637。

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

4

優化結果展示

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

圖8：閉環室外廠區130x240米（俯視圖：上，正視圖：下）

圖9：閉環室內+室外廠區205x240米（俯視圖：上，正視圖：下）

圖10：室內廠區120x300米（俯視圖：上，正視圖：下）

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

圖11：室內地庫50x70米（俯視圖：上，正視圖：下）

圖12：室內房間22x35米（俯視圖：上，正視圖：下）

5

總結

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

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

審核編輯：劉清