GR-Fusion：強魯棒低漂移的多傳感器融合系統（IROS2021）

來源：GR-Fusion Multi-Sensor Fusion SLAM for Ground Robots with High Robustness and Low Drift IROS 2021

單位：中科院沈陽自動化研究所

針對問題：

針對地面機器人的多傳感器融合SLAM系統

提出方法：

系統融合緊耦合IMU、輪速計、視覺、LiDAR和GNSS多源觀測信息，并結合地面機器人平面運動約束，構建了融合多模態觀測信息的因子圖優化模型，針對各種傳感器退化場景提出了有效的運動退化檢測方法，使得系統能夠在復雜環境中仍保持較強的魯棒性和較低的漂移。

達到效果：

所提出系統在室內數據集、地下數據集、開放式草坪數據集以及校園場景數據集中進行了廣泛測試，與現有方案對比達到了最先進的效果。數據集開源地址https://drive.google.com/drive/folders/110Hko3z PcDmY0_bnZdXxJXJKe6wr3t10?usp=sharing

Abstract

本文提出了一個緊耦合LiDAR、相機、IMU、輪速計和GNSS信息的SLAM系統。系統通過將LiDAR點云和地面投影到圖像中恢復視覺特征深度，選擇跟蹤的高質量視覺特征和LiDAR特征，并緊耦合IMU和輪速計的預積分值來優化機器人的狀態增量。我們使用估計的相對位姿來重新度量局部滑窗中特征之間的關聯距離，并去除動態物體和異常值。在建圖節點中，我們使用精煉的特征，并緊耦合GNSS測量值、增量因子和局部平面約束，通過將LiDAR特征與全局地圖對齊，進一步優化機器人的全局狀態。此外，該方法可以檢測傳感器的退化情況并自動重新配置優化過程。系統基于一個六輪地面機器人在室內和室外環境中進行了廣泛的實驗，證明了所提出的GR-Fusion在精度和魯棒性方面優于最先進的SLAM方法。

Introduction

主要貢獻：

本文提出了一個可以融合來自LiDAR、相機、IMU、輪速計、GNSS和局部平面約束的多模態觀測的框架，用于機器人狀態估計；

系統將LiDAR點云和地面被投影到圖像中，以提取視覺特征的深度。該方法可以檢測每個傳感器的退化情況，并重新配置優化過程，以應對各種具有挑戰性的情況；

所構建系統在真實的地面機器人上進行了廣泛的實驗，表明GR-Fusion具有高魯棒性和低漂移性。

The proposed GR-Fusion

1.系統概述

所提方法的流程圖如上圖所示，系統包含四個節點。每個傳感器的測量量分別為：輪速計100HZ，IMU 100HZ，相機30HZ，LiDAR 10HZ，GNSS 5HZ。測量預處理節點首先進行數據緩存和同步。然后根據時間戳對輪速計和IMU的測量進行預積分，以消除LiDAR點云的運動失真。根據我們之前研究中提出的地面測量模型，提取地面點，并擬合地平面參數。然后將計算出的局部平面和LiDAR點云投影到圖像中，計算出視覺特征的深度。里程計節點通過緊耦合多模態的局部約束因素來估計機器人在滑動窗口中的運動增量。建圖節點將LiDAR特征與全局地圖對齊，并將本地約束和GNSS約束緊耦合起來，以優化機器人的全局狀態。閉環節點接收來自GR-mapping的位姿和特征，并進行閉環檢測和優化以消除漂移。

2.Measurement pre-processing 測量數據預處理

(1)輪速計IMU預積分

我們使用了[10]中提出的流形上的輪速計增量模型，它可以融合輪速計和IMU的測量值，計算機器人在復雜地形（如斜坡、樓梯和草坪）上的三維位置增量，如下圖所示。接下來，輪速計和IMU的測量結果根據LiDAR和圖像的時間戳進行預積分，以計算幀之間的位姿增量。在正常情況下，位置增量由輪速計提供，旋轉增量由IMU提供。如果檢測到機器人的車輪在打滑，則由IMU提供全部增量。

（2）LiDAR和相機觀測預處理

首先，根據幀間的運動增量，去除點云的運動失真。為了去除噪聲和不穩定的點，我們按照[13]提出的方法進行點云分割。根據粗糙度提取了角和平面特征。由于機器人總是在地面上運行，我們提出使用局部平面約束來優化機器人的位姿。我們使用之前研究中提出的地面測量模型來提取地面點，之后我們擬合地面平面參數來計算當前可見的地面區域。

