面向動態(tài)環(huán)境的機器人同步定位與建圖技術(shù)

賴尚祥，楊忠，姜遇紅，張弛，方千慧

1. 南京航空航天大學(xué) 自動化學(xué)院，江蘇 南京 211106

2. 南京航空航天大學(xué) 無人機研究所，江蘇 南京 211106

同步定位與建圖技術(shù)(simultaneous localization and mapping，SLAM)是提高機器人自主性和智能性的關(guān)鍵技術(shù)。近年來，它一直是機器人領(lǐng)域研究的熱點。SLAM技術(shù)僅通過機載傳感器的數(shù)據(jù)流，聯(lián)合評估未知環(huán)境的地圖和機器人在地圖中的姿態(tài)。該地圖允許機器人在相同的環(huán)境中不斷地定位而不積累漂移。由于視覺傳感器具有成本低、數(shù)據(jù)量大等優(yōu)點，視覺SLAM越來越受到人們的關(guān)注[1]。常用的視覺傳感器有單目相機、雙目相機、RGB-D相機等。單目相機方案在尺寸、功率和成本方面有實際優(yōu)勢，但也面臨一些挑戰(zhàn)，如尺度、初始化困難問題。通過使用更復(fù)雜的視覺傳感器可以避免這些問題，如雙目相機和RGB-D相機?，F(xiàn)代視覺SLAM系統(tǒng)的框架由特征提取、狀態(tài)估計、建圖、回環(huán)檢測等幾個基本部分組成。SVO[2]、LSD-SLAM[3]、ORB-SLAM2[4]等SLAM算法已經(jīng)取得了令人滿意的視覺效果。然而絕大多數(shù)方法和數(shù)據(jù)集都采用靜態(tài)環(huán)境。這類方法通過將動態(tài)內(nèi)容分類為靜態(tài)模型的異常值，從而處理一小部分動態(tài)內(nèi)容。但是在人口稠密的現(xiàn)實環(huán)境中，如行人、動物，這些均是不可避免的內(nèi)容，同時這些環(huán)境正是服務(wù)機器人或自動駕駛汽車等相關(guān)應(yīng)用的目標(biāo)。

因此有效地解決動態(tài)環(huán)境下的機器人同步建圖技術(shù)顯得尤為重要?？紤]到上述算法的局限性，動態(tài)SLAM算法的主要挑戰(zhàn)為：

1)如何檢測動態(tài)目標(biāo)；

2)如何防止跟蹤算法使用屬于動態(tài)對象的特征點。

1 相關(guān)工作

在典型的SLAM系統(tǒng)中，動態(tài)目標(biāo)被視為噪聲數(shù)據(jù)，因此它既不包含在地圖中，也不用于跟蹤攝像機。在ORB-SLAM[4-5]中最具代表性的是RANSAC算法。RANSAC算法假設(shè)存在一個模型，所有的局內(nèi)點符合模型，局外點不符合模型。只要符合模型的局內(nèi)點足夠多，則模型便足夠合理。整個過程采用反復(fù)迭代、優(yōu)化模型、更新內(nèi)點集的方式尋找最優(yōu)模型。但是，當(dāng)動態(tài)對象的比例較大，甚至覆蓋了相機的主要視場時，該方法將失效。

Kundu等[6]提出一種基于幾何方法的動態(tài)目標(biāo)檢測。通過構(gòu)造機器人運動的基本矩陣，定義對極幾何約束。如果匹配的特征點離外極線太遠，則最可能被認為是動態(tài)的。這種方法的關(guān)鍵點在于對基本矩陣的估計，如果能獲得相對可靠的基本矩陣，則可以很容易地檢測到大部分動態(tài)特征。由于RGB序列中包含大量動態(tài)特征，則對這種方法的初始基礎(chǔ)矩陣計算造成很大的影響，從而影響整個系統(tǒng)的精度。

