基于RGB-D的移動機器人地圖構(gòu)建方法研究

富 裕，劉振宇

（沈陽工業(yè)大學(xué)信息科學(xué)與工程學(xué)院, 沈陽110870）

1 引 言

近幾年，隨著廉價高可用性RGB-D 相機的流行，RGB-D SLAM 得到廣泛應(yīng)用。RGB-D SLAM 的實現(xiàn)與激光雷達方法相似，都是利用距離圖像之間的密集三維點云來估計對應(yīng)點云之間作為剛體變換的位姿間的運動。華盛頓大學(xué)（University of Washington）的Henry 等人于2010 年提出了一種室內(nèi)三維視覺SLAM 方法[1]，該法通過對Kinect 采集到的RGB 圖像提取SIFT（Scale-Invariant Feature Transform，尺度不變特征變換)特征點，結(jié)合深度數(shù)據(jù)與ICP（Iterative Closest Point，迭代最近點)算法[2]估計攝像機的位姿，最終建立起室內(nèi)環(huán)境下的三維地圖。其后，德國弗萊堡大學(xué)（University of Freiburg）計算機科學(xué)系的Nikolas Engelhard、Felix Endres 和Jurgen Hess 提出一套基于手持Kinect 相機的RGBD SLAM 方法[3]，在SLAM 算法的視覺里程計部分使用SURF（Speeded Up Robust Features，加速魯棒性特征）[4]算法，然后根據(jù)特征點計算出三維空間坐標。與SIFT 相比，在保證具有尺度和旋轉(zhuǎn)不變性的同時，其時間復(fù)雜度稍有降低，但目前為止計算機仍然無法實時地進行圖像特征提取及匹配。另外，SLAM算法的誤差主要來源于行進過程中機器人帶來的累計誤差。當(dāng)移動機器人進行定位時，其可通過觀測位置處已知的特征標志對前端的誤差進行補償，每一次觀測使機器人的位置誤差趨近特征標志的位置誤差與觀測誤差之和[5]。然而在SLAM 中，由于移動機器人的位置和環(huán)境中特征標志的位置均未知，故觀測信息不能有效糾正前端的誤差，機器人的位置誤差會隨機器人運動距離的增大而增大。因此，機器人的位置誤差與特征標志的位置誤差緊密相關(guān)。它們之間的相互影響使機器人和特征標志的位置估計產(chǎn)生累計誤差，難以保證地圖的全局一致性。

針對上述研究現(xiàn)狀，提出一種室內(nèi)環(huán)境下的移動機器人RGB-D SLAM 方法。該方法利用Kinect獲取圖像彩色信息及深度信息，提取ORB（Oriented FAST and Rotated BRIEF）特征點進行特征匹配，在保證特征點匹配精度的同時，有效提高了算法實時性；引入回環(huán)檢測方法，有效抑制累積誤差造成的位姿漂移，最終完成室內(nèi)環(huán)境下的三維地圖重建。

2 視覺SLAM 框架

SLAM（Simultaneous Localization and Mapping，同時定位與地圖構(gòu)建）是指機器人在其自身位置不確定且無先驗環(huán)境模型的條件下創(chuàng)建地圖，同時基于該地圖實現(xiàn)自定位[6]。在此提出一種基于深度圖像的移動機器人RGB-D SLAM 框架，框架的結(jié)構(gòu)與流程圖如圖1 所示，其主要包括前端（視覺里程計)、后端和回環(huán)檢測三個部分。

圖1 提出的RGB-D SLAM 框架

a.前端

VO（Visual Odometry，視覺里程計）用來估計兩個時刻間機器人的相對運動。在視覺SLAM 中，將觀測到的信息與全局地圖進行匹配，采用RANSAC（Random Sample Consensus，隨機抽樣一致性）[7]算法去除誤點得到位姿的初始估計；接著采用ICP 算法進行點云匹配改善位姿；最后得到深度相機的位姿。其中基于特征的方法是目前視覺里程計中流行的方式。若使用RGB-D 相機，利用已知的深度特征點，可直接估計移動機器人的運動。給定一組特征點及配準關(guān)系求解機器人的位姿，稱為PnP（Perspective-n-Point）問題。用非線性優(yōu)化求解PnP，得到幀間位置關(guān)系。最常用的是點特征，例如Harris 角點、SIFT、SURF 和ORB 等。

