一種基于深度相機的機器人室內導航點云地圖生成方法

馬躍龍，曹雪峰，萬 剛,李登峰

(1.信息工程大學，河南 鄭州 450052；2.炮兵訓練基地，河北 宣化 075100)

地圖是人類空間認知的工具，無論早期的紙質地圖還是如今的電子地圖，都在人類的日常生活中發揮著重要作用[1]。隨著以無人車、無人機為典型代表的智能機器人平臺逐漸進入人們的視野，智能機器人的自主導航成為當前的研究熱點，而作為導航基礎的地圖生成方法更是成為智能機器人自主導航的關鍵。

點云是智能機器人導航地圖的一種有效表示形式。目前智能機器人的點云獲取主要有激光點云與圖像點云兩種主要方式。三維激光掃描儀能夠直接快速地獲取高精度的三維激光點云，在城市建模[2]、文物與古建筑修復[3-4]等領域得到了成功的應用。三維激光掃描儀造價高昂，體積較大，常常需要車載或機載配合使用；所獲取的點云數據量較大，處理較為復雜，需要人工對點云數據進行編輯。

圖像點云的生成主要有SfM與SLAM兩種方法。計算機視覺領域的SfM(Structure from Motion)方法通過對大量無序圖片的離線處理，恢復相機的運動軌跡并重建相應的圖像點云，進而恢復三維場景結構[5-6]。經過幾十年的發展，SfM方法相對較為成熟，并且有PhotoScan與Pix4D等成熟的商業軟件可供選用。由于SfM方法大多使用單目相機作為圖像獲取手段，因而在處理過程中不容易獲得三維場景結構的真實尺度，且其離線處理方式使其不適用于實時性要求較高的應用場景。

機器人領域的SLAM(Simultaneous Localization and Mapping)問題自20世紀80年代提出，至今已經過近30年的發展[7]。早期的SLAM研究主要以激光、聲納等測距類傳感器為主。隨著傳感器技術以及計算機視覺技術的發展，特別是多視圖幾何技術[8]的逐漸成熟，簡單輕便的單目、雙目以及深度(RGBD)相機等圖像類傳感器越來越受到研究者的青睞。

以微軟Kinect代表的深度相機可同時獲取彩色圖像與深度圖像，為室內模型構建與機器人導航地圖的生成提供了新工具。KinectFusion[9]通過ICP(Iterative Closest Point)算法實現點云數據之間的配準，估計相機在空間中的運動；基于體素格網實現室內物體的表面重建，所有的測量直接融合為體素表示。KinectFusion可實時計算，但由于算法的復雜性，需要高性能顯卡的支持，且體素的表達方式內存占用較大，僅適用于小范圍場景。Kintinuous[10]通過移動帶有當前相機位姿的體素格網動態改變融合區域，改進KinectFusion的內存占用問題，采用基于BoW(Bag of Words)[11]的環路檢測算法降低了位姿估計誤差的累積影響，通過CUDA運算實現相機位姿的實時估計與圖像點云的實時拼接，但位姿估計的精度有待進一步提高。RGBD-SLAM[12]是為開源社區廣泛使用的實時SLAM系統，通過相鄰圖像幀之間的視覺特征提取與匹配估計相機運動，并通過ICP算法對估計得到的相機運動進行驗證以保證相機位姿估計的正確性，而位姿估計的實時性與精度有待進一步提高。DVO[13-14]通過直接方法(Direct-Method)利用像素強度實現相鄰圖像幀的配準并估計相機位姿。相比于ICP算法，DVO可通過CPU實現實時運算。DVO適用于窄基線場景(相機運動變化較小)，對于相機運動變化較大的情況魯棒性不高，且直接方法受光照條件的影響較大，因而對所使用的相機有較高的要求。

為了能夠實時精確地生成機器人室內導航所需的點云地圖，本文提出一種基于深度相機的視覺特征與ICP相融合的點云地圖生成方法。

1 算法設計

本文提出一種基于深度相機的機器人室內導航點云地圖實時生成方法：①利用ORB(Oriented FAST and Rotated BRIEF)算子對深度相機所拍攝的彩色序列圖像進行視覺特征的快速提取與匹配，結合ICP算法，實時估計相機位姿；②在相機位姿優化操作中添加ICP誤差約束，應用圖優化算法[15]對深度相機所獲取的彩色圖像的投影誤差以及深度圖像的反投影誤差進行聯合優化，得到相對精確的相機位姿與三維稀疏點云；③利用估計得到的相機位姿對關鍵幀所對應的稠密圖像點云進行拼接融合，得到稠密點云表示的機器人室內導航點云地圖。算法流程如圖1所示。

1.1 相機模型

由深度相機可以同時獲得彩色圖像IRGB→R3與深度圖像ID→R。設標準針孔相機的焦距為f，相機投影中心沿圖像x軸與y軸的偏移分別為cx與cy，將三維空間中某點P=(X,Y,Z)T映射為圖像像素位置p=(u,v)的投影函數π[8]定義為