對于來自雙目相機的圖像，進行角點檢測。通過設置兩個相鄰特征之間的最小像素間隔，實現了特征的均勻分布。這些特征使用KLT光流算法進行追蹤，同時我們還使用KLT跟蹤器來匹配左右圖像之間的特征。

在大規模環境中，由于基線較小，使用視覺特征的三角測量法得到的深度有很大的誤差。因此，我們通過[20]介紹的方法，將LiDAR點云投射到圖像中，以提取視覺特征的更魯棒的深度。地面點被投影到圖像中，以分割地面上的視覺特征。相機投影模型被用來計算地面特征的深度。實驗發現，位于地面的視覺特征移動速度快，跟蹤時間短，往往無法通過三角測量法獲得準確的深度。因此，利用LiDAR擬合的地面參數可以直接計算出地面特征的深度。此外，它還隱式地提供了地面視覺特征的共面約束。

3.GR-odometry

里程計節點被設計為選擇少量但高質量的特征，以快速估計機器人在局部滑窗內的運動增量。同時，優化過程根據傳感器的退化情況進行了重新配置。此外，在當前LiDAR幀被發送到建圖線程之前，利用里程計的結果對當前幀的特征進行細化調整，位于動態目標上的特征和不穩定的特征點將被剔除。

（1）狀態向量

本文所提出的方法以LiDAR幀為優化節點，選擇最近的圖像作為視覺約束。狀態向量表示如下：

我們使用IMU時間戳作為基準，將LiDAR和相機的測量時間對齊IMU的時間戳。然后與IMU的觀測一起進行優化，以估計傳感器之間的時間偏移。

（2）因子圖優化模型

我們在里程計部分維護一個滑窗，如上圖所示。有n+1個節點，優化窗口包含三個最新的連續節點。其他節點根據移動距離被選為關鍵幀，并將其保留在滑動窗口中，為優化窗口提供約束。滑動窗口中的LiDAR特征被用來維護本地點云地圖，最新的幀被投影到圖像中。對于每個視覺特征，相鄰的點云塊被提取出來并進行深度估計。對于深度已經從地面參數獲得的特征點，其深度值保持不變。此外，當從新的圖像中提取特征時，在點云覆蓋的區域，視覺特征間距被設定為其他區域的一半。這種策略能夠從點云覆蓋的圖像區域中提取更多的特征，而且深度可以直接估算，更加魯棒。

一個特征點被追蹤的次數越多，它的質量就越高。因此，我們將跟蹤次數超過閾值的點加入到優化中。接下來，根據跟蹤結果構建了以下重投影誤差約束：

我們將從LiDAR點云中提取的角點和平面特征與當前的局部地圖相匹配，并使用點到線和點到面的距離來構建LiDAR約束因子。此外，輪速計和IMU的測量結果通過預積分構建一個增量約束因子。

（3）局部平面約束

我們認為，當機器人在地面上移動時，局部平面在兩個連續的幀之間是不變的，如果平坦的地面被擬合，那么在局部平面的Z軸上的位移接近于零。為了確保我們的公式的魯棒性，我們考慮了法向量、地面粗糙度和地面連續性等因素來決定是否添加地面約束。

地面的法向量和機器人的Z軸之間的角度必須小于設定的閾值;

分割的地面點與擬合的地面之間的距離的平方之和必須小于設定的閾值;

最遠點和最近點地面之間的距離必須小于設定的閾值。

（4）傳感器退化檢測

在弱紋理環境或黑暗環境中，相機無法提取魯棒的特征進行跟蹤。當場景中存在大量的動態目標時，也會出現較大的跟蹤誤差。首先，我們統計了當前幀中每個特征的跟蹤時間，如果小于設定的閾值，就降低視覺特征的權重。如果所有特征的跟蹤時間之和小于設定的閾值，則被認為是不穩定的，這時，視覺約束將不被用于后續的優化過程。

LiDAR在高度重復的場景中會退化，例如走廊或開放的戶外環境。因此，我們區分了最新一幀點云的深度和優化窗口外的第一幀點云的深度，并計算出平均深度差。接下來，我們得到了所有點深度差的直方圖統計。如果深度差大于閾值點的數量很少，則LiDAR被認為是退化的。這時，LiDAR約束將不會被添加到優化中，優化滑窗的大小也被調整。該系統退化為GR-SLAM，并將相機、輪速計和IMU緊耦合在一起進行狀態估計。然而，局部的點云仍然被維護，建圖節點仍然運行以維護全局地圖。

