基于RGB－D傳感器的3D室內(nèi)環(huán)境地圖實時創(chuàng)建

朱笑笑，曹其新，楊 揚，陳培華

（上海交通大學 機器人所 機械系統(tǒng)與振動國家重點實驗室，上海200240）

0 引 言

在自主移動機器人領(lǐng)域，對環(huán)境地圖的建立一直以來都是一個研究熱點問題。近年來，在以微軟公司發(fā)布的Kinect傳感器為代表的RGB－D傳感器迅速普及后，機器人從2D環(huán)境地圖的建立全面步入到了3D地圖的建立階段。本文主要研究利用Kinect傳感器進行3D地圖的建立方法。目前國內(nèi)外的相關(guān)研究情況如下：

在Kinect發(fā)布后不久，華盛頓大學與微軟實驗室［1］，開發(fā)了基于圖優(yōu)化算法——TORO［2，3］， 實時的視覺SLAM［4］（simultaneous localization and mapping）系統(tǒng)來建立3D地圖。該SLAM方法是用SIFT特征匹配得到當前幀的相對于第一幀的位姿的初始估計，然后使用ICP方法進行點云匹配來改善初始估計，在檢測到loop－closure后，將所有的幀輸入到TORO圖中進行全局優(yōu)化，從而得到更高精度的地圖。德國Freiburg大學［5］提出了RGBD－SLAM算法，采用了與華盛頓大學類似的方法，但是為了提高實時性使用了Hog－man［6］圖優(yōu)化算法，同時在相對位姿檢測上采用了SURF特征進行對應(yīng)點匹配。他們開放了源程序?？突仿〈髮W［7］為了進一步的提高實時性，沒有直接采用點云的地圖，而是在原始數(shù)據(jù)中提取出平面特征，他們開發(fā)了基于快速平面濾波算法多邊形重構(gòu)和融合算法。Willow Garage實驗室［8］開發(fā)了一種基于ICP （iteratively closest point）及BA （bundle adjustment）的地圖創(chuàng)建方法，這種方法不僅可以對相機位姿進行優(yōu)化同時可以對場景中的特征點進行優(yōu)化。

可以看到這些方法都是實時SLAM的方法以基于圖優(yōu)化的算法為主，同時在幾種地圖表達形式中，以3D點云地圖最具有通用性，一方面，它非常的直觀符合人類的視覺感官，可以直接展示給用戶。另一方面，最近幾十年來對點云的處理理論的研究也有了很深的積累，為機器人處理提供了很堅實的基礎(chǔ)。

本文將對具有代表性的RGBD－SLAM算法進行研究，重點考慮地圖的完整性和冗余度兩個方面，對該算法進行的改進。這里提出完整性是指建立的地圖盡量的包含環(huán)境的所有信息，減少空洞的出現(xiàn)，這就要求地圖建立方法有較高的實時性，可以實時將建立結(jié)果反饋給用戶，讓用戶知道那些地方還需要增加觀測。本文的結(jié)構(gòu)如下，首先介紹RGB－D傳感器及RGBD－SLAM算法原理，然后提出直接使用RGBD－SLAM傳感器在建立地圖中出現(xiàn)的兩個問題，并分別提出解決方案，最后通過實驗對兩個改進方法進行驗證。

1 RGB－D傳感器—Kinect

本節(jié)主要對Kinect傳感器的進行介紹，并將其和傳統(tǒng)的激光和視覺傳感器進行比較，同時給出其測量誤差方程。

1.1 Kinect傳感器的結(jié)構(gòu)與原理

Kinect是一個多傳感器的結(jié)合體，如圖1［9］所示，它包括一個隨機紅外點云投射器，一個紅外相機和一個彩色相機。發(fā)射器和紅外相機構(gòu)成了一個隨機點云結(jié)構(gòu)光3D成像系統(tǒng)，其得到RGB－D數(shù)據(jù)的原理是使用結(jié)構(gòu)光成像獲得環(huán)境的深度信息，然后通過坐標變換和顏色信息融合在一起。

圖1 Kinect內(nèi)部結(jié)構(gòu)

1.2 Kinect與激光傳感器和視覺傳感器比較

為了體現(xiàn)Kinect傳感的一些特點，我們將其和服務(wù)機器人上比較常用的HOKUYO URG－04LX激光傳感器進行比較，結(jié)果見表1?？梢钥吹酱蟛糠值闹笜硕汲^了該激光傳感器，只是在檢測角度范圍上要比激光傳感器小，也就是水平視場比較小，但是由于有垂直視場在很大程度上彌補了這個缺陷。

