基于Kinect的改進(jìn)移動(dòng)機(jī)器人視覺(jué)SLAM

蔡軍，陳科宇，張毅

近年來(lái)，隨著SLAM研究的不斷深入，使三維重建成為了機(jī)器人研究領(lǐng)域的熱潮[1]。從本質(zhì)上來(lái)說(shuō)，SLAM所有的計(jì)算都是對(duì)傳感器數(shù)據(jù)的處理，因此不同傳感器的基本方程的參數(shù)化有很大的差別。常見(jiàn)的SLAM傳感器有IMU、激光傳感器。IMU通常包含陀螺儀和加速度計(jì)。陀螺儀具有良好的動(dòng)態(tài)響應(yīng)，但會(huì)產(chǎn)生累計(jì)誤差；加速度計(jì)雖然不會(huì)產(chǎn)生累計(jì)誤差，但易受到機(jī)器運(yùn)行時(shí)的振動(dòng)干擾；激光傳感器精度高、探測(cè)范圍廣，但價(jià)格昂貴、功耗大；相機(jī)主要分為單目相機(jī)、雙目相機(jī)和RGB-D[2]相機(jī)，單目相機(jī)無(wú)法直接獲取圖像深度信息，需要通過(guò)移動(dòng)相機(jī)來(lái)產(chǎn)生深度；雙目相機(jī)通過(guò)視差計(jì)算深度，在推算圖像中物體距離時(shí)計(jì)算量大，實(shí)時(shí)性較差；典型的RGBD相機(jī)，如Kinect，具有價(jià)格低廉、信息豐富、更新速度快、能直接獲取圖像深度等優(yōu)點(diǎn)，因此其成為了RGB-D SLAM系統(tǒng)的主流視覺(jué)傳感器，受到廣大視覺(jué)SLAM研究者的青睞。因此，國(guó)內(nèi)外學(xué)者對(duì)Kinect在室內(nèi)移動(dòng)機(jī)器人上的應(yīng)用研究取得了一定的研究成果。

一個(gè)完整的RGB-D SLAM系統(tǒng)應(yīng)該包含傳感器數(shù)據(jù)、視覺(jué)里程計(jì)、非線性優(yōu)化、建圖與回環(huán)檢測(cè)等主要5個(gè)部分。視覺(jué)里程計(jì)的作用是估計(jì)相機(jī)運(yùn)動(dòng)，其中包括特征點(diǎn)檢測(cè)與提取、相鄰幀間配準(zhǔn)、迭代最近點(diǎn)等。非線性優(yōu)化是從帶有噪聲的傳感器數(shù)據(jù)中優(yōu)化機(jī)器人運(yùn)行軌跡和在運(yùn)行過(guò)程中增量式構(gòu)建環(huán)境地圖。回環(huán)檢測(cè)[3]是機(jī)器人通過(guò)識(shí)別圖像間的相似性來(lái)判斷是否回到之前到過(guò)的位置。在視覺(jué)里程計(jì)部分，通常采用ICP算法去估計(jì)相機(jī)的運(yùn)動(dòng)。然而傳統(tǒng)的ICP算法[4]搜尋對(duì)應(yīng)點(diǎn)對(duì)相當(dāng)耗時(shí)，且容易出現(xiàn)大量誤匹配點(diǎn)對(duì)。針對(duì)以上問(wèn)題，許多學(xué)者提出了優(yōu)秀的改進(jìn)ICP算法。文獻(xiàn)[5]針對(duì)多場(chǎng)景三維重建問(wèn)題，在基于2D局部特征匹配的基礎(chǔ)上，提出了一種新穎且高效的3D配準(zhǔn)方法。該方法將三維點(diǎn)云轉(zhuǎn)換為2D軸承角圖像，然后在兩幅圖像之間利用SURF特征搜尋匹配的像素對(duì)，獲取3D點(diǎn)云的對(duì)應(yīng)點(diǎn)，再通過(guò)基于SVD的方法求出最優(yōu)旋轉(zhuǎn)矩陣，將點(diǎn)云與矩陣對(duì)齊來(lái)重建3D模型。該方法雖大大減少了算法運(yùn)算時(shí)間，但在精度上卻不如傳統(tǒng)ICP算法。文獻(xiàn)[6]針對(duì)服務(wù)機(jī)器人手眼協(xié)調(diào)抓取提出了一種利用RGB-D傳感器對(duì)家庭常見(jiàn)物體進(jìn)行3-D建模的方法。該方法僅需環(huán)繞物體采取數(shù)十幀即可重建三維模型，但無(wú)法估計(jì)相機(jī)位姿。

