龐智華,齊臣坤

(上海交通大學(xué) 機械與動力工程學(xué)院,上海 200240)

0 概述

視覺里程計[1-2]是移動機器人自主建圖與定位的關(guān)鍵技術(shù),其在要求傳感器體積小、重量輕、功耗小以及價格低的場合具有較大優(yōu)勢。應(yīng)用單目視覺里程計[3]時存在的一個重要問題是相機在快速或劇烈運動時,里程計算法的魯棒性較低[4-5],原因是相機快速運動時難以建立可靠的特征匹配關(guān)系,此外地圖擴展更新模塊存在一定的延遲,沒有及時生成地圖點,導(dǎo)致在相機快速或劇烈運動時沒有足夠多的地圖點用于跟蹤[6]。

對于單目視覺里程計算法,特征匹配需要綜合考慮算法的實時性和魯棒性。MonoSLAM算法[7]先采用卡爾曼濾波[8]預(yù)測特征點在下一幀的位置,然后進行主動搜索加速特征匹配。并行跟蹤和建圖(PTAM)算法[9]和ORB-SLAM算法[10]利用相機勻速運動模型估計當(dāng)前幀位姿,再將3D地圖點投影到當(dāng)前幀中,從而縮小搜索范圍以加速匹配。ENGLE等人[11]提出的大尺度直接單目SLAM算法(LSD-SLAM)假設(shè)相機運動非常緩慢,直接用上一幀的位姿初始化當(dāng)前幀位姿,然后進行圖像對齊。在相機運動較為劇烈時,勻速[9-10]或者靜止[11]等運動模型都存在較大誤差,濾波方法[6]由于誤差累積也會導(dǎo)致特征匹配失敗,因此,需要提高系統(tǒng)的魯棒性。文獻[12]通過增加線特征來提高特征匹配的成功率,文獻[13]引入機器人輪子編碼器信息從而提高特征匹配的魯棒性。

地圖的擴展速度也是影響算法實時性和魯棒性的一個重要因素。目前,較多性能良好的視覺SLAM算法,如ORB-SLAM、半直接視覺里程計(SVO)[14]、直接稀疏里程計(DSO)[15]等,為了提高算法的實時性和精度,將地圖的擴展更新與位姿跟蹤分離,導(dǎo)致地圖擴展更新有數(shù)幀甚至數(shù)十幀的延遲。然而,在相機快速或劇烈運動的情況下,對視角較小的單目視覺而言,地圖擴展更新延遲意味著沒有足夠多的地圖點用于跟蹤,從而降低了系統(tǒng)的魯棒性。

本文針對單目視覺里程計在相機快速或劇烈運動時魯棒性較低的問題,提出一種基于樹結(jié)構(gòu)特征匹配和實時地圖更新的單目視覺里程計算法。設(shè)計基于樹結(jié)構(gòu)和RANASC算法[16]的特征匹配算法,采用無運動假設(shè)的樹結(jié)構(gòu)進行特征匹配,以保證算法的實時性和特征匹配的魯棒性,通過結(jié)合漢明距離的RANASC算法剔除誤匹配。建立地圖實時擴展更新框架,在跟蹤線程,基于當(dāng)前幀與關(guān)鍵幀的匹配關(guān)系實時擴展更新地圖,并針對視差大小采取不同的深度估計策略來生成不同類型的地圖點,在局部地圖優(yōu)化線程中進行局部地圖的優(yōu)化。建立基于視差的權(quán)重矩陣估計模型,根據(jù)視差大小估計優(yōu)化函數(shù)中殘差項的權(quán)重矩陣,使精度高的地圖點在優(yōu)化函數(shù)中占主導(dǎo)地位。

1 單目視覺里程計算法框架

如圖1所示,單目視覺里程計算法包括跟蹤與局部地圖優(yōu)化2個線程。在跟蹤線程,首先根據(jù)特征樹建立當(dāng)前幀與前n幀的特征匹配關(guān)系,用于求解當(dāng)前幀的粗位姿,然后通過跟蹤局部地圖來優(yōu)化粗位姿,最后基于當(dāng)前幀與關(guān)鍵幀之間的匹配關(guān)系,根據(jù)視差大小采用2種策略生成不同類型的3D地圖點,完成地圖的實時擴展更新。在局部地圖優(yōu)化線程,首先將關(guān)鍵幀位姿與高精度的地圖點聯(lián)合優(yōu)化,提高地圖精度,然后將優(yōu)化后的高精度位姿作為約束來優(yōu)化臨時壞地圖點從而完成地圖優(yōu)化。將跟蹤與局部地圖優(yōu)化相分離,有利于提高地圖優(yōu)化的精度和效率。

