張建博，袁 亮，何 麗，冉 騰，唐鼎新

(新疆大學(xué) 機(jī)械工程學(xué)院，新疆 烏魯木齊 830047)

0 引 言

由于單目相機(jī)具有成本低、適用范圍廣和校準(zhǔn)過程簡單等優(yōu)勢，使得單目視覺同時定位與地圖構(gòu)建(simulta-neous localization and mapping，SLAM)成為機(jī)器人在未知環(huán)境中自主定位的一個重要研究方向。但是單目存在尺度不確定性的缺點(diǎn)，無法通過單幀圖像得到場景中真實的深度信息，使單目SLAM出現(xiàn)尺度模糊的問題，因此需要一種單目的絕對尺度估計方法來恢復(fù)單目的絕對尺度。Ji Zhang等[1]使用激光雷達(dá)來輔助單目恢復(fù)場景的真實尺度；文獻(xiàn)[2-4]利用慣性測量單元(inertial measurement unit，IMU)輔助單目恢復(fù)絕對尺度；Shiyu Song等[5]提出利用物體先驗尺寸來恢復(fù)單目尺度；文獻(xiàn)[6]利用相機(jī)的先驗高度信息獲得單目絕對尺度；Sen Wang等提出的DeepVO[7]和陳宗海等提出的LSTMVO[8]是一種端到端的深度學(xué)習(xí)框架，通過輸入離線的視頻序列可以同時輸出深度圖和相機(jī)位姿。

此外，實現(xiàn)精確的自主定位是SLAM最重要的任務(wù)之一，而單目SLAM的尺度不確定性會引起尺度漂移的問題，從而嚴(yán)重影響了定位精度。Engel J等提出基于直接法的DSO[9]利用光束法平差(bundle adjustment，BA)減少尺度漂移。Mur-Artal等提出基于特征點(diǎn)的ORB-SLAM[10]，利用局部BA和全局回環(huán)檢測方法減少尺度漂移。

本文利用MonoDepth[11]深度預(yù)測網(wǎng)絡(luò)估計單目的絕對尺度，以此來彌補(bǔ)單目固有的尺度模糊的缺點(diǎn)。利用絕對尺度實時校正尺度漂移，從而提高單目SLAM的定位精度。

1 單目視覺SLAM

單目視覺SLAM中一般采用針孔相機(jī)模型，其成像原理如圖1所示，空間中一點(diǎn)投影在2維相機(jī)成像平面上，得到一個2維像素點(diǎn)P，但僅通過成像平面上的一個像素點(diǎn)P無法確定出該空間點(diǎn)的具體位置，因為從相機(jī)光心過歸一化平面整條射線上的所有點(diǎn)都可以投影到該像素點(diǎn)，所以當(dāng)確定了點(diǎn)P的真實深度時才能得到它的空間位置。

圖1 單目尺度不確定性原理

傳統(tǒng)單目視覺SLAM是通過三角化過程來恢復(fù)像素點(diǎn)相對深度的。首先對連續(xù)兩幀二維圖像提取特征點(diǎn)，再根據(jù)圖像特征進(jìn)行特征點(diǎn)匹配，最后利用對級約束關(guān)系恢復(fù)出兩幀圖像之間相機(jī)的運(yùn)動，求解原理如圖2所示，單目相機(jī)獲取的相鄰幀圖像序列I1，I2經(jīng)過特征提取和匹配，得到匹配好的其中一對特征點(diǎn)p1,p2，分別對應(yīng)空間中一點(diǎn)P=[X,Y,Z]T在I1，I2上的投影，在針孔相機(jī)模型中有以下空間幾何關(guān)系

圖2 單目相機(jī)運(yùn)動求解原理

s1p1=KP，s2p2=K(RP+t)

(1)

式中：K為單目相機(jī)的內(nèi)參矩陣，R、t為兩幀圖像的相機(jī)位姿變換，s1、s2為尺度變換因子。

齊次坐標(biāo)下，取x1、x2為兩個像素點(diǎn)p1、p2在歸一化坐標(biāo)平面上的點(diǎn)，則有

