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

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

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

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

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

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

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

為了能夠?qū)崟r精確地生成機器人室內(nèi)導(dǎo)航所需的點云地圖，本文提出一種基于深度相機的視覺特征與ICP相融合的點云地圖生成方法。

1 算法設(shè)計

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

1.1 相機模型

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

(1)

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

(2)

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

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

(3)

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

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

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

(4)

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

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

g=exp(ξ).

(5)

圖1 算法流程

1.2 相機跟蹤

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

(6)

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

(7)

1.3 關(guān)鍵幀處理

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

(8)

式中：W是一個對角矩陣，表示ξji中每個參數(shù)的權(quán)重。如果當(dāng)前幀與上一個關(guān)鍵幀Ki-1之間的距離超過所設(shè)定的閥值時，則創(chuàng)建新的關(guān)鍵幀Ki。

對關(guān)鍵幀的處理：

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

2)執(zhí)行數(shù)據(jù)關(guān)聯(lián)操作融合重復(fù)的地圖點，并剔除誤差較大的地圖點。

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

4)進行局部優(yōu)化，優(yōu)化相機位姿與3D地圖點坐標(biāo)。

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

(9)

彩色圖像的投影誤差為

(10)

對彩色圖像的投影誤差與深度圖像的反投影誤差進行聯(lián)合優(yōu)化：

(11)

其中wrgb為彩色圖像投影誤差的權(quán)重參數(shù)。

1.4 環(huán)路檢測與地圖優(yōu)化

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

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

在檢測到SLAM環(huán)路之后，執(zhí)行數(shù)據(jù)關(guān)聯(lián)操作融合重復(fù)的地圖點，利用位姿圖優(yōu)化方法對構(gòu)成環(huán)路的關(guān)鍵幀以及所對應(yīng)的3D地圖點進行投影誤差與反投影誤差的聯(lián)合優(yōu)化，提高SLAM位姿估計精度，得到全局一致地圖。

2 實驗結(jié)果

2.1 機器人測試

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

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

圖2 相機估計軌跡

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

圖3 三維稀疏點云

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

圖4 稠密點云表示的室內(nèi)三維場景

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

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

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

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

2.2 數(shù)據(jù)集測試

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

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

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

3 結(jié)束語

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

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

[2] 康永偉. 車載激光點云數(shù)據(jù)配準(zhǔn)與三維建模研究[D]. 北京：首都師范大學(xué), 2009.

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

[4] 詹慶明, 張海濤, 喻亮. 古建筑激光點云-模型多層次一體化數(shù)據(jù)模型[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.