李明燦，賈永興，王 聰

（陸軍工程大學(xué)，江蘇 南京 210000）

0 引言

隨著芯片技術(shù)和傳感器技術(shù)的發(fā)展，移動機(jī)器人技術(shù)逐步進(jìn)入生活的方方面面。無人機(jī)、掃地機(jī)器人等功能各異的機(jī)器人正深刻改變著人們的生產(chǎn)生活方式。想要完成復(fù)雜的任務(wù)，機(jī)器人需要能夠自主規(guī)劃自身的運(yùn)動。在沒有操作者的情況下，自主移動機(jī)器人失去了輔助判斷狀態(tài)的先決條件，所以必須具備感知外部信息的能力，憑借傳感器數(shù)據(jù)估算自身的位置與周圍環(huán)境的信息，從而依靠這些信息計(jì)算出最佳的運(yùn)動路線和動作軌跡，而準(zhǔn)確的機(jī)器人信息感知是困難的。機(jī)器人搭載的傳感器只能測量到局部的環(huán)境數(shù)據(jù)，不能直觀了解環(huán)境的整體情況，必須依靠位置狀態(tài)進(jìn)行環(huán)境信息的提取與合成。這種相互依存的關(guān)系導(dǎo)致移動機(jī)器人學(xué)中一個重大的開放性問題——要精確定位，因此機(jī)器人必須有精確的環(huán)境地圖。為了構(gòu)建精確的地圖，必須精確知道機(jī)器人觀測的位置。為了實(shí)現(xiàn)高精度的信息感知，研究者們進(jìn)行了長期探索，經(jīng)過建模分析與實(shí)際環(huán)境實(shí)驗(yàn)，提出了許多有意義的解決方案，其中，一個重要的領(lǐng)域是同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)，目前已成為整個問題框架中的核心研究方向。

1 視覺SLAM技術(shù)介紹

SLAM技術(shù)旨在利用傳感器數(shù)據(jù)，通過計(jì)算機(jī)估算出機(jī)器人平臺的位置變化，并同時性地構(gòu)建出周圍環(huán)境的仿真地圖。其中，利用視覺傳感機(jī)器獲取外部信息的SLAM技術(shù)稱為視覺SLAM。視覺SLAM前端被叫做視覺里程計(jì)，目的是通過計(jì)算機(jī)視覺方法估算出機(jī)器人的位姿變化；視覺SLAM后端使用圖優(yōu)化[1]或束調(diào)整[2]等方式，優(yōu)化機(jī)器人的運(yùn)動軌跡。在視覺里程計(jì)研究領(lǐng)域，不少開發(fā)者推出許多優(yōu)秀的算法，實(shí)現(xiàn)了使用不同類型的攝像頭估算運(yùn)動中位姿的變化。按是否需要提取圖像特征[3]，它主要分為特征點(diǎn)法和直接法。

在當(dāng)前視覺SLAM領(lǐng)域，最受關(guān)注的是利用深度相機(jī)完成感知任務(wù)。深度相機(jī)又稱RGBD相機(jī)，可以用來觀測視角范圍的彩色圖像和深度圖像，輸出一連串的三維圖像?；赗GBD數(shù)據(jù)的特征點(diǎn)法利用彩色圖像拾取特征點(diǎn)，而后利用對應(yīng)的距離信息還原出其真實(shí)的空間分布，并根據(jù)3D-3D空間位姿變換的約束關(guān)系，依靠部分特征點(diǎn)使用純幾何手段估算關(guān)鍵幀之間的變換矩陣，進(jìn)而分解出機(jī)器人運(yùn)動的平移向量和旋轉(zhuǎn)矩陣。在得到一段時期的位姿變換估計(jì)后，算法后端利用優(yōu)化算法對輸出結(jié)果進(jìn)行高維度濾波，得到更符合實(shí)際變化特點(diǎn)的定位信息。系統(tǒng)的另一個任務(wù)是建圖，即利用優(yōu)化獲得的定位信息，將三維特征點(diǎn)或者點(diǎn)云圖變換參考系并進(jìn)行耦合，得到稀疏或者稠密的仿真地圖。

