富 裕，劉振宇

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

1 引 言

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

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

2 視覺(jué)SLAM 框架

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

圖1 提出的RGB-D SLAM 框架

a.前端

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

b.后端

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

c.回環(huán)檢測(cè)

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

3 特征提取及匹配

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

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

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

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

3.1 Oriented FAST 關(guān)鍵點(diǎn)

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

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

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

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

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

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

圖2 Oriented FAST 特征點(diǎn)的檢測(cè)

因?yàn)橹暗腇AST 角點(diǎn)不具有方向信息，取半徑為3 的圓會(huì)存在尺度問(wèn)題：從遠(yuǎn)處看是角點(diǎn)的地方，近距離看可能就不是角點(diǎn)。針對(duì)此問(wèn)題，首先構(gòu)建圖像金字塔，檢測(cè)每層金字塔的角點(diǎn)，然后采用灰度質(zhì)心法（Intensity Centroid），具體步驟如下：

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

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

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

綜上，改進(jìn)后的FAST 角點(diǎn)在具有了方向性的同時(shí)，也顯著提升了在不同圖像間描述的魯棒性。

3.2 BRIEF 二進(jìn)制描述子

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

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

4 回環(huán)檢測(cè)

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

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

預(yù)測(cè)：

更新：

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

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

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

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

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

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

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

其誤差函數(shù)為：

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

5 實(shí)驗(yàn)結(jié)果及分析

5.1 硬件平臺(tái)搭建

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

圖3 移動(dòng)機(jī)器人實(shí)驗(yàn)裝置

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

5.2.1 實(shí)驗(yàn)場(chǎng)景選取

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

圖4 實(shí)驗(yàn)場(chǎng)景

5.2.2 特征提取

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

圖5 ORB 特征提取

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

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

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

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

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

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

6 結(jié)束語(yǔ)

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