一種基于深度相機(jī)的機(jī)器人室內(nèi)導(dǎo)航點(diǎn)云地圖生成方法

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

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

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

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

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

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

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

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

1 算法設(shè)計(jì)

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

1.1 相機(jī)模型

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

(1)

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

(2)

相機(jī)剛體變換[16]定義為

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

(3)

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

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

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

(4)

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

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

g=exp(ξ).

(5)

圖1 算法流程

1.2 相機(jī)跟蹤

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

(6)

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

(7)

1.3 關(guān)鍵幀處理

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

(8)

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

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

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

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

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

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

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

(9)

彩色圖像的投影誤差為

(10)

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

(11)

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

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

由于估計(jì)誤差的不斷累積，在視覺SLAM中相機(jī)跟蹤漂移是不可避免的。為了有效地抑制誤差累積的影響，得到更好的全局一致地圖，在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)路候選項(xiàng)，計(jì)算其與當(dāng)前關(guān)鍵幀的相似度，并選擇相似度超過一定閥值的關(guān)鍵幀作為環(huán)路候選項(xiàng)。為了提高系統(tǒng)的魯棒性與環(huán)路檢測的準(zhǔn)確性，僅選擇其相鄰幀作為環(huán)路候選項(xiàng)，并使用RANSAC方法計(jì)算其與當(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ù)的地圖點(diǎn)，利用位姿圖優(yōu)化方法對構(gòu)成環(huán)路的關(guān)鍵幀以及所對應(yīng)的3D地圖點(diǎn)進(jìn)行投影誤差與反投影誤差的聯(lián)合優(yōu)化，提高SLAM位姿估計(jì)精度，得到全局一致地圖。

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

2.1 機(jī)器人測試

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

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

圖2 相機(jī)估計(jì)軌跡

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

圖3 三維稀疏點(diǎn)云

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

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

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

圖5 稠密點(diǎn)云的八叉樹表示

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

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

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

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

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

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

3 結(jié)束語

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

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

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

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

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