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

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

0 引 言

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

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

本文利用MonoDepth[11]深度預測網絡估計單目的絕對尺度，以此來彌補單目固有的尺度模糊的缺點。利用絕對尺度實時校正尺度漂移，從而提高單目SLAM的定位精度。

1 單目視覺SLAM

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

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

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

圖2 單目相機運動求解原理

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

(1)

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

齊次坐標下，取x1、x2為兩個像素點p1、p2在歸一化坐標平面上的點，則有

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

(2)

(3)

(4)

對極約束中包括了旋轉和平移，其中

(5)

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

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

(6)

中間項e1到e9組成本質矩陣E，對E進行SVD奇異值分解，恢復出相機的運動R、t，即得到了兩幀圖像I1、I2之間的位姿坐標變化T

(7)

對同一空間點P，求解方程如下

s1x1=s2Rx2+t

(8)

s2R2x2+t=0

(9)

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

2 基于深度預測的絕對尺度估計

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

2.1 深度預測

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

(10)

式中：上下標l和r分別表示深度預測網絡訓練時所用的雙目相機的左圖和右圖，α為各部分權重，C表示總的損失函數，它包括3部分：外觀匹配損失Cap、視差平滑損失Cds和左右視差一致性損失Clr。

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

(11)

式中：d表示預測的深度值，disp表示網絡預測的視差值。網絡的訓練需要通過雙目相機獲取的雙目圖像，b表示雙目相機的基線長度。f表示雙目相機的焦距。

2.2 特征點選取

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

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

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

2.3 絕對尺度估計

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

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

(12)

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

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

2.4 特征點匹配與位姿估計

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

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

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

(13)

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

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

圖3 本文算法流程

3 實驗與結果

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

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

3.1 SLAM定位精度對比實驗

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

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

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

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

本文算法通過加入場景的絕對尺度減少單目SLAM的尺度漂移，而不依賴回環(huán)檢測來校正相機全局位姿。從表1中可以看出，KITTI數據集的07序列場景下，本文的定位精度相比于單目ORB-SLAM提高了64.23%，而KITTI-07序列在起點處有一個回環(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)的場景下，都能夠表現出較高的定位精度和較強的魯棒性。

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

3.2 基于深度學習的VO定位精度對比實驗

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

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

表2 與深度學習算法的對比結果

3.3 軌跡對比實驗

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

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

圖4 序列10的軌跡、平移和旋轉誤差曲線

圖5 序列07的軌跡、平移和旋轉誤差曲線

圖6 序列05的軌跡、平移和旋轉誤差曲線

圖7 序列00的軌跡、平移和旋轉誤差曲線

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

4 結束語

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