馮一博，張小俊，王金剛

(河北工業(yè)大學(xué) 機械工程學(xué)院，天津 300130)

0 引言

近年來，同步定位與地圖構(gòu)建技術(shù)(Simultaneous Localization and Mapping，SLAM)[1]作為高精度地圖的代表被廣泛應(yīng)用到智能汽車領(lǐng)域。智能汽車SLAM問題可以描述為:智能汽車從未知環(huán)境的未知地點出發(fā)，在移動的過程中，通過傳感器信息定位自身的位置和姿態(tài)，進而構(gòu)建增量式地圖，使智能汽車達到同步定位與地圖構(gòu)建的目的。因此，SLAM技術(shù)的發(fā)展會極大地影響智能汽車的發(fā)展，研究智能汽車SLAM技術(shù)具有一定的實際意義。

1986年由Smith Self 和Cheeseman首次提出SLAM技術(shù)，截止目前已經(jīng)發(fā)展三十多年，主要經(jīng)歷了三個發(fā)展階段。第一個階段是傳統(tǒng)階段：SLAM問題的提出，并將該問題轉(zhuǎn)換為一個狀態(tài)估計問題，利用擴展卡爾曼濾波[2]、粒子濾波[3]及最大似然估計[4]等手段來求解。將視覺同步定位與地圖構(gòu)建問題轉(zhuǎn)換為一個狀態(tài)估計問題，傳統(tǒng)階段的傳感器主要依據(jù)就激光雷達為主，該階段的缺陷在于忽略了地圖的收斂性質(zhì)，把定位問題與地圖構(gòu)建問題分開處理。第二個階段是算法分析階段(2004-2015)：2007年A. J. Davison教授提出的MonoSLAM[5]是第一個實時的單目視覺SLAM系統(tǒng)，但它不能在線運行，只能離線地進行定位與建圖；同年Klein等人提出了PTAM[6](Parallel Tracking and Mapping),該算法雖然第一個使用非線性優(yōu)化作為后端并且實現(xiàn)了跟蹤與建圖過程的并行化，但是它只適用于小場景下，而且在跟蹤過程中容易丟失；2015 年MurArtal 等人提出的ORB-SLAM[7]是 PTAM 的繼承者之一，它圍繞ORB特征進行位姿估計，以詞袋模型的方式克服累積誤差問題，但它在旋轉(zhuǎn)時容易丟幀。第三階段就是魯棒性-預(yù)測性階段(2015-)：魯棒性、高級別的場景理解、計算資源優(yōu)化、任務(wù)驅(qū)動的環(huán)境感知等，這些都伴隨著人工智能技術(shù)的發(fā)展，讓深度學(xué)習(xí)逐漸融入到視覺SLAM之中。而在SLAM系統(tǒng)中，當(dāng)一個新的幀到來時，通過應(yīng)用YOLO[8]、SSD[9]、SegNet[10]、Mask R-CNN[11]等先進的CNN結(jié)構(gòu)，可以獲得特征的語義標(biāo)簽。文獻[12]使用目標(biāo)檢測網(wǎng)絡(luò)SSD來檢測可移動的物體，如人、狗、貓和汽車。文獻[13]使用YOLO來獲取語義信息，他們認(rèn)為總是位于運動物體上的特征是不穩(wěn)定的，并將其過濾掉。文獻[14]使用SegNet在一個單獨的線程中獲得像素語義標(biāo)簽，如果一個特征被分割成“人”，則使用外極幾何約束進行進一步的移動一致性檢查；如果檢查結(jié)果是動態(tài)的，那么語義標(biāo)簽為“person”的所有特征都將被分類為動態(tài)并被刪除。這種方法實際上是將帶有標(biāo)簽“person”的特征作為一個整體來處理，并且取兩個結(jié)果的交集：只有在語義上和幾何上都是動態(tài)的特征才被認(rèn)為是動態(tài)的。文獻[15]結(jié)合了Mask R-CNN和多視圖幾何的語義分割結(jié)果，將語義動態(tài)或幾何動態(tài)的特征都被認(rèn)為是動態(tài)的。