由于IMU是一個內置的傳感器，不受外部環境的影響，在短時間內的估計結果是相對準確的。機器人車輪的滑動不可避免地增加了輪速計的預積分值。因此，我們計算了輪速計的預積分值與IMU的預積分值比率。如果它大于設定的閾值，則認為車輪在打滑，輪速計的約束就不會被添加到優化中。此外，實驗結果顯示，機器人在轉彎時很容易打滑，而輪速計在直線運動時的精度更高。因此，我們也用機器人的轉動來調整輪速計的優化權重。

（5）局部因子圖優化

通過BA優化所有因素的殘差，并獲得機器人狀態的最大后驗估計，如下所示。

其中ro(.)代表局部地面約束。值得注意的是，這里優化的是圖4所示優化窗口中節點的狀態增量。只有當節點滑出優化窗口后，才會被送至建圖節點進行全局優化。

另外，我們使用GR-odometry的結果來去除動態物體和不穩定的特征。首先，當前的LiDAR特征被投影到局部滑窗的起始幀，以計算匹配距離。對于靜態環境，投影的LiDAR點是重合的，或者匹配距離很小。對于移動物體，投影點的匹配距離較大。因此，根據匹配距離對LiDAR特征進行排序，匹配距離超過設定閾值的點被判斷為動態物體而被剔除。最大剔除率被設定為10%。這一策略利用了動態物體的運動特征，并使用滑動窗口的時間跨度，這樣可以突出動態物體的投影誤差。最后，提煉出的特征點將被送到GR-Mapping節點。

4.GR-建圖和閉環檢測

（1）GNSS 權重

盡管我們可以融合多模態的測量來進行機器人的狀態估計，但長期運行仍會產生累積誤差。GNSS可以提供絕對位置。然而，實驗結果顯示，當GNSS受到建筑物或樹木的阻擋時，GNSS的測量會發生跳躍，導致大的誤差甚至失敗。因此，我們根據衛星使用數量、精度衰減因子、局部增量方差評估GNSS的質量。

搜索的衛星越多，GNSS的測量就越準確。然而，這也與衛星的空間幾何分布有關。因此，我們也使用GNSS輸出的DOP值來評估測量誤差。此外，我們還計算了局部GNSS增量和里程計增量的方差來評價GNSS的測量質量。盡管里程計測量法會產生累積誤差，但其局部精度相對較高。因此，里程計可以用來確定GNSS測量是否有跳躍，以及本地測量的質量是否下降，以避免大的誤差。

（2）全局因子圖優化

我們在全局優化中加入的制約因素有里程計增量因子、局部平面約束因子、來自IMU的角度預測因子、GNSS因子、地圖因子。閉環因子使用里程計測量的估計結果作為初始值，我們將LiDAR特征與全局地圖對齊，同時耦合其他局部約束和全局約束，以優化機器人的全局狀態。閉環因子來自于閉環檢測節點，我們使用了[21]提出的點云描述符進行位置識別。它還可以保存和重用地圖，并快速重定位到當前地圖，值得注意的是，我們的方法支持只包含輪速計、IMU和GNSS的低成本配置，并對地面機器人進行有效的狀態估計。

Experiments

我們在不同的環境中收集了多個數據集。這些數據集分別被稱為室內、地下、露天草坪和校園。

1.室內數據集

這個數據集包含了室內走廊環境，以及樓梯。軌跡的起點和終點是重合的，我們用離線批量優化和閉環優化的結果作為真值。結果顯示在圖6中。機器人在圖6(b)中圓圈標記的地方上下樓。由于LiDAR視角的急劇變化，LIO_SAM和LOAM_IMU都產生了很大的誤差。LIOM[14]與IMU緊耦合，誤差很小。GR_Fusion將攝像頭、IMU、輪速計和LiDAR緊耦合在前端，快速估計機器人的狀態增量，可以應對觀察角度急劇變化的情況。圖6(b)底部的曲線是算法在Z軸上的位移。由于GR_Fusion在優化時考慮了局部平面的約束，所以它的精度更高。

