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

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

針對問題：

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

提出方法：

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

達到效果：

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

Abstract

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

Introduction

主要貢獻：

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

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

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

The proposed GR-Fusion

1.系統(tǒng)概述

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

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

(1)輪速計IMU預積分

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

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

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

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

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

3.GR-odometry

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

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

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

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

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

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

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

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

（3）局部平面約束

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

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

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

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

（4）傳感器退化檢測

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

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

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

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

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

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

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

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

（1）GNSS 權重

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

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

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

我們在全局優(yōu)化中加入的制約因素有里程計增量因子、局部平面約束因子、來自IMU的角度預測因子、GNSS因子、地圖因子。閉環(huán)因子使用里程計測量的估計結果作為初始值，我們將LiDAR特征與全局地圖對齊，同時耦合其他局部約束和全局約束，以優(yōu)化機器人的全局狀態(tài)。閉環(huán)因子來自于閉環(huán)檢測節(jié)點，我們使用了[21]提出的點云描述符進行位置識別。它還可以保存和重用地圖，并快速重定位到當前地圖，值得注意的是，我們的方法支持只包含輪速計、IMU和GNSS的低成本配置，并對地面機器人進行有效的狀態(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)化的結果作為真值。結果顯示在圖6中。機器人在圖6(b)中圓圈標記的地方上下樓。由于LiDAR視角的急劇變化，LIO_SAM和LOAM_IMU都產(chǎn)生了很大的誤差。LIOM[14]與IMU緊耦合，誤差很小。GR_Fusion將攝像頭、IMU、輪速計和LiDAR緊耦合在前端，快速估計機器人的狀態(tài)增量，可以應對觀察角度急劇變化的情況。圖6(b)底部的曲線是算法在Z軸上的位移。由于GR_Fusion在優(yōu)化時考慮了局部平面的約束，所以它的精度更高。

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

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

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

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

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

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

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

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

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

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

我們還測試了環(huán)境中出現(xiàn)動態(tài)物體時GR_Fusion的建圖性能。圖10顯示了車輛經(jīng)過時的建圖結果?？梢钥闯觯渌惴ㄔ诘貓D中包括了所有移動車輛的點云。GR_Fusion可以有效地消除動態(tài)物體，建立一個更合理的靜態(tài)環(huán)境地圖。

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

Conclusions And Future work

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

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