x1=K-1p1，x2=K-1p2

(2)

(3)

(4)

對極約束中包括了旋轉(zhuǎn)和平移，其中

(5)

求解相對位姿變化R、t需要求解本質(zhì)矩陣E。

由于對級約束是等式為0的約束，使得E具有尺度等價性，由E分解得到的R、t也具有尺度等價性。對匹配點(diǎn)的歸一化坐標(biāo)x1=[u1,v1,1]、x2=[u2,v2,1],其對級約束關(guān)系為

(6)

中間項e1到e9組成本質(zhì)矩陣E，對E進(jìn)行SVD奇異值分解，恢復(fù)出相機(jī)的運(yùn)動R、t，即得到了兩幀圖像I1、I2之間的位姿坐標(biāo)變化T

(7)

對同一空間點(diǎn)P，求解方程如下

s1x1=s2Rx2+t

(8)

s2R2x2+t=0

(9)

解得s1即可得到p1點(diǎn)相對深度z，最終得到相機(jī)坐標(biāo)系下估計的空間點(diǎn)P的相對坐標(biāo)P=(x,y,z)T，但由于無法確定求解的平移量t與對應(yīng)世界坐標(biāo)系下的實際移動距離的比例，也即t與實際位移相差一個比例因子。由于尺度模糊，相機(jī)位姿估計缺乏絕對尺度的校正，以及相機(jī)的觀測噪聲和特征點(diǎn)的誤匹配等其它因素的影響，最終會導(dǎo)致更嚴(yán)重的尺度漂移問題，給相機(jī)位姿估計帶來隨時間逐步增大的累積誤差，并最終使處于未知環(huán)境中的機(jī)器人定位精度嚴(yán)重下降。

2 基于深度預(yù)測的絕對尺度估計

針對上述的問題，本文研究了單目SLAM的位姿估計過程，提出利用MonoDepth深度預(yù)測網(wǎng)絡(luò)獲取單目圖像的絕對深度信息，來估計單目SLAM的絕對尺度，并通過特征點(diǎn)選取算法篩選含有可靠深度值的ORB特征點(diǎn)，最終用真實深度值進(jìn)行相機(jī)的位姿估計與優(yōu)化。

2.1 深度預(yù)測

本文采用深度預(yù)測網(wǎng)絡(luò)MonoDepth進(jìn)行單目圖像的深度估計。MonoDepth是一個端到端的單目無監(jiān)督深度估計網(wǎng)絡(luò)，網(wǎng)絡(luò)是全卷積架構(gòu)，主要包括兩部分：編碼器和解碼器。MonoDepth深度預(yù)測網(wǎng)絡(luò)不需要深度圖的真值來訓(xùn)練，而是以雙目圖像的左圖作為輸入，經(jīng)過卷積神經(jīng)網(wǎng)絡(luò)，生成左圖的預(yù)測視差圖，并加入僅在訓(xùn)練中使用的雙目攝像機(jī)的右圖，用左右視圖一致性損失函數(shù)來增強(qiáng)左右視差圖的一致性，從而使結(jié)果更準(zhǔn)確，最終得到左圖的深度預(yù)測圖。MonoDepth網(wǎng)絡(luò)總的損失函數(shù)如下

(10)

式中：上下標(biāo)l和r分別表示深度預(yù)測網(wǎng)絡(luò)訓(xùn)練時所用的雙目相機(jī)的左圖和右圖，α為各部分權(quán)重，C表示總的損失函數(shù)，它包括3部分：外觀匹配損失Cap、視差平滑損失Cds和左右視差一致性損失Clr。