所有方法的絕對平移誤差見表二。LOAM_IMU是優化的LOAM。我們增加了IMU的局部姿態增量和全局重力約束，以提高其估計的準確性。

2.地下數據集

在這個數據集中，機器人從校園出發，經過地下車庫，然后返回地面校園。地下車庫的通道有一個旋轉通道和一個直線通道，如圖7(a)所示。這些通道的環境是高度重復的，造成LiDAR的退化。

圖7(b)中顯示了每種算法的結果。可以看出，LIOM、LIO_SAM和LOAM_IMU由于LiDAR的退化，在地下通道中都有明顯的錯誤。GR_Fusion仍然可以依靠視覺、IMU和輪速計增量模型進行連續狀態估計。當GR_Fusion檢測到LiDAR的退化時，它將減少LiDAR的優化權重。而在現有狀態的基礎上，局部滑動窗口的大小將逐漸增加，并包含更多的數據進行優化。在保證位姿不跳躍的同時，保證了狀態估計的準確性。

3.開放式草坪數據集

在這個測試中，我們評估了所提方法在戶外大規模開放環境中的表現。在開放草坪數據集中，機器人穿越了一個非常開放的草坪，在遠處只能看到一些稀疏的樹木，如圖8所示。在穿越草坪時，從LiDAR中提取的大部分特征都分布在地面上，使運動估計發生退化。從圖8(b)中可以看出，LIOM和LOAM_IMU由于LiDAR的退化而產生了明顯的誤差。因為LIO_SAM融合了GNSS測量，所以估計的全局姿態可以保證一定的精度。但是當我們去掉GNSS測量時，LIO_SAM估計的狀態會出現跳變，由此產生的誤差比LOAM_IMU更大。這是因為LIO_SAM在估計時使用的特征比LOAM_IMU少。此外，我們發現在室外環境中，從LiDAR中提取的特征相對稀疏，所以與室內環境相比，所評估的其他算法的性能有所下降。GR_Fusion結合了LiDAR、相機、IMU和輪速計的測量結果，以確保無論在室內還是室外環境下都有相同的精度。而通過融合GNSS測量，GR_Fusion可以輸出全局無漂移的估算結果。

4.校園數據集

在這個測試中，我們評估了GR_Fusion在不同配置下的性能。GR_Fusion_Local使用LiDAR、照相機、IMU和輪速計作為傳感器。GR_Fusion_GNSS進一步融合了GNSS測量。GR_Fusion_Low_Cost只使用低成本的傳感器配置，包括GNSS、IMU和輪速計。GR_Fusion_Loop有閉環優化。應該注意的是，在以前的實驗中沒有使用閉環優化。

結果顯示在圖9中。GR_Fusion_Loop的精度最高，可以消除累積誤差。GR_Fusion_Local的精度比GR_Fusion_GNSS的精度高。這是因為在軌跡的某一段中，GNSS的測量質量降低了，這導致了全局優化的誤差。然而，GR_Fusion_GNSS在長時間運行時具有更高的精度。GR_Fusion_LowCost只使用IMU和輪速計增量模型進行局部估計，并使用GNSS進行全局狀態優化。它的精度很容易受到GNSS測量的影響，但它仍然可以提供可接受的結果，對低成本機器人非常有用。

5.動態物體剔除

我們還測試了環境中出現動態物體時GR_Fusion的建圖性能。圖10顯示了車輛經過時的建圖結果。可以看出，其他算法在地圖中包括了所有移動車輛的點云。GR_Fusion可以有效地消除動態物體，建立一個更合理的靜態環境地圖。

此外，我們還對算法的實時性能進行了統計，如下表所示。可以看出，雖然GR_Fusion融合了更多的傳感器，但通過對信息的仔細處理和選擇，以及工程實施中的有效策略，它可以提供高效和準確的估計結果。

Conclusions And Future work

我們提出了一種多模態的傳感器融合方法，可以魯棒且準確地估計機器人的狀態。通過對各傳感器數據的精心選擇和細化，在提高精度的同時，還可以消除動態目標和不穩定的特征。它可以實時檢測傳感器的退化情況，并可以靈活地配置為多種工作模式。目前，視覺信息僅在前端用于估計機器人的局部狀態。在未來，我們將結合視覺和LiDAR來創建具有全局描述符的地標，以進一步優化機器人的全局狀態。

備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超干貨的3D視覺學習社區