針對(duì)以上研究現(xiàn)狀，本文提出了一種新的室內(nèi)移動(dòng)機(jī)器人視覺(jué)SLAM方法。通過(guò)Kinect相機(jī)獲取室內(nèi)環(huán)境信息，對(duì)獲取的彩色圖像進(jìn)行特征提取與匹配，結(jié)合標(biāo)定后的Kinect內(nèi)參與像素點(diǎn)深度值，得到對(duì)應(yīng)的空間對(duì)應(yīng)點(diǎn)云，運(yùn)用RANSAC方法篩選出可靠的內(nèi)點(diǎn)進(jìn)行初始位姿估計(jì)完成初匹配。在精匹配過(guò)程中，初匹配的結(jié)果作為精匹配的初值，通過(guò)添加點(diǎn)對(duì)間距離和角度閾值雙重限制，剔除誤匹配點(diǎn)對(duì)，提高點(diǎn)云配準(zhǔn)的精度。在優(yōu)化端的關(guān)鍵幀選取中引入權(quán)重，通過(guò)回環(huán)檢測(cè)消除累積誤差，實(shí)現(xiàn)三維地圖的優(yōu)化與更新。

1 基于Kinect的改進(jìn)移動(dòng)機(jī)器人VSLAM算法基本原理

1.1 系統(tǒng)結(jié)構(gòu)

本文提出的室內(nèi)移動(dòng)機(jī)器人視覺(jué)SLAM系統(tǒng)框架如圖1所示，在進(jìn)行傳感器數(shù)據(jù)采集之前，需對(duì)Kinect傳感器進(jìn)行標(biāo)定以獲取相機(jī)內(nèi)部參數(shù)，對(duì)獲取到的彩色圖像進(jìn)行特征檢測(cè)與提取，進(jìn)行初步篩選后，利用RANSAC算法剔除離群點(diǎn)獲取初始位姿估計(jì)，再用改進(jìn)的ICP算法完成點(diǎn)云的精準(zhǔn)匹配，在關(guān)鍵幀選取中引入權(quán)重，通過(guò)回環(huán)檢測(cè)消除累積誤差，實(shí)現(xiàn)三維點(diǎn)云地圖的優(yōu)化與更新，最終獲得機(jī)器人運(yùn)行軌跡和室內(nèi)環(huán)境點(diǎn)云地圖。

圖1 室內(nèi)環(huán)境下基于移動(dòng)機(jī)器人的RGB-D SLAM系統(tǒng)框架圖Fig. 1 The frame of RGB-D SLAM system for mobile robot in indoor environment

1.2 特征檢測(cè)與匹配

SLAM包含兩個(gè)問(wèn)題，一個(gè)是“定位”，一個(gè)是“建圖”。解決定位問(wèn)題的關(guān)鍵是在相機(jī)獲取了機(jī)器人所在的室內(nèi)環(huán)境信息后，如何根據(jù)相鄰圖像的信息，粗略估計(jì)出相機(jī)運(yùn)動(dòng)，這一過(guò)程也被稱為視覺(jué)里程計(jì)。視覺(jué)里程計(jì)的實(shí)現(xiàn)方法主要分為特征法和直接法。特征法運(yùn)行穩(wěn)定，對(duì)光照、動(dòng)態(tài)物體不敏感，是目前比較成熟的解決方案，因此本文采用特征法。