經(jīng)過深度預(yù)測網(wǎng)絡(luò)得到圖像中各像素的雙目視差值。根據(jù)實際需求，本文調(diào)整了MonoDepth網(wǎng)絡(luò)的輸入和輸出部分，在輸入部分將網(wǎng)絡(luò)調(diào)整為可以適應(yīng)KITTI數(shù)據(jù)集的輸入圖片大小，而不受原始網(wǎng)絡(luò)本身只能輸入固定尺寸的限制，并通過批量預(yù)測以提高效率；而在網(wǎng)絡(luò)的輸出部分，原始網(wǎng)絡(luò)輸出為雙目視差圖，而為得到可以直接用于位姿估計的真實深度值，我們在網(wǎng)絡(luò)的后處理階段通過如下公式計算得到對應(yīng)像素的真實深度值

(11)

式中：d表示預(yù)測的深度值，disp表示網(wǎng)絡(luò)預(yù)測的視差值。網(wǎng)絡(luò)的訓(xùn)練需要通過雙目相機(jī)獲取的雙目圖像，b表示雙目相機(jī)的基線長度。f表示雙目相機(jī)的焦距。

2.2 特征點(diǎn)選取

利用MonoDepth深度預(yù)測網(wǎng)絡(luò)獲得的深度值來恢復(fù)二維圖像中特征點(diǎn)的真實三維坐標(biāo)，但并不是所有深度值都會用作估計。由于只有距離較近的點(diǎn)對位姿估計起較大作用，而距離較遠(yuǎn)的特征點(diǎn)在相鄰幀中位置變化非常小，如在室外場景下，天空與前方樹木交界處或道路盡頭等距離遠(yuǎn)但像素梯度變化大的地方檢測到的特征點(diǎn)。在SLAM中很難利用這些特征點(diǎn)進(jìn)行相機(jī)的位姿估計，但這些特征點(diǎn)仍進(jìn)行了大量重復(fù)的計算，這不但占用了較多的計算資源，還會帶來更大的噪聲和誤差，故需要剔除距離較遠(yuǎn)的特征點(diǎn)。

本文采用的策略是判斷深度值是否為可靠深度，將從圖像中提取的ORB特征點(diǎn)分為近點(diǎn)和遠(yuǎn)點(diǎn)，實驗中，設(shè)置近點(diǎn)深度閾值為20 m，將超出閾值的深度作為不可靠深度，然后剔除與不可靠深度值關(guān)聯(lián)的特征點(diǎn)，僅保留近點(diǎn)的特征點(diǎn)用于進(jìn)行后續(xù)的特征點(diǎn)匹配以及相機(jī)位姿估計，此處描述的特征點(diǎn)選取算法的偽代碼可以表示為如算法1所示：

