GR-Fusion：強(qiáng)魯棒低漂移的多傳感器融合系統(tǒng)（IROS2021）

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

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

針對問題：

針對地面機(jī)器人的多傳感器融合SLAM系統(tǒng)

提出方法：

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

達(dá)到效果：

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

Abstract

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

Introduction

主要貢獻(xiàn)：

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

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

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

The proposed GR-Fusion

1.系統(tǒng)概述

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

2.Measurement pre-processing 測量數(shù)據(jù)預(yù)處理

(1)輪速計IMU預(yù)積分

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

（2）LiDAR和相機(jī)觀測預(yù)處理

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

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

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

3.GR-odometry

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

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

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

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

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

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

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

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

（3）局部平面約束

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

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

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

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

（4）傳感器退化檢測

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

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

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

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

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

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

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

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

（1）GNSS 權(quán)重

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

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

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

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

Experiments

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

1.室內(nèi)數(shù)據(jù)集

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

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

2.地下數(shù)據(jù)集

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

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

3.開放式草坪數(shù)據(jù)集

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

4.校園數(shù)據(jù)集

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

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

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

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

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

Conclusions And Future work

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

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