基礎(chǔ)的視覺SLAM算法在結(jié)構(gòu)上已趨于完善，但是在實(shí)際的應(yīng)用實(shí)現(xiàn)中并不能完全穩(wěn)定的工作，需要使用許多特別的技巧強(qiáng)化系統(tǒng)的健壯性。經(jīng)過長期的探索，研究者推出了許多優(yōu)秀的框架，在原有理論的基礎(chǔ)上采用了新的理念，完成了準(zhǔn)確的定位和建圖?？紤]到傳感器適配、稠密建圖以及準(zhǔn)確性等方面，本文選擇最新的RGB-D SLAM V2來進(jìn)行典型算法的分析和評估。

2 算法結(jié)構(gòu)

RGB-D SLAM V2由Endres提出，是一個基于深度相機(jī)的稠密視覺SLAM系統(tǒng)。RGB-D SLAM V2算法流程，如圖1所示。它的代碼直接兼容機(jī)器人操作系統(tǒng)（Robot Operating System，ROS），基本不用配置就可以非常方便地運(yùn)行。對比基礎(chǔ)的系統(tǒng)架構(gòu)，它在位姿估計(jì)、重定位、回環(huán)檢測和建圖等方法上使用了創(chuàng)新算法，提高了系統(tǒng)的容錯性，使系統(tǒng)對機(jī)器人的運(yùn)動追蹤更加快速和穩(wěn)定，同時能夠產(chǎn)生較為完整的環(huán)境地圖，為后續(xù)的應(yīng)用提供了更全面的信息。

圖1 RGB-D SLAM V2流程

2.1 位姿估計(jì)

RGB-D SLAM V2使用的是標(biāo)準(zhǔn)的特征點(diǎn)法，支持SIFT、SUFT以及ORB[4]等特征選取方法，利用稀疏的3維特征點(diǎn)估計(jì)幀與幀之間的變換矩陣。如圖2所示，比較3種特征點(diǎn)，有GPU時SIFT綜合表現(xiàn)最好，而綜合實(shí)時性、硬件成本和準(zhǔn)確率來看，ORB具有均衡的優(yōu)勢。

圖2 3種特征點(diǎn)效果比較

視覺SLAM基礎(chǔ)的位姿估算方法利用的是連續(xù)兩幀的空間變換關(guān)系。假設(shè)上一幀觀測位置為p1，當(dāng)前幀觀測位置為p2，兩幀之間的變換矩陣描述為T12，則觀測位置之間的關(guān)系為：

RGB-D SLAM V2采用的估計(jì)算法不僅利用了一種策略，而且采用RANSAC[5]和ICP相結(jié)合的方式進(jìn)行運(yùn)動估計(jì)。初始階段，前端使用3對特征點(diǎn)進(jìn)行快速計(jì)算作為RANSAC的參考。在每一輪迭代中，利用最小二乘法估計(jì)其余點(diǎn)對相關(guān)程度，不斷優(yōu)化得到最佳的變換矩陣。

2.2 環(huán)境測量模型

Endres提出了一個專屬的環(huán)境測量模型（Environmental Measurement Model，EMM），如圖3所示，指出了觀測圖像的特征點(diǎn)服從高斯分布，開創(chuàng)性地利用高斯分布的吻合特性來考慮點(diǎn)位之間的一致性問題。模型把兩幀之間的特征點(diǎn)進(jìn)行空間初始估計(jì)后，將特征點(diǎn)的高斯分布空間按照設(shè)定的閾值劃分邊界?？臻g能夠重疊的相同特征會被視作在可接受誤差范圍內(nèi)的可靠點(diǎn)，可以被用來進(jìn)行進(jìn)一步的優(yōu)化，而無法重疊的匹配點(diǎn)會被視作誤匹配或者不合格的特征而被丟棄。另外，要考慮在當(dāng)前幀中出現(xiàn)的新特征點(diǎn)，既沒有舊特征點(diǎn)與之匹配，也不存在未匹配特征點(diǎn)與之高斯分布重合。

圖3 環(huán)境測量模型

2.3 重定位