圖1 單目視覺里程計算法框架

2 跟蹤線程

2.1 位姿粗估計

2.1.1 基于特征樹和RANSAC算法的特征匹配

基于運動假設(shè)的運動模型無法適用于劇烈運動的情況,本文基于特征樹建立特征匹配關(guān)系,并用RANSAC算法來剔除誤匹配，具體過程如下:

1)特征樹構(gòu)建

如圖2所示,將每一幀圖像的描述子{dj}都構(gòu)建為一棵類二叉樹的特征樹。特征樹的非葉節(jié)點Ni儲存描述子的位索引號ki,ki∈[0,1,…,dim(d)-1],dim(d)指描述子的維數(shù),示例圖像中的描述子維數(shù)為4。根據(jù)輸入的描述子在位索引號上的值將其分配到左、右節(jié)點中。最終,特征樹的每個葉節(jié)點Li都儲存了描述子集合{dj}的一個子集合{di,j}。

圖2 特征樹結(jié)構(gòu)

位索引號通過如下方式確定:

(1)任意指定。該方法操作簡單,但是不能保證建立的二叉樹是近似平衡的,會提高搜索的代價。

(2)統(tǒng)計輸入描述子中ki位為1所占的比例。當(dāng)ki位為1所占的比例ratio(ki)足夠接近0.5時,ki即為位索引號,該方法可以保證特征樹近似平衡。ratio(ki)的計算方法如下:

(1)

2)基于特征樹的特征匹配

描述子之間的漢明距離表征了匹配的可靠性,正確匹配點對之間的漢明距離應(yīng)小于閾值ρ。描述子ds在一棵深度為h的特征樹中能找到匹配描述子dm的概率為:

(2)

(3)

結(jié)合后文中的跟蹤前n幀策略,對于能被前k幀觀測到的特征點,最終能找到對應(yīng)匹配描述子的概率為:

(4)

結(jié)合工程應(yīng)用可知,一般匹配點的漢明距離閾值ρ小于最小漢明距離的2倍,最大不超過25,這里取ρ=20,深度h=4,則不同幀數(shù)下能找到特征匹配的概率如表1所示。

表1 特征匹配的概率Table 1 Probability of feature matching

3)基于RANSAC算法的誤匹配剔除

2.1.2 基于前n幀的位姿粗估計

在相機快速平移或旋轉(zhuǎn)時,當(dāng)前幀與前一幀之間或當(dāng)前幀與參考關(guān)鍵幀之間的視角重疊較小,無法得到足夠數(shù)量的特征匹配來估計穩(wěn)定的相機位姿,從而導(dǎo)致跟蹤失敗。

針對上述問題,本文提出跟蹤前n幀的策略來增加視角的重疊并引入更多的信息量。假定當(dāng)前幀的幀號為k,則這n幀中至少包含k-1幀、參考關(guān)鍵幀以及最近的關(guān)鍵幀。若當(dāng)前幀跟蹤到的3D地圖點數(shù)量小于閾值γ,則會依次跟蹤k-3幀、k-5幀,直至匹配的特征點數(shù)目大于γ或超出最大的n值。前n幀中包含參考關(guān)鍵幀、最近關(guān)鍵幀的原因是:1)關(guān)鍵幀可以為后面的實時地圖更新模塊提供具有一定視差的2D-2D的特征匹配點;2)參考關(guān)鍵幀、最近關(guān)鍵幀與當(dāng)前幀最有可能具有大量的共視點,從而保證了當(dāng)前幀能跟蹤到足夠的3D地圖點。

結(jié)合上文中提到的特征匹配算法,可以得到足夠的3D-2D、2D-2D特征匹配關(guān)系。本文僅使用3D-2D匹配關(guān)系,通過最小化重投影誤差的平方和來估計相機的粗位姿Tk,w,代價函數(shù)為:

(5)

ei=xi-π(KTk,wPi)

(6)

(7)

其中,Σ-1為權(quán)重矩陣,由后文中的估計方法給定,Pi為當(dāng)前幀觀測到的地圖點,xi為匹配特征點的像素坐標(biāo),K為相機的內(nèi)參矩陣。

2.2 局部地圖跟蹤

局部地圖是根據(jù)兩級共視關(guān)系而確定的,局部地圖中關(guān)鍵幀能觀測到的地圖點稱為局部地圖點。在建立局部地圖點與當(dāng)前幀之間的3D-2D匹配關(guān)系后,通過最小化重投影誤差來優(yōu)化粗位姿,代價函數(shù)如下:

(8)