(1)

給定圖像像素坐標p=(u,v)T以及所對應的深度值Z(p),重建空間三維點的反投影函數π-1可定義為

(2)

相機剛體變換[16]定義為

g=[R|t]∈SE(3).

(3)

其中R∈SO(3)為正交旋轉矩陣，t∈R3為平移矩陣。

將相機位姿定義為剛體變換矩陣，并使用曲線坐標系(twist coordinate)表示：

ξ=(v1,v2,v3,w1,w2,w3)T.

(4)

其中，[v1,v2,v3]∈R3表示相機的線性速度，[w1,w2,w3]∈R3表示相機的角速度。

由李群與李代數的運算規則，相機在空間中的剛體變換(位姿)可表示為

g=exp(ξ).

(5)

圖1 算法流程

1.2 相機跟蹤

將第一幀圖像所對應的相機位姿ξ0=(0,0,0,0,0,0)T定義為世界坐標系的原點，通過相鄰兩幀圖像之間的相對變化估計相機位姿，并描述相機在三維空間中的運動。

(6)

通過優化彩色圖像的投影誤差，可以得到相機位姿為

(7)

1.3 關鍵幀處理

在SLAM點云地圖中并不會保存所有的圖像幀，而是選擇保存滿足一定條件的關鍵幀。將兩幀圖像之間的相對距離定義為相機位姿的平移與旋轉的權重組合：

(8)

式中：W是一個對角矩陣，表示ξji中每個參數的權重。如果當前幀與上一個關鍵幀Ki-1之間的距離超過所設定的閥值時，則創建新的關鍵幀Ki。

對關鍵幀的處理：

1)三角化匹配的特征點生成3D地圖點。為了盡可能多地生成3D地圖點，對于跟蹤中未成功匹配的所有特征點，沿極線在相鄰關鍵幀中查找更多的匹配。

2)執行數據關聯操作融合重復的地圖點，并剔除誤差較大的地圖點。

3)計算并注冊新關鍵幀的BoW向量，以用于后續操作的快速搜索與匹配。

4)進行局部優化，優化相機位姿與3D地圖點坐標。

對新添加的關鍵幀K，通過BoW向量快速檢索與關鍵幀K共享相同視覺單詞(Visual Words)的關鍵幀構成局部關鍵幀集合KL，同時與關鍵幀集合KL相對應的3D地圖點集合。對關鍵幀集合KL以及相對應的3D地圖點集合進行投影誤差與反投影誤差的聯合優化，以提高關鍵幀所對應的相機位姿以及3D地圖點坐標的估計精度。

(9)

彩色圖像的投影誤差為

(10)

對彩色圖像的投影誤差與深度圖像的反投影誤差進行聯合優化：

(11)

其中wrgb為彩色圖像投影誤差的權重參數。

1.4 環路檢測與地圖優化

由于估計誤差的不斷累積，在視覺SLAM中相機跟蹤漂移是不可避免的。為了有效地抑制誤差累積的影響，得到更好的全局一致地圖，在SLAM處理流程中引入基于BoW[11]的環路檢測。

在關鍵幀處理階段，為新生成的關鍵幀注冊BoW向量。在環路檢測階段，對于當前關鍵幀，在已構建的BoW向量數據庫中檢索與當前關鍵幀共享相同視覺單詞的關鍵幀作為環路候選項，計算其與當前關鍵幀的相似度，并選擇相似度超過一定閥值的關鍵幀作為環路候選項。為了提高系統的魯棒性與環路檢測的準確性，僅選擇其相鄰幀作為環路候選項，并使用RANSAC方法計算其與當前關鍵幀的變換關系，選擇滿足一定數量的關鍵幀作為環路關鍵幀，確保與當前關鍵幀的幾何一致性。

在檢測到SLAM環路之后，執行數據關聯操作融合重復的地圖點，利用位姿圖優化方法對構成環路的關鍵幀以及所對應的3D地圖點進行投影誤差與反投影誤差的聯合優化，提高SLAM位姿估計精度，得到全局一致地圖。

2 實驗結果

2.1 機器人測試

使用Turtlebot作為機器人平臺，搭載Asus Xtion Pro深度相機，并使用安裝有Intel Core I5-3230 CPU，8G內存，64位Debian Sid系統的筆記本電腦作為主控計算機，負責Turtlebot機器人的運行控制與SLAM信息處理，通過手動遙控方式控制機器人在室內行走，構建機器人室內導航點云地圖。

對深度相機所拍攝的序列圖像進行視覺特征提取，通過相鄰圖像幀之間的特征點匹配集合實時估計相機位姿，得到相機在空間中的運動軌跡，如圖2所示。

圖2 相機估計軌跡

利用所獲取的深度圖像，對相鄰圖像幀之間的特征點匹配集合執行三角化操作，重建圖像像素點所對應的空間三維點坐標，得到稀疏三維點云，如圖3所示。