隨著在計(jì)算機(jī)視覺(jué)領(lǐng)域的長(zhǎng)年探索，研究者們提出了許多優(yōu)秀的圖像特征檢測(cè)方法。如著名的 FAST[7]、SIFT[8]以及 SURF[9]。FAST 特征點(diǎn)檢測(cè)計(jì)算速度快，但FAST不具有方向性。SURF算法采用盒子濾波器，積分圖像的運(yùn)用減少了采用盒子濾波進(jìn)行卷積的時(shí)間，盒子濾波可以加速卷積運(yùn)算過(guò)程，在尺度空間建立上比SIFT更迅速。由于圖像特征檢測(cè)與匹配僅僅是整個(gè)SLAM系統(tǒng)諸多環(huán)節(jié)中的一個(gè)，為了滿足實(shí)時(shí)性，采用具有旋轉(zhuǎn)和尺度不變性的SURF特征。在提取了圖像特征以后，需確定兩個(gè)點(diǎn)集元素的對(duì)應(yīng)關(guān)系。最簡(jiǎn)單的方法是暴力匹配(brute-force matcher，BFM)，然而當(dāng)特征點(diǎn)數(shù)量很大時(shí)，暴力匹配法的運(yùn)算量將變得很大，運(yùn)行過(guò)程非常耗時(shí)，因此選用更適合于匹配點(diǎn)數(shù)量極多情況的快速近似最近鄰[10](fast library for approximate nearest neighbour，F(xiàn)LANN)算法。

利用SURF算法進(jìn)行特征點(diǎn)檢測(cè)與匹配的主要過(guò)程如下：

1) 對(duì)圖像關(guān)鍵點(diǎn)進(jìn)行檢測(cè)；

2) 使用“金字塔”構(gòu)建尺度空間；

3) 關(guān)鍵點(diǎn)定位；

4) 確定每個(gè)關(guān)鍵點(diǎn)的主方向；

5) 生成特征描述子；

6) 運(yùn)用FLANN對(duì)特征描述子進(jìn)行匹配。

1.3 Kinect標(biāo)定與點(diǎn)云生成

彩色圖像和深度圖像需結(jié)合Kinect內(nèi)參生成三維點(diǎn)云。Kinect相機(jī)在出廠之后具有固定的內(nèi)參，但這些參數(shù)會(huì)隨著溫度、濕度、氣壓等環(huán)境因素的變化而改變，在使用前有必要進(jìn)行相機(jī)標(biāo)定[11]。本文采用文獻(xiàn)[12]的方法對(duì)Kinect進(jìn)行標(biāo)定。在相機(jī)完成標(biāo)定之后，圖像中任意一點(diǎn)可結(jié)合該點(diǎn)深度值得到其對(duì)應(yīng)的三維空間點(diǎn)坐標(biāo)，進(jìn)而生成三維點(diǎn)云數(shù)據(jù)。二維圖像中的像素點(diǎn)(其中d指深度數(shù)據(jù))與其空間點(diǎn)的對(duì)應(yīng)關(guān)系為

1.4 點(diǎn)云配準(zhǔn)與位姿估計(jì)

1.4.1 RANSAC算法完成粗匹配

ICP算法依賴良好的初值選取，如果初值選取不合適，迭代易陷入局部最優(yōu)，以至不能達(dá)到全局最優(yōu)效果。RANSAC是一種隨機(jī)參數(shù)估計(jì)算法，通過(guò)隨機(jī)抽選樣本子集、計(jì)算模型參數(shù)、設(shè)定閾值等步驟獲取最佳模型參數(shù)[13]。本文采用RANSAC算法對(duì)兩組相鄰RGB圖的特征點(diǎn)進(jìn)行粗匹配，通過(guò)預(yù)先設(shè)定一個(gè)閾值d將全部匹配點(diǎn)對(duì)區(qū)分為內(nèi)點(diǎn)和外點(diǎn)，剔除大于此閾值的匹配點(diǎn)對(duì)即剔除了外點(diǎn)對(duì)粗匹配的影響。將篩選后的內(nèi)點(diǎn)進(jìn)行最小二乘法下對(duì)Kinect相機(jī)的初始位姿估計(jì)，可將源點(diǎn)集大致配準(zhǔn)到目標(biāo)點(diǎn)集，粗匹配后的兩片點(diǎn)云分別記為和。相機(jī)在第i時(shí)刻的位姿與第時(shí)刻的位姿的位姿轉(zhuǎn)換關(guān)系為

1.4.2 傳統(tǒng)ICP算法

傳統(tǒng)ICP算法通過(guò)解決最小二乘均方差問(wèn)題，求解包含旋轉(zhuǎn)矩陣R和平移矩陣t的剛體變換T，從而實(shí)現(xiàn)相機(jī)相鄰位姿的運(yùn)動(dòng)估計(jì)。傳統(tǒng)ICP算法步驟如下。

