基于RGB-D的云機器人3D SLAM實驗系統

王 帥, 張云洲, 段 強

(東北大學 機器人科學與工程學院, 遼寧 沈陽 110819)

服務機器人大多呈現為移動型,需要在室內環境靈活運動。視覺、激光、慣性導航等傳感器的廣泛應用,極大地增強了移動機器人對環境的認知能力,同時也形成了沉重的運算負擔,對機器人的處理能力提出了更高的要求,產生了性能與成本之間的矛盾。為此,研究者提出將機器人與云計算相結合,把復雜的運算卸載至云端,同時憑借云端的數據交互賦予機器人互相學習的能力,由此形成了“云機器人”的技術概念[1]。近年來,研究者從體系架構、通信協議、數據庫構建等方面對云機器人開展了研究,出現了RoboEarth、GostaiNET等技術平臺[2-3],但大多數研究仍然停留在概念階段,缺乏實際應用的案例。

本文針對移動機器人構圖,借助云計算解決同步定位與地圖構建(SLAM)過程中密集型數據運算的問題,具有云機器人的典型技術特征。SLAM可以描述為：在未知環境中運動的機器人逐步構建周圍環境的地圖,同時在該地圖確定自身的位置和姿態[4-5]。在機器人領域,SLAM被認為是實現真正自主機器人的核心環節[6]。近年來,基于Kinect的三維視覺SLAM方法憑借其成本低廉、環境信息描述豐富等特點,在室內移動機器人領域迅速成為研究者關注的熱點[7-8]。

為了提高運算效率,研究者引入了并行運算、GPU編程等方法。文獻[9]將多核CPU用于視覺SLAM的特征匹配環節,通過并行處理實現加速運算；文獻[10]提出了一種三維VSLAM方法,建立環境的三維模型；文獻[11]提出了一種利用RGB-D數據進行快速視覺里程計和地圖構建的方法,使用Shi-Tomasi算法進行角點檢測,并通過卡爾曼濾波進行模型更新。上述方法均依靠機器人本地的計算資源進行實時求解,需要性能先進的機載硬件支持,存在計算開銷大、執行速度慢等缺陷。

為此,研究者開始借助云計算解決機器人SLAM過程的數據負擔,在低成本機器人硬件的基礎上實現高性能運算。Ayush K等[12]利用基礎設施即服務(Infrastructure as a service, IaaS)和并行運算實現視覺SLAM,獲得了良好的實時性與魯棒性,但并未考慮在實際應用時如何將機器人應用到該框架下。Benavidez等[13]以多臺計算機節點組成了云計算平臺,將視覺SLAM的復雜處理卸載于云端,通過并行處理在低成本硬件上實現了高性能運算,實現了二維環境構圖。Mohanarajah等人[14]采用云計算實現了多臺機器人的協同3D構圖。上述研究,大多采用了私有云框架實現SLAM流程,其應用范圍受到了較大的限制。

本文針對移動機器人機載嵌入式控制器處理能力有限的問題,依托公共云平臺提出了一種基于云計算的實時三維視覺SLAM實驗系統,利用金字塔Lucas-Kanade算法實現特征點的實時追蹤與匹配,通過將復雜運算卸載于云端從而提高了執行速度。實驗結果表明,本文有效降低了視覺SLAM對硬件的依賴度,大幅降低了運算時間,并且具有較好的實時性和構圖精度。

1 視覺SLAM算法及步驟

本文采用基于圖優化的SLAM方法,主要包括前端任務和后端任務兩部分。前端任務包括特征提取與匹配、配準和閉環檢測等過程。特征提取與匹配是指順序數據關聯問題,而配準用于解決幀間位姿估計問題,閉環檢測則根據觀測到的場景信息來判斷機器人是否回到之前訪問過的區域。后端任務主要完成地圖的優化,構建全局一致的地圖。

1.1 關鍵幀提取

1.1.1 金字塔Lucas-Kanade算法

Lucas-Kanade算法(LK)是光流法的經典算法,

基于以下3個假設：(1)亮度恒定；(2)時間連續或者小運動；(3)空間一致。LK算法適用于小而連貫的運動,但在實際應用中跟蹤效果并不好。本文采用基于金字塔分層的LK算法[15],在圖像金字塔的最高層計算光流,將得到的運動估計結果作為下一層金字塔的起始點,重復該過程直至到達金字塔的最底層。這樣可以有效解決小運動的限制問題。

