李 博，段中興

(西安建筑科技大學(xué) 信息與控制工程學(xué)院，西安 710055)

1 引 言

視覺同步定位與建圖(Visual Simultaneous Localization and Mapping，VSLAM)是指機器人系統(tǒng)利用相機作為唯一的外部傳感器，在進行自身定位的同時創(chuàng)建環(huán)境地圖[1].視覺里程計(Visual Odometry，VO)[2]是VSLAM系統(tǒng)的前端，它能夠根據(jù)相機采集的圖像信息來估算相機的當(dāng)前位姿并構(gòu)建周圍環(huán)境的局部地圖.視覺里程計根據(jù)是否需要提取特征點可以分為基于特征點法的視覺里程計以及不需要提取特征點的直接法視覺里程計[3].

多年來，經(jīng)過研究人員們的不斷努力，視覺里程計與VSLAM領(lǐng)域涌現(xiàn)出了許多優(yōu)秀的成果，例如Mono-SLAM[4]，ORB-SLAM2[5]，PTAM(Parallel Tracking and Mapping)[6]，RGB-D SLAM[7]，LSD(Large Scale Direct)-SLAM[8]等.其中，Raúl Mur-Artal等人提出的基于特征點法的ORB-SLAM2系統(tǒng)能夠在大型環(huán)境中實時運行，具有良好的精度與穩(wěn)定性，并且支持單目、雙目以及RGB-D 3種模式.然而，這些傳統(tǒng)的VSLAM系統(tǒng)都是基于外部環(huán)境為靜態(tài)的假設(shè)[9]，并未考慮場景中存在動態(tài)物體的情況.但是在實際環(huán)境中由于動態(tài)物體的存在，使得系統(tǒng)在進行特征提取時會不可避免地提取到動態(tài)物體范圍內(nèi)的特征點，從而影響相機位姿的估算精度.因此如何減少動態(tài)物體的干擾，提高VSLAM系統(tǒng)在動態(tài)環(huán)境下的定位精度與魯棒性，成為了目前VSLAM領(lǐng)域的一個研究熱點.

為了解決這一問題，Kundu[10]等人利用對極幾何約束來剔除特征點中的動態(tài)點；Fang[11]等通過光流法來過濾環(huán)境中的動態(tài)物體；Tan[12]等采用自適應(yīng)的RANSAC算法來篩選圖像中的靜態(tài)點.這些僅依靠幾何或光流的算法雖然在一定程度上能夠改善系統(tǒng)的定位精度，但由于缺乏先驗的語義信息，提升能力有限，并且當(dāng)環(huán)境中的動態(tài)物體較多時很容易失效.

近年來隨著深度學(xué)習(xí)的發(fā)展，越來越多的結(jié)合深度學(xué)習(xí)的方案被提出，Bescos[13]等人提出的DynaSLAM在ORB-SLAM2基礎(chǔ)上通過Mask R-CNN語義分割網(wǎng)絡(luò)和多視角幾何來剔除動態(tài)特征點.Yu[14]等也在ORB-SLAM2的基礎(chǔ)上提出了DS-SLAM，該算法采用SegNet網(wǎng)絡(luò)與運動一致性檢驗結(jié)合的方式濾除動態(tài)特征點.這些結(jié)合深度學(xué)習(xí)的方式相較于之前的方法能夠更好的過濾動態(tài)點，大幅提高定位精度，但實時性較差，如，DynaSLAM中在語義分割時使用Mask-RCNN網(wǎng)絡(luò)，其速度只能達到5Hz，很難做到實時運行；而DS-SLAM雖然采用了比較快速的SegNet網(wǎng)絡(luò)，但在進行運動一致性檢測時使用光流跟蹤法，極大增加了運算量，對系統(tǒng)的實時性產(chǎn)生了一定影響.

為了能夠有效提高VSLAM系統(tǒng)在室內(nèi)動態(tài)環(huán)境下的定位精度，同時滿足實時性的要求，本文以O(shè)RB-SLAM2的視覺里程計為基礎(chǔ)進行改進，主要工作及貢獻如下：

1)開辟了一條獨立的動態(tài)目標檢測線程，使用YOLOv4進行動態(tài)目標檢測，將其與ORB-SLAM2的視覺里程計相結(jié)合.