2) 通過(guò)最小化均方目標(biāo)函數(shù)來(lái)求解剛體變換T。

式中：R為旋轉(zhuǎn)矩陣，t為平移矩陣。

3) 運(yùn)用剛體變換轉(zhuǎn)換點(diǎn)云。

式中m表示迭代次數(shù)。

1.4.3 改進(jìn)的ICP算法完成精匹配

通過(guò)RANSAC算法進(jìn)行特征點(diǎn)的粗匹配可粗略地重合相鄰兩幀點(diǎn)云。傳統(tǒng)的ICP算法是基于兩組點(diǎn)云完全重合這一假設(shè)理論，然而在實(shí)際應(yīng)用中點(diǎn)云P中的一點(diǎn)并不能總在點(diǎn)云Q中找到其對(duì)應(yīng)點(diǎn)，傳統(tǒng)ICP算法存在大量錯(cuò)誤匹配點(diǎn)對(duì)，算法易陷入局部最優(yōu)，甚至不能收斂。為了減少配準(zhǔn)誤差，提高點(diǎn)云配準(zhǔn)精度，需要利用改進(jìn)的ICP算法對(duì)點(diǎn)云進(jìn)行精確配準(zhǔn)。在精匹配過(guò)程中，粗匹配的結(jié)果作為精匹配的初值，采用點(diǎn)對(duì)間歐氏距離閾值法和角度閾值法對(duì)誤匹配點(diǎn)對(duì)進(jìn)行剔除，以篩選出滿足閾值條件的點(diǎn)對(duì)，進(jìn)行精確相機(jī)位姿估計(jì)。所提閾值法原理如圖2所示。

圖2 歐氏距離閾值法及角度閾值法原理示意圖Fig. 2 Euclidean distance threshold method and angle threshold method

1) 歐氏距離閾值

正確的匹配點(diǎn)對(duì)，兩點(diǎn)對(duì)之間的歐氏距離不應(yīng)過(guò)大。本文添加了點(diǎn)對(duì)間歐氏距離閾值限制，去除小于平均點(diǎn)對(duì)距離的匹配，且認(rèn)為同一片點(diǎn)云下任一點(diǎn)與其鄰點(diǎn)的拓?fù)浣Y(jié)構(gòu)不會(huì)隨剛體變換而變化。因此在粗匹配后，配對(duì)點(diǎn)云中的任一點(diǎn)與其鄰點(diǎn)，在另一個(gè)配對(duì)點(diǎn)云中的也應(yīng)是鄰近點(diǎn)，給出了式(11)和式(12)兩個(gè)距離閾值限制：

2) 角度閾值

經(jīng)過(guò)歐氏距離閾值法可剔除大部分點(diǎn)云數(shù)據(jù)噪聲點(diǎn)，再結(jié)合角度閾值，能進(jìn)一步檢測(cè)點(diǎn)對(duì)匹配的正確性，提高點(diǎn)云初值選取的正確率。本文采用了點(diǎn)到切平面的最近搜索算法，對(duì)于給定點(diǎn)及其對(duì)應(yīng)點(diǎn)，通過(guò)其鄰近點(diǎn)集擬合平面，并求出各自的近似法向量。提出的角度閾值限制為

1.4.4 改進(jìn)后的ICP算法步驟

3) 應(yīng)用角度閾值法進(jìn)一步剔除誤匹配點(diǎn)對(duì)，得到精配對(duì)點(diǎn)集和。

4) 采用奇異值分解(singular value decomposition，SVD)求得點(diǎn)集和之間的旋轉(zhuǎn)矩陣R與平移矩陣t。

6) 重復(fù) 3)～5)，直到滿足式 (15)：

1.5 三維地圖的更新與優(yōu)化

1.5.1 關(guān)鍵幀選取