為了提高光流計算速度,可以僅對少量簡單特征點進行光流計算跟蹤。根據亮度恒定和時間連續假設可以得到對于相鄰兩幀圖像：

I(x,y,t)=I(x+dx,y+dy,t+dt)

(1)

對I(x+dx,y+dy,t+dt)進行一階泰勒級數展開可以得到：

(2)

根據空間一致的假設,在像素局部小鄰域內可建立如下n個方程：

(3)

求解可以求得像素點處的概略光流矢量。

1.1.2 關鍵幀更新

假設移動機器人在某一時刻的位姿為P=(x,y,zi,φ,θ,ψ),當前關鍵幀與新幀的姿態分別為Pi和Pj,則兩幀之間的相對距離Di,j[15]為

(4)

式中,函數dis(·)和angle(·)分別代表轉換矩陣的平移與旋轉部分。當Di j較小時,表明機器人獲取當前關鍵幀與新幀時的姿態Pi和Pj變化程度不足,放棄新幀；當Di,j大于某個閾值時,則將新幀替換為當前關鍵幀。

本文采用RANSAC算法完成機器人位姿的初始估計。首先提取當前關鍵幀的FAST特征點,然后對這些特征點進行跟蹤,利用跟蹤上的特征點估計機器人獲取當前幀時的位姿,并判斷當前幀是否為關鍵幀。為了解決跟蹤過程中特征點不斷減少的問題,每提取一次關鍵幀,對跟蹤模板進行一次更新。關鍵幀提取流程如圖1所示。

圖1 關鍵幀提取流程圖

1.2 精確配準與閉環檢測

1.2.1 ICP精確配準

作為一種基于迭代的配準算法,ICP算法[16]能夠配準自由曲面和曲線以及各種點集。它每次迭代都在目標數據集中找到最近的點,將其作為控制點估計出變換矩陣參數,直到滿足收斂準則,使得兩次迭代的目標函數值變化小于某個閾值,或者達到預設的迭代次數迭代計算才會停止。本文的目標函數采用點到點距離平方和：

(5)

其中,M為源點集,N為目標點集,R和T分別為旋轉矩陣和平移向量。

對于數據集M中的每個數據點,可以在目標數據集中找到相應的最近點,組成對應點集N。

初始時令M0=M,R0=I,T0=0,k=0,則ICP算法迭代步驟為：

Step 1 計算最近點：對Mk中的每個數據點,估計在目標數據集中的最近點,組成數據集Nk；

Step 3 更新源集：用Step2得到的變換矩陣Hk根據公式(6)：

Pk+1=Hk(P0)=Rk(P0)+Tk

(6)

對數據Pk進行更新。

Step 4重復迭代：當兩次估計得到的誤差變化小于閾值τ時,迭代停止。

對于2個數據集A和B的配準,如果A是B的嚴格子集,則ICP算法能夠使其在全局上達到某種度量準則下的最佳配準。然而,從不同視角下獲得的物體表面深度數據,通常僅存在部分的重疊,很容易導致ICP算法陷入局部的最小值。因此,對2個數據集的初始相對位置要求較高,否則初始誤差過大將導致匹配錯誤。另外,如果考慮全部點集,不僅會影響尋找最近點的精度,還會影響ICP算法的運算速率,從而嚴重妨礙算法的實時性。為了解決上述問題,本文使用ORB特征對相鄰關鍵幀進行特征匹配,然后將彩色圖像中的特征匹配對對應到深度圖中,將匹配上的特征點對轉化為點云作為ICP算法的初始點集,從而在提高精度的同時滿足實時性的要求。

1.2.2 閉環檢測

對點云進行逐步配準時,匹配誤差將逐漸累積,在最終閉合場景的匹配處會產生位姿漂移現象,形成閉環問題。閉環檢測是指根據傳感器信息判斷機器人當前是否處在之前已經訪問過的某個區域。準確的閉環信息可以有效修正點云配準誤差,得到信息一致的地圖。本文的閉環檢測方法如下：

根據兩幀圖像特征匹配對在總特征點中所占的比率,初步判斷是否為相同場景,然后利用兩幀的相對位姿變化程度進一步確定是否形成閉環。

假設ni和nj分別為圖像Ii和Ij提取的特征點個數,n為匹配上的特征對個數,則兩幀圖像的距離可以用下式衡量[17]:

(7)

兩幀圖像的相似度定義為：

(8)