和普通攝像頭相比Kinect是有絕對優(yōu)勢的，首先它含有普通的彩色攝像頭，同時還解決普通攝像頭應(yīng)用中的兩個難題，即受光線的影響非常大和無法確定物理尺度。

表1 Kinect與HOKUYO激光傳感器比較

1.3 傳感器的誤差曲線

根據(jù)Kinect的成像原理，其測量誤差主要來自于幾個方面，特征匹配定位的誤差，數(shù)據(jù)離散化誤差和前期標定誤差。其中前期標定誤差是相對比較小，而其它兩個誤差結(jié)合起來使得測量精度和距離值呈一個非線性的關(guān)系。在文獻 ［9］中對Kinect精度進行了測量，其得到的測量誤差曲線方程為式 （1），這個誤差方程將被用在后續(xù)的圖像融合過程中

2 RGB－D－SLAM 介紹

這里對RGBD－SLAM算法的原理進行簡要的介紹，同時提出在實際使用中的兩個問題。

2.1 整體算法流程

RGB－D－SLAM算法流程如圖2所示，其核心是一個Hog－man圖優(yōu)化算法，在2.2節(jié)中將對其進行介紹。從圖中可以看到整個算法對RGB－D信息的充分利用，首先使用RGB顏色信息進行特征點的提取和匹配，然后結(jié)合深度信息一起進行初始位姿的計算，隨后再次使用深度信息進行ICP算法來優(yōu)化初始位姿。將得到的初始位姿作為節(jié)點和鄰近幀之間的位姿關(guān)系作為邊輸入到圖中，然后使用Hogman圖優(yōu)化算法進行優(yōu)化得到全局一致的地圖。最后在得到了多幀數(shù)據(jù)后，根據(jù)圖優(yōu)化的結(jié)果進行疊加就得到了3D點云地圖。使用該算法建立得到的一個實驗室場景如圖3所示。

2.2 Hog－man分層圖優(yōu)化算法

Hog－man分層圖優(yōu)化算法是為了實時SLAM而設(shè)計的，核心思想是根據(jù)一些標準將原始的圖劃分為多個子圖，以子圖中的一個節(jié)點來表示該子圖，從而得到原始圖一層的抽象，對得到的圖再依次進行抽象，就可以得到一個多層圖結(jié)構(gòu)，提取出原始圖中的拓撲結(jié)構(gòu)。當有新節(jié)點加入到圖中時，首先是加入到原始圖中，然后查看是否改變子圖劃分，如果有改變就需要更新高層圖，同時對最頂層進行優(yōu)化，只有當頂層的拓撲結(jié)構(gòu)發(fā)生很大改變的時候才由頂層圖向底層圖反向傳遞，更新底層圖。這樣就可以保證實時的優(yōu)化。

下面以一個兩層圖為例來介紹一下Hog－man多層圖的建立過程，因為這個過程是一個增量式的過程，我們假設(shè)已經(jīng)有了圖4中左邊的圖結(jié)構(gòu)。其中S表示子圖，L表示抽象層次，黃色點表示L0層節(jié)點，L0層中深藍色被選為L1層的節(jié)點。可以看到已經(jīng)有了3個子圖，當又有兩個新的節(jié)點如圖中綠色的節(jié)點A和B需要加入時，根據(jù)節(jié)點到最近的子圖的代表節(jié)點的距離來判斷是否加入現(xiàn)有子圖（如B）或者創(chuàng)建新的子圖 （如A）。當?shù)蛯拥膱D發(fā)生改變時，要依次傳遞到頂層圖，然后優(yōu)化頂層圖。

2.3 存在的問題

在建立圖4中場景時，Kinect的軌跡只是圍繞屋子中心進行旋轉(zhuǎn)，所以建立的地圖并不完整，只是建立了視角的方向（藍色的區(qū)域就是空缺的區(qū)域）。在實際進行地圖建立的時候，為了建立完整的3D環(huán)境，一些比較復(fù)雜的位置往往需要多次取像，如圖5所示為一個室內(nèi)場景，節(jié)點顏色的表示方法和圖4一樣。可以看到開發(fā)軌跡的特點的是在3個主要物體周圍形成了以物體為中心的取像軌跡，而在L1層節(jié)點中則沒有突出這種拓撲結(jié)構(gòu)反而有所弱化，如圖中紅色圈的區(qū)域。同時因為沒有正確的提取出拓撲結(jié)構(gòu)導致了L1層節(jié)點數(shù)仍然比較多。在實際建立地圖時，相機軌跡是一個3D的軌跡，將涉及到的節(jié)點更多，所以目前算法中的子地圖分割方法，對于建立完整室內(nèi)環(huán)境并不是最優(yōu)的。