傳統(tǒng)的視覺SLAM在建圖的過程中，需要考慮兩方面，一方面如何依據(jù)拍攝的圖像序列估計車輛的運動軌跡，另一方面如何在三維場景中構(gòu)建幾何結(jié)構(gòu)。而解決SLAM問題的關(guān)鍵是從獲取的環(huán)境圖像中快速提取精確特征，描述相機環(huán)境的局部特征，最后計算空間三維坐標(biāo)和其他信息實現(xiàn)視覺的自主定位和地圖構(gòu)建。

特征是圖像信息的另一種表達方式，特征點是圖像中一些特別的地方，所以需要特征點在相機運動之后保持穩(wěn)定，而傳統(tǒng)的特征提取方法，如著名的SIFT[16]、SURF[17]、ORB[18]等在復(fù)雜環(huán)境下，容易受灰度值、動態(tài)物干擾，面對圖像時會出現(xiàn)性能下降。因此，在傳統(tǒng)的特征點提取中加入了語義信息，改進了SLAM系統(tǒng)，主要內(nèi)容包括：

1) 深度學(xué)習(xí)與特征提取相結(jié)合，本文選用YOLO v3端對端的目標(biāo)檢測方法，準(zhǔn)確識別地下車庫中的動態(tài)物體，并且算法適應(yīng)地下環(huán)境變化，有效提取圖像特征。

2) 能夠過濾出動態(tài)物體，快速提取穩(wěn)定的局部特征點進行幀間匹配、位姿估計，提高了SLAM定位精度，增強車輛在地下出庫場景中的環(huán)境感知。

1 基于YOLO v3與ORB-SLAM2的動態(tài)建圖算法

本文算法依托ORB-SLAM2[19]為主體框架，視覺前端的特征點提取融合了深度學(xué)習(xí)的算法，通過分割圖像中的靜態(tài)和動態(tài)特征并將動態(tài)部分視為離群值，可以提高SLAM算法的魯棒性。根據(jù)選擇性跟蹤算法，在視覺前端中計算出靜態(tài)特征點，進行姿態(tài)估計和非線性優(yōu)化，可以避免動態(tài)物體的干擾。圖1給出了本文SLAM的主體框架。

圖1 算法流程圖Fig. 1 Algorithm flow chart

1.1 目標(biāo)檢測原理

YOLO v3主干網(wǎng)絡(luò)由Darknet-53構(gòu)成，它利用53個卷積層進行特征提取，而卷積層對于分析物體特征最為有效。

在前向過程中，YOLO v3輸出了3個不同尺度的特征圖：y1，y2，y3，其中y1檢測大型物體，y2檢測中型物體，y3檢測小型物體，輸出特征圖輸出維度為

R=N×N(3×(4+1+M))，

(1)

其中，N×N為輸出特征圖格點數(shù)，3表示每個特征圖y一共3個錨框，4表示4維的檢測框位置(tx,ty,tw,th)，1表示檢測置信度1維，M為分類個數(shù)。

在反向過程中，預(yù)測框共分3種情況：正例、負(fù)例、忽略樣例。損失函數(shù)[20]為3個特征輸出圖的損失函數(shù)值和，分別為置信度損失、分類損失和定位損失。置信度損失用于優(yōu)化框架的準(zhǔn)確性，分類損失用于優(yōu)化檢測和分類，定位損失用于優(yōu)化中心點坐標(biāo)和寬度檢測框。

損失的計算：

L(O,o,C,c,l,g)=λ1Lconf(o,c)+
λ2Lcla(O,C)+λ3Lloc(l.g)

(2)