定位技術(shù)的方法有很多，可以將其劃分為絕對定位和相對定位。絕對定位是以固定的坐標(biāo)為參考，計(jì)算每次定位的絕對位置。相對定位以初始位置為坐標(biāo)原點(diǎn)，依次估計(jì)相鄰兩次觀測點(diǎn)位間的相對運(yùn)動，等效得到定位信息。但是，只是依靠連續(xù)幀簡單估計(jì)變換矩陣時，誤差累計(jì)會導(dǎo)致最終的定位發(fā)生嚴(yán)重偏移。重定位技術(shù)利用變換參考的方式很好地解決了這個問題，技術(shù)原理如圖4所示。

重定位的具體方法是在每次位姿估計(jì)之前對當(dāng)前幀的光照條件進(jìn)行一次估算，將滿足閾值的圖像作為關(guān)鍵幀抽選出來，作為接下來三維幀位姿估計(jì)的參考。這種方式的實(shí)質(zhì)是將連續(xù)的相對位姿估計(jì)轉(zhuǎn)化成可變窗口的短時絕對定位，而參考的定位點(diǎn)是窗口的第一幀圖像的位置。在后端的優(yōu)化過程中，參考幀組成的稀疏圖像集會被看作運(yùn)動的節(jié)點(diǎn)，進(jìn)一步優(yōu)化估計(jì)的結(jié)果，從而實(shí)現(xiàn)對運(yùn)動噪聲的一種數(shù)字化濾波。由于關(guān)鍵幀的低數(shù)量級和可存儲的特點(diǎn)，優(yōu)化可以高效完成，不需要再在整段圖像序列上進(jìn)行操作，節(jié)約了后端處理的時間。

圖4 重定位技術(shù)

2.4 圖優(yōu)化

位姿圖優(yōu)化是利用圖論的算法優(yōu)化位姿的手段。在RGB-D SLAM V2中，圖優(yōu)化只優(yōu)化位姿圖，并沒有優(yōu)化三維圖像點(diǎn)。具體實(shí)現(xiàn)中，估算的位姿被畫成了一個全向圖，使用g2o圖優(yōu)化庫來對被環(huán)境測量模型接受的運(yùn)動估計(jì)結(jié)果進(jìn)行噪聲濾除，將兩幀相機(jī)位姿作為優(yōu)化頂點(diǎn)，將它們之間的運(yùn)動估計(jì)結(jié)果作為優(yōu)化邊，利用兩者的約束關(guān)系，在多次迭代后得到最佳的估計(jì)。值得注意的是，后端模塊對檢測到的回環(huán)也加入優(yōu)化頂點(diǎn)和邊，實(shí)際上對全局優(yōu)化進(jìn)行了一定的矯正。

優(yōu)化邊的誤差函數(shù)：

式中，xi、xj為優(yōu)化頂點(diǎn)，即位姿的估計(jì)值；zij是約束，也就是xi和xj之間的變換；e(xi,xj,zij)是滿足xi到xj的誤差程度；中間的Ωij是優(yōu)化邊的信息矩陣（協(xié)方差矩陣的逆），表示對邊的精度的估計(jì)。

2.5 回環(huán)檢測

回環(huán)檢測是利用運(yùn)動中相同路標(biāo)的重復(fù)出現(xiàn)對軌跡進(jìn)一步優(yōu)化的方法，實(shí)質(zhì)是一種采用反饋調(diào)節(jié)消除誤差的策略。實(shí)現(xiàn)回環(huán)檢測的關(guān)鍵是要依靠特征點(diǎn)包含的信息。健壯的圖像特征點(diǎn)要在保證視角變換和旋轉(zhuǎn)的情況下仍能被檢測出來，并具有相同的描述信息，因此每個特征點(diǎn)要同時包含點(diǎn)位的坐標(biāo)及其描述子，用來完成特征的準(zhǔn)確匹配?；丨h(huán)檢測就是利用特征點(diǎn)描述子的重復(fù)出現(xiàn)來判別機(jī)器人是否來到了自己走過的路線。

RGB-D SLAM V2系統(tǒng)的回環(huán)檢測使用基于最小生成樹的方法——一種隨機(jī)森林的隨機(jī)回環(huán)。算法使用描述子之間直接相減的差值，產(chǎn)生了一棵有限深度的最小生成樹，去掉連續(xù)多幀內(nèi)的最近點(diǎn)來避免重復(fù)比較，再隨機(jī)從樹上選k個偏向于更早時間的幀來判斷閉環(huán)是否滿足。