隨著深度學(xué)習(xí)技術(shù)的發(fā)展，圖像中的高級語義信息逐漸被用來處理場景中的動態(tài)對象，以提高SLAM的性能。在SLAM系統(tǒng)中，利用YOLO[7]、SSD[8]、SegNet[9]、Mask R-CNN[10]等先進的CNN架構(gòu)，獲得每一幀特征的語義標(biāo)簽。根據(jù)人類的先驗知識，如人、動物被認為是動態(tài)物體，附屬于該類對象上的特征點將被剔除。現(xiàn)實環(huán)境中存在一類物體，如椅子，它在常規(guī)情況下屬于靜態(tài)的，因此在卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural networks,CNN)中的標(biāo)簽也為靜態(tài)。但與人發(fā)生接觸后，其三維空間位置大概率會發(fā)生剛性變換，這對后續(xù)相機位姿估計精度造成一定的影響。

Cui等[11]采用光流法追蹤經(jīng)過CNN去動態(tài)的RBD圖，從而獲得一個較為準(zhǔn)確的基礎(chǔ)矩陣，進而進行運動一致性檢測，去除潛在的動態(tài)物體。由于算法采用了光流法，勢必對系統(tǒng)的實時性造成影響。

針對上述情況，提出一種動態(tài)SLAM系統(tǒng)，通過結(jié)合語義分割和幾何分割來檢測動態(tài)對象。采用Mask R-CNN去除先驗動態(tài)物體，使用慣性模型實現(xiàn)幀間基礎(chǔ)矩陣求解，根據(jù)對極約束進行運動一致性檢測。最后，使用區(qū)域生長法標(biāo)記動態(tài)對象。相對其他方法，本文方法提升了定位精度。

圖1 系統(tǒng)跟蹤線程框架圖

2 系統(tǒng)跟蹤線程框架

2.1 系統(tǒng)跟蹤線程

圖1為本系統(tǒng)的跟蹤線程框圖。首先，RGB序列經(jīng)過一個語義分割網(wǎng)絡(luò)，將所有先驗的動態(tài)對象逐像素分割出來。其次，我們使用幾何分割的方法來標(biāo)記在Mask R-CNN階段未被檢測到的其他動態(tài)物體。為此，有必要估計兩幀間的相機姿態(tài)變化。因此我們設(shè)計了一種基于慣性模型快速求解相對可靠的相機位姿方法。值得注意的是，CNN無法檢測到的其他動態(tài)對象將通過幾何分割進行檢測。剩余的靜態(tài)特征被保留，以便用于后續(xù)跟蹤線程新幀的姿態(tài)估計。動態(tài)物體去除模塊流程如圖2所示。

圖2 動態(tài)物體去除模塊流程

2.2 語義分割先驗動態(tài)物體

為了檢測動態(tài)對象，本文采用了一種通用的實例分割結(jié)構(gòu)Mask R-CNN。Mask R-CNN可以同時獲得實例標(biāo)簽和像素級語義分割。

根據(jù)掩碼的標(biāo)簽，可以獲得一些動態(tài)對象的先驗信息，如人、貓、狗等。如果對象被標(biāo)記為“人”，那么我們假設(shè)落在該對象上的所有像素都是動態(tài)的可能性非常大，因為一個人在常規(guī)情況下傾向于在移動。同樣地，如果像素的標(biāo)簽是“桌子”，我們將假設(shè)這些像素所在的物體是靜態(tài)的可能性非常大。如果像素的標(biāo)簽是“椅子”，這種情況就不同了，因為在人的影響下，椅子是可以移動的。所以我們需要使用其他方法來檢測這些潛在的動態(tài)對象。

有了先驗知識，我們可以將物體上的像素大致分為3類：動態(tài)的、潛在的動態(tài)的、靜態(tài)的。為了減少計算量，本文只考慮特征點，而不考慮像素點。這樣，落在動態(tài)對象上的特征點將被移除，而靜態(tài)特征點將被保留。如果將潛在的動態(tài)點簡單地看作動態(tài)點或靜態(tài)點，將對SLAM系統(tǒng)的定位精度產(chǎn)生不可逆轉(zhuǎn)的影響。因此，本文采用了一種將語義分割和幾何分割相結(jié)合的動態(tài)目標(biāo)檢測方法。

2.3 慣性模型