b.后端

在前端完成兩個幀之間圖像特征匹配后，理論上是可以得到移動機器人的軌跡的，但在前端會產(chǎn)生誤差，且隨著機器人的行進這些誤差將會一直堆積，產(chǎn)生累計誤差。例如，若在兩個時刻間，估計的轉(zhuǎn)角比真實轉(zhuǎn)角少1 度，那么之后的軌跡將全部減少1 度，如此反復(fù)，重建的地圖將會失真，估計的軌跡也會有嚴重的漂移。故在SLAM 中，還會把幀間相對運動放到后端的程序中進行優(yōu)化。

c.回環(huán)檢測

回環(huán)檢測（Loop closure detection）是指機器人識別曾到達場景的能力，實質(zhì)上是一種檢測觀測數(shù)據(jù)相似性的算法。回環(huán)檢測主要用來解決移動機器人位置估計隨時間漂移的問題，為了降低視覺里程計過程中的積累誤差，利用各關(guān)鍵幀的相互關(guān)聯(lián)性進行有效的閉合回環(huán)，在本質(zhì)視圖上優(yōu)化位姿。這樣就可以將累計的誤差分散到位姿圖中，并通過相似變換矯正尺度偏移。如果回環(huán)檢測成功，則認為機器人曾到達過這個地方，把比對信息輸送給后端的優(yōu)化算法，后端再根據(jù)回環(huán)檢測的信息調(diào)整移動機器人軌跡和地圖。通過回環(huán)檢測，可有效抑制累積誤差造成的位姿漂移[8]。

3 特征提取及匹配

在此采用ORB 算法進行特征提取和匹配。首先利用Oriented FAST（一種改進的FAST 角點）檢測子在相鄰圖像上檢測特征點，再通過Rotated BRIEF（Binary Robust Independent Elementary Features)描述符生產(chǎn)特征點的二進制描述向量，最后相鄰圖像的特征點的匹配采用漢明距離比值準則得到最終的特征點匹配對。此ORB 算法改進了FAST 檢測子的方向性問題，并采用速度極快的二進制描述子BRIEF，大大提高整個圖像特征提取實時性。

用此法提取ORB 特征主要包括以下兩個步驟：

第一步，提取Oriented FAST 角點。首先，找出圖像中的“角點”；然后，計算特征點的主方向，以便增加BRIEF 描述子的旋轉(zhuǎn)不變性。

第二步，產(chǎn)生BRIEF 描述子。采用二進制，描述所提取特征點的周圍圖像區(qū)域。

3.1 Oriented FAST 關(guān)鍵點

Oriented FAST 關(guān)鍵點是一種角點，主要對局部像素灰度變化明顯的地方進行快速檢測。如果一個像素與其鄰域的像素間差別較大（過亮或過暗）, 那它極有可能是角點。相比于其他角點檢測算法，Oriented FAST 關(guān)鍵點只需比較像素灰度值的大小，使檢測子具有尺度方向性的同時又提升了實時性。其檢測過程如圖2 所示，可歸納為：

1)在圖像中任選像素p，它的亮度為Ip;

2)設(shè)定一個閾值T（示例中T=0.7 );

3)以像素p 為中心，選取半徑為r 的圓上的n個像素點（示例中r=3, n=16）;

4)若該圓上有m 個連續(xù)的點亮度在Ip±0.7 范圍內(nèi)，則像素p 被認為是特征點（示例中m=9);

5)循環(huán)以上四步，對每個像素執(zhí)行相同操作。

圖2 Oriented FAST 特征點的檢測

因為之前的FAST 角點不具有方向信息，取半徑為3 的圓會存在尺度問題：從遠處看是角點的地方，近距離看可能就不是角點。針對此問題，首先構(gòu)建圖像金字塔，檢測每層金字塔的角點，然后采用灰度質(zhì)心法（Intensity Centroid），具體步驟如下：

1.在一個小的圖像塊B 中，定義圖像塊的矩：

2.通過矩找到圖像塊的質(zhì)心：

3.由圖像塊的幾何中心O 指向質(zhì)心C，得到方向向量OC，則所求特征點的方向如公式(3)所示：

綜上，改進后的FAST 角點在具有了方向性的同時，也顯著提升了在不同圖像間描述的魯棒性。

3.2 BRIEF 二進制描述子