2)提出了一種動態(tài)特征點剔除策略，首先根據(jù)動態(tài)目標檢測結(jié)果進行動態(tài)點一次過濾，接著分別利用對極幾何約束以及光流約束完成第2、3次過濾.

3)在TUM RGB-D公開數(shù)據(jù)集上對所提方法的定位精度以及實時性進行了評估.

2 算法介紹

2.1 算法框架

改進的視覺里程計框架如圖1所示，在ORB-SLAM2視覺里程計的基礎(chǔ)上新增了一條動態(tài)目標檢測線程，使用快速目標檢測網(wǎng)絡(luò)YOLOv4得到輸入圖像中的語義信息并確定動態(tài)目標.在追蹤線程中加入動態(tài)點剔除環(huán)節(jié)，該環(huán)節(jié)首先根據(jù)目標檢測的結(jié)果來剔除該動態(tài)物體范圍內(nèi)的特征點.接著為防止在目標檢測過程中出現(xiàn)漏檢，以及環(huán)境中還存在潛在動態(tài)物體的情況，在完成相鄰兩幀圖像的特征匹配后采用RANSAC算法求解出兩圖像間的基礎(chǔ)矩陣，再利用對極幾何約束對環(huán)境中可能存在的剩余動態(tài)點進行二次過濾.然后使用金字塔LK光流法獲得其余特征點的光流信息，根據(jù)靜態(tài)點與動態(tài)點光流大小的不同采用統(tǒng)計學(xué)的方式徹底濾除動態(tài)點.后續(xù)依靠過濾后剩余的靜態(tài)特征點進行相機的位姿估計.

圖1 本文視覺里程計結(jié)構(gòu)Fig.1 Overview of proposed visual odometry

2.2 動態(tài)目標檢測

YOLO[15]是一種基于深度神經(jīng)網(wǎng)絡(luò)的目標檢測算法，其最大的特點就是快，能夠在保持較高檢測精度的同時實現(xiàn)對整幅圖片和視頻的實時檢測.YOLOv4[16]是Alexey Bochkovskiy等人在YOLOv3[17]的基礎(chǔ)上提出的，巧妙結(jié)合了多種優(yōu)化技巧，實現(xiàn)了檢測速度和精度的最佳權(quán)衡，并且使模型能夠在常規(guī)GPU上進行訓(xùn)練和檢測，極大降低了訓(xùn)練成本.YOLOv4在MS COCO數(shù)據(jù)集上的AP達到了43.5%，在Tesla V100上的檢測速度達到了65FPS，因此本文選用YOLOv4作為目標檢測網(wǎng)絡(luò)來檢測環(huán)境中的動態(tài)物體，能很好地兼顧檢測精度與實時性.YOLOv4的網(wǎng)絡(luò)結(jié)構(gòu)如圖2所示.

圖2 YOLOv4網(wǎng)絡(luò)結(jié)構(gòu)Fig.2 YOLOv4 network structure

對比YOLOv3，YOLOv4的主干網(wǎng)絡(luò)框架由Darknet53變?yōu)榱薈SPDarknet53，該主干網(wǎng)絡(luò)較Darknet53有兩個主要的改進：1)改變了殘差結(jié)構(gòu)，運用了CSPnet網(wǎng)絡(luò)的殘差結(jié)構(gòu)；2)改變了激活函數(shù)，采用Mish激活函數(shù).同時Yolov4引入了空間金字塔池化(SSP)，采用不同尺寸的池化核，極大地增加感受野，分離出最顯著的上下文特征.此外，為最大限度的提取特征，YOLOv4將YOLOv3中的參數(shù)聚合方法FPN替換為PANet.

選用MS COCO數(shù)據(jù)集對YOLOv4網(wǎng)絡(luò)進行訓(xùn)練，COCO數(shù)據(jù)集是一個圖像識別，分割以及字幕的大型數(shù)據(jù)集，其中包含了80個類別.一般情況下認為在室內(nèi)環(huán)境中最主要的動態(tài)物體為人和小動物，因此針對COCO數(shù)據(jù)集中的類別，將人，貓，狗作為動態(tài)物體，剩下的都暫時默認為靜態(tài)物體，在使用YOLOv4進行目標檢測時只檢測這些動態(tài)物體.

2.3 動態(tài)特征點剔除