其中,xi與Pi分別為對應(yīng)的像素點坐標(biāo)和局部地圖點坐標(biāo),ρ(e)為魯棒核函數(shù),本文采用Huber核函數(shù):

(9)

由式(9)可以看出,當(dāng)誤差e大于閾值δ時,函數(shù)由二次函數(shù)變成了一次函數(shù),相當(dāng)于限制了迭代求解中梯度的最大值,減弱了錯誤特征匹配的影響,提高了位姿估計的精度。

2.3 地圖擴展與關(guān)鍵幀插入

實時地圖擴展更新能保證當(dāng)前幀跟蹤到足夠多的地圖點,使算法對快速運動具有較好的魯棒性。

2.3.1 實時地圖擴展更新

對于視差α(k,i)大于閾值βα的點對,直接通過三角測量法進行深度估計。如果α(k,i)小于βα且大于γα,則先將xk臨近點的平均深度作為初始深度值dinit,然后通過最小化重投影誤差來優(yōu)化深度值。對于視差過小的匹配點對,如無窮遠處的點,則不做處理,以避免相機保持靜止時進行地圖擴展[18]。求xk和xi對應(yīng)的地圖點P時需要解如下的優(yōu)化問題:

(10)

2.3.2 基于視差的權(quán)重矩陣估計

本文將地圖點分成3類,并結(jié)合視差的大小和地圖點類型計算地圖點對應(yīng)殘差項的權(quán)重矩陣。跟蹤線程中通過三角化得到的地圖點視差較大、位置精度較高,將其記為臨時好地圖點,可直接用于跟蹤;通過優(yōu)化得到的地圖點初始深度值較差,且僅依靠兩幀的觀測來優(yōu)化深度值,位置精度較低,將其標(biāo)記為臨時壞地圖點。將經(jīng)過局部地圖優(yōu)化線程多視角約束優(yōu)化后仍被保留下來的臨時好地圖點標(biāo)記為永久地圖點,并對其分配最大的權(quán)重矩陣。

權(quán)重矩陣估計步驟具體如下:

1)判斷地圖點的類型。對于永久地圖點,則權(quán)重矩陣為單位矩陣;對于臨時地圖點,根據(jù)后續(xù)步驟計算權(quán)重矩陣。

2)統(tǒng)計優(yōu)化方程中所有臨時地圖點生成時的最大視差αmax。

(11)

在優(yōu)化方程中添加上述權(quán)重矩陣,可以使精度高的地圖點對應(yīng)的殘差項在優(yōu)化過程中占主導(dǎo)作用,從而提高位姿的估計精度。

2.3.3 關(guān)鍵幀選擇

及時生成關(guān)鍵幀有利于提高位姿和地圖點的估計精度?？紤]到局部地圖優(yōu)化線程無需進行地圖擴展,計算代價較低,本文采取較為簡單的關(guān)鍵幀插入策略。插入一個新的關(guān)鍵幀至少需要滿足以下條件中的一個條件:

1)在當(dāng)前幀跟蹤到的地圖點中,臨時地圖點比例超過30%。

2)局部地圖優(yōu)化線程是空閑的,且距離上一個關(guān)鍵幀已經(jīng)超過了1 s。

3)當(dāng)前幀跟蹤到的地圖點少于50個。

4)當(dāng)前幀跟蹤參考關(guān)鍵幀的永久地圖點所占比例少于80%。

3 局部地圖優(yōu)化線程

局部地圖優(yōu)化線程主要實現(xiàn)關(guān)鍵幀位姿和地圖點的優(yōu)化以及臨時地圖點的處理。

3.1 共視圖更新和地圖點選擇

將局部地圖的3D點投影到當(dāng)前關(guān)鍵幀中建立更多匹配,同時也將當(dāng)前關(guān)鍵幀的3D地圖點投影到局部地圖的關(guān)鍵幀中,完成共視圖的更新。局部地圖點包含永久地圖點、臨時好地圖點和臨時壞地圖點3類點。所有的永久地圖點都用于位姿優(yōu)化,所有的臨時壞地圖點都不用于位姿優(yōu)化。對于臨時好地圖點,先計算地圖點在關(guān)鍵幀中的平均投影誤差δe,丟棄投影誤差大于1.2δe的匹配關(guān)系,然后將所有臨時好地圖點用于位姿優(yōu)化。

3.2 局部BA優(yōu)化與臨時壞地圖點優(yōu)化

將局部地圖中的當(dāng)前關(guān)鍵幀、一級共視關(guān)鍵幀、永久地圖點和臨時好地圖點一起聯(lián)合優(yōu)化,而二級共視關(guān)鍵幀在優(yōu)化過程中會被固定,用于為地圖點提供約束。在優(yōu)化結(jié)束后,所有參與優(yōu)化的臨時好地圖點精度進一步提高,因此,將其標(biāo)記為永久地圖點。

