基于ESKF 與圖優化的輕量級多傳感器融合SLAM 系統

李文豪

（廣東工業大學，廣東 廣州510006）

1 概述

狀態估計、定位和建圖是移動機器人的關鍵技術，是反饋控制、避障和規劃等任務的先決條件。目前主流的是基于視覺以及激光的建圖定位方案，并且投入大量的研究在如何確保系統的穩定實時運行中。基于視覺方法通常使用單目或立體相機對連續圖像中的特征進行分析以確定攝像機的運動。盡管基于視覺的方法是可行的，但它們對初始化、光照以及環境的紋理比較敏感，這使它們不可靠，但是基于視覺的方案特別適合位置識別。另一方面，激光雷達對環境不敏感，光照變化基本不會影響激光雷達。尤其是遠程高分辨率三維激光雷達的應用，例如Velodyne VLS-128 和Outster OS1-128 激光雷達,使得激光更適合直接捕捉三維空間中的環境因此，本文的研究重點是基于激光雷達的狀態估計和SLAM方法。我們提出的方法有如下幾個特點：

1.1 結合點線、點面ICP 以及ndt 算法實現激光里程計的匹配，并采用了一種高頻幀間匹配與低頻Map 優化的策略。

1.2 采用一種以機器人坐標為中心的ESKF 對IMU 進行融合，并實現對先驗觀測的融合，可更好適配室內平面環境。

1.3 實現一種簡單直接的去除激光匹配過程中動態物體影響的方法。

2 相關工作

對于激光SLAM任務，核心是根據傳感器兩幀的數據推斷其運動，其最經典的方法即ICP 算法[1],ICP 算法衍生出了多種變體，如IMLS-ICP、NICP、GICP[2]等,近年，Zhang 提出了一種基于特征提取的ICP 變種方法[3]，它首先進行特征提取減少匹配點云量并實現點云的配準。由于機械式激光的旋轉機制，當激光傳感器運動時，產生的點云會產生較大的畸變，因此單獨使用激光雷達姿態估計效果不理想，必須要另外的傳感器提供信息去進行補償，此外，單獨的激光傳感器無法適應劇烈的運動以及退化環境，多傳感器融合提供了解決上述問題可靠的方法，多傳感器融合通常分為松耦合與緊耦合，松耦合通常使用EKF，例如[4-6]采用EKF 去融合IMU、GPS、Lidar 等傳感器，緊耦合由于可以獲得更好的精度，所以獲得了更大的關注，[7]實現了一種基于滑動窗口優化的緊耦合系統，[8]實現了一種基于IEKF 的緊耦合激光慣導里程計。

3 系統框架

系統主要由數據處理、激光慣導里程計、后端優化、回環檢測等幾個模塊構成。系統采用ROS 作為通信框架，ROS 是一種適用于機器人的開源系統，可方便實現進程間的通信，以及進程的管理，并有豐富的數據分析工具如可視化工具RVIZ 等。

4 數據處理

數據處理的主要完成濾波與畸變去除，原始的激光雷達點云數據包含大量的噪聲，并且過于稠密，因此需要通過濾波去處理，濾波方法如下：首先采用距離濾波提取出ROI 區域范圍內的點云，然后采用基于角度的快速聚類算法[9]對點云進行聚類，剔除掉聚類后數量少于一定閾值的點，只保留一定體積的點云進行匹配，聚類濾波并且過濾掉地面后的效果如圖1 所示。

圖1 濾波提取后的點云

然后基于曲率提取特征點，特征點有兩類平面點與曲面點，具體的實現方法參考LOAM[3]。經過優化，濾波以及特征提取的的任務可以在5ms 以內的時間內完成。

5 激光慣導里程計

激光慣導里程計輸入IMU 數據以及提取的特征點數據，輸出系統運動的狀態信息。出于輕量化以及運行效率考慮，我們采用高頻的scan-scan 匹配結合低頻的scan-map 優化的前端思路，可以最大限度的平衡快速性以及精確性，同時在scan-scan匹配的階段，我們采用eskf 融合IMU 與激光的估計結果，同樣為了追求快速性以及高效率，我們采用松耦合的策略，激光慣導里程計模塊的結構如圖2。

圖2 激光慣導里程計模塊

激光慣導里程計主要由兩個模塊構成，LIO 模塊- 融合scan-scan 的匹配結果與IMU 的運動預測結果，校正模塊則負責對其結果進行優化。Scan-scan 匹配通過最小化點線、點面的殘差從而求解最優變換T，即求解一個非線性最小二乘問題。點線，點面的殘差構造如下：

上式可以采用高斯牛頓法迭代求解，首先求解出，如下式所示：

6 圖優化融合

圖優化信息融合主要負責融合激光慣導里程計、GNSS 信息、回環檢測信息，結構如圖3 所示，我們采用g2o 實現，執行圖優化的策略如下：

(1)具備回環信息時，執行全局圖優化。

(2)由于激光里程計的飄逸很慢，沒有必要一直添加GNSS 因子，當激光里程計輸出的狀態協方差過大，且GNSS 協方差足夠小時，添加GNSS 約束因子，并取前m 個關鍵幀執行局部圖優化。

(3)當機器人出于平面場景下運動時，可以添加先驗全局平面約束，由于LIO 模塊融合了對于平面信息的觀測，因此全局平面約束可以每間隔n 個關鍵幀添加一次。

圖3 后端優化框架

7 實驗

為了測試系統的實際性能，搭建了載人車輛并在校園環境下以及開源數據集下進行了測試，實驗車輛如圖4 所示。

圖4 實驗自動駕駛車輛

我們使用velodyne VLP16 激光雷達、MTI-300S 慣導以及差分式GPS 作為實驗傳感器，工控機的配置為i7-8550u, 8g 內存。在校園環境進行了測試，并將最終輸出的點云地圖與實際的衛星地圖進行比較，可以看到點云地圖可以與衛星地圖很好的進行匹配，這說明該系統估計的位姿與GPS 真值之間具有很小的誤差，另外，由于高頻scan-scan 的匹配與低頻scan-map 的匹配是通過兩個獨立的進程完成的，系統狀態輸出頻率是由scan-scan 的頻率決定的，而scan-scan 的匹配由于具有較少的約束，因此可以在很短的時間完成，經過實際的測試，對于16 線的激光雷達，scan-scan 的匹配可以在10ms 左右的時間完成，完全達到實時的需求。

圖5 實驗自動駕駛車輛

Kitti 數據集下測試的結果如下，其中綠色軌跡為經過后端優化后的結果，紅色軌跡為激光里程計結果。通過EVO 工具對最終輸出的結果與GNSS 真值進行比較具有極小的誤差。

圖6 后端優化與前端的對比

圖7 GNSS 融合后evo 評估結果