其中,Dmin和Dmax為自己設定的距離可以取的最小與最大閾值。

當Sij大于某個閾值時,初步認為兩幀是同一個場景,然后利用ICP算法判斷幀間位姿變換程度,進而確定是否回到先前位置。

1.3 相機姿態的圖優化求解

SLAM過程中檢測到閉合回路后,可以利用閉環信息消除視覺里程計過程中的累積誤差,在SLAM中這被稱為圖優化問題,即根據已建立的約束關系求解最優的位姿序列。

(9)

即：

(10)

在圖結構建立之后,本文采用開源框架g2o對最小二乘問題進行優化求解。

2 基于云計算的3D VSLAM

2.1 系統描述

復雜室內環境的視覺SLAM屬于計算密集型任務,運算量較大,對硬件的要求也比較高。對于機器人通常采用的嵌入式處理器,承擔SLAM任務時的運算負擔較重、耗時過長,無法滿足移動機器人的實際需求。本文結合云計算的優勢,在機載嵌入式系統的基礎上構建了一種新的快速SLAM系統,具體結構如圖2所示。

圖2 云計算SLAM的系統架構

本文云端平臺選用AWS公共云計算平臺,以虛擬機的形式申請了計算資源,其性能相當于2 GB內存、2.5 GHz主頻的雙核計算機處理器,承擔機器人SLAM的閉環檢測和全局優化任務。移動機器人的機載嵌入式系統采用了i.MX6四核ARM處理器,其主頻為1.2 GHz,內存為1 GB DDR3,構成了低成本的數據處理平臺。云存儲采用了MySQL數據庫,能夠對多個請求進行多線程共同處理。在云平臺提供的計算服務包括VSLAM過程中的閉環檢測和全局優化。隨著關鍵幀數量的增多,閉環檢測過程會極大地消耗計算資源,考慮到閉環檢測的處理過程是相互獨立的,適用于云端分布式處理的架構,因此機器人可以卸載復雜的運算到云端,以提高構圖的效率。

本文的SLAM系統主要包含本地和云端兩部分。在本地端,采用低成本的RGB-D傳感器Kinect獲取實際場景中的彩色圖和深度圖,在嵌入式ARM處理器上完成關鍵幀的提取過程,減少本地與云端的傳輸數據量以縮短執行時間。在云端,使用云存儲和云計算處理傳感器數據并存儲視覺及圖像信息。在數據傳輸層采用C/S架構,機器人與云平臺分別作為客戶端與服務端,以TCP/IP協議的套接字(socket)進行數據傳輸,數據通信流程圖圖3所示。

圖3 數據傳輸流程圖

云平臺作為服務端,提供構建地圖的服務。本地端獲取的關鍵幀信息全部存儲在云端的MySQL數據庫中,云端利用分布式計算方法完成閉環檢測過程,然后利用優化后的機器人位姿信息完成地圖的構建,并對本地機器人的位姿進行更新與修正。

2.2 數據傳輸

TCP/IP協議是網絡通信的經典協議之一,提供點對點的連接機制,對數據傳輸涉及的封裝、尋址、傳輸等過程均加以標準化,在保證數據穩定傳輸的同時提高傳輸效率。

TCP協議的優勢在于流量控制,能夠保證傳輸數據的完整性,這對于本地傳輸給云端的關鍵幀數據至關重要,因為關鍵幀在傳輸過程中丟失將影響地圖信息的完整性,甚至導致地圖構建失敗。本文需要將Kienct獲取的場景信息發送到云端服務器,同時將更新后的機器人位姿返回給本地端,更新與修正機器人位姿,對數據的穩定性要求較高?？紤]到SLAM的效率和數據傳輸速度,本文選擇以TCP/IP協議進行數據傳輸。

3 實驗與討論

為了驗證所提方法的有效性,本文分別在標準數據集和實際場景下進行了實驗。

3.1 數據仿真

3.1.1 TUM數據集

仿真所選用的標準數據集為慕尼黑工業大學的辦公室場景[19]。針對本地運算和本文系統(云端-嵌入式系統)運算這兩種方式,執行時間對比結果如表1所示?？梢钥闯?引入云計算之后,雖然數據傳輸需要消耗一定的時間,但是計算效率卻提高了4～5倍。

表1 執行時間對比(本地優化)

如果本地運算要達到本文系統的構圖速度,則需要舍棄閉環檢測和全局優化兩個過程,執行時間對比如表2所示。在這種情況下,由于本地運算不需要傳輸數據到云端,耗時少于本文云計算SLAM系統,但是構圖精度卻大幅下降。