幾何分割根據(jù)相機姿態(tài)進一步篩選特征點中的動態(tài)點。因此，需要一種快速準(zhǔn)確的姿態(tài)估計方法。本文采用慣性模型對圖像中的靜態(tài)點進行跟蹤，這些圖像的靜態(tài)點由語義分割去動態(tài)之后獲得。由于幀率較高，假設(shè)幀之間的速度不變，因而相機將以上一幀的速度移動。相機姿態(tài)變換矩陣應(yīng)為

式中：Tcur、Tlast表示當(dāng)前幀與上一幀的姿態(tài)變換矩陣；Min表示慣性系數(shù)，當(dāng)上一幀跟蹤失敗時，該值等于0，否則為1。

將地圖上的特征點投影到圖像幀中，在投影點附近搜索相應(yīng)的匹配點。如果搜索到的匹配點對數(shù)量不夠，則增大搜索范圍。如果依然搜索不到足夠數(shù)量的匹配點對，則認為模型失效。若模型失效，將對上一個關(guān)鍵幀采用慣性模型求解，或者進行重定位。

在模型成功的情況下，將觀測方程抽象記為

式中：x代指此刻相機位姿，即相機變換矩陣T；y代指路標(biāo)點，即三維特征點P；z為觀測值。獲得匹配特征點對與初始位姿可以得到此次觀測誤差為

式中：err 為誤差項，z為觀測數(shù)據(jù)z?[us,vs]。將具有共視關(guān)系的幾幀觀測量加入方程，設(shè)zij為在位姿Ti處觀察點Pj產(chǎn)生的數(shù)據(jù)，則整體的代價函數(shù)為

為求得投影誤差最小時對應(yīng)的待優(yōu)化變量Ti，采用圖優(yōu)化[12]的方法，從初始估計值開始，尋找目標(biāo)函數(shù)下降方法，對Ti進行修正，找到最優(yōu)解。此時Pj不參與被優(yōu)化，以減少計算量。

慣性模型求解相機位姿使用的特征點包含潛在動態(tài)物體，如與人發(fā)生互動的椅子、書本等物體。這些潛在動態(tài)物體對應(yīng)的特征點三維空間坐標(biāo)發(fā)生了變化，相關(guān)的投影誤差項失效。因此需要通過進一步使用幾何分割檢測動態(tài)點。

2.4 幾何分割

對于一對匹配好的靜態(tài)特征點p1、p2，兩點滿足極線約束，即E=t^R。二者在空間中的關(guān)系如圖3所示。

圖3 多視圖幾何中特征的對極約束

I1、I2分別代表2幀圖像；p1、p2是一對匹配好的 點，2個 相 機 中 心 分 別 為O1、O2；連 線在三維空間中相交于P。 這時O1O2P三點可以確定一個平面，稱為極平面。O1O2連線與像平面I1、I2的交點分別為e1、e2。e1、e2稱為極點，O1O2被稱為基線。

極平面O1O2P與I1、I2兩幀的交線l1、l2稱為極線。極線l2可以視為在幀I2上的投影線。在三維點P(P0,P00)不確定深度的情況下，可以知道它在I2上的投影必定在極線l2上。這種對極幾何約束實際上描述了從一幀中的一個點到另一幀中相應(yīng)的極線的投影。投影關(guān)系可以用基本矩陣F表示：

式中：E為 本質(zhì)矩陣，t為平移向量，R為旋轉(zhuǎn)矩陣，K為相機內(nèi)參矩陣。

由于特征提取和基本矩陣F估計中存在不可避免的誤差，實際上特征點p2并沒有完全落在極線l2上，如圖2中pˉ2所示。此時，由匹配點對可知坐標(biāo)：P點 三維坐標(biāo)：

根據(jù)RGB-D相機測量的特征點深度值，恢復(fù)

P=dK?1p1

其中d為該點深度值，P點在I2上的投影為

p2=RP+t

其中R、t由基礎(chǔ)矩陣F分解得到。位于I2上的實際匹配點為則誤差項定義為

通過運動一致性檢測，可以知道哪些特征點落在動態(tài)對象上。為了避免遺漏動態(tài)對象上的特征點，我們通過區(qū)域生長獲得幾何分割掩碼。區(qū)域生長算法的基本思想是將具有相似屬性的像素合并在一起。RGB圖包含過多的冗余信息，而深度圖上一個對象的像素分布始終是連貫的，因此本文選擇在深度圖上采用區(qū)域生長算法[13]。