第二個問題是在算法執(zhí)行完成后輸出3D點云結(jié)果時，只是簡單的將各幀點云數(shù)據(jù)相加。兩個點云的直接相加產(chǎn)生了大量的冗余點，一方面冗余點將導致點云數(shù)據(jù)量非常的龐大，另一方面，冗余的點云為后續(xù)處理帶來了干擾。例如，ICP方法是點云之間配置的標準方法［7］，它的原理是將最近點作為近似的對應(yīng)點來計算兩個點云之間的位姿關(guān)系，如果建立的點云有很多的冗余點時，ICP算法得到的結(jié)果就會比較差，如圖6是一幀點云 （較亮色點云）和已經(jīng)融合后的點云 （淺色點云）的ICP結(jié)果，其中白色點云是建立好的3D點云地圖，點云是一幀點云數(shù)據(jù)，ICP算法給出的最近點誤差已經(jīng)非常小，但是得到的位姿是不正確的。

3 算法改進

本節(jié)將針對上節(jié)中的兩個實際問題分別提出改進方法。

圖6 由冗余點導致的ICP誤差

3.1 改進子地圖分割標準

在圖6中可以看到，當我們想要建立一塊區(qū)域完整的信息時我們會在這塊區(qū)域周圍采集較多的點是一種包圍式的建立，所以我們將子圖的判斷標準修改為相機觀察中心的距離值如圖7（a）所示。圖中O為相機中心，IP為興趣中心點到O距離為d。根據(jù)Kinect測量有效范圍，將d設(shè)置為2m。假設(shè)當前相機的位姿用一個4×4的矩陣P表示，則IP的坐標為

使用觀察中心代替相機中心作為子圖分割標準可以更好地反應(yīng)建立完整地圖時的節(jié)點分布特點。如圖7（b）為3種節(jié)點分布，第一個是觀察中心集中的情況，第二個是相機中心和觀察中心平行的情況，第三個是相機中心集中的情況。顯然在第一和第三情況下，使用觀察中心可以得到正確的子圖分割結(jié)果，而在第二種情況下兩種是等效的。由式 （2）也可以看到，觀察中心的位置既包含了相機的位置信息也包含了相機選擇信息，所以在有轉(zhuǎn)角的情況下會明顯的優(yōu)于使用相機中心。

圖7 興趣中心點及節(jié)點分布

3.2 地圖冗余點去除

在進行地圖冗余點的去除前，我們首先在程序中添加了一個去噪模塊，對單幀數(shù)據(jù)進行一個濾波。主要是根據(jù)一個點的鄰域內(nèi)是否有點來判斷該點是否是孤立點，如果是孤立點則直接中這一幀點云中去除掉。

為了去除直接疊加所有幀帶來的冗余點，我們采用［10］中的基于空間體的點云融合方法。其主要思想是在對多幀點云數(shù)據(jù)進行融合時，首先建立一個可以包含所有幀的空間體，空間體以一定的分辨率劃分為空間像素點，每個點上將記錄到環(huán)境中物體的距離，然后依次對每一幀進行處理，修改空間像素點的值。該距離值的定義如圖8［10］所示，沿相機中心到曲面上每一點的視線軸上的一個有帶符號的距離信息，每一幀數(shù)據(jù)都對應(yīng)了一組距離值信息，通過一個累加操作將各幀的數(shù)據(jù)融合起來。最后空間體中的零值像素即為最后的曲面上的點，這樣得到的曲面是具有最小二乘性質(zhì)的曲面。同時為了反映采集到的每幀數(shù)據(jù)的一些特點，在進行距離值疊加的時候，采用了一個權(quán)值函數(shù)，最后的疊加方程如式 （3）、式 （4）。我們根據(jù)Kinect傳感器的誤差方程式 （1）權(quán)值函數(shù)的選取如式 （5）。其中x＊為單幀曲面上的點到相機中心的距離，如圖8中所示。

圖8 基于空間體的點云融合

4 實 驗

4.1 子圖劃分實驗