算法1: 特征點(diǎn)選取算法 輸入: 單目圖像I 設(shè)定深度閾值th. 輸出: 經(jīng)過篩選可以用于位姿估計的ORB特征點(diǎn) (1)對單目圖像提取所有特征點(diǎn)KPS (2) I經(jīng)過MonoDepth網(wǎng)絡(luò)預(yù)測各像素點(diǎn)視差值disp (3) for 第i個ORB特征點(diǎn): (4) 根據(jù)式(11)計算第i個特征點(diǎn)對應(yīng)深度值di (5) if (th

2.3 絕對尺度估計

由于對每一幀圖像都進(jìn)行了深度估計，每個像素點(diǎn)都具有真實的尺度信息，故在初始化階段，不用像傳統(tǒng)單目SLAM算法通過三角化過程才能得到缺乏絕對尺度的相對位姿，而是從初始幀即可獲得空間中點(diǎn)的真實3維位置信息。

首先提取圖像中ORB特征點(diǎn)，ORB特征點(diǎn)包含了Oriented FAST關(guān)鍵點(diǎn)和BRIEF描述子，具有檢測速度快、魯棒性好的優(yōu)點(diǎn)。由于我們在初始幀即可獲得真實深度，故通過2.1節(jié)獲得的真實深度信息以及ORB特征點(diǎn)的像素位置索引，對ORB特征點(diǎn)進(jìn)行絕對深度值的數(shù)據(jù)關(guān)聯(lián)，即可以直接獲得空間點(diǎn)P的真實深度d，根據(jù)給定ORB特征點(diǎn)的像素坐標(biāo)(u,v)和得到的真實深度估計值d，計算該像素點(diǎn)在相機(jī)坐標(biāo)系上的世界坐標(biāo)(X,Y,Z)

(12)

式中：fx、fy是相機(jī)的焦距，cx、cy是圖像原點(diǎn)相對于光心成像點(diǎn)的偏移量。

通過上述計算便得到了初始幀圖像中特征點(diǎn)的真實尺度信息，然后在提取初始幀中ORB特征點(diǎn)的過程中，通過執(zhí)行特征點(diǎn)選取算法，剔除與不可靠深度值進(jìn)行數(shù)據(jù)關(guān)聯(lián)的ORB特征點(diǎn)，最后利用保留的特征點(diǎn)進(jìn)行特征點(diǎn)匹配和相機(jī)位姿估計。

2.4 特征點(diǎn)匹配與位姿估計

根據(jù)真實深度值估計出初始幀中特征點(diǎn)的絕對尺度，繼續(xù)對第二幀圖像同樣進(jìn)行ORB特征點(diǎn)的提取，然后通過特征點(diǎn)選取算法剔除含有不可靠深度值的特征點(diǎn)，將保留的含有可靠深度值的ORB特征點(diǎn)與第一幀圖像上的特征點(diǎn)進(jìn)行特征點(diǎn)匹配，傳統(tǒng)的暴力匹配算法計算效率低，本文采用計算速度快、而且更加適合大量特征點(diǎn)匹配的FLANN算法進(jìn)行特征點(diǎn)匹配。

當(dāng)特征點(diǎn)匹配成功后，我們根據(jù)得到的3D-2D特征點(diǎn)對(其中3D點(diǎn)是指第一幀相機(jī)坐標(biāo)系下已知3D位置的空間點(diǎn)，而2D點(diǎn)是指第二幀圖像中與第一幀圖像特征點(diǎn)匹配的2D像素點(diǎn))，利用EPNP算法計算同一空間點(diǎn)P在第二幀相機(jī)坐標(biāo)系下的3D空間點(diǎn)位置，這樣便獲得了連續(xù)兩幀圖像各相機(jī)坐標(biāo)系下同一空間點(diǎn)的3D位置，最后根據(jù)ICP算法對連續(xù)幀間相機(jī)的位姿T進(jìn)行估計。

由于觀測點(diǎn)噪聲的存在，空間點(diǎn)P在第二幀相機(jī)坐標(biāo)系下的2D投影點(diǎn)與通過特征點(diǎn)匹配獲得的p2存在誤差(也稱為重投影誤差)，解決辦法是繼續(xù)構(gòu)建非線性最小二乘BA公式，同時優(yōu)化相機(jī)位姿和空間點(diǎn)位置，使得系統(tǒng)整體的重投影誤差最小，以此來進(jìn)一步降低相機(jī)位姿估計的誤差，SLAM系統(tǒng)中非線性最小二乘的BA公式如下

(13)

同理，后續(xù)的相機(jī)位姿同樣根據(jù)已知絕對尺度的3D-2D點(diǎn)對進(jìn)行求解，用式(13)來優(yōu)化相機(jī)位姿和空間點(diǎn)位置，并通過真實的深度值實時校正尺度漂移。

本文算法流程如圖3所示。

圖3 本文算法流程

3 實驗與結(jié)果

為評估本文所提出的方法，在室外公共數(shù)據(jù)集KITTI上進(jìn)行了實驗。實驗主要分為3部分，第一部分為SLAM定位精度對比實驗，與當(dāng)前最主流的兩種單目SLAM算法進(jìn)行了定量的對比：基于特征點(diǎn)的單目ORB-SLAM(關(guān)閉回環(huán)檢測)算法和基于直接法的DSO算法；在第二部分，為更好地驗證基于深度預(yù)測網(wǎng)絡(luò)的SLAM絕對尺度估計方法用于機(jī)器人位姿估計的性能，本文還與同樣基于深度學(xué)習(xí)的視覺里程計方法在定位精度方面進(jìn)行了對比；最后，為更直觀表現(xiàn)算法構(gòu)建全局軌跡的準(zhǔn)確性，在實驗的第三部分定性對比了本文算法與ORB-SLAM算法得到的全局軌跡結(jié)果。

本文算法運(yùn)行的硬件配置為Intel Core i5-4460M CPU，主頻為3.2 GHz，內(nèi)存為8 G，GTX1030顯卡電腦。實驗系統(tǒng)采用Ubuntu16.04操作系統(tǒng)。

3.1 SLAM定位精度對比實驗

為定量對比在沒有回環(huán)檢測功能的情況下各算法的定位精度，在實驗中關(guān)閉了ORB-SLAM回環(huán)檢測，DSO算法本身不具備回環(huán)檢測功能，本文也只用絕對深度信息來估計相機(jī)位姿。

表1展示了在KITTI數(shù)據(jù)集上3種算法的均方根誤差(root mean squared error，RMSE)，每組數(shù)據(jù)都進(jìn)行了5次實驗并取其平均值,測試圖像來自于KITTI數(shù)據(jù)集中0號相機(jī)的灰度圖。

表1 KITTI數(shù)據(jù)集上的均方根誤差(RMSE)

在實際情況下，機(jī)器人經(jīng)常處于沒有回環(huán)路徑的未知場景中，想要獲得準(zhǔn)確的全局定位，只能依靠穩(wěn)定精確的位姿增量估計，從而使機(jī)器人在陌生環(huán)境中更好確定自身位置信息。為驗證算法在所有場景下的定位精度，實驗中關(guān)閉了ORB-SLAM算法的回環(huán)檢測功能。

本文算法通過加入場景的絕對尺度減少單目SLAM的尺度漂移，而不依賴回環(huán)檢測來校正相機(jī)全局位姿。從表1中可以看出，KITTI數(shù)據(jù)集的07序列場景下，本文的定位精度相比于單目ORB-SLAM提高了64.23%，而KITTI-07序列在起點(diǎn)處有一個回環(huán)路徑。同樣具有多處回環(huán)的00、05和06序列，也分別提高了34.20%、65.49%和63.56%。除了在有回環(huán)的場景下定位精度提高外，在09序列和10序列兩個無回環(huán)但長距離的場景下，相對于DSO算法在定位精度上也分別提高了21.62%和75.10%，而對比ORB-SLAM算法分別提高了11.38%和67.36%。這驗證了本文算法在無論有、無回環(huán)的場景下，都能夠表現(xiàn)出較高的定位精度和較強(qiáng)的魯棒性。

由于01序列較為空曠的場景和較大的光強(qiáng)變化，3種算法均無法完成測試。而在較短直線路段的03、04序列，本文算法相比于DSO算法定位精度較高，但相對ORB-SLAM定位精度略有所下降。主要原因是本文算法將場景中被列為遠(yuǎn)點(diǎn)的ORB特征點(diǎn)剔除，而近處的路面特征點(diǎn)較少，從而產(chǎn)生了誤差。

3.2 基于深度學(xué)習(xí)的VO定位精度對比實驗

為了更好地體現(xiàn)算法定位精度的性能，本文還使用KITTI官方評價指標(biāo)對同樣基于深度學(xué)習(xí)的DeepVO算法和LSTMVO算法進(jìn)行了對比，其中平移均方根誤差trel以“%”來度量，旋轉(zhuǎn)均方根誤差rrel以“°/100 m”來度量。兩種算法的數(shù)據(jù)來源于對應(yīng)論文，文中只提供了KITTI數(shù)據(jù)集的部分序列上的結(jié)果，而本文算法分別在KITTI數(shù)據(jù)集00-10所有序列上進(jìn)行了測試，每組結(jié)果取5次實驗的平均值。

如表2所示，對3種算法進(jìn)行了定量評估，加粗?jǐn)?shù)據(jù)表示更優(yōu)的結(jié)果。DeepVO算法和LSTMVO算法屬于視覺里程計范疇，并采用訓(xùn)練好的端到端的神經(jīng)網(wǎng)絡(luò)來估計相機(jī)位姿，而本文算法是通過幾何以及優(yōu)化的方式來估計位姿，從表中可以看出，在定位精度上相對另外兩種算法的均值分別提高了50.77%、59.72%和29.87%、81.37%。在KITTI數(shù)據(jù)集00-10所有序列上，僅有04序列上DeepVO算法在平移均方根誤差方面比本文高31.40%，其它結(jié)果均低于本文算法得到的結(jié)果。這也驗證了相比于純使用訓(xùn)練端到端的神經(jīng)網(wǎng)絡(luò)來預(yù)測相機(jī)位姿的方法，通過深度預(yù)測網(wǎng)絡(luò)估計的單目絕對尺度來輔助SLAM進(jìn)行位姿估計的定位精度更高。

