基于Kinect的改進移動機器人視覺SLAM

蔡軍，陳科宇，張毅

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

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

針對以上研究現狀，本文提出了一種新的室內移動機器人視覺SLAM方法。通過Kinect相機獲取室內環境信息，對獲取的彩色圖像進行特征提取與匹配，結合標定后的Kinect內參與像素點深度值，得到對應的空間對應點云，運用RANSAC方法篩選出可靠的內點進行初始位姿估計完成初匹配。在精匹配過程中，初匹配的結果作為精匹配的初值，通過添加點對間距離和角度閾值雙重限制，剔除誤匹配點對，提高點云配準的精度。在優化端的關鍵幀選取中引入權重，通過回環檢測消除累積誤差，實現三維地圖的優化與更新。

1 基于Kinect的改進移動機器人VSLAM算法基本原理

1.1 系統結構

本文提出的室內移動機器人視覺SLAM系統框架如圖1所示，在進行傳感器數據采集之前，需對Kinect傳感器進行標定以獲取相機內部參數，對獲取到的彩色圖像進行特征檢測與提取，進行初步篩選后，利用RANSAC算法剔除離群點獲取初始位姿估計，再用改進的ICP算法完成點云的精準匹配，在關鍵幀選取中引入權重，通過回環檢測消除累積誤差，實現三維點云地圖的優化與更新，最終獲得機器人運行軌跡和室內環境點云地圖。

圖1 室內環境下基于移動機器人的RGB-D SLAM系統框架圖Fig. 1 The frame of RGB-D SLAM system for mobile robot in indoor environment

1.2 特征檢測與匹配

SLAM包含兩個問題，一個是“定位”，一個是“建圖”。解決定位問題的關鍵是在相機獲取了機器人所在的室內環境信息后，如何根據相鄰圖像的信息，粗略估計出相機運動，這一過程也被稱為視覺里程計。視覺里程計的實現方法主要分為特征法和直接法。特征法運行穩定，對光照、動態物體不敏感，是目前比較成熟的解決方案，因此本文采用特征法。

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

利用SURF算法進行特征點檢測與匹配的主要過程如下：

1) 對圖像關鍵點進行檢測；

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

3) 關鍵點定位；

4) 確定每個關鍵點的主方向；

5) 生成特征描述子；

6) 運用FLANN對特征描述子進行匹配。

1.3 Kinect標定與點云生成

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

1.4 點云配準與位姿估計

1.4.1 RANSAC算法完成粗匹配

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

1.4.2 傳統ICP算法

傳統ICP算法通過解決最小二乘均方差問題，求解包含旋轉矩陣R和平移矩陣t的剛體變換T，從而實現相機相鄰位姿的運動估計。傳統ICP算法步驟如下。

2) 通過最小化均方目標函數來求解剛體變換T。

式中：R為旋轉矩陣，t為平移矩陣。

3) 運用剛體變換轉換點云。

式中m表示迭代次數。

1.4.3 改進的ICP算法完成精匹配

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

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

1) 歐氏距離閾值

正確的匹配點對，兩點對之間的歐氏距離不應過大。本文添加了點對間歐氏距離閾值限制，去除小于平均點對距離的匹配，且認為同一片點云下任一點與其鄰點的拓撲結構不會隨剛體變換而變化。因此在粗匹配后，配對點云中的任一點與其鄰點，在另一個配對點云中的也應是鄰近點，給出了式(11)和式(12)兩個距離閾值限制：

2) 角度閾值

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

1.4.4 改進后的ICP算法步驟

3) 應用角度閾值法進一步剔除誤匹配點對，得到精配對點集和。

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

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

1.5 三維地圖的更新與優化

1.5.1 關鍵幀選取

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

1.5.2 位姿優化

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

在各個位姿轉換過程中均存在誤差，如果不進行優化，隨著轉換過程的疊加，會產生累計誤差。本文采用g2o優化算法[14]對位姿轉換過程中產生的累計誤差進行優化，誤差函數定義為

2 實驗結果及分析

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

圖3 實驗場景及實驗設備Fig. 3 Experimental scene and experimental installation

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

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

圖4 室內環境的兩幀數據Fig. 4 Two frames in indoor environment

圖5 第一幀數據的三維彩色點云圖Fig. 5 3-D color point cloud of the first frame data

圖6 粗匹配實驗結果Fig. 6 The experimental results of coarse matching

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

圖7 室內環境三維重建結果Fig. 7 The experimental results of 3D reconstruction in indoor environment

圖8 g2o優化圖Fig. 8 g2o optimization diagram

圖9 機器人運行軌跡估計Fig. 9 Robot trajectory estimation

3 結束語

本文提出了一種新的基于Kinect的室內移動機器人同時定位與地圖構建系統。針對相機位姿估計過程中，點云配準精度不高，易陷入局部最優等問題，提出了一種新的歐氏距離與角度閾值的雙重限制法，提高了點云配準精度，減少了配準誤差。結合關鍵幀選取機制和g2o優化算法，實現了三維地圖的優化與更新。在真實室內環境下進行在線實驗，實驗結果證明了本文算法的有效性與可行性。進一步改進工作將集中在：1)進一步研究歐氏距離與角度閾值的選取標準；2)提出移動機器人在同一環境下持續運動所產生的關鍵幀冗余的情況的解決方案。