基于語義與幾何約束的動態SLAM系統

丁國強，趙朋朋，李 濤，婁泰山

(鄭州輕工業大學 電氣信息工程學院，河南 鄭州 450002)

0 引 言

當前的SLAM研究大多基于靜態場景的假設[1，2]，但是在實際應用中，動態對象是不可避免的。這些動態對象將改變圖像信息，從而導致錯誤的數據關聯并會降低定位的精度。此外，一旦將動態信息加載到地圖中，將會導致“重影”[3]，從而影響地圖的可視化和應用。基于靜態場景的假設極大地限制了SLAM的發展和在現實生活中的應用。傳統SLAM[4]將動態對象視為錯誤數據關聯，并通過RANSAC[5，6](random sample consensus)對其進行過濾，但當動態目標太多時，RANSAC將失效。

在面向動態環境的視覺SLAM系統中[7-10]，特征可以分為兩類，靜態特征和動態特征。各種動態SLAM算法的目標基本上都是剔除動態特征，僅保留靜態特征，以提高系統的精度。楊世強等[11]首先利用幾何約束雙向投票模型進行動態特征粗過濾，然后通過RANSAC計算系統基礎矩陣，最終采用極線幾何來精確判斷動態特征并進行剔除；Wang等[12]應用基本矩陣檢測特征點的不一致性，然后對深度圖像進行聚類，當聚類區域中的異常值超過閾值時，該區域將被識別為移動對象。

為了應對動態環境的挑戰，使視覺SLAM更好得到應用，本文提出了一種基于語義和幾何約束的動態SLAM系統。將語義先驗信息與幾何約束相結合來處理動態特征，并且通過加入圖像補全線程來降低剔除動態特征后對位姿估計的影響，從而提高視覺SLAM系統在動態環境中的定位精度。最后，在公開數據集上進行測試，實驗驗證了本文算法的有效性。

1 系統設計

1.1 SLAM系統框架

目前大部分有關SLAM的研究基本上都是基于ORB-SLAM2開展的，ORB-SLAM2是目前較為完善的SLAM方案之一，因此本文也在此基礎上進行改進，如圖1所示。原始的ORB-SLAM2系統主要包括3個線程：跟蹤、局部建圖、回環檢測；本文針對動態環境的挑戰，加入了語義分割網絡，用來檢測特征點的狀態，針對剔除動態目標上的特征點后產生的掩膜空洞，加入了圖像補全線程，有利于提高位姿估計和地圖構建的精度。在輸入圖像后，并行運行語義分割網絡和跟蹤線程，進行運動一致性檢測，利用先驗的語義信息，判斷圖中的靜態區域，待判斷區域，背景區域，然后用靜態區域的特征點迭代出基礎矩陣，來確定待判斷區域的狀態。將動態區域的特征點剔除后，該區域的點將不參與后續的位姿估計與地圖構建，以致于會導致圖像特征的缺失，從而影響系統精度，因此采用圖像補全線程，通過相關算法，搜索動態區域的背景圖像塊來形成一個合成幀，就相當于SLAM運行在一個不包含動態物體的靜態區域，利用合成幀來估計相機位姿，可以有效提高相機位姿估計精度。

圖1 系統框架

1.2 語義分割網絡

為了檢測動態對象，本文采用了SegNet語義分割網絡，并在此基礎上加入了注意力機制。引入注意機制是為了提高語義分割模型的性能。注意力機制的想法是允許解碼器以靈活的方式利用輸入序列中最相關的部分，通過所有編碼輸入向量的加權組合，使相關性最高的向量被賦予最高權重。本文改進后的語義網絡結構如圖2所示。

圖2 改進的SegNet網絡框架

Badrinarayanan等提出了SegNet網絡結構，主要為了處理智能機器人領域中的圖像語義分割問題。SegNet是一種具有深度卷積的對稱網絡結構，并且提出了編碼器和解碼器的概念。該網絡的編碼器部分主要由VGG16網絡的前13個卷積層組成，并且沒有使用全連接層，與其它分割網絡相比，其網絡參數數量顯著下降。在Max pooling過程中，通過添加池化索引來記錄池化過濾器中最大特征值的位置，以便保留池化操作的位置信息。然后，將最大值恢復到池化索引所記錄的位置，并對其它位置用零填充，以確保上采樣過程中信息的準確性，上采樣池化恢復過程如圖3所示。解碼器部分則使用來自編碼過程所記錄的最大池化索引進行上采樣，確保了解碼器的上采樣特征信息的恢復，并很好地保證了檢測目標的邊緣信息。上采樣僅在解碼器部分使用，與編碼過程的池化層對應，并且上采樣步驟不用進行學習，只需要通過池化索引就可完成。最終解碼器輸出高維的特征到分類器中，獨立的對每個像素進行分類，預測的分割對應于每個像素具有最大概率處的類別。

