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

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

單位：中科院沈陽(yáng)自動(dòng)化研究所

針對問(wèn)題：

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

提出方法：

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

達到效果：

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

Abstract

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

Introduction

主要貢獻：

本文提出了一個(gè)可以融合來(lái)自L(fǎng)iDAR、相機、IMU、輪速計、GNSS和局部平面約束的多模態(tài)觀(guān)測的框架，用于機器人狀態(tài)估計；

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

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

The proposed GR-Fusion

1.系統概述

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

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

(1)輪速計IMU預積分

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

（2）LiDAR和相機觀(guān)測預處理

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

對于來(lái)自雙目相機的圖像，進(jìn)行角點(diǎn)檢測。通過(guò)設置兩個(gè)相鄰特征之間的最小像素間隔，實(shí)現了特征的均勻分布。這些特征使用KLT光流算法進(jìn)行追蹤，同時(shí)我們還使用KLT跟蹤器來(lái)匹配左右圖像之間的特征。

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

3.GR-odometry

里程計節點(diǎn)被設計為選擇少量但高質(zhì)量的特征，以快速估計機器人在局部滑窗內的運動(dòng)增量。同時(shí)，優(yōu)化過(guò)程根據傳感器的退化情況進(jìn)行了重新配置。此外，在當前LiDAR幀被發(fā)送到建圖線(xiàn)程之前，利用里程計的結果對當前幀的特征進(jìn)行細化調整，位于動(dòng)態(tài)目標上的特征和不穩定的特征點(diǎn)將被剔除。

（1）狀態(tài)向量

本文所提出的方法以L(fǎng)iDAR幀為優(yōu)化節點(diǎn)，選擇最近的圖像作為視覺(jué)約束。狀態(tài)向量表示如下：

我們使用IMU時(shí)間戳作為基準，將LiDAR和相機的測量時(shí)間對齊IMU的時(shí)間戳。然后與IMU的觀(guān)測一起進(jìn)行優(yōu)化，以估計傳感器之間的時(shí)間偏移。

（2）因子圖優(yōu)化模型

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

一個(gè)特征點(diǎn)被追蹤的次數越多，它的質(zhì)量就越高。因此，我們將跟蹤次數超過(guò)閾值的點(diǎn)加入到優(yōu)化中。接下來(lái)，根據跟蹤結果構建了以下重投影誤差約束：

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

（3）局部平面約束

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

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

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

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

（4）傳感器退化檢測

在弱紋理環(huán)境或黑暗環(huán)境中，相機無(wú)法提取魯棒的特征進(jìn)行跟蹤。當場(chǎng)景中存在大量的動(dòng)態(tài)目標時(shí)，也會(huì )出現較大的跟蹤誤差。首先，我們統計了當前幀中每個(gè)特征的跟蹤時(shí)間，如果小于設定的閾值，就降低視覺(jué)特征的權重。如果所有特征的跟蹤時(shí)間之和小于設定的閾值，則被認為是不穩定的，這時(shí)，視覺(jué)約束將不被用于后續的優(yōu)化過(guò)程。

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

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

（5）局部因子圖優(yōu)化

通過(guò)BA優(yōu)化所有因素的殘差，并獲得機器人狀態(tài)的最大后驗估計，如下所示。

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

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

4.GR-建圖和閉環(huán)檢測

（1）GNSS 權重

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

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

（2）全局因子圖優(yōu)化

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

Experiments

我們在不同的環(huán)境中收集了多個(gè)數據集。這些數據集分別被稱(chēng)為室內、地下、露天草坪和校園。

1.室內數據集

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

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

2.地下數據集

在這個(gè)數據集中，機器人從校園出發(fā)，經(jīng)過(guò)地下車(chē)庫，然后返回地面校園。地下車(chē)庫的通道有一個(gè)旋轉通道和一個(gè)直線(xiàn)通道，如圖7(a)所示。這些通道的環(huán)境是高度重復的，造成LiDAR的退化。

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

3.開(kāi)放式草坪數據集

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

4.校園數據集

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

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

5.動(dòng)態(tài)物體剔除

我們還測試了環(huán)境中出現動(dòng)態(tài)物體時(shí)GR_Fusion的建圖性能。圖10顯示了車(chē)輛經(jīng)過(guò)時(shí)的建圖結果?？梢钥闯?，其他算法在地圖中包括了所有移動(dòng)車(chē)輛的點(diǎn)云。GR_Fusion可以有效地消除動(dòng)態(tài)物體，建立一個(gè)更合理的靜態(tài)環(huán)境地圖。

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

Conclusions And Future work

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

備注：作者也是我們「3D視覺(jué)從入門(mén)到精通」特邀嘉賓：一個(gè)超干貨的3D視覺(jué)學(xué)習社區