面向嵌入式平臺的單目ORB-SLAM稠密化建圖實現

馬靖煊，王紅雨，曹 彥，喬文超，韓佼志，吳昌學

1.上海交通大學 電子信息與電氣工程學院，上海 200240

2.上海宇航系統工程研究所，上海 200240

同時定位與地圖構建（simultaneous localization and mapping，SLAM）技術是機器人技術領域的一個研究熱點和關鍵技術。在室外環境中，機器人可以依靠以衛星定位為基礎的技術實現對其自身的定位，進一步實現路徑規劃與避障探索。但人類活動時間超過70%是在室內，而室內環境復雜，需要從多種測量數據中估計移動節點的實時位置[1]。在未知的室內環境中，機器人需要構建地圖來執行各種任務，但構建地圖的前提條件是機器人知道自己的位置。

在過去的十幾年里，很多學者嘗試使用多種傳感器來實現SLAM技術。Davison使用單目攝像頭實現了實時SLAM系統[2]，是第一個實時的單目視覺SLAM系統；Asada 等人設計了自學習雙目立體視覺伺服系統，通過雙目立體視覺三角測量原理跟蹤移動方向未知的目標[3]；在2007 年，Klein 等人提出了并行追蹤與建圖（parallel tracking and mapping，PTAM）方法[4]，提出并實現了跟蹤與建圖過程的并行化，并且PTAM是第一個使用非線性優化，而非傳統的濾波器（如卡爾曼濾波[5]、粒子濾波[6]和組合濾波[7]）作為后端的方案。國內學者在文獻[8]中對基于RGB-D的SLAM系統也進行了深刻的研究。隨著PTAM的提出，許多優秀的SLAM系統在其基礎上被設計出來，ORB-SLAM 正是PTAM 繼承者中最完善易用的SLAM 系統之一。2020 年7 月提出了最新的ORBSLAM3 系統[9]，該系統是第一個同時支持單目、雙目、RGB-D 相機的SLAM 系統。與它的前身ORB-SLAM2系統相比，ORB-SLAM3融合了基于緊耦合方法的視覺-慣性里程計，完全依賴于最大后驗概率（maximum-aposteriori，MAP）估計，精度比前者高2～5 倍；第二個創新點是多地圖系統，可以在丟失追蹤時創建一張新的地圖，并在重新追蹤時與先前的地圖無縫合并。

與其他的SLAM方案相比，ORB-SLAM3在保留了回環檢測的基礎上，在跟蹤階段加入了IMU，在地圖構建階段引入了多地圖系統，這些特點使得ORB-SLAM3有著遠超其他方案的穩健性。當然也存在一些不足之處，如其構建的稀疏特征點地圖僅能滿足機器人對定位的需求，而無法提供導航、避障、交互等諸多功能。為了克服上述缺點，為機器人導航避障提供基礎，本文將論述基于ORB-SLAM3 系統實現的稠密建圖，并在Nvidia Jetson AGX Xavier 嵌入式平臺上測試了基于TUM 數據集本文方法的效率與精度，取得了一定的實驗效果。

1 ORB-SLAM3系統概述

ORB-SLAM 是一種基于ORB（oriented fast and rotated BRIEF）關鍵點和非線性優化方式的，適用于單目、雙目、RGB-D 等多種相機模型的視覺SLAM 系統。該系統圍繞ORB 特征進行計算，包括視覺里程計部分和用于回環檢測詞袋（bag of words，BoW）[10]，這一特征是現階段計算平臺的一種優秀的效率與精度之間的折中方案。ORB不像SIFT（scale-invariant feature transform）或SURF（speed up robust feature）那樣費時，在CPU上面即可實時計算；亦比Harris等簡單角點具有更好的旋轉和縮放不變性。同時，ORB-SLAM 創新式地使用了三個線程來完成其整個SLAM系統：實時跟蹤特征點的Tracking跟蹤線程、局部地圖構建線程以及回環檢測線程，如圖1所示。下文將分別介紹這三個線程的作用。

圖1 ORB-SLAM3系統線程Fig.1 ORB-SLAM3 system thread

1.1 Tracking跟蹤線程