圖3 上采樣恢復過程

本文在SegNet網絡的基礎上加入了通道注意力模塊，其中Max pooling是將目標區域輸出最大值，這種方式得到的特征圖可以保留更多的紋理特征；Average pooling則是對池化區域內的圖像取平均值，這種方式可以得到更多的背景信息。經過池化操作，得到兩組1×1×512維的向量，然后用1×1的卷積使它們降到1×1×256維，接著經過ReLU激活層增加模型的非線性能力，最后使兩組特征向量恢復到512維，將得到的權重乘以編碼器的輸出，可以有效提高網絡的分割能力。其中，語義分割效果與部分先驗信息如圖4所示。

圖4 語義分割效果與語義先驗信息

1.3 運動一致性檢測

對于兩幀圖片進行特征匹配，可以粗略得到特征點之間的匹配關系，但是因為場景中可能包含動態物體，因此會對位姿求解產生影響。一般來說，靜態物體的匹配精度的置信度比較高。因此，本文選擇使用先驗靜態匹配點對來估計模型。

根據之前的目標檢測網絡，我們已經得到了圖像中的先驗語義信息，根據一般經驗，可以事先定義訓練數據庫所包含物體的類別的靜態率，例如人、車、動物具有較低的靜態率，此類目標是本文主要關注的對象，需要進行優化；而電視，桌子具有較高的靜態率，如果將這個指標量化，可以表示為θ∈[0，1]， 本文定義靜態率大于0.9的區域為靜止區域，其余部分為待判斷區域。本文主要利用靜止區域的特征點和背景點去計算幀間圖像的基礎矩陣。

通過語義分割網絡，可以得到圖片中所有的掩膜區域，和掩膜的分類信息，讀取事先定義的物體的靜態率。因此可以將掩膜區域分為靜態掩膜，其中的特征點定義為staticP={P1，P2，P3，…Pn}， 和待判斷掩膜，其中的特征點集合為unknownQ={Q1，Q2，Q3…Qn}， 而除去所有掩膜區域的部分，定義為背景區域，靜態率為1，其中的特征點集合為backgroundM={M1，M2，M3…Mn}， 定義staticS為當前幀所有靜態特征點的集合，則

staticS=staticP+backgroundM

(1)

對兩幀圖片staticSK與staticSK-1中的特征點進行匹配，計算其漢明距離，可以得到兩幀圖片靜態特征點的對應關系。在此處使用RANSAC算法進行迭代，主要步驟為：

(1)在樣本空間staticS中隨機選擇8對匹配點；

(2)利用式(6)使用8點法計算基礎矩陣F；

(3)利用式(8)驗證其余點的幾何約束，統計符合約束的點的對數n；

(4)重復迭代步驟(1)～(3)，將n達到最大時的F作為兩幀圖像的基礎矩陣。

利用先驗靜態區域的特征點計算的基礎矩陣精度較高，本文采用運動一致性檢驗來驗證unknownQ中特征點的狀態。定義約束條件如圖5所示。

圖5 幾何約束

圖5中O1與O2代表相機光心，PA與PB分別為3D點P在圖像A與圖像B上的投影點，歸一化坐標分別表示為

pa=[u1v11]T，pb=[u2v21]T

(2)

對于pa與pb滿足關系

s1pa=KP，s2pb=K(RP+t)

(3)

其中，K為相機內參矩陣，R、t為相機間的變換關系，s1、s2表示尺度因子。

在理想情況下，圖片中一對匹配點滿足約束

(4)

其中可以將基礎矩陣F改寫為

f=[f11f12f13f21f22f23f31f32f33]T

(5)

然后對式(4)進行改寫，可以得到

[u1u2u1v2u1v1u2v1v2v1u2v21]f=0

(6)

其中，F中的元素全都包含在f中，pa與pb是一對匹配點，共有n對匹配點。而f中共有9個參數，且一般令f33=1來歸一化矩陣，共有8個未知數，因此需要8個方程來求解，對應于取圖片中8對匹配點，所以該方法稱為8點法。

l1與l2分別代表兩圖中的對極線，在同一平面內可用Ax+By+C=0來表示，則像點PA映射到極線l2上用方程可表示為

(7)

其中，F為前一步RANSAC迭代出來的基礎矩陣。

根據像點與極線的映射關系，可以得出特征點是否符合約束，從而判斷特征點的狀態。如果pb不在極線l2上，則需要計算像點到極線的距離

(8)