其中，λ1,λ2,λ3為平衡系數(shù)。Lconf(o,c)為置信度損失，Lcla(O,C)為分類損失，Lloc(l,g)為定位損失。

置信度損失：

(3)

類別損失：

(4)

定位損失：

(5)

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

圖2是動態(tài)目標(biāo)剔除算法的整體框架圖，它在傳統(tǒng)的視覺前端加入了一個并行線程：YOLO v3模塊。本文使用的YOLO v3目標(biāo)檢測方法能夠輸出3個特征圖，分別用于檢測大型和中小型物體；并且自制了地下車庫VOC數(shù)據(jù)集，標(biāo)注了可能移動的類(汽車、人、自行車、公共汽車等)，之后對測試集進行訓(xùn)練，得到預(yù)訓(xùn)練權(quán)重，這樣能夠更好地檢測識別出地下車庫中的動態(tài)目標(biāo)，從而剔除動態(tài)特征點。

圖2 動態(tài)目標(biāo)剔除算法框架Fig. 2 Dynamic target elimination algorithm framework

1.3 軌跡重建

優(yōu)化的算法主體框架與ORB-SLAM2相當(dāng)，它可以說是ORB-SLAM2的延伸，主要是由跟蹤線程、局部建圖線程與閉環(huán)檢測線程組成。各個線程的主要任務(wù)如下：

1) 跟蹤線程：根據(jù)目標(biāo)檢測傳回的目標(biāo)區(qū)域進行特征剔除，接著進行位姿跟蹤。

2) 局部建圖線程：最大的特點就是借助跟蹤線程的關(guān)鍵幀，更新地圖點和位姿，從而進行局部束極調(diào)整，最后對每一個匹配好的特征點建立方程，解出最優(yōu)的位姿矩陣和空間點坐標(biāo)。

3) 閉環(huán)檢測線程：首先計算圖像的詞袋向量，根據(jù)詞袋之間的相似度判斷是否閉環(huán)，若閉環(huán)則使用閉環(huán)關(guān)鍵幀對地圖點進行閉環(huán)融合處理，最終位姿通過本質(zhì)圖優(yōu)化來校正累計誤差。

進入跟蹤線程的前提是需要對算法進行初始化，而初始化的關(guān)鍵就是通過評分的方式選取合適的基本矩陣(Fundamental)或者單應(yīng)矩陣(Homograph)。由于跟蹤線程中使用雙目相機或者RGB-D相機可以對一些3D點進行恢復(fù)，因此，單幀可創(chuàng)建關(guān)鍵點，關(guān)鍵幀；然而對于單目攝像頭，恢復(fù)3D點只能借助時間序列上的兩幀恢復(fù)。無論哪種相機，都需要借助評分選擇一個更好的模型來初始化。

Fundamental模型評分：

(6)

Homograph模型評分：

(7)

其中,當(dāng)x≤0時，ρ(x)=0，當(dāng)x>0時,ρ(x)=x。TF取固定閾值3.84，TH取固定閾值5.99。式(6)和式(7)中x′和x為同一個3D點在o和o′坐標(biāo)系下坐標(biāo)，F(xiàn)為基本矩陣，H為單應(yīng)矩陣，i表示特征點序號，n表示特征點個數(shù)，σ為標(biāo)準(zhǔn)差。

兩個模型的選擇條件為

(8)

當(dāng)RH≥0.45時，表示當(dāng)前場景較為平坦并且視差較低，選擇單應(yīng)矩陣；其他的情況，選擇基本矩陣。

局部建圖線程主要是為了進行束極調(diào)整，先根據(jù)相機模型和A、B圖像特征匹配好的像素坐標(biāo)，求出A圖像上的像素坐標(biāo)的歸一化的空間點坐標(biāo)，然后根據(jù)空間點的坐標(biāo)計算投影到B圖像上的像素坐標(biāo)，可以發(fā)現(xiàn)，重投影的像素坐標(biāo)(估計值)與匹配好的B圖像上的像素坐標(biāo)(測量值)不會完全重合，這時候就需要束極調(diào)整。束極調(diào)整的目的就是對每一個匹配好的特征點建立方程，然后聯(lián)立形成超定方程，解出最優(yōu)的位姿矩陣或空間點坐標(biāo)。

