動態環境下的語義視覺SLAM算法研究

華春生，郭偉豪

(遼寧大學 信息學院，遼寧 沈陽 110036)

0 引言

同步定位與地圖構建(Simultaneous localization and mapping，SLAM)是指載體利用傳感器在未知環境移動的過程中估計自身位姿的同時，進行地圖的構建.由于機器人的主要傳感器為視覺SLAM，它具有性價比高、體積小、可獲得豐富的環境信息等特點，所以近年來SLAM在機器人領域的應用得到廣泛關注.研究者提出了很多優秀的視覺SLAM算法，例如MonoSLAM[1]、PTAM[2](Parallel tracking and mapping)、LSD-SLAM[3]、DSO[4]、ORB-SLAM2[5]等.但是這些算法都是建立在所遇到的場景是靜態的假設基礎上，即圖像的信息變化只取決于相機本身的運動.然而在實際的應用場景中很難滿足這種假設，會不可避免地出現動態物體，例如行走的人、移動的物體等.雖然可以采用隨機抽樣一致性檢測(RANSAC)[6]來減少一些小物體的異常值，但是較大物體和移動較多的情況卻很難處理.這不但會給系統帶來誤差還會影響到位姿估計和地圖的構建.深度學習近年來發展迅速，將視覺SLAM和深度學習相結合的解決方案應運而生.利用深度學習技術來提取語義信息并融合到視覺SLAM中，不僅可以有效去除動態點，而且可以在幾何和語義兩個層面上對周圍環境進行一些高層次的理解，建立具有語義信息的三維地圖，為機器人執行高級任務做準備.Mclormac等[7]利用卷積神經網絡和Whelan等[8]結合構建語義地圖，都沒有考慮動態物體移動較多的問題.Runz等[9]和He等[10]將MASK-RCNN和基于幾何分割的算法相結合，實現了多物體跟蹤識別和重建，但是對非剛性物體無法重建.Bescos等[11]利用MASK-RCNN和多視角幾何相融合，能夠剔除動態物體并進行背景修復，但是，該算法會把有一定概率移動的對象移除掉，例如靜止的車.這會導致留下的特征點太少，進而影響到系統的位姿估計等.Lu等[12]提出了一種基于分布和局部的算法(DM-SLAM)，并設計了一種基于鄰域互斥的候選地圖選擇機制，減少了動態物體的影響.這種算法局限性在于大量靜止物體出現在場景中，此后在跟蹤移動物體時，算法就會失效.Yang等[13]提出語義和幾何約束算法提高了位姿估計精度，但是當動態對象占據當前幀的一半時，基礎矩陣不能準確反映整個幀，會導致對極約束的錯誤.

由于在動態環境下收到動態對象的干擾(類似于行人等移動物體的出現)，一方面會導致定位精度的降低，累計誤差不斷增大，嚴重時會導致系統崩潰；另一方面保存在地圖中的動態對象會嚴重影響地圖的使用價值.針對現有的視覺SLAM在動態環境中存在的魯棒性與穩定性的問題，本文在ORB-SLAM2系統的基礎上進行改進，提出動態點檢測和語義分割相結合的算法，剔除動態點，減少動態目標的影響，并構建出三維語義地圖.

1 系統介紹

ORB-SLAM2算法是目前應用最廣泛、系統最完整的視覺SLAM系統之一，同時支持單目、雙目和RGB-G相機.因此本文將基于ORB-SLAM2算法進行改進，在提高精度的同時增加系統的穩定性.改進后的系統框架如圖1所示，在ORB-SLAM2算法的跟蹤線程里嵌入動態點檢測，另外增加了語義分割線程和地圖構建線程.系統的流程是先將RGB圖像序列同時輸入到跟蹤線程和分割線程，通過動態點檢測算法和語義分割獲得的結果來檢測動態物體，然后刪除動態對象的特征點.最后通過穩定的靜態特征點來估計姿態，并構建語義地圖.

圖1 系統框架

2 動態特征點檢測與剔除

2.1 光流法

光流法是一種利用圖像序列中的像素在時間域上的變化和相鄰的圖像幀之間的相關性來求解前一幀和當前幀之間的對應關系，進而求解對象運動信息的方法.光流法可以分為稀疏光流和稠密光流，稀疏光流只是針對圖像中的部分像素進行計算，并且計算開銷要比稠密光流小.其中較為典型的是LK光流[14].本文利用LK光流提取特征點和求解基礎矩陣.

LK光流的使用需要遵循以下3個假設條件.

1)灰度不變假設：圖像中相同空間點的像素灰度值在每個圖像中是不變的.

2)小運動假設：相鄰幀間的運動位移要小.

3)空間一致假設：相鄰的像素要具有相似的運動.

空間中某個點，在t時刻，假設其像素坐標為(x，y)，在t+dt時刻設它運動到了(x+dx，y+dy)處.根據灰度不變假設，可以得到

I(x+dx，y+dy，t+dt)=I(x，y，t)