理想狀態下，d應該等于0，而在現實場景中，由于相機采集的照片不可避免的存在一定程度的畸變和噪聲，使得相鄰幀間的點無法完美匹配在極線上。因此設定一個閾值T，當d>T，則判定該點為動態點。由于像點與極線的距離與特征點的視差值相關，因此閾值T定義為

T=αf+ε

(9)

其中，α代表視差系數，f代表特征點的視差值，也就是相鄰兩幀像素坐標的變化。通過對特征點與極線關系判斷，確定動態特征點并進行篩除，如圖6所示。

圖6 動態特征點剔除效果

1.4 圖像補全

經過運動一致性檢測環節，環境中運動的物體并不參與位姿估計，而直接將運動目標濾除，必然會導致特征缺失，并在圖像中產生掩碼空洞，如圖7所示，影響后續的位姿估計和建圖，因此需要補充空洞區域。

圖7 掩碼空洞

視頻補全方法旨在使用每幀中其余部分相似的內容來填充目標空洞。目前很多視頻補全算法[13]僅針對固定攝像機，無法很好應用到SLAM中。因此本文采用一種非參數的基于圖像塊的補全算法[14]。該算法可以處理使用移動攝像機捕捉的動態場景，目前大部分方法還未涉及此類問題，因為視點變化會提升問題的復雜度。主要通過聯合估計缺失區域中的光流和顏色來解決這個問題。

在該算法中，首先使用兩幀光流算法計算所有相鄰幀的已知區域中的正向和反向光流。經過初始化后，在每個尺度下，主要迭代3個步驟：

(1)最近鄰場估計：在局部外觀最相似的源圖像塊中尋找與目標圖像塊近似的圖像塊來最小化顏色空間誤差；

(2)顏色更新：顏色更新通過圖像塊投票進行重建，其中未知像素處的像素顏色通過對所有重疊圖像塊中相應最近鄰區域的顏色進行平均來估計，并且沿著光流向量搜索已知像素來合并已知區域中最近的相鄰時刻的顏色，使用前向、后向流動一致性來識別不可靠流動的區域；

(3)光流更新：在估計目標區域的顏色后，細化正向和反向光流場。固定源區域中的光流，僅更新目標區域中的光流，從而得到具有時空孔邊界約束的光流估計算法。

該算法補全效果如圖8所示，最終任務歸結為求解目標函數

圖8 補全結果

(10)

其中，Ecolor-spatial表示目標區域中基于空間圖像塊的顏色重建誤差；Ecolor-temporal表示合成顏色的時間不一致性；Eflow-spatial表示正向和反向流場的空間正則化。

對于顏色空間誤差，可以使目標區域的局部鄰域Pi近似為源區域的局部鄰域Qi。由于考慮了重疊區域，因此可以保證空間一致。目標圖像塊Pi是一個以ti為中心的5×5軸對稱區域，而Qi可通過Pi以si為中心，以θi為比例變換得到。顏色空間誤差表示為所有重疊圖像塊的平方損失之和

(11)

而顏色時間誤差可以保證正向和反向流向量的相鄰幀之間顏色時間的一致性

(12)

對于光流空間誤差，為了促進分段平滑光流場，引入了空間正則化

(13)

1.5 位姿估計

經過之前的運動一致性檢測剔除動態點，并對空洞區域進行補全后，利用合成幀提取特征點，求解相機位姿，更新位姿優化初值。假設有n個3D空間點Pi=[Xi，Yi，Zi]， 以及這些點在二維圖像中的像點pi=[ui，vi]， 建立起Pi與pi的關系如式(14)所示，變換為參數形式如式(15)所示。其中si為尺度因子，K為相機的內參，ξ∧為位姿變換的李代數矩陣，exp(ξ∧) 表示為向量到矩陣的指數映射，e代表誤差項

(14)

sipi=K·exp(ξ∧)Pi+e

(15)

對相鄰幀構建最小二乘問題，就可以得到優化后的相機位姿

(16)

2 實 驗

本文運行測試平臺為筆記本電腦，其硬件配置為Intel Core i5-9400H，主頻為2.3 GHz，內存為8 G，顯卡為GTX 1050，搭載系統為Ubuntu 16.04。

2.1 數據集

實驗所需的TUM RGB-D數據集是慕尼黑工業大學提供的一個大型開源數據集，其中包含RGB圖、深度圖和真實軌跡，可以用來評估視覺SLAM系統。

