朱笑笑，曹其新，楊 揚(yáng)，陳培華

（上海交通大學(xué) 機(jī)器人所 機(jī)械系統(tǒng)與振動國家重點(diǎn)實(shí)驗(yàn)室，上海200240）

0 引 言

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

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

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

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

1 RGB－D傳感器—Kinect

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

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

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

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

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

為了體現(xiàn)Kinect傳感的一些特點(diǎn)，我們將其和服務(wù)機(jī)器人上比較常用的HOKUYO URG－04LX激光傳感器進(jìn)行比較，結(jié)果見表1。可以看到大部分的指標(biāo)都超過了該激光傳感器，只是在檢測角度范圍上要比激光傳感器小，也就是水平視場比較小，但是由于有垂直視場在很大程度上彌補(bǔ)了這個缺陷。

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

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

1.3 傳感器的誤差曲線

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

2 RGB－D－SLAM 介紹

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

2.1 整體算法流程

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

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

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

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

2.3 存在的問題

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

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

3 算法改進(jìn)

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

圖6 由冗余點(diǎn)導(dǎo)致的ICP誤差

3.1 改進(jìn)子地圖分割標(biāo)準(zhǔn)

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

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

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

3.2 地圖冗余點(diǎn)去除

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

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

圖8 基于空間體的點(diǎn)云融合

4 實(shí) 驗(yàn)

4.1 子圖劃分實(shí)驗(yàn)

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

4.2 冗余點(diǎn)去除實(shí)驗(yàn)

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

5 結(jié)束語

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