表2 與深度學(xué)習(xí)算法的對比結(jié)果

3.3 軌跡對比實驗

為定性的評估全局軌跡結(jié)果，且由于ORB-SLAM的定位精度明顯優(yōu)于DSO，故只選擇與單目ORB-SLAM進(jìn)行了軌跡對比。實驗分別測試了在無回環(huán)、一個回環(huán)、較多回環(huán)和多回環(huán)4種場景下的軌跡對比，分別對應(yīng)著KITTI數(shù)據(jù)集中的10、07、05和00這4個序列，并對平移和旋轉(zhuǎn)誤差曲線也進(jìn)行了對比。

圖4～圖7展示了本文算法和單目ORB-SLAM(無回環(huán))在KITTI數(shù)據(jù)集中10、07、05和00序列的全局軌跡誤差、平移誤差和旋轉(zhuǎn)誤差的對比曲線，其中GT表示KITTI數(shù)據(jù)集提供的標(biāo)準(zhǔn)真值數(shù)據(jù)，OUR表示的是本文算法得到的結(jié)果。

圖4 序列10的軌跡、平移和旋轉(zhuǎn)誤差曲線

圖5 序列07的軌跡、平移和旋轉(zhuǎn)誤差曲線

圖6 序列05的軌跡、平移和旋轉(zhuǎn)誤差曲線

