面向動態環境的機器人同步定位與建圖技術

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

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

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

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

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

1)如何檢測動態目標；

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

1 相關工作

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

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

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

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

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

圖1 系統跟蹤線程框架圖

2 系統跟蹤線程框架

2.1 系統跟蹤線程

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

圖2 動態物體去除模塊流程

2.2 語義分割先驗動態物體

為了檢測動態對象，本文采用了一種通用的實例分割結構Mask R-CNN。Mask R-CNN可以同時獲得實例標簽和像素級語義分割。

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

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

2.3 慣性模型

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

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

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

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

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

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

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

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

2.4 幾何分割

對于一對匹配好的靜態特征點p1、p2，兩點滿足極線約束，即E=t^R。二者在空間中的關系如圖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上。這種對極幾何約束實際上描述了從一幀中的一個點到另一幀中相應的極線的投影。投影關系可以用基本矩陣F表示：

式中：E為 本質矩陣，t為平移向量，R為旋轉矩陣，K為相機內參矩陣。

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

根據RGB-D相機測量的特征點深度值，恢復

P=dK?1p1

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

p2=RP+t

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

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

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

3 實驗分析

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

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

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

圖4 動態環境下本系統與ORB-SLAM2特征點檢測結果比較

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

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

圖5 本系統與ORBSLAM2軌跡誤差對比

圖6 本系統與ORBSLAM2平移誤差對比

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

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

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

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

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

由表1可以看出，與ORB-SLAM2相比，本系統在準確性上有了很大的提高，并且幾乎所有結果都提高了一個數量級。在“w.half”序列中，RMSE、SD、平均誤差、中值誤差的準確度分別提高了95.59%、92.55%、95.80%和96.18%。在其他序列中也會發生類似情況。這是因為語義分割與幾何分割相結合有助于去除動態特征，從而大大提高系統的準確性。

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

表1 本系統與ORB-SLAM2系統絕對軌跡誤差的RMSE[m]對比結果

為體現本文算法優越性，將其與近代較為先進的動態環境RGB-D SLAM系統DS-SLAM[15]、Detect-SLAM[16]進行對比。表2的實驗結果表明，與這些系統相比，本文方法在動態序列定位方面具有更好的性能。

表2 本系統與先進動態SLAM的RMSE[m]對比

4 結束語

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

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