臨時壞地圖點之所以不直接用于BA優(yōu)化,是考慮到其不準(zhǔn)確的位置會導(dǎo)致優(yōu)化結(jié)果錯誤。本文采用固定優(yōu)化后的位姿以及多視角的約束關(guān)系來優(yōu)化臨時壞地圖點,這樣既不降低位姿的精度,同時也可以提高臨時壞地圖點的精度。將優(yōu)化后的臨時壞地圖點標(biāo)記為臨時好地圖點用于跟蹤。每一個臨時壞地圖點Pi都通過最小化重投影來單獨優(yōu)化,由于地圖點之間相互獨立,可以通過并行計算來加速。優(yōu)化方程為:

(12)

其中,TPi為所有能觀測到地圖點Pi的關(guān)鍵幀集合,xj,i為地圖點Pi在關(guān)鍵幀j中的觀測值。

4 實驗結(jié)果與分析

在本文實驗中,CPU為Inter i5-7200U,主頻為2.5 Hz,內(nèi)存為8 GB,無GPU加速,操作系統(tǒng)為Ubuntu16.04。采用公開的KITTI數(shù)據(jù)集[19](室外)和EuRoC數(shù)據(jù)集[20](室內(nèi))進行實驗。

4.1 特征匹配實驗

為了驗證基于樹結(jié)構(gòu)和RANASC的特征匹配算法的高效性和精確性,將本文算法與OpenCV自帶的暴力匹配算法進行對比。用于特征匹配實驗的圖像分別來自EuRoC的V1_01_easy序列和KITTI-02序列,每張圖片上提取的ORB特征點數(shù)量均為2 000,實驗結(jié)果如圖3、圖4、表2、表3所示,其中,“×”表示包含大量錯誤匹配,無法統(tǒng)計匹配比例。

圖3 EuRoC數(shù)據(jù)集的匹配結(jié)果

圖4 KITTI數(shù)據(jù)集的匹配結(jié)果

表2 EuRoC數(shù)據(jù)集特征匹配結(jié)果對比

表3 KITTI數(shù)據(jù)集特征匹配結(jié)果對比Table 3 Comparison of feature matching results of KITTI dataset

在圖3、圖4中,左上角表示提取ORB特征點的原始圖像,右上角表示使用OpenCV自帶的暴力匹配算法匹配的結(jié)果,左下角表示將初始暴力匹配的結(jié)果通過嚴格的距離閾值和角度閾值剔除誤匹配后的優(yōu)化結(jié)果(可視為匹配的真值),右下角表示本文特征匹配算法的結(jié)果。從圖3、圖4可以看出,初始的暴力匹配雖然能為每個特征點找到匹配點,但是存在大量的誤匹配,本文特征匹配算法和優(yōu)化后的暴力匹配算法雖然丟失了一些匹配,但是都取得了較高的匹配精度。

從表2、表3可以看出,特征樹構(gòu)建約需要3 ms,具有很高的效率。而基于樹結(jié)構(gòu)和RANASC算法的特征匹配算法相對于優(yōu)化后的暴力匹配算法,雖然會丟失部分正確的匹配,但是計算速度提高了一個數(shù)量級。綜上,本文特征匹配算法具有很高的精度和效率。對于丟失部分正確匹配的問題,本文跟蹤前n幀的策略可以在一定程度上提高當(dāng)前幀特征匹配的比例。

4.2 KITTI數(shù)據(jù)集室外大尺度實驗

在本次實驗中將本文所提單目視覺里程計算法稱為mono-vo算法。KITTI數(shù)據(jù)集包含從00號到10號的基準(zhǔn)數(shù)據(jù),該數(shù)據(jù)集是通過快速運動車輛上的攝像頭采集到的大尺度真實環(huán)境。ORB-SLAM是目前最優(yōu)的純視覺SLAM算法之一,本文使用不包含閉環(huán)線程的ORB-SLAM算法與mono-vo算法進行對比,使用evo工具評測算法效果,用絕對軌跡的均方根誤差(RMSE)作為評價指標(biāo),實驗結(jié)果如表4所示,最優(yōu)結(jié)果加粗表示。

表4 KITTI數(shù)據(jù)集室外大尺度實驗結(jié)果Table 4 Outdoor large scale experiment results of KITTI dataset