在完成Oriented FAST 關(guān)鍵點提取后，得到特征點的主方向，就可以據(jù)此計算對每個點的描述子。本算法對BRIEF 特征描述做了改進。

此處，BRIEF 采用二進制，描述向量由許多個0和1 組成。這里的0 和1 表示的是兩個附近關(guān)鍵點的像素（假如兩個像素為x, y）的大小關(guān)系：若x 大于y，則BRIEF 描述子等于1，反之就等于0。如果取了n 對這樣的x 和y，最后就得到n 維由0 和1 組成的向量。BRIEF 使用隨機選點來進行比較，速度非?？?；而由于采用了二進制來表示，儲存方便所占空間小，適用于實時的圖像匹配。原始的BRIEF 描述子不具有旋轉(zhuǎn)不變性，因此在圖像發(fā)生旋轉(zhuǎn)時容易丟失。而改進后的Oriented FAST 在特征點提取階段計算了關(guān)鍵點的方向，所以可以利用這一方向信息，計算旋轉(zhuǎn)之后的“Steer BRIEF”特征，使ORB 的描述子具有較好的旋轉(zhuǎn)不變性。

4 回環(huán)檢測

回環(huán)檢測用來判斷當(dāng)前位置是否曾是已訪問過的環(huán)境區(qū)域。改進后方法的視覺回環(huán)檢測是通過比較兩幅圖像數(shù)據(jù)的相似性來實現(xiàn)的。如果回環(huán)檢測成功，則認為機器人曾經(jīng)來過這個地方，將比對信息輸送給后端算法進行優(yōu)化。后端再根據(jù)回環(huán)檢測的信息調(diào)整機器人軌跡和地圖[9]。

在此采用貝葉斯濾波更新進行機器人位姿回環(huán)檢測。隨機變量St表示t 時刻的回環(huán)假設(shè)狀態(tài)，即回環(huán)假設(shè)的可能分布，通過不斷更新粒子的位置和權(quán)重來逼近系統(tǒng)的真實后驗概率分布[10]。整個濾波更新過程分為預(yù)測/更新兩步，遞歸執(zhí)行，如下式：

預(yù)測：

更新：

在圖像Ii與It結(jié)束匹配循環(huán)時，令St=i，則Ii與It表示相同的位置。當(dāng)t 時刻沒有發(fā)生閉環(huán)時，令St=-1，則It表示新圖像。粒子濾波使用tN+2 個相關(guān)權(quán)重的樣本集合計算相應(yīng)時刻t 的后驗概率密度,如下式：

其中tn是機器人的工作記憶（WM）中與新場景圖像相關(guān)的時間指數(shù)；It表示t 時刻獲取的圖像序列，僅包含WM 中的圖像，It=I-1,...It。用似然函數(shù)L(St/It)評估觀測模型p(It/St)，比較當(dāng)前圖像It和每個閉環(huán)狀態(tài)中St=j（j=-1,...,tn）對應(yīng)圖像的相似度，得出一個分數(shù)Sj=R(zt,zj)。對每個得分進行標準差σ 和均值μ 的歸一化處理，計算t 時刻的似然值，即：

對每個新圖像的狀態(tài)St=-1 作似然估計如下：

若似然函數(shù)L(St=-1/It)的值很高，則說明It與WM 中的一個圖像不相似，且當(dāng)σ＜μ 時，則It極可能是個新場景圖像。預(yù)測St的分布用時間演變模型p(St/St-1=i)來表示，其中St-1的狀態(tài)均為已知，與機器人在t 時刻和t-1 時刻的狀態(tài)相關(guān)。

此處的時間演變模型有以下四種取值情況：

此概率函數(shù)為一個以j 為中心的離散高斯曲線，鄰域正好有八個值是非空的，且高斯系數(shù)之和是0.9。

通過比較圖像特征的匹配率來衡量圖像間的相似度。定義匹配率ROM（Ratio of Match）如下：

其誤差函數(shù)為：

其中，T 表示深度相機位姿的描述集合；Tij表示不同時刻相機的相對位姿；e(Tij,Tij,Tij)表示不同時刻位姿觀測值與預(yù)測值之間的誤差，即閉環(huán)誤差；ψij表示不同時刻相機位姿間觀測相對性矩陣。當(dāng)e(Tij,Tij,Tij)=0 時，表示從i 到j(luò) 時刻相機位姿估計滿足閉環(huán)調(diào)節(jié)，該時刻相機的位姿漂移為零。