然而，一些位于動態(tài)對象邊界上的特征點可能會引起問題。因為掩碼的邊緣會大致覆蓋整個對象，邊緣特性不是很明顯，而邊緣又屬于容易被檢測出特征點的區(qū)域。為了彌補該缺點，本文對每個掩碼進行了形態(tài)計算以優(yōu)化掩模的邊界。

3 實驗分析

本文系統(tǒng)在公共TUM RGB-D數(shù)據(jù)集中進行了評估，并與其他動態(tài)SLAM系統(tǒng)進行了比較。首先，將本文系統(tǒng)與原始的ORB-SLAM2系統(tǒng)進行比較，該系統(tǒng)被認為是相對更出色且穩(wěn)定的SLAM系統(tǒng)之一。所有實驗均在Intel i5-9400F、Nvidia GTX-1660、16 GB RAM的計算機上執(zhí)行。

TUM RGB-D數(shù)據(jù)集[14]包含多個動態(tài)環(huán)境序列。包含深度圖像和RGB圖像，以及由高精度運動捕捉系統(tǒng)記錄的地面真實軌跡。由于序列中移動的人可能會占據(jù)圖像的很大一部分，因此序列中的動態(tài)對象給SLAM系統(tǒng)的定位精度帶來了極大的挑戰(zhàn)。我們將本系統(tǒng)與ORB-SLAM2進行了比較，以說明此動態(tài)SLAM系統(tǒng)的作用。

根據(jù)語義分割以及幾何分割去除動態(tài)特征點，效果如圖4所示。圖4(a)、(c)、(e)為本系統(tǒng)結(jié)果，圖4(b)、(d)、(f)為ORB-SLAM2的結(jié)果。圖中紅色框為先驗動態(tài)物體，藍色框為潛在動態(tài)物體。圖4(a)、(c)、(e)中紅色框內(nèi)由Mask R-CNN獲得相關(guān)掩碼并去除對應(yīng)的特征點，藍色框內(nèi)由幾何分割檢測得到的動態(tài)物體并去除相應(yīng)的特征點。由圖可知，動態(tài)物體對應(yīng)的特征點均被有效剔除，從而減少其對系統(tǒng)的影響。

圖4 動態(tài)環(huán)境下本系統(tǒng)與ORB-SLAM2特征點檢測結(jié)果比較

在SLAM系統(tǒng)中，誤匹配點對，動態(tài)點對參與圖優(yōu)化的整個過程。由于他們的數(shù)據(jù)是錯誤的，即誤差項很大，意味著算法將試圖調(diào)整這條邊所連接的所有節(jié)點的估計值，以緩和錯點帶來的大誤差項。這將導(dǎo)致優(yōu)化方向偏離真實方向，因此一般采用魯棒核函數(shù)削減這些點的影響：

當(dāng)誤差項e大于閾值 δ時，函數(shù)由二次增長變?yōu)橐淮涡问?，限制了梯度的最大值。?dāng)SLAM系統(tǒng)面對一個高動態(tài)環(huán)境時，動態(tài)特征點對成為優(yōu)化函數(shù)中具有分量的一類數(shù)據(jù)，則魯棒核函數(shù)收效甚微。包含了大量動態(tài)點的誤差項整體數(shù)值偏大，算法優(yōu)化方向偏向于錯點，導(dǎo)致估計位姿偏離真值，對跟蹤效果造成不可逆的影響。因此去除動態(tài)點對能夠使得優(yōu)化函數(shù)準(zhǔn)確表達真實三維場景約束。由圖5、6可看出，沒有進行動態(tài)點提出的ORBSLAM2系統(tǒng)軌跡與真實軌跡大相徑庭，而本文提出的算法很好地對相機位姿進行了跟蹤。

圖5 本系統(tǒng)與ORBSLAM2軌跡誤差對比

圖6 本系統(tǒng)與ORBSLAM2平移誤差對比

本實驗采用絕對平移誤差(absolute trajectory error, ATE)定量評估系統(tǒng)軌跡的整體一致性。絕對平移誤差是估計位姿和真實位姿的直接差值，可以非常直觀地反應(yīng)算法精度和軌跡全局一致性。第i幀的ATE定義為