在測試中，本文主要使用數據集中的7個序列，且將它們定義為：freiburg2_desk序列是靜態環境；freiburg3_sitting_xyz、freiburg3_sitting_static序列是低動態環境；freiburg3_walking_static、freiburg3_walking_xyz、reiburg3_walking_halfsphere、freiburg3_walking_rpy序列是高動態環境，關于數據集的介紹可參見文獻[15，16]。為了后續表述簡便，本文將數據集序列中的freiburg簡寫為fr，將sitting簡寫為s，將walking簡寫為w，將halfsphere簡寫為hs。

目前，視覺SLAM系統主要使用兩種評估指標，包括絕對軌跡誤差(absolute trajectory error，ATE)和相對位姿誤差(relative pose error，RPE)，具體定義請參見文獻[17]。此外，我們還使用均方根誤差(root mean square error，RMSE)和標準差(standard deviation，SD)來判斷本文算法的準確性。

2.2 相機位姿誤差分析

定量比較結果見表1～表3，比較了本文算法和ORB-SLAM2在7個數據集序列中的實驗結果。評價指標為RMSE、平均誤差(Mean)和SD。這些評價指標顯示了估計軌跡與系統真實值之間的差異，可以此來分析系統的性能。

表1 絕對軌跡誤差對比結果

表中本文算法相較于ORB-SLAM2性能的提升率可以表示定義為

(17)

其中，δ表示提升率，α表示ORB-SLAM2算法的軌跡誤差，β表示本文算法的軌跡誤差。

在表1中展示了算法之間的ATE對比結果，表2與表3分別是在不同序列中，算法之間的相對軌跡平移誤差和相對軌跡旋轉誤差的對比結果。分析表1～表3中的數據，在高動態場景中，相較于ORB-SLAM2，本文算法ATE和RPE的各項數值明顯降低了，其中ATE部分的RSME最高降低了98.4%，RPE平移和旋轉誤差部分最高分別降低了98.35%和97.52%，因為ORB-SLAM2無法處理相機的復雜運動疊加場景高速運動，容易定位丟失，而本文如入了運動一致性檢測模塊和語義分割模塊來檢測動態物體，使SLAM可以在高動態場景中工作。而對于靜態和低動態場景，本文算法提升幅度不大，因為對于ORB-SLAM2算法本身具有一定的魯棒性，可以處理一些低動態場景。

表2 相對軌跡誤差平移部分對比結果

表3 相對軌跡誤差旋轉部分對比結果

對于圖9、圖10中，主要展示了本文算法與ORB-SLAM2在數據集的高動態序列上估計軌跡與真實軌跡的誤差和兩算法的相對位姿誤差，從圖中可以看出，ORB-SLAM2算法不能很好處理高動態場景，具有很大的軌跡誤差。而觀察本文算法的誤差圖，可以發現可以有效減小動態對象對系統的影響，因為本文利用語義網絡提取先驗信息，然后與幾何約束相結合剔除了場景中的動態特征點，并且圖像補全線程補充了動態特征剔除后的圖像信息，相當于把原來的動態場景轉化為靜態場景，可以有效降低系統軌跡誤差，提高位姿估計的精度。

圖9 fr3_w_xyz序列上的ATE和RPE

圖10 fr3_w_hs序列上的ATE和RPE

然后為了進一步驗證本文算法的有效性，計算出其它面向動態環境的視覺SLAM系統的各項誤差指標，并與本文算法進行對比，由表4可以得知，與目前一些先進的應用于動態環境的SLAM相比，本文算法也具有一定的競爭性。結果表明，該方法可以有效消除動態對象的干擾，從而降低優化過程中的位姿誤差。

表4 同類算法ATE對比

2.3 算法實時性測試

這部分實驗用于評估每個系統的跟蹤時間。平均跟蹤時間見表5。本文SLAM算法主要側重于位姿估計和軌跡誤差，因此對SLAM系統實時性能提升不大，主要是由于本文加入了語義分割網絡來提取語義信息，分割圖像，加入了運動一致性檢測，用來剔除動態點，之后還對剔除動態點后的圖像進行了補全，來優化位姿估計，這些線程都會產生時間成本。因此下一步需要進一步優化網絡結構，進一步提高SLAM系統的實時性。

表5 不同系統的平均定位跟蹤時間

3 結束語

本文利用改進的語義分割網絡提取語義先驗信息，結合幾何約束剔除動態特征，并且對產生掩碼空洞的圖片進行補全。最后，在公開的TUM數據集上進行了評估。結果表明，在動態環境下，該算法可以顯著提高定位精度。然而，在室外大規模、多動態目標場景中，仍會出現定位丟失的情況。目前，該算法可以應用在各種靜態場景，以及一些室內動態場景中。在未來研究中需要進一步優化網絡結構，以應對復雜環境中的各種挑戰，更好的實現機器人的導航與定位。