閉環(huán)檢測線程主要由檢測回閉環(huán)、計算Sim3、閉環(huán)融合以及優(yōu)化本質(zhì)圖四個部分所組成。在閉環(huán)檢測中，首先要求閉環(huán)與閉環(huán)之間必須超過十幀，然后計算當(dāng)前幀與相鄰幀之間的詞袋得分，從而得到最小相似度,之后以最小相似度作為基準(zhǔn)，若大于該基準(zhǔn)則可能是關(guān)鍵幀，這就將閉環(huán)檢測轉(zhuǎn)變?yōu)橐粋€類似于模式識別的問題，當(dāng)相機再次來到之前到過的場景，就會因為看到相同的景物，而得到類似的詞袋描述，從而檢測到閉環(huán)。其次在計算Sim3中，單目系統(tǒng)中存在尺度的不確定性，因為針對一個三維點P來說，在單目拍攝的兩幅中可以匹配到PL和PR，但是無法確定其在三角化里的具體位置，所以存在尺度模糊，而對于雙目相機或者RGB-D相機，尺度可以唯一確定。之后閉環(huán)矯正是融合重復(fù)的點云，并且在共視圖中插入新的邊以連接閉環(huán)，接下來當(dāng)前幀的位姿會根據(jù)相似變換而被矯正，與此同時所有與其相連的幀也會被矯正，矯正之后所有的被閉環(huán)處的關(guān)鍵幀觀察到的地圖通過映射在一個小范圍里，然后去搜索它的近鄰匹配，這樣就可以對所有匹配的點進行更有效的數(shù)據(jù)融合。最后為了更好地完成閉環(huán)，將回環(huán)誤差均攤到所有關(guān)鍵幀上，使用本質(zhì)圖優(yōu)化所有關(guān)鍵幀的位姿姿態(tài)。

2 實驗與分析

2.1 數(shù)據(jù)準(zhǔn)備

采集制作含有動態(tài)目標(biāo)的地下車庫VOC數(shù)據(jù)，該數(shù)據(jù)集分為有車有人、有車無人、無車無人、無車有人4種情況。利用目標(biāo)檢測工具labelimg對采集的地下車庫圖片進行標(biāo)注，分為person、car、bus、rider類，制作成VOC數(shù)據(jù)集。將數(shù)據(jù)集按照8∶1∶1的比例劃分為訓(xùn)練集、驗證集和測試集，即訓(xùn)練集包括3 200張圖片，驗證集和測試集各包括400張照片。

之后對測試集訓(xùn)練，用平均損失曲線以及平均交并比(Intersection Over Union，IOU)曲線作為訓(xùn)練的評價標(biāo)準(zhǔn)。由圖3、4可以看出在經(jīng)過40 000多次迭代后平均損失曲線接近于0.06趨向于擬合，平均交并比曲線代表預(yù)測的邊界框和真實框的交集與并集之比接近于1，由此訓(xùn)練結(jié)果可以用來進行地下車庫試驗。

圖3 平均損失曲線Fig. 3 Average loss curve

圖4 平均交并比曲線 Fig. 4 Average IOU curve

2.2 檢測驗證

傳統(tǒng)的視覺SLAM難以從平面像素中定義動態(tài)對象，也難以檢測跟蹤動態(tài)對象，因此，本文測試了ORB-SLAM2以及優(yōu)化的SLAM的操作效果。