Tracking 跟蹤線程負責處理傳感器采集到的數據，并實時計算當前幀相對于當前活動地圖的姿態，盡可能地降低符合地圖要素的重投影誤差。同時，跟蹤線程還判斷當前幀是否可以作為關鍵幀使用。在視覺-慣性模式下，跟蹤線程通過在優化中加入慣性殘差來估計相機速度和IMU 的偏差。當跟蹤丟失時，跟蹤線程會嘗試在所有地圖集中重新定位當前幀，如果成功重新定位的話會恢復跟蹤，并在適當的時候切換活動地圖；如果重定位失敗，在一定時間后活動地圖會存儲為非活動地圖，并創建新的活動地圖。在傳統的純視覺SLAM系統中，估計的狀態只包括當前的相機姿態，而在ORBSLAM3 的視覺慣性SLAM 系統中，還需要考慮以下參數：真實世界坐標系下的姿態Ti、速度vi，以及陀螺儀和加速度計的漂移和bai，這兩個漂移假設為布朗運動，i為當前時刻的視頻幀。可以得出如下狀態向量：

對于視覺-慣性SLAM，通過文獻[11]中的方法在連續的視頻幀i和i+1 之間預積分IMU，并通過文獻[12]的方法形成公式，從而得到預測量的旋轉、速度和位置信息。最終，視覺慣性優化問題可通過下式表示：

式中，sˉk是一組數量為k+1 的關鍵幀的狀態，x是一組數量為h的3D點的狀態，rli-1,i是慣性殘差，Vli,i+1是整個測量向量的信息矩陣，ρHub是魯棒Huber 核，kj是一組數量為j的3D觀測點。這種優化方法需要較為精確的初始值，從而實現更快地收斂和得到更精確的解。

1.2 局部地圖構建線程

局部地圖構建線程負責向當前活動的地圖添加關鍵幀和地圖點，同時剔除冗余的關鍵幀和地圖點，并通過視覺或視覺-慣性約束來細化地圖。剔除冗余地圖點的原則有以下幾條：（1）這些點在接下來的圖像幀中無法跟蹤匹配；（2）投影光線通過三角化計算點處于低視差；（3）三角測量點產生的重投影誤差會較大。這三條地圖點篩選原則保證了所構建地圖的魯棒性。為了保證地圖的簡潔性，降低BA過程的復雜度，提高系統的運行速度，在ORB-SLAM3 的關鍵幀集合中，如果某個關鍵幀中90%的地圖點在其他至少3 個關鍵幀中被檢測到，則會剔除該關鍵幀。

1.3 回環檢測線程

在ORB-SLAM2系統中，回環檢測線程利用各個關鍵幀的相互關聯性進行有效的閉合回環，在本質圖上優化位姿，這樣就可以將累計的誤差分散到位姿圖中，并通過相似變換校正尺度偏差，從而降低視覺里程計過程中的累計誤差[13]。在ORB-SLAM3系統中，回環檢測線程不光具有ORB-SLAM2中的功能，還會以關鍵幀速率來檢測活動地圖和整個地圖集之間的公共區域。如果公共區域屬于活動地圖，回環檢測線程會執行前文所述的循環校正；如果屬于不同的地圖，則兩張地圖會無縫地合并成一張地圖，并成為新的活動地圖。循環校正后，ORB-SLAM3系統會在獨立線程中啟動一個完整的BA來進一步細化地圖映射，而不會影響實時性能。

ORB-SLAM3 雖然魯棒性更好，性能更強，但其構建的仍是三維稀疏特征地圖，無法滿足機器人的導航、避障、路徑規劃等需求。為了克服這個缺點，本文在ORB-SLAM3系統上進行改進，使其能對三維場景進行稠密化建圖。

2 稠密化建圖實現

改進后的OS3-D（ORB-SLAM3-Dense）系統框架圖如圖2所示，圖中虛線框內為OS3-D系統新增的稠密建圖線程。在Tracking 跟蹤線程中對滿足要求的關鍵幀進行特征點二次采樣，二次采樣僅僅是為了提取ORB特征點，不需要進行匹配，也不用于估計相機姿態和運動。相機位姿則由局部BA進行初始化，并在回環檢測和全局BA階段進行更新，使點云拼接更為精確。

圖2 加入稠密建圖線程系統結構圖Fig.2 System structure diagram of adding dense mapping thread

稠密建圖線程主要分為三部分：數據輸入、位姿估計和地圖構建。數據輸入部分采集RGB 圖像、深度圖和ORB特征點，為地圖構建提供環境源數據；位姿估計部分利用ORB-SLAM3 為每一幀滿足建圖需求的關鍵幀提供姿態估計；地圖構建節點接收前兩部分的圖像數據和位姿信息，進行三維點云拼接，并維護和管理點云數據，后續可增加點云濾波、語義識別等功能。稠密建圖線程結構如圖3所示。

圖3 稠密建圖線程結構圖Fig.3 Structure diagram of dense mapping thread

三維點云可以根據針孔相機成像原理，由二維RGB圖像和深度圖像生成：式中，cx、cy、fx和fy是相機內參，u、v是像素坐標，x、y、z是像素坐標系坐標，d是像素點深度（即像素點到成像平面距離），單位為毫米（mm），s是深度尺度，根據經驗取1 000。