從表4可以看出,除序列01外,mono-vo與ORB-SLAM均可成功測試其他10個序列。其中,在平緩的直線運動中,mono-vo與ORB-SLAM具有相近的精度,而在具有較大旋轉(zhuǎn)的數(shù)據(jù)集上,mono-vo取得了更好的定位精度,驗證了該算法在相機視角變換較快時具有更好的魯棒性。在10個序列的整體效果中,mono-vo具有更高的精度,此外,帶權(quán)重矩陣的mono-vo算法比無權(quán)重矩陣的mono-vo算法的定位精度更高,驗證了基于視差的權(quán)重矩陣估計方法的有效性。

結(jié)合序列01、02、03和04的具體環(huán)境及軌跡來分析表4結(jié)果以及算法的實時性表現(xiàn)。從圖5可以看出,序列01的環(huán)境是高速公路,道路兩邊非常空曠,能觀測到的特征點絕大部分都是很遠的點,太遠的觀測點導(dǎo)致生成地圖點時視差過小,無法正確估計地圖點深度,不適用于純單目視覺算法。

圖5 序列01環(huán)境

在序列02、03和04的數(shù)據(jù)集實驗中,每幀提取2 000個ORB特征點,統(tǒng)計當(dāng)前幀能跟蹤到的地圖點數(shù)量和單幀跟蹤耗時,結(jié)果如表5所示。

表5 當(dāng)前幀能跟蹤到的地圖點數(shù)量和單幀跟蹤耗時Table 5 The number of map points that can be tracked in the current frame and the tracking time of a single frame

從表5、圖6、圖7可以看出,序列02、03包含大量的曲線運動,且具有較大的旋轉(zhuǎn)角度,mono-vo算法中跟蹤前n幀的策略可以增加視角的重疊,引入更多的信息,同時基于樹結(jié)構(gòu)的特征匹配算法和實時地圖更新策略可以保證當(dāng)前幀跟蹤到更多的地圖點,從而提高了位姿估計的精度。從表5、圖8可以看出,在以直線運動為主的數(shù)據(jù)集中,mono-vo算法取得了與ORB-SLAM算法相近的精度,這是由于在直線運動中,幀間的視角重疊較大,跟蹤前n幀的策略并不會引入太多的信息。由單幀消耗的平均時間可知,mono-vo算法的跟蹤頻率在18 Hz～22 Hz范圍內(nèi),其具有很好的實時性。

圖6 序列02跟蹤結(jié)果

圖7 序列03跟蹤結(jié)果

圖8 序列04跟蹤結(jié)果

4.3 EuRoC數(shù)據(jù)集室內(nèi)小尺度實驗

EuRoC數(shù)據(jù)集是在室內(nèi)小尺度人工環(huán)境下通過無人機采集的圖像序列。本文實驗采用V1_01、V1_02、V1_03 3個序列,難度依次增加,V1_03序列包含大量的劇烈旋轉(zhuǎn)與平移,且存在跳幀情況。實驗結(jié)果如表6、圖9～圖11所示。從中可以看出,在運動較平緩時,mono-vo與ORB-SLAM都有很好的定位精度,但在劇烈運動和跳幀場景下,ORB-SLAM算法無法跟蹤到足夠的地圖點,而mono-vo算法通過基于樹的特征匹配算法和實時地圖擴展更新策略,依然可以順利地測試整個數(shù)據(jù)集,表現(xiàn)出良好的魯棒性和較高的定位精度。

表6 RuRoC數(shù)據(jù)集室內(nèi)小尺度實驗結(jié)果Table 6 Indoor small scale experimental results of RuRoC dataset

圖9 序列V1_01跟蹤結(jié)果

圖10 序列V1_02跟蹤結(jié)果

圖11 序列V1_03跟蹤結(jié)果

5 結(jié)束語

針對單目視覺里程計在相機快速運動時魯棒性較低的問題,本文提出一種改進的單目視覺里程計算法。通過結(jié)合基于樹結(jié)構(gòu)的特征匹配算法、基于前n幀的位姿粗估計方法、實時地圖擴展更新策略以及基于視差的權(quán)重矩陣估計方法,使得算法在相機快速或劇烈運動下取得較高的定位精度和魯棒性。在大尺度室外數(shù)據(jù)集KITTI和小尺度室內(nèi)數(shù)據(jù)集EuRoC中進行實驗,結(jié)果表明,該算法的跟蹤頻率為18 Hz～22 Hz,具有較好的實時性,在平緩的直線運動中,本文算法與無回環(huán)線程的ORB-SLAM算法具有相近的精度,在曲線或劇烈運動中,本文算法取得了更高的魯棒性和定位精度。下一步將考慮卷簾快門對定位結(jié)果的影響,探究并消除由于同一幀圖像各行曝光時刻不一致而引入的誤差。