使用ORB-SLAM2的特征提取如圖5所示，可以看出特征點集中在人車這些動態(tài)物上，ORB特征點大多是雜亂無章，并且對于SLAM的建圖有很大影響，不但會增加冗余計算量、影響建圖速度，甚至?xí)a(chǎn)生漂移等影響。圖6表示的是應(yīng)用YOLO v3的目標(biāo)檢測方法進行的動態(tài)目標(biāo)檢測，圖7表示的是識別全部的ORB特征點，圖8表示的是動態(tài)特征點的剔除。

圖5 ORB-SLAM2的特征提取Fig. 5 Feature extraction of ORB-SLAM

圖6 YOLO v3的目標(biāo)檢測Fig. 6 Target detection of YOLO v3

圖7 全部特征點的識別Fig. 7 Recognition of all feature points

圖8 動態(tài)特征點的剔除Fig. 8 Removal of dynamic feature points

2.3 車庫軌跡驗證

地下車庫數(shù)據(jù)集是由智能汽車平臺搭載的RGB-D相機錄制，數(shù)據(jù)集經(jīng)歷了3個場景，分別是靜態(tài)場景、低動態(tài)場景、高動態(tài)場景。

用激光雷達算法LOAM測試完整個數(shù)據(jù)集，當(dāng)作該數(shù)據(jù)集的真實軌跡圖，圖9展示了不同算法的運行軌跡，本文改進的SLAM算法相機軌跡估計比ORB-SLAM2更接近地面真實情況。在靜態(tài)場景下，本文算法和ORB-SLAM2都貼近于真實軌跡；隨后經(jīng)歷低動態(tài)場景，ORB-SLAM2產(chǎn)生了些許的漂移，根據(jù)回環(huán)校正，依舊可以跟蹤上一幀，進行幀間匹配建圖，然而到了高動態(tài)場景中，ORB-SLAM2算法中由于提取的特征點大多集中在車中，出現(xiàn)了嚴(yán)重的漂移，導(dǎo)致建圖未能成功；相反改進的SLAM算法由于特征點提取范圍從整幅圖像縮小到非動態(tài)區(qū)域，能夠進行有效的幀間匹配，完成建圖。與ORB-SLAM2算法相比，改進的SLAM算法在動態(tài)場景中具有更強的魯棒性，攝像機軌跡估精度也有很大提高。

圖9 不同算法的軌跡對比圖Fig. 9 Trajectory comparison diagram of different algorithms

3 評價指標(biāo)

對優(yōu)化的SLAM算法評估可以從多個方面考慮，例如時耗、復(fù)雜度、精度。其中，算法對精度要求最高，本文采用絕對軌跡誤差(Absolute Trajectory Error, ATE)和相對位姿誤差(Relative Pose Error，RPE)兩個精度指標(biāo)。ATE被廣泛用于定量評估，它評估兩條軌跡之間的絕對姿態(tài)差，采用均方根誤差(Root Mean Square Error, RMSE)、平均誤差(Mean Error)、中位誤差(Median Error)和標(biāo)準(zhǔn)差(Standard Deviation, SD)等指標(biāo)進行測量。在這些指標(biāo)中，RMSE和SD在反映視覺SLAM系統(tǒng)性能力方面更為重要。RMSE是測量誤差的標(biāo)準(zhǔn)差，用ERMSE表示，它很好反映測量的精度，ERMSE越小，估計軌跡越接近地面真值軌跡。

(9)

其中，n表示觀測次數(shù)，i表示第i個關(guān)鍵幀，xobs,i表示第i個關(guān)鍵幀姿態(tài)的地面真實值，xmodel,i表示第i個關(guān)鍵幀姿態(tài)的觀測值。

SD可以反映觀測值的分散程度,用ESD表示

(10)

其中，n表示關(guān)鍵幀的數(shù)目，Xmodel,i表示第i個關(guān)鍵幀的觀測姿態(tài)，μ是Xmodel,i的平均值。

ME是觀測姿態(tài)與地面真值之間誤差的算術(shù)平均值,用Emean表示：

(11)