此時得到的某一幀圖像的三維點云是基于此時刻的相機坐標系，為了得到像素點在真實世界中的坐標，需要利用前文所述的位姿信息將像素點的相機坐標變換為世界坐標：

式中，Ti為前文中第i個關鍵幀相機在真實世界坐標系下的位姿，Xci,j是第i個關鍵幀在相機坐標系上的坐標，Xw,j是變換后的世界坐標。

在實際使用過程中，深度相機的depth 量程是有限的，通過如下公式去掉“不可能”采集到的點：

式中，dmin、dmax分別是實際深度相機的上下量程，單位為毫米（mm），dtrust是實際深度相機可以采集到的點。以Intel Realsense D455相機為例，dmin為400，即0.4 m，dmax為8 000，即8 m。

3 實驗結果和分析

為了驗證OS3-D算法有效性，對文獻[14]所提供的RGB-D 標準數據集進行了測試，包括彩色圖、深度圖、相機內參與高精度動作捕捉系統獲得的相機運動位姿，并且該數據集還提供了計算估計位姿和真實位姿的均方根誤差（root mean square error，RMSE）工具來評估算法的定位精度。值得一提的是，為了后續針對無人機室內建圖研究，用于評測OS3-D的測試平臺是Nvidia公司的ARM架構嵌入式平臺Jetson AGX Xavier，其CPU是8個ARM V8.2核心，GPU是擁有32 TOPS計算能力的Volta核心，整個系統的最大功耗僅為30 W，不帶電源適配器重量672 g，十分適合無人機這種重量、功耗敏感的平臺。之前已有學者在ORB-SLAM2 上增加半稠密建圖線程，并在筆記本平臺測試了性能[15]。

測試OS3-D 主要采用TUM 數據集中的RGB-D 標準數據進行實驗，估計相機的運動軌跡，并進行稠密建圖。三維稠密建圖效果如圖4所示，估計軌跡與真實軌跡對比圖如圖5所示。

圖4 部分fr數據集稠密建圖效果圖Fig.4 Dense mapping renderings of some fr data sets

從圖4 中可以看出，OS3-D 算法可以較好地實現三維稠密建圖，不論是對于細節較多的（a）fr1/desk、（b）fr1/desk2、（c）fr1/xyz 和（e）fr2/xyz 等場景，還是大范圍場景（d）fr2/desk 和（f）fr3/long_office，都可以較好地還原。從圖5可以看出，OS3-D對于軌跡的估計有一定的漂移，但總體來說估計軌跡較為準確。表1為OS3-D與ORB-SLAM3 算法的性能對比，表中“OS3”為ORBSLAM3縮寫。

圖5 真實軌跡與估計軌跡圖Fig.5 Real trajectory and estimated trajectory

從表1中可以知道：因為OS3-D算法與ORB-SLAM3算法相比，增加了稠密建圖線程，構建了三維稠密地圖場景，并且使用了性能較弱的計算平臺，故本文算法的每幀耗時、合計耗時均弱于ORB-SLAM3 算法，并且由于每幀耗時增加，進而影響了算法的定位精度。

表1 本文算法與ORB-SLAM3性能對比Table 1 Performance comparison between algorithm in this paper and ORB-SLAM3

與ORB-SLAM3算法相比，OS3-D算法的每幀平均耗時為0.092 2 s，即每秒10.8 幀，每幀處理耗時增加了3.7%；合計處理耗時增加了7%；對于定位精度RMSE，本文算法僅增加了2%，在6個數據集的測試中，本文算法的平均定位精度達到了0.213%。綜合來看，整個系統可以滿足機器人定位與建圖在精度和速度上的要求。

4 結束語

為了實現機器人在室內的定位、導航、避障與建圖需求，本文基于ORB-SLAM3算法設計了三維稠密地圖重建系統，并在嵌入式平臺Jetson AGX Xavier上進行了數據集測試。實驗表明該系統相較于ORB-SLAM3系統，實現了三維稠密建圖，同時定位速度可達10.8 frame/s，定位精度可達0.213%，滿足機器人室內定位需求，為機器人室內導航避障提供了基礎。

目前本文方法仍存在不足，僅實現了在TUM 數據集上的測試。并且因為本文方法是基于ORB角點特征實現位置識別和回環檢測，所以在角特征點較少的環境中容易定位失敗。未來，將進一步改進實現三維稠密地圖的實時構建，加入濾波算法和基于像素的直接SLAM，提高系統的運行效率和定位效果。