(1)

左邊進行泰勒級數展開并保留一階項得到

(2)

由于灰度不變假設，故下一時刻的灰度等于之前的灰度，從而有

(3)

兩邊除以dt，得

(4)

其中令Vx=dx/dt，表示像素在x軸上的速度.令Vy=dy/dt，表示像素在y軸上的速度.令Ix=?I/?x，Iy=?I/?y，分別表示像素在該點處x方向的梯度，y方向上的梯度.圖像灰度對連續時間的變化量可以記為It，上式可以寫成如下形式：

IxVx+IyVy=-It

(5)

考慮一個窗口的大小為ω×ω，即含有ω2個數量的像素，因此可建立如下方程：

AV=-b

(6)

V=-(ATA)-1ATb

(7)

這樣就可以求得像素在圖像間的運動速度，進而求得像素在若干塊圖像中出現的位置.但以上3種假設在實際的應用中不能完全滿足.當物體移動較快時往往會破壞這種假設，進而會導致計算的光流值出現較大的誤差，為了改善此問題可以采用圖像金字塔LK光流法.圖像金字塔通過縮放等操作，獲得不同分辨率的圖像.計算光流時，可以首先從頂部的圖像計算，得到結果后，將其作為下一層要計算的初始值.這種方法的好處在于，當原始圖像的像素運動較大時，對于金字塔的頂層來說，運動仍然是在一個很小的范圍內，進而滿足了基本假設.

2.2 動態點檢測

本文提出的動態點檢測算法主要分為預檢測和極線約束兩個步驟.

圖2 對極幾何約束

第一步，預檢測：①使用稀疏LK光流得到上一幀和當前幀圖像相匹配的特征點；②根據以下方法處理匹配的特征點，如果匹配的特征點特別靠近圖像的邊緣，則丟棄該對特征點.

第二步，對極幾何約束：①對預檢測之后當前幀和上一幀中剩余的特征點進行特征匹配；②從匹配的特征點對中使用八點法來計算基礎矩陣F；③利用基礎矩陣和上一幀中的特征點計算出當前幀對應的極線；④計算當前幀中的匹配特征點到極線的距離，和預設閾值比較，如果大于閾值則將其視為動態點.

(8)

式中A，B，C為線向量.對于當前幀的每個匹配點，計算其到極線的距離，如果大于預設閾值dthr，則視為動態點.距離計算公式為

(9)

2.3 剔除動態特征點

通過動態點檢測算法檢測完動態點后，無法獲得完整動態區域邊界輪廓，而語義分割可以完整地分割出動態區域.考慮到本系統是為實際場景的應用而設計的，所以要在實時性和準確性之間取得平衡.本文采用SegNet[15]語義分割網絡模型，并使用Pascal VOC數據集訓練該模型.SegNet網絡是一種適合實時應用的快速、小存儲空間的網絡模型，它可以將圖像中的對象進行分割，能夠達到像素的級別.本文將動態點檢測結果和語義分割結果相結合，去除動態點的示意圖如圖3所示，圈出來的點為檢測出的動態點，語義分割可以分割出精確的輪廓.如果檢測出一定數量的動態點是在語義分割的動態對象區域內，則將該對象視為動態物體，然后將該對象內的特征點去除，而剩下的特征點可以用于后續的跟蹤和建圖.通過這種方式可以極大地提高魯棒性和精度.

圖3 剔除動態點

3 地圖構建

3.1 單幀點云生成

單幀點云是構建全局地圖的基礎，每一個點都包含位置信息和深度信息，根據相機模型將二維圖像上的點映射到三維空間，從而得到對應的點云.根據相機模型，空間點與對應的像素坐標之間變換關系如下：

(10)

其中f表示焦距，c表示相對于原點的偏移量，K表示相機內參.設圖像中某一點的深度為d，則對應的空間點為

(11)

進而得到

(12)

將所有的像素按照式(12)進行轉換即可得到相應的點云.

3.2 點云的拼接與濾波

圖4 點云拼接示意圖

將關鍵幀對應的點云生成后進行拼接即可形成局部點云地圖.由于局部點云在拼接的過程中會出現關鍵幀重疊的情況，導致視野重疊區域存在很多位置相近的點，極大地浪費了存儲空間，因此需要進行體素濾波.通過體素濾波后可以使相應的空間內只有一個點，從而節約了存儲空間.點云拼接的示意圖如圖4所示.數學表達式為

(13)

其中：Ck表示單幀點云，Tk表示相機位姿，m表示局部點云.

4 實驗結果與分析

4.1 實驗環境

ORB-SLAM2算法作為視覺SLAM領域最為經典的算法之一，因其優秀的穩定性和實用性被廣泛應用到各個領域，而本文的算法也是在其基礎上進行改進.因此本文算法實驗的各種數據將與ORB-SLAM2相比較，以此來驗證在動態環境下本文算法的魯棒性.本文使用TUM RGB-D公開數據集中的walking序列進行測試，分別是freiburg3_walking_static，freiburg3_walking_xyz，freiburg3_walking_halfsphere.walking序列中兩名測試人員會圍繞桌子來回走動，屬于高動態場景.