Kinect相機(jī)以一定的幀率采集室內(nèi)環(huán)境信息，即使相機(jī)固定不動(dòng)，程序的內(nèi)存占用也會(huì)越來(lái)越高。在RGB-D SLAM系統(tǒng)中，過(guò)多的關(guān)鍵幀會(huì)增加閉環(huán)檢測(cè)與全局優(yōu)化的計(jì)算耗時(shí)，過(guò)少的關(guān)鍵幀會(huì)導(dǎo)致關(guān)鍵幀間間隙增大，造成幀間配準(zhǔn)易出現(xiàn)失敗的情況。為了滿足幀間配準(zhǔn)的成功率和系統(tǒng)實(shí)時(shí)性要求，引入一種關(guān)鍵幀選取機(jī)制。在關(guān)鍵幀選取上，旋轉(zhuǎn)的變化比平移更加敏感，因此在旋轉(zhuǎn)出現(xiàn)微小變化或平移相對(duì)大一段距離后添加關(guān)鍵幀。當(dāng)新幀到來(lái)時(shí)，檢測(cè)其圖像特征并與前一關(guān)鍵幀進(jìn)行變換計(jì)算，如果變換超過(guò)閾值，則添加關(guān)鍵幀，反之，則丟棄。具體選取標(biāo)準(zhǔn)如式(16)所示：

1.5.2 位姿優(yōu)化

在進(jìn)行機(jī)器人同時(shí)定位與地圖構(gòu)建過(guò)程中，兩幀之間的位姿變換可通過(guò)式(5)計(jì)算得到，各時(shí)刻相對(duì)于起始時(shí)刻的變換矩陣可通過(guò)式(17)獲得：

在各個(gè)位姿轉(zhuǎn)換過(guò)程中均存在誤差，如果不進(jìn)行優(yōu)化，隨著轉(zhuǎn)換過(guò)程的疊加，會(huì)產(chǎn)生累計(jì)誤差。本文采用g2o優(yōu)化算法[14]對(duì)位姿轉(zhuǎn)換過(guò)程中產(chǎn)生的累計(jì)誤差進(jìn)行優(yōu)化，誤差函數(shù)定義為

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

本文實(shí)驗(yàn)平臺(tái)由3個(gè)主要部分組成：第一部分為一臺(tái)Intel雙核4.0 GHz主頻的筆記本電腦，裝配以Linux為內(nèi)核的Ubuntu 14.04操作系統(tǒng)；第二部分為Kinect XBOX360，圖像分辨率為640×480，最高幀率為30幀/秒，水平視場(chǎng)角為52°；第三部分為Pioneer3-DX機(jī)器人。實(shí)驗(yàn)環(huán)境為一間約80 m2的小型實(shí)驗(yàn)室，圖3(a)為實(shí)驗(yàn)真實(shí)場(chǎng)景，圖3(b)為實(shí)驗(yàn)設(shè)備。

圖3 實(shí)驗(yàn)場(chǎng)景及實(shí)驗(yàn)設(shè)備Fig. 3 Experimental scene and experimental installation

在實(shí)驗(yàn)過(guò)程中，Pioneer3-DX機(jī)器人搭載筆記本電腦與Kinect相機(jī)，并以0.1 m/s的速度在實(shí)驗(yàn)室內(nèi)移動(dòng)，Kinect相機(jī)通過(guò)獲取室內(nèi)環(huán)境的彩色信息與深度信息，利用關(guān)鍵幀的選取機(jī)制，對(duì)新來(lái)的關(guān)鍵幀進(jìn)行圖像特征檢測(cè)與提取，結(jié)合相機(jī)標(biāo)定內(nèi)參與像素點(diǎn)深度值，生成三維點(diǎn)云數(shù)據(jù)。采用改進(jìn)的基于RANSAC的ICP點(diǎn)云配準(zhǔn)算法將兩片相鄰點(diǎn)云進(jìn)行配準(zhǔn)，獲取相機(jī)位姿估計(jì)。實(shí)驗(yàn)過(guò)程中各參數(shù)設(shè)置為：，，，，，cm，，，。不同真實(shí)場(chǎng)景采集圖像不同，提取的特征點(diǎn)數(shù)量和點(diǎn)對(duì)間距離也不同，因此為不定值。點(diǎn)云分辨率設(shè)為0.02，最大迭代次數(shù)為100次，內(nèi)點(diǎn)數(shù)量閾值為，對(duì)每個(gè)關(guān)鍵幀完成特征點(diǎn)匹配、誤匹配剔除以及地圖更新的時(shí)間約為。