5 實驗結(jié)果及分析

5.1 硬件平臺搭建

實驗采用的移動機器人平臺為全向輪移動機器人，如圖3 所示。機器人頂端采用深度相機Kinect，其彩色圖像分辨率為640×480，深度圖像分辨率為320×240，深度感應(yīng)有效距離為1.2～3.5m，最高幀率為30 幀每秒。全向輪小車上搭載一臺筆記本電腦，它配備主頻為2.5GHz 的Intel Core i5 處理器，配置Ubuntu 16.04 LTS 操作系統(tǒng)，運行ROS 操作系統(tǒng)，作為數(shù)據(jù)發(fā)送端，通過ROS 系統(tǒng)采集、傳輸圖像到遠程數(shù)據(jù)處理端PC。

圖3 移動機器人實驗裝置

5.2 真實室內(nèi)場景下三維點云地圖構(gòu)建

5.2.1 實驗場景選取

實驗選取6m×6m 的辦公室封閉場景，如圖4 所示。場景中物品較多，環(huán)境較復(fù)雜，以此可檢驗復(fù)雜場景下特征點提取的可信度，進而驗證本算法的可靠性。

圖4 實驗場景

5.2.2 特征提取

對Kinect 相機采集的圖像進行ORB 特征提取，驗證算法的效果。提取效果如圖5，可見，算法準確提取了那些高信任度的角點，且運用灰度質(zhì)心法估計了每個角點的方向。用BRIEF 描述子進行簡單的二進制測試，產(chǎn)生一個位串來作為特征描述，從而有效提高了RGB-D SLAM 的實時性。

圖5 ORB 特征提取

5.2.3 三維點云地圖構(gòu)建

為驗證所采用的回環(huán)檢測算法的有效性，要利用離線數(shù)據(jù)。選用的離線數(shù)據(jù)為Kinect 相機采集到的室內(nèi)非結(jié)構(gòu)化環(huán)境信息。以此來獲取該實驗環(huán)境下的全局三維點云地圖，并對圓環(huán)檢測算法添加前后的效果進行對比。

未經(jīng)圓環(huán)檢測的三維點云地圖構(gòu)建如圖6 所示。圖6(a)為未經(jīng)回環(huán)檢測的軌跡，其中以稀疏點云來顯示路徑，以稠密點云來構(gòu)建地圖，圖中軌跡較為狹小，地圖重建不完善；圖6(b)為未經(jīng)回環(huán)檢測的三維地圖構(gòu)建效果，可以看到圖中產(chǎn)生嚴重的位姿漂移，導(dǎo)致三維地圖無法閉合創(chuàng)建。

圖7 則為引入回環(huán)檢測算法的三維點云地圖構(gòu)建。圖7(a)為引入回環(huán)檢測算法的軌跡，圖中軌跡形成回路，地圖重建完善；圖7(b)為引入回環(huán)檢測算法的三維地圖構(gòu)建效果，可以看到重建后的三維地圖輪廓清晰、完整、光滑，與真實的實驗環(huán)境基本一致，三維重建效果良好。

圖6 未經(jīng)圓環(huán)檢測的三維點云地圖構(gòu)建

圖7 引入回環(huán)檢測算法的三維點云地圖構(gòu)建

6 結(jié)束語

針對室內(nèi)環(huán)境提出了一種改進的移動機器人RGB-D SLAM 方法。該方法提取ORB 特征進行特征匹配，引入回環(huán)檢測算法，顯著減小了累積誤差造成的位姿漂移，最終在實驗場景中得到了全局一致的軌跡及三維地圖。實驗結(jié)果表明，運用該方法能夠較好地抑制配準過程中的位姿漂移，創(chuàng)建出更為精確的室內(nèi)環(huán)境三維地圖。然而由于Kinect 相機拍攝范圍有限，也可能會導(dǎo)致提取特征點不足從而影響位姿的估計，且本方法采用的全向輪小車的輪隙較大，在上端的深度相機會產(chǎn)生顛簸，影響畫面感，導(dǎo)致特征點匹配不準，影響了位姿的估計，此類問題還需在后續(xù)的研究中通過進一步改進方法和設(shè)備來加以解決。