2.6 建 圖

RGB-D SLAM V2系統(tǒng)適配點(diǎn)云地圖和八叉樹地圖兩種地圖。點(diǎn)云是把二維像素三維化后得到的結(jié)果，使得所有像素點(diǎn)的空間位置被直白表示出來。在無失準(zhǔn)的情況下，點(diǎn)云保留了觀測的所有信息。經(jīng)過空間變換擬合后的地圖最大限度地還原了原始環(huán)境，但是這對運(yùn)算、存儲和顯示資源的消耗都是巨大的。一個小場景的點(diǎn)云地圖往往需要1×109bit的存儲，顯然后續(xù)應(yīng)用的輸入是難以適用的。此外，這種地圖保留的信息未經(jīng)進(jìn)一步處理，后續(xù)信息的提取在輕量級平臺上也難以達(dá)到實(shí)時性。

對比而言，八叉樹地圖具有更高的應(yīng)用優(yōu)勢。如圖5所示，八叉樹地圖使用濾波方式，將設(shè)定分辨率內(nèi)的像素轉(zhuǎn)換為單一的體素，利用算法中樹的結(jié)構(gòu)，逐級把相鄰8個體素中顏色最多的體素作為上級分辨率節(jié)點(diǎn)形成一個八叉樹，從而將整個地圖更優(yōu)化地存儲，提高地圖的可查找性和可利用性。

圖5 八叉樹地圖原理

3 性能評估

實(shí)驗(yàn)階段，利用標(biāo)準(zhǔn)的數(shù)據(jù)集衡量了系統(tǒng)定位的準(zhǔn)確性，評估輸出地圖的視覺效果。

MIT推出了使用RGB-D相機(jī)測量的數(shù)據(jù)集，結(jié)合高精度的慣導(dǎo)設(shè)備重現(xiàn)運(yùn)動軌跡，用以驗(yàn)證和評價SLAM算法的定位精度。由手持設(shè)備三維平行運(yùn)動觀測（圖6、圖7）、圍繞目標(biāo)運(yùn)動觀測（圖8、圖9）和360°運(yùn)動觀測（圖10、圖11）等場景測試結(jié)果可以看出，算法對光心移動的定位較為準(zhǔn)確，基本可以還原真實(shí)的軌跡，對場景的構(gòu)建較為真實(shí)，但是建圖存在區(qū)域性的空洞現(xiàn)象。造成這個問題的原因在于算法使用了直接耦合的方式，對觀測中的運(yùn)動速度對數(shù)據(jù)的影響沒有進(jìn)行適配，導(dǎo)致速度快速變化時像素點(diǎn)的漂移較大，使得擬合后的地圖具有空間點(diǎn)疏離的特點(diǎn)。

圖6 三維平行運(yùn)動地圖構(gòu)建效果

圖7 三維平行運(yùn)動定位估計(jì)效果

圖9 圍繞目標(biāo)運(yùn)動定位估計(jì)效果

圖10 360°運(yùn)動地圖構(gòu)建效果

圖11 360°運(yùn)動定位估計(jì)效果

4 結(jié)語

本文對RGB-D SLAM V2系統(tǒng)進(jìn)行了詳細(xì)講解，并評估算法的數(shù)據(jù)集測試效果和真實(shí)場景的建圖效果。系統(tǒng)在基礎(chǔ)的視覺SLAM框架的基礎(chǔ)上，利用一些創(chuàng)新的輔助技巧提高了系統(tǒng)的容錯性，更健壯地完成了定位和建圖的整個流程，在適配的RGBD相機(jī)上性能表現(xiàn)優(yōu)秀。從實(shí)驗(yàn)結(jié)果可以看出，算法定位的精度基本擬合真實(shí)的運(yùn)動，對環(huán)境的重現(xiàn)也可用于視覺觀測，具有極好的可擴(kuò)展性。下一步工作將對定位和建圖的輸出結(jié)果應(yīng)用機(jī)器人導(dǎo)航進(jìn)行更加細(xì)致的研究。