圖3 三維稀疏點云

由關鍵幀對應的彩色圖像與深度圖像可得到該關鍵幀所對應的三維點云，利用估計得到的相機位姿對關鍵幀所對應的三維點云進行拼接融合，得到以稠密點云表示的三維空間場景結構，如圖4所示。

圖4 稠密點云表示的室內三維場景

為了對所生成的稠密點云進行有效的管理與使用，使用八叉樹對稠密點云進行剖分與管理，所得到結果如圖5所示(分辨率0.01 m)。

圖5 稠密點云的八叉樹表示

為了測試本文方法的效率，對關鍵步驟進行速度度量，結果如表1所示。

由定位速度的測試結果可以看出，本文方法中的相機跟蹤平均耗時為23.5 ms，即可以實現每秒42幀的處理速度，由程序性能度量以及實際的機器人測試結果來看，本文方法可滿足機器人室內導航點云地圖生成的實時性需求。

2.2 數據集測試

為了評估本文方法位姿估計的精確性，應用TUM-RGBD數據集[17]進行精度評估測試，度量估計得到的相機軌跡與相機軌跡真值之間絕對軌跡誤差(ATE)，并與典型的KinectFusion[9]以及RGBD-SLAM[12]的精度測試結果進行對比，以絕對軌跡誤差(ATE)作為度量指標，如表2所示。由測試結果可以看出，本文方法估計得到的相機位姿精度與魯棒性優于另外兩種方法。

表2 絕對軌跡誤差(ATE)對比 m

本文方法估計得到的相機軌跡與數據集相機軌跡真值之間的對比如圖6所示。由對比結果可以看出，本文方法估計得到的相機軌跡可以很好地接近數據集的相機軌跡真值。

3 結束語

通過處理深度相機所拍攝的序列圖像實時估計相機位姿，利用圖優化算法，綜合考慮深度相機所獲取的彩色圖像的投影誤差與深度圖像的反投影誤差，對相機位姿與稀疏三維點云進行聯合優化；利用優化后的相機位姿對關鍵幀所對應的圖像點云進行拼接融合，從而得到表示三維空間場景結構的稠密點云地圖。通過機器人實驗以及標準數據集測試，驗證本文方法的有效性、精確性與實時性。

[1] 高俊. 地圖學尋跡[M]. 北京：測繪出版社, 2012.

[2] 康永偉. 車載激光點云數據配準與三維建模研究[D]. 北京：首都師范大學, 2009.

[3] 趙煦, 周克勤, 閆利,等. 基于激光點云的大型文物景觀三維重建方法[J]. 武漢大學學報(信息科學版), 2008, 33(7): 684-687.

[4] 詹慶明, 張海濤, 喻亮. 古建筑激光點云-模型多層次一體化數據模型[J]. 地理信息世界, 2010(4):6-11.

[5] SNAVELY N, SEITZ S, SZELISKI R. Photo Tourism: Exploring Image Collections in 3D. ACM Transactions on Graphics[J]. ACM Transactions on Graphics, 2006.

[6] SNAVELY N, SEITZ S M, SZELISKI R. Modeling the world from internet photo collections[J]. International Journal of Computer Vision, Springer, 2008, 80(2): 189-210.

[7] BAILEY T, DURRANT-WHYTE H. Simultaneous localization and mapping(SLAM): Part I[J]. IEEE Robot. Autom. Mag., 2006.

[8] HARTLEY R I, ZISSERMAN A. Multiple View Geometry in Computer Vision[M]. 第2版. Cambridge University Press, ISBN: 0521540518, 2004.

[9] NEWCOMBE R A, IZADI S, HILLIGES O, et al. KinectFusion: Real-time dense surface mapping and tracking[C]//Mixed and augmented reality (ISMAR), 2011 10th IEEE international symposium on. IEEE, 2011: 127-136.

[10] WHELAN T, KAESS M, FALLON M, et al. Kintinuous: Spatially extended kinectfusion[C]. National Conference on Artificial Intelligence. 2012.

[12] ENDRES F, HESS J, STURM J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, IEEE, 2014, 30(1): 177-187.

[13] KERL C, STURM J, CREMERS D. Robust Odometry Estimation for RGB-D Cameras[C]//International Conference on Robotics and Automation. 2013.

[14] KERL C, STURM J, CREMERS D. Dense Visual SLAM for RGB-D Cameras[C]//Proc. of the Int. Conf. on Intelligent Robot Systems (IROS). 2013.

[15] KüMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: A general framework for graph optimization[J]. Proceedings - IEEE International Conference on Robotics and Automation, 2011: 3607-3613.

[16] MA Y, SOATTO S, KOSECKA J, et al. An invitation to 3-d vision: from images to geometric models[M]. Springer Science & Business Media, 2012, 26.

[17] STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012: 573-580.