實驗環境為Ubuntu 18.04，實驗平臺為一臺配備Intel Xeon CPU E5-2678 v3，12GB內存和一個NVIDIA Tesla K80 GPU的計算機.絕對軌跡誤差(Absolute trajectory error，ATE)和相對位姿誤差(Relative pose error，RPE)[16]是用來評價算法優劣的最常用的指標.本文將用以上兩項指標作為算法精度評估的指標.絕對軌跡誤差用于衡量估計軌跡與真實軌跡的一致程度，即軌跡準確度.相對位姿誤差用于評估在一定時間間隔內的局部軌跡精度，并可用于評價位姿估計時產生的漂移量.

4.2 誤差分析

如圖5(a)所示，在以活動的人為對象的動態場景中，從表1和圖6、圖7可以觀察出原始的ORB-SLAM2算法估計軌跡存在很大的誤差.而經過本文的改進，在加入了語義分割之后可以精確地分割出動態目標，把動態對象(即人身上)的特征點去除.不僅消除了動態對象對位姿估計的影響，而且定位精度也取得了很大的提升，圖6最能直觀地看出增加了語義分割后定位精度的提升效果.

圖5 動態環境測試圖

實驗的結果如表1所示，其中第一列有3種相機運動類型：①static表示相機處于靜止狀態；②xyz表示相機會沿著xyz軸運動；③halfsphere表示的是相機會按照半球面來運動.其中提升率計算公式為

(10)

式中α表示ORB-SLAM2算法，β表示本文算法.在實驗結果中計算了均方根誤差(Root mean squared error，RMSE)，平均值(Mean)和標準差(Standard deviation，S.D.)這3個參數，平均值反映估計位姿的精度，均方根誤差和標準差更能體現系統的穩定性和魯棒性.

表1 絕對軌跡誤差(ATE)

由表1可知，在高動態場景下，本文算法的RMSE數值相比較于ORB-SLAM2算法平均下降了95.32%，反映系統魯棒性的標準差S.D.則下降了94.00%.實驗結果表明了在高動態環境中，本文算法比ORB-SLAM2算法具有更好的魯棒性和穩定性.

圖6(a)是ORB-SLAM2算法在數據集walking_xyz下的相機軌跡圖，圖6(b)是本文算法所估計的相機軌跡圖.圖中虛線表示的是數據集的真值，實線表示的是各個算法的相機估計軌跡值.通過觀察圖中兩種線條的重合度，明顯可以看出本文算法估計的相機軌跡和真值基本重合，相比較于ORB-SLAM2算法，準確度有很大的提升.圖7(a)和(b)展示了ORB-SLAM2算法和本文算法的絕對軌跡誤差圖.通過圖7(a)可以觀察出ORB-SLAM2算法的絕對軌跡誤差主要分布在0.3 m到0.5 m之間，還有一些時間的誤差快達到了1.0 m，誤差在0.1 m內的時間則極短.而從圖7(b)中可以看出本文算法估計的軌跡誤差基本都是在0.015 m到0.028 m的區間內，最小的誤差能達到0.005 m，最大的誤差都不超過0.08 m.從圖6和圖7中都能非常直觀地看出本文算法相較ORB-SLAM2算法，在動態環境中的軌跡誤差有很大幅度的降低，大大提高了位姿估計的準確度.

圖6 軌跡真實值與估計軌跡

圖7 絕對軌跡誤差圖

4.3 語義建圖實驗

在語義建圖實驗中，本文選擇了walking_static數據集.在此序列中會有兩個人坐在椅子上，或者圍繞著桌子進行走動，屬于高動態場景.圖8(a)展示了ORB-SLAM2算法構建的地圖，從圖中可以看出ORB-SLAM2沒有去除動態目標，會影響機器人執行路徑規劃等任務.圖8(b)展示了經過語義分割和動態點檢測算法結合去除動態特征點之后的語義地圖效果，從圖中可以看出本文算法去除了場景中大部分動態特征點，建立了點云地圖，可以用機器人執行高級任務.

圖8 ORB-SLAM2算法與本文算法構建地圖效果

5 結束語

本文為了解決環境中出現動態對象而影響視覺SLAM算法的精度和缺乏語義信息的問題，基于ORB-SLAM2算法提出了一個動態點檢測算法，嵌入到ORB-SLAM2算法的追蹤線程中，并增加一個語義分割線程和稠密建圖線程.通過動態點檢測算法與語義分割相結合去除動態對象上的特征點，以減少動態對象對系統位姿估計的影響，并建立了語義地圖.在公開數據集上的實驗結果表明，在動態場景下本文算法比ORB-SLAM2算法大幅度提升了位姿估計的準確度，提高了系統的魯棒性和穩定性.在未來的工作中將改進稠密語義建圖的功能，使之用于機器人執行高級任務.