本文提出的新的子圖劃分方法主要是針對一些節(jié)點密集區(qū)域的節(jié)點的抽象的處理，所以這個部分的實驗環(huán)境如圖9（a），是圍繞實驗室中的一張辦公桌來進行3D地圖建立。建立得到的原始圖如圖9（b）共有56個節(jié)點。分別通過觀察中心標準和相機中心標準進行分割，分割的距離閾值為0.6m，得到的結(jié)果如圖9（c）、（d）。從結(jié)果對比可以看到使用觀察中心得到了節(jié)點數(shù)減少了一半，同時選出的結(jié)果更能表示原圖的拓撲結(jié)構(gòu)，產(chǎn)生3個節(jié)點分別對應(yīng)了正面，側(cè)面和頂面。

4.2 冗余點去除實驗

為了清楚的反映冗余點去除的效果，這里只對一個小場景拍攝的兩幀數(shù)據(jù)進行處理。實驗數(shù)據(jù)如圖10所示，其中圖10（a）為實驗場景的兩個顯示器。圖10（b）是第一幀點云數(shù)據(jù)，圖10（c）為第二幀點云。圖10（d）為兩幀直接相加的結(jié)果，圖10（e）為圖10（c）和圖10（d）的左側(cè)試圖，可以看到點云注冊位置誤差的存在，兩個點云直接疊加后，顯示器屏幕厚度變厚了，也就是前面2.3節(jié)中提到的情況，同時重復(fù)的點數(shù)也非常多。進行冗余點去除處理的結(jié)果如圖10（f）所示，兩個圖融合在了一起，從圖10（g）的網(wǎng)格化視圖可以看到，顯示器屏幕僅有一張網(wǎng)格組成，成功消除了冗余點的情況。

5 結(jié)束語

本文對當前使用RGB－D傳感器進行室內(nèi)3D點云地圖建立的方法進行了介紹，重點研究了最為流行的RGB－DSLAM算法。在對該算法原理進行研究基礎(chǔ)上提出了它的兩個不足之處，并提出了相應(yīng)的解決方法。一方面，通過將子地圖分割標準修改為根據(jù)觀察中心的方法，使得上層抽象地圖能夠更好的反映環(huán)境的拓撲結(jié)構(gòu)，在建立完成的較大場景時有更高的效率。另一方面，在最后地圖生成部分，提出了使用經(jīng)典的基于空間體的多幀融合方法來減少冗余點。實驗結(jié)果表明，兩方面的改進在建立完成室內(nèi)環(huán)境的應(yīng)用中得到很好的效果。總的說來，RGB－D傳感器可以迅速的發(fā)展主要是得益于前期相關(guān)理論的積累，但是RGB－D傳感器其自身的特點，也帶了很多新的挑戰(zhàn)，因此還有很多方面需要進一步研究。

［1］Peter H，Michael K，Evan H，et al.RGB－D mapping：Using depth cameras for dense 3Dmodeling of indoor environments［C］／／RSS Workshop on RGB－D Cameras，2010.

［2］Grisetti G，Slawomir G，Cyrill S，et al.Efficient estimation of accu－rate maximum likelihood maps in 3D ［C］／／IEEE／RSJ International Conference on Intelligent Robots and Systems，2007.

［3］Giorgio G，Cyrill S，Wolfram B.Non－linear constraint network optimization for efficient map learning ［J］.IEEE Transactions on Intelligent Transportation Systems，2009，10 （3）：428－439.

［4］Durrant－Whyte H，Bailey T.Simultaneous localization and mapping（SLAM）：Part I the essential algorithms ［J］.Robotics and Automation Magazine，2009，13 （2）：99－110.

［5］Fioraio N，Konolige K.Realtime visual and point cloud SLAM［C］／／RSS Workshop on RGB－D Cameras，2011.

［6］Ni K，Dellaert F.Multi－level submap based SLAM using nested dissection ［C］／／Intelligent Robots and Systems，2010：2558－2565.

［7］Biswas J，Veloso M.Depth camera based localization and navigation for indoor mobile robots［C］／／RSS Workshop on RGBD Cameras，2011.

［8］Engelhard N，F(xiàn)elixEndres，UrgenHess J，et al.Real－time 3D visual SLAM with a hand－h(huán)eld RGB－D camera ［C］／／RSS Workshop on RGB－D Cameras，2010.

［9］Herrera C，Daniel K，Heikkil J，et al.Depth and color camera calibration with distortion correction ［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2012，34 （10）：2058－2064.

［10］Cui Y，Schuon S，Thrun S，et al.Algorithms for 3Dshape scanning with a depth camera ［C］／／IEEE Trans on Pattern Analysis and Machine Intelligence，2012.