圖7 序列00的軌跡、平移和旋轉(zhuǎn)誤差曲線

由于單目ORB-SLAM存在尺度漂移問題，嚴(yán)重依賴回環(huán)檢測校正全局位姿，但大多數(shù)真實場景下缺少回環(huán)路徑，當(dāng)機(jī)器人在探索沒有回環(huán)路徑的未知場景時，機(jī)器人很難通過回環(huán)檢測來校正全局位姿，所以會隨時間產(chǎn)生累積誤差。而本文通過深度預(yù)測網(wǎng)絡(luò)為單目SLAM增加了準(zhǔn)確的絕對尺度信息，從而在增量的估計過程中減少了單目SLAM算法的累積漂移誤差，而不再需要回環(huán)檢測來校正全局位姿。從圖中可以看出，本文算法不僅在多回環(huán)場景下的00序列相比于單目ORB-SLAM更靠近真實軌跡,而且也可以適用于如10序列沒有回環(huán)的場景。這也進(jìn)一步驗證了通過深度預(yù)測網(wǎng)絡(luò)來估計單目SLAM的絕對尺度，能夠顯著地降低累積漂移誤差，從而提高相機(jī)的定位精度。

4 結(jié)束語

本文提出一種通過結(jié)合深度預(yù)測網(wǎng)絡(luò)MonoDepth恢復(fù)單目絕對尺度的低成本、高定位精度的單目SLAM算法，能夠利用真實深度信息來有效解決單目SLAM的尺度漂移問題，并通過剔除不可靠深度值的特征點(diǎn)可以消除遠(yuǎn)點(diǎn)對定位精度的影響，從而利用準(zhǔn)確的深度信息減少累積定位誤差。KITTI數(shù)據(jù)集上的實驗結(jié)果表明，結(jié)合深度預(yù)測網(wǎng)絡(luò)的單目SLAM絕對尺度估計方法，在無論有、無回環(huán)路徑的室外場景下都能有效提高SLAM的定位精度。