2.3.1 動態(tài)點一次過濾

在完成動態(tài)目標檢測后，先將動態(tài)物體范圍內(nèi)的特征點去除.假設(shè)輸入的第K幀圖像中的特征點集合如式(1)所示：

(1)

第K幀圖像中動態(tài)物體的集合如式(2)所示：

(2)

通常情況下在經(jīng)過第一次動態(tài)點過濾后已經(jīng)可以剔除全部的動態(tài)特征，但還可能存在以下特殊情況：

1)動態(tài)目標檢測時僅將人和貓，狗視為動態(tài)物體而忽略了環(huán)境中還可能存在的潛在動態(tài)物體，如被人拿起的書本或是被人挪動的桌椅等.這些物體在一般情況下都默認為靜態(tài)物體，但是如果發(fā)生以上特殊情況就會變?yōu)閯討B(tài)物體.

2)由于動態(tài)物體被大面積遮擋等原因，在進行動態(tài)目標檢測時可能會出現(xiàn)漏檢的情況.

因此，僅僅依靠第一次過濾并不能保證將圖像中的動態(tài)點全部剔除，在此基礎(chǔ)上還需要對剩余特征點進一步過濾.

2.3.2 對極幾何約束

1)從匹配點對中隨機選出4組用來求解基礎(chǔ)矩陣F.

2)計算其余特征點到矩陣F所對應(yīng)極線的距離d，將該距離與事先設(shè)定好的閾值dn比較，當(dāng)ddn，判定該點為外點.記下內(nèi)點的數(shù)目為n.

3)重復(fù)執(zhí)行上述1)，2)步迭代N次，選取其中內(nèi)點數(shù)目最多的一次，將該次所求得的基礎(chǔ)矩陣作為最終的基礎(chǔ)矩陣.

求得基礎(chǔ)矩陣之后對所有的匹配點對進檢驗，判斷它們是否滿足對極幾何約束.如圖3所示.

圖3 對極幾何約束Fig.3 Epipolar geometry constrain

(3)

(4)

(5)

(6)

dthr1

(7)

dthr1，dthr2為設(shè)定好的閾值.當(dāng)特征點到極線的距離超出閾值范圍，則將該點視為動態(tài)點或誤匹配點并剔除.

雖然用對極幾何約束的方式在一定程度上能夠剔除動態(tài)點，但是當(dāng)動態(tài)物體的運動方向平行于相機的運動方向時，該動態(tài)物體上的特征點也是滿足對極幾何約束的，而這時對極幾何約束就不再適用.因此，經(jīng)過此次檢驗后仍有可能存在動態(tài)特征點，這時就還需要結(jié)合光流約束進行第3次過濾.

2.3.3 光流約束

光流是空間運動物體在觀察成像平面上的像素運動的瞬時速度.在室內(nèi)環(huán)境下，當(dāng)相機運動時，背景中所有靜態(tài)點的光流大小應(yīng)該基本相同，因為它們都是隨著相機的運動而運動.但是動態(tài)物體往往是獨立于背景運動，因此動態(tài)點的光流大小通常與靜態(tài)點不同.根據(jù)這個原理，計算出剩余特征點的光流大小，然后用統(tǒng)計學(xué)的方式剔除剩余的動態(tài)點.

計算光流的大小使用LK光流法，該算法基于3個假設(shè)：

1)亮度恒定不變，即當(dāng)同一目標隨時間變化而移動時，其亮度值不變;

2)時間連續(xù)或運動是“小運動”，即時間的變化不會引起目標位置的劇烈變化，相鄰幀之間位移要比較小;

3)空間一致，即前一幀圖像中相鄰的目標在后一幀中也是相鄰的.

根據(jù)亮度不變假設(shè)得到式(8)：

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

(8)

t和t+dt為獲取上一幀和下一幀圖像的時刻，I(x,y,t)，I(x+dx,y+dy,t+dt)分別為某像素點在上一幀和下一幀圖像中的位置.

根據(jù)小運動假設(shè)，對上式右側(cè)進行泰勒級數(shù)展開，根據(jù)小運動假設(shè)，對上式右側(cè)進行泰勒級數(shù)展開，結(jié)果如式(9)所示：

(9)

根據(jù)式(8)、式(9)可得式(10)：

(10)

IxVx+IyVy=-It