表2 執行時間對比(本地未優化)

圖4和圖5分別為本地SLAM(未優化)和本文系統在地圖構建過程中移動機器人的姿態和位置誤差對比,其中姿態包括俯仰角(Pitch)、偏航角(Yaw)和翻滾角(Roll)。實驗所使用的關鍵幀是手持Kinect在大約10 m的移動距離內采集的?？梢钥闯?經過云端優化,機器人姿態估計具有更小的誤差,偏航角的優化效果尤為明顯,本文系統Yaw誤差保持在0.004 rad以內。對于軌跡誤差,隨著時間推移,本地SLAM系統會引入大于0.12 m的定位誤差,而本文系統定位誤差大約為0.04 m,更貼合機器人移動的真實路徑,具有較好的定位精度。

圖4 姿態誤差對比

圖5 位置誤差對比

圖6為機器人運動的真實路徑(groundtruth)以及本文系統和本地(未優化)所估計出的機器人路徑?？梢钥闯霰疚南到y所估計出的路徑與真實路徑(groundtruth)更為相近。圖7為本文系統構建的數據集實驗室場景點云地圖。該地圖是由1 000幀數據集中提取出的144幀關鍵幀拼接而成,可以看出已較完整地構建出了實驗室場景。根據上述誤差對比結果,也可看出本文系統構建的地圖較為精確。

圖6 構建的路徑對比

圖7 地圖構建

3.1.2 NYUV1數據集

NYUV1數據集[17]提供了64個不同的室內場景,本文采用其中的一個動態場景,測試云計算SLAM系統的性能。首先對比機載處理器(未優化)和本文系統的構圖效果,如圖8所示。在這種情況下,兩種方法的構圖效率大致相等,可以看出機載處理器自身構建的地圖存在明顯的偏差,本文系統經閉環檢測和全局優化之后構建出全局一致的地圖。

圖8 構圖效果對比

為繼續驗證本文系統的有效性,對機器人機載處理器SLAM過程同樣加入了閉環檢測和全局優化部分,結果對比如圖9所示?？梢钥闯?盡管在云端構圖需要數據傳輸,而通信需要消耗一定的時間,但由于云端強大的計算能力,其構圖與優化過程仍然具有較高的效率。

圖9 本地與云端構圖速度對比

3.2 實際環境測試

為進一步驗證本文系統的效果,選取實驗室環境進行測試。機器人的運動平臺采用TurtleBot,傳感器為Kinect RGB-D傳感器,機器人實物及實際環境如圖10所示。本地與云端的構圖過程均在機器人操作系統(ROS)框架下實現。

圖10 機器人實物及實際測試場景

機器人的移動速度為0.3 m/s,實際的構圖效果如圖11所示。其為實驗室真實場景點云地圖的俯視圖,可以看出地圖符合全局一致的要求,并未出現明顯的由于累積誤差而導致的漂移現象,驗證了本文系統的可行性和有效性。

圖11 實際環境構圖效果

綜上所述,借助云端計算資源,本文為低成本機器人賦予了高性能的運算能力。本文采用ARM嵌入式系統,其經濟成本遠低于搭載工控機或便攜式計算機的常規方案；為執行SLAM運算所申請的公共云計算資源,其使用成本可以忽略不計(本文所申請的云計算資源使用成本為0.18元/h,而SLAM運算過程僅持續數分鐘；即使以10 min計算,成本僅有0.03元/次)。因此,與配備高性能硬件的常規做法相比,本文可以有效地避免機器人硬件資源的浪費,形成更高的性價比。

4 結論

基于RGB-D傳感器的機器人SLAM已成為國內外的研究熱點,然而由于數據計算量較大,依靠機器人的機載處理器很難執行SLAM運算。本文引入云計算構建一種新的移動機器人SLAM運算架構,機器人本身不再需要進行大量運算,極大減輕機載處理器的負擔,縮短地圖構建時間,對機器人硬件資源的需求也大為降低。實驗結果也表明,本文所提方法及算法的有效性。與通過機器人配備高性能硬件以提高性能的做法相比,本文所構建的實驗系統將低成本硬件與云端運算相結合,不僅賦予機器人更為強大的運算能力,而且可以大幅降低其對硬件資源的依賴,為未來高性價比機器人的發展提供了一種新的思路。