在實(shí)驗(yàn)場(chǎng)景中，圖4(a)為相機(jī)獲取到的第一幀數(shù)據(jù)(包含彩色信息和深度信息)，圖4(b)為相機(jī)獲取到的第二幀數(shù)據(jù)，兩幀數(shù)據(jù)的視角只在水平方向發(fā)生了較小的平移。圖5為利用標(biāo)定后的Kinect相機(jī)獲取到的第一幀彩色信息，結(jié)合像素點(diǎn)深度值生成的彩色點(diǎn)云數(shù)據(jù)。圖6(a)為未經(jīng)RANSAC算法的匹配點(diǎn)對(duì)，兩幀數(shù)據(jù)只在水平方向上發(fā)生了移動(dòng)，因此只有水平的匹配線才是正確的匹配。圖6(b)為經(jīng)RANSAC算法后的匹配點(diǎn)對(duì)，可以看出，剔除誤匹配后，基本消除了噪聲對(duì)匹配結(jié)果的影響，提高了點(diǎn)對(duì)匹配正確率。

圖4 室內(nèi)環(huán)境的兩幀數(shù)據(jù)Fig. 4 Two frames in indoor environment

圖5 第一幀數(shù)據(jù)的三維彩色點(diǎn)云圖Fig. 5 3-D color point cloud of the first frame data

圖6 粗匹配實(shí)驗(yàn)結(jié)果Fig. 6 The experimental results of coarse matching

圖7(a)為采用文獻(xiàn)[6]的方法進(jìn)行三維重建的效果圖，所重構(gòu)出的點(diǎn)云圖中的貨物架、AGV小車以及墻板等物體存在輪廓不清晰的情況，左邊窗簾冗余點(diǎn)較多。圖7(b)為采用本文改進(jìn)后的ICP算法進(jìn)行三維重建的效果圖。相比之下，利用本文算法得到的點(diǎn)云圖精度明顯提高，貨物架、AGV小車以及墻板等物體輪廓變得清晰，窗簾上的冗余點(diǎn)明顯得到減少。圖8為采用g2o算法對(duì)地圖進(jìn)行優(yōu)化與更新，深藍(lán)部分為關(guān)鍵幀，其頂點(diǎn)表示相機(jī)的各個(gè)姿態(tài)，連接兩頂點(diǎn)的邊即表示兩點(diǎn)之間的位姿變換。局部回環(huán)表示當(dāng)前幀與就近幀間的匹配，大回環(huán)表示當(dāng)前幀與關(guān)鍵幀序列中隨機(jī)幀的匹配。本文中設(shè)置，n =5。圖9計(jì)算機(jī)記錄機(jī)器人移動(dòng)的位姿數(shù)據(jù)后，利用MATLAB仿真生成的機(jī)器人運(yùn)行軌跡估計(jì)。

圖7 室內(nèi)環(huán)境三維重建結(jié)果Fig. 7 The experimental results of 3D reconstruction in indoor environment

圖8 g2o優(yōu)化圖Fig. 8 g2o optimization diagram

圖9 機(jī)器人運(yùn)行軌跡估計(jì)Fig. 9 Robot trajectory estimation

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

本文提出了一種新的基于Kinect的室內(nèi)移動(dòng)機(jī)器人同時(shí)定位與地圖構(gòu)建系統(tǒng)。針對(duì)相機(jī)位姿估計(jì)過(guò)程中，點(diǎn)云配準(zhǔn)精度不高，易陷入局部最優(yōu)等問(wèn)題，提出了一種新的歐氏距離與角度閾值的雙重限制法，提高了點(diǎn)云配準(zhǔn)精度，減少了配準(zhǔn)誤差。結(jié)合關(guān)鍵幀選取機(jī)制和g2o優(yōu)化算法，實(shí)現(xiàn)了三維地圖的優(yōu)化與更新。在真實(shí)室內(nèi)環(huán)境下進(jìn)行在線實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明了本文算法的有效性與可行性。進(jìn)一步改進(jìn)工作將集中在：1)進(jìn)一步研究歐氏距離與角度閾值的選取標(biāo)準(zhǔn)；2)提出移動(dòng)機(jī)器人在同一環(huán)境下持續(xù)運(yùn)動(dòng)所產(chǎn)生的關(guān)鍵幀冗余的情況的解決方案。