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

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

2.上海宇航系統(tǒng)工程研究所，上海 200240

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

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

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

1 ORB-SLAM3系統(tǒng)概述

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

圖1 ORB-SLAM3系統(tǒng)線程Fig.1 ORB-SLAM3 system thread

1.1 Tracking跟蹤線程

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

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

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

1.2 局部地圖構建線程

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

1.3 回環(huán)檢測線程

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

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

2 稠密化建圖實現(xiàn)

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

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

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

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

三維點云可以根據(jù)針孔相機成像原理，由二維RGB圖像和深度圖像生成：式中，cx、cy、fx和fy是相機內(nèi)參，u、v是像素坐標，x、y、z是像素坐標系坐標，d是像素點深度（即像素點到成像平面距離），單位為毫米（mm），s是深度尺度，根據(jù)經(jīng)驗取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 標準數(shù)據(jù)集進行了測試，包括彩色圖、深度圖、相機內(nèi)參與高精度動作捕捉系統(tǒng)獲得的相機運動位姿，并且該數(shù)據(jù)集還提供了計算估計位姿和真實位姿的均方根誤差（root mean square error，RMSE）工具來評估算法的定位精度。值得一提的是，為了后續(xù)針對無人機室內(nèi)建圖研究，用于評測OS3-D的測試平臺是Nvidia公司的ARM架構嵌入式平臺Jetson AGX Xavier，其CPU是8個ARM V8.2核心，GPU是擁有32 TOPS計算能力的Volta核心，整個系統(tǒng)的最大功耗僅為30 W，不帶電源適配器重量672 g，十分適合無人機這種重量、功耗敏感的平臺。之前已有學者在ORB-SLAM2 上增加半稠密建圖線程，并在筆記本平臺測試了性能[15]。

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

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

從圖4 中可以看出，OS3-D 算法可以較好地實現(xiàn)三維稠密建圖，不論是對于細節(jié)較多的（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個數(shù)據(jù)集的測試中，本文算法的平均定位精度達到了0.213%。綜合來看，整個系統(tǒng)可以滿足機器人定位與建圖在精度和速度上的要求。

4 結束語

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

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