式中：Gi為相機位姿真值，F(xiàn)i為估計值，S為相機估計位姿坐標(biāo)系與真值坐標(biāo)系的轉(zhuǎn)換矩陣，由最小二乘法估計得到。通常使用均方根誤差(root mean squared error, RMSE)統(tǒng)計前n幀的ATE值[14]：

為獲得高度可信的實驗數(shù)據(jù)，對每個序列運行5次，分別獲得均方根誤差、誤差標(biāo)準(zhǔn)差(standard deviation, SD)、誤差中位數(shù)(median)、誤差平均值(mean)，取5次數(shù)據(jù)的中位數(shù)作為實驗結(jié)果。

序列名稱中，第一個字母表示序列中人的狀態(tài)，“s”表示人的狀態(tài)為靜坐(弱動態(tài)場景)，“w”表示人的狀態(tài)為移動(強動態(tài)場景)。后續(xù)字母表示相機運動方式。該系統(tǒng)的改進值通過式（1）計算：

式中：η表示提升值，εorb表示ORB-SLAM2的誤差值，εours表示本系統(tǒng)的誤差值。

由表1可以看出，與ORB-SLAM2相比，本系統(tǒng)在準(zhǔn)確性上有了很大的提高，并且?guī)缀跛薪Y(jié)果都提高了一個數(shù)量級。在“w.half”序列中，RMSE、SD、平均誤差、中值誤差的準(zhǔn)確度分別提高了95.59%、92.55%、95.80%和96.18%。在其他序列中也會發(fā)生類似情況。這是因為語義分割與幾何分割相結(jié)合有助于去除動態(tài)特征，從而大大提高系統(tǒng)的準(zhǔn)確性。

值得注意的是，在弱動態(tài)序列“s.half”中，系統(tǒng)改進并不明顯。這是因為人在這些序列中的位置總是非常靠前。在本文方法中，即使它們沒有移動，它們也會被標(biāo)記為動態(tài)的。在這種情況下，ORB特征點在圖像中的分布將不均勻，這將影響系統(tǒng)姿態(tài)確定的準(zhǔn)確性。并且，原始的ORBSLAM2系統(tǒng)可以通過非線性優(yōu)化來處理一些低動態(tài)情況。但是，這不會影響該系統(tǒng)的優(yōu)越性，它的整體準(zhǔn)確性仍然得到提高。

表1 本系統(tǒng)與ORB-SLAM2系統(tǒng)絕對軌跡誤差的RMSE[m]對比結(jié)果

為體現(xiàn)本文算法優(yōu)越性，將其與近代較為先進的動態(tài)環(huán)境RGB-D SLAM系統(tǒng)DS-SLAM[15]、Detect-SLAM[16]進行對比。表2的實驗結(jié)果表明，與這些系統(tǒng)相比，本文方法在動態(tài)序列定位方面具有更好的性能。

表2 本系統(tǒng)與先進動態(tài)SLAM的RMSE[m]對比

4 結(jié)束語

本文提出了一種基于RGB-D傳感器的方法，該方法能夠一致地映射包含多個動態(tài)元素的場景。通過結(jié)合語義分割和幾何分割來檢測動態(tài)對象。跟蹤序列以獲得相機姿勢，其中先驗的動態(tài)對象由Mask R-CNN移除。移動一致性檢查用于檢測其他動態(tài)特征點。最后，使用區(qū)域生長法標(biāo)記動態(tài)對象。實驗結(jié)果表明，該方法減少了運動物體的影響。在動態(tài)序列中，本文系統(tǒng)在定位精度方面比原始的ORB-SLAM2有了很大的提高。此外，在動態(tài)場景中與其他最新動態(tài)SLAM系統(tǒng)的比較表明，本文系統(tǒng)在大多數(shù)情況下可以達到最高的精度。

本系統(tǒng)依然存在需要完善的工作，未來的工作包括：1)使用其他對象的語義標(biāo)簽，這將會在地圖中提供更豐富的直觀信息。2)使用系統(tǒng)計算得到的信息建立一個更加直觀的語義稠密地圖。