其中，n表示觀測次數(shù)，i表示第i個關(guān)鍵幀，Xobs,i表示第i個關(guān)鍵幀姿態(tài)的地面真實值，Xmodel,i表示第i個關(guān)鍵幀姿態(tài)的觀測值。

中位誤差是觀測姿態(tài)和地面真實值之間的誤差的中間值。

RPE主要描述的是相隔固定時間差兩幀位姿差的精度(相比真實位姿)，相當(dāng)于直接測量里程計的誤差。因此第i幀的RPE定義如下：

(12)

其中，Pi表示第i幀算法估計位姿，Qi表示第i幀真實位姿，Δt表示時間間隔t,Ei表示i時刻絕對或相對位姿估計誤差。

采用ERMSE統(tǒng)計誤差總體值：

(13)

其中，n表示觀測次數(shù)，Δt表示時間間隔t，得到m，m=n-Δt×ERPE,Ei表示i時刻絕對或相對位姿估計誤差，trans(Ei)代表相對位姿誤差中的平移部分。

但是，在大多數(shù)情況下，Δt有許多選擇，為了綜合衡量算法的表現(xiàn)，計算遍歷了所有時間間隔的ERMSE的平均值：

(14)

其中，n表示觀測次數(shù)，Δt表示時間間隔t(t從1開始取直到觀察結(jié)束n次為止)，ERMSE表示均方根誤差。

若遍歷所有時間間隔，則存在計算復(fù)雜程度高、耗時問題，因此通過計算固定數(shù)量的RPE樣本獲取估計值作為最終結(jié)果。

采用系數(shù)K對比優(yōu)化的SLAM算法與ORB-SLAM2的改進值：

(15)

其中，α是優(yōu)化的SLAM獲得的值，β是ORB-SLAM2獲得的值。

圖10、11對ORB-SLAM2和優(yōu)化的SLAM進行ATE評測：對比真值位姿，ORB-SLAM2的均方根誤差為22.49 m，優(yōu)化的SLAM的均方根誤差為1.46 m，提升了93.50%；ORB-SLAM2的標(biāo)準(zhǔn)差為11.92 m，優(yōu)化的SLAM的標(biāo)準(zhǔn)差為0.65 m，提升了94.53%?？梢钥闯鰞?yōu)化的SLAM算法漂移量以及分散程度明顯優(yōu)于ORB-SLAM2。

圖10 ORB-SLAM2位姿與真值位姿之間的絕對軌跡誤差Fig. 10 ATE between ORB-SLAM2 pose and true value pose

圖11 優(yōu)化的算法位姿與真值位姿之間的絕對軌跡誤差Fig. 11 ATE between optimized algorithm and true value pose

圖12、13對ORB-SLAM2和優(yōu)化的SLAM分別進行RPE評測：對比真值位姿，ORB-SLAM2的均方根誤差為0.24 m；優(yōu)化的SLAM的均方根誤差為0.040 8 m；提升了83.03%?？梢钥闯?，優(yōu)化的SLAM算法漂移量明顯低于ORB-SLAM2。

圖12 ORB-SLAM2位姿與真值位姿之間的相對位姿誤差Fig. 12 RPE between ORB-SLAM2 pose and true value pose

圖13 優(yōu)化的算法位姿與真值位姿之間的相對位姿誤差Fig. 13 RPE between optimized algorithm and true value pose

4 結(jié)論

針對在地下車庫光照不足，容易受動態(tài)物影響的問題，本文構(gòu)造了一個完整的SLAM框架，提出了一種基于YOLO v3和ORB-SLAM2相結(jié)合的動態(tài)場景視覺SLAM算法。實驗結(jié)果表明，在綜合場景下本文提出的算法位姿估計精度相比于ORB-SLAM2提升了93.50%，改進后的算法大幅度提高了SLAM系統(tǒng)在動態(tài)環(huán)境下精度與魯棒性，并且保證了實時性。