(11)

根據(jù)空間一致性假設(shè)，選取3×3窗口內(nèi)的9個像素點聯(lián)立，建立如式(12)所示：

AV=b

(12)

ATAV=ATb

(13)

V=(ATA)-1ATb

(14)

在求解過程中應(yīng)用了泰勒展開，泰勒展開只有在變量變化很小的情況下才能使用，如果幀間的像素運動比較大，泰勒展開便不適用了.而在實際場景中是很難滿足LK光流法的3條假設(shè)的，尤其是假設(shè)二.因此為防止像素運動太快，無法使用泰勒展開，在實際計算時采用金字塔LK光流法.其基本思想是：將整張圖片進行縮小，降低其分辨率，對于運動較快的像素點，總能在圖像分辨率降到一定程度時，其運動變得足夠小，滿足泰勒展開的條件.

通過金字塔LK光流法求得剩余特征點的光流大小后，計算其平均值與標準差，采用一種統(tǒng)計學(xué)的方式來判斷特征點是否為動態(tài)點.判斷條件如式(15)和式(16)所示：

|Li-Lavg|>2×Lstd

(15)

或

|Li-Lavg|>Lthr1(Lstd

(16)

其中，Li為第i個特征點的光流大小，Lavg，Lstd分別為所有特征點光流的均值以及標準差，Lthr1，Lthr2為設(shè)定的閾值.當(dāng)Li符合上面的關(guān)系時，便認為該點為動態(tài)點并剔除.

在使用對極幾何約束與光流約束來濾除動態(tài)點時，若圖像中的動態(tài)點較多，則計算的基礎(chǔ)矩陣以及光流的平均大小會更偏向于擬合動態(tài)對象，這種情況下很難通過這兩種方式來過濾掉動態(tài)特征點.因此本文所提的動態(tài)點剔除策略首先使用YOLOv4進行目標檢測剔除動態(tài)目標區(qū)域特征點，可以使圖像中的大部分動態(tài)點被過濾，僅剩余少量的誤匹配點及動態(tài)點，此時再利用對極幾何約束與光流約束能夠有效的將這些特征點全部剔除.

圖4 3次過濾結(jié)果Fig.4 Three times filtering results

經(jīng)過3次過濾后的結(jié)果如圖4所示，該圖片取自TUM數(shù)據(jù)集中的freiburg3_walking_xyz 高動態(tài)場景序列.圖片中的場景為人物手持書本坐在桌前做小范圍運動，此時人與手中書本均為運動狀態(tài).

圖4(a)為對圖像特征提取后的結(jié)果，圖中的標記點即為特征點，由圖片可以看出此時靜態(tài)物體與動態(tài)物體內(nèi)均存在特征點；圖4(b)為使用YOLOv4進行動態(tài)目標檢測后的結(jié)果，檢測框范圍內(nèi)的人即為動態(tài)目標；圖4(c)為動態(tài)特征點第1次剔除后的結(jié)果，可以看到此時處于動態(tài)目標范圍內(nèi)的特征點均已被剔除，但由于在動態(tài)目標檢測時只檢測環(huán)境中主要的運動物體即圖中的人物，并未檢測其手中所持書本，因此此時書本上的特征點仍然被保留；動態(tài)特征點經(jīng)過第2、3次過濾后的結(jié)果如圖4(d)所示，此時書本上的特征點也已經(jīng)被成功過濾.

3 實驗驗證

3.1 實驗數(shù)據(jù)集與環(huán)境

本次實驗主要對改進方法的精度與實時性進行評估，使用TUM RGB-D公開數(shù)據(jù)集中的7組動態(tài)場景數(shù)據(jù)進行測試.TUM數(shù)據(jù)集是由德國慕尼黑工業(yè)大學(xué)制作的RGB-D室內(nèi)數(shù)據(jù)集，它采用8臺高速攝像機在真實環(huán)境中拍攝而成，包括彩色圖像，深度圖像以及位姿真實值等內(nèi)容.TUM數(shù)據(jù)集里面包含了39個不同室內(nèi)環(huán)境的圖像序列，其中動態(tài)場景分為低動態(tài)的sitting和高動態(tài)的walking兩個子序列，它們分別對應(yīng)著場景中人物不同的狀態(tài)，在sitting序列中兩個人物坐在桌前，肢體或桌上一些小物體有輕微移動，walking序列中兩人則是繞著桌子進行大范圍走動并伴隨著行走，坐立之間的狀態(tài)切換.這兩個序列中又包含著相機4種不同的運動軌跡，分別為halfsphere，rpy，static，xyz.在halfsphere中相機沿著半球面運動，在rpy中相機做翻轉(zhuǎn)，俯仰等運動，在static中相機近似靜止，在xyz中相機沿x，y，z軸方向運動.本次實驗選用的7組動態(tài)場景包括freiburg3_walking_halfsphere，freiburg3_walking_rpy，freiburg3_walking_static，freiburg3_walking_xyz 4組高動態(tài)場景以及freiburg3_sitting_halfspher，freiburg3_sitting_rpy，freiburg3_sitting_static 3組低動態(tài)場景.

實驗平臺為聯(lián)想拯救者R7000P筆記本電腦，CPU型號為R7-4800H，主頻2.9GHz，16GB內(nèi)存，顯卡為NVIDIA GeForce RTX2060，6GB顯存，系統(tǒng)環(huán)境為Ubuntu18.04.

3.2 精度評估

精度的評估一般采用的評價指標為絕對軌跡誤差(Absolute Trajectory Error，ATE)以及相對位姿誤差(Relative Pose Error，RPE).ATE估計位姿與真實位姿的直接差值，能夠直觀地反應(yīng)出估計軌跡的精度和全局一致性.RPE主要描述的是相隔固定時間差內(nèi)估計位姿的變化量與真實位姿變化量之間的差值，包含了相對平移誤差(Relative Translation Error)和相對旋轉(zhuǎn)誤差(Relative Rotation Error)兩部分.使用均方根誤差(Root Mean Squared Error，RMSE)，平均值(Mean)以及標準差(Standard Deviation，S.D.)這3個參數(shù)對ATE和RPE這兩個指標進行統(tǒng)計.其中，平均值能夠反應(yīng)估計位姿的精度，均方根誤差以及標準差則體現(xiàn)了系統(tǒng)的魯棒性.

3.2.1 與其他算法對比

為了能更直觀地體現(xiàn)出本文方法在精度方面的提升，首先選用ORB-SLAM 2來與本文方法進行對比.經(jīng)過在數(shù)據(jù)集測試后，兩者的絕對軌跡誤差，相對平移誤差，相對旋轉(zhuǎn)誤差以及性能提升的百分比如表1-表3所示.通過對比可以看出，在高動態(tài)場景下本文方法能夠大幅度提高SLAM系統(tǒng)的定位精度，其中walking_xyz，walking_half和walking_static這3個序列上的誤差可減小至少90%以上，而在walking_rpy序列中雖然精度有所降低但精度仍然能提高85%以上.ORB-SLAM 2 作為目前最優(yōu)秀的SLAM系統(tǒng)之一，其在低動態(tài)場景下有著良好的表現(xiàn)，但即使如此，從整體上看本文方法較之仍舊有所提升.

表1 絕對軌跡誤差典型值(米)Table 1 Typical value of ATE(m)

表2 相對平移誤差典型值(米)Table 2 Typical value of translation(m)

表3 相對旋轉(zhuǎn)誤差典型值(度)Table 3 Typical value of rotation(deg)

圖5和圖6展示了在walking_xyz以及walking_half兩個高動態(tài)序列中用ORB-SLAM 2所估計的軌跡和用本文方法所估計的軌跡與相機真實運動軌跡的對比.由圖可以看出，在高動態(tài)場景下ORB-SLAM 2所估計的軌跡與真實軌跡差別較大，而本文方法所估計的軌跡基本接近真實軌跡且擁有良好的魯棒性.

圖5 walking_xyz序列Fig.5 walking_xyz sequence

圖6 walking_half序列Fig.6 walking_half sequence

為了進一步驗證改進視覺里程計的效果，將其與同類型的其他基于深度學(xué)習(xí)的視覺SLAM算法進行比較，表4為在walking_xyz，walking_half和walking_static 3個高動態(tài)場景下本文方法與DS-SLAM，Semantic-Fusion，Mask-Fusion的絕對軌跡誤差對比.通過對比可以看出，本文方法的精度與DS-SLAM基本持平，而相比Semantic-Fusion，Mask-Fusion均有大幅提升，這充分證明了所提方法的有效性.

表4 同類型算法絕對軌跡誤差對比(米)Table 4 Comparison of ATE of the same type of algorithms(m)

3.2.2 消融實驗

本文在進行動態(tài)點剔除時采取了YOLOv4檢測動態(tài)目標以剔除動態(tài)目標區(qū)域特征點、通過對極幾何約束剔除動態(tài)點以及通過光流約束剔除動態(tài)點這3種方法結(jié)合的方式，為驗證每個步驟的有效性，將這3種方法分別與原始的ORB-SLAM2結(jié)合并進精度評估的實驗，表5為ORB-SLAM2與這3種方法在walking_xyz，walking_half和walking_static這3個場景下的絕對軌跡誤差對比.其中method1表示用YOLOv4檢測動態(tài)目標以剔除動態(tài)目標區(qū)域特征點的方法，method2為通過對極幾何約束剔除動態(tài)點的方法，method3表示通過光流約束剔除動態(tài)點的方法.

表5 3種方法絕對軌跡誤差對比(米)Table 5 Comparison of ATE of three methods(m)

通過實驗結(jié)果可以看出使用YOLOv4目標檢測來剔除動態(tài)點的方式對系統(tǒng)精度的提升最為有效，而在單獨使用對極幾何約束或光流約束時，定位精度的提升則比較有限.這主要是因為在TUM數(shù)據(jù)集中，作為主要動態(tài)物體的人在圖像中所占的空間較大，正如前文所提，這種情況下對極幾何約束與光流約束很難有效過濾掉圖像中的動態(tài)點，而在使用YOLOv4目標檢測來剔除動態(tài)點時，可以將人身上的動態(tài)點完全過濾，從而剔除圖像中的大部分動態(tài)點，只殘留少量誤匹配點以及動態(tài)點.實驗結(jié)果也表明僅靠某種單一的方法很難完全剔除圖像中的動態(tài)點，只有將3種方法組合起來才能達到最佳效果.

3.3 實時性評估

實時性是評估SLAM系統(tǒng)是否具有實用性的一項重要指標.本次實驗中對實時性的評估所采用的評價指標為系統(tǒng)跟蹤線程處理每幀圖像所用時間，并且將本文方法與同樣使用深度學(xué)習(xí)的DynaSLAM以及DS-SLAM進行對比，結(jié)果如表6所示.本文方法的平均跟蹤時間為85ms，運行速度可以達到12幀/秒左右，能夠滿足實時性的要求.雖然同樣使用了深度學(xué)習(xí)的方法，但本文在動態(tài)目標檢測時采用的是快速檢測網(wǎng)絡(luò)YOLOv4，相較于DynaSLAM以及DS-SLAM所使用的Mask-RCNN與SegNet網(wǎng)絡(luò)有很大的速度優(yōu)勢，并且在計>算光流時只計算經(jīng)兩次過濾后的特征點光流，并未計算稠密光流，一定程度上節(jié)省了運算時間，因此在追蹤時間上相比兩者均有較大提升.通過對比也證明了相較于其他的SLAM算法，本文算法能夠更好地兼顧定位精度與實時性.

表6 跟蹤時間對比(毫秒)Table 6 Tracking timecomparison(ms)

4 結(jié)束語

本文提出了一種面向室內(nèi)動態(tài)環(huán)境的視覺里程計.該方法在ORB-SLAM2的視覺里程計基礎(chǔ)上開辟一條獨立的動態(tài)目標檢測線程，為滿足檢測精度與實時性的要求，采用YOLOv4作為檢測網(wǎng)絡(luò)，相應(yīng)提出一種動態(tài)特征點剔除策略，根據(jù)檢測結(jié)果對圖像中的特征點進行第1次過濾，接著為防止動態(tài)點剔除不徹底，分別利用對極幾何約束和光流約束進一步過濾特征點.在TUM數(shù)據(jù)集上的實驗結(jié)果表明，改進后的視覺里程計能夠大幅度提高SLAM系統(tǒng)在動態(tài)環(huán)境下的定位精度，并且能滿足實時性的要求.在后續(xù)研究中將嘗試構(gòu)建含有語義信息的三維地圖，以便于機器人能夠執(zhí)行更高級的任務(wù).