劉 晙，袁培燕，李永鋒

(1.河南工學(xué)院 計(jì)算機(jī)科學(xué)與技術(shù)系,河南 新鄉(xiāng) 453002； 2.河南師范大學(xué) 計(jì)算機(jī)與信息工程學(xué)院,河南 新鄉(xiāng) 453007；3.96367部隊(duì)，青海 德令哈 817099)

基于完整可見(jiàn)性模型的改進(jìn)魯棒OctoMap

劉 晙1*，袁培燕2，李永鋒3

(1.河南工學(xué)院 計(jì)算機(jī)科學(xué)與技術(shù)系,河南 新鄉(xiāng) 453002； 2.河南師范大學(xué) 計(jì)算機(jī)與信息工程學(xué)院,河南 新鄉(xiāng) 453007；3.96367部隊(duì)，青海 德令哈 817099)

(*通信作者電子郵箱liujun_hbtm@tom.com)

從移動(dòng)機(jī)器人自主導(dǎo)航對(duì)3D地圖精度的需求出發(fā)，在魯棒OctoMap的基礎(chǔ)上提出一種基于完整可見(jiàn)性模型的改進(jìn)魯棒OctoMap并應(yīng)用于基于Kinect的RGB-D同時(shí)定位與地圖創(chuàng)建(SLAM)中。首先，通過(guò)考慮相機(jī)和目標(biāo)體素的相對(duì)位置關(guān)系及地圖分辨率進(jìn)行可連通性判斷，獲得滿足可連通性的相鄰體素的個(gè)數(shù)及位置；其次，根據(jù)不同的可連通性情況分別建立目標(biāo)體素的可見(jiàn)性模型，從而構(gòu)建普適性更強(qiáng)的完整可見(jiàn)性模型，有效克服了魯棒OctoMap可見(jiàn)性模型的局限性，提高了建圖精度；再次，使用基于高斯混合模型的Kinect深度誤差模型代替簡(jiǎn)單深度誤差模型，進(jìn)一步克服傳感器測(cè)量誤差對(duì)地圖精度的影響，降低了地圖的不確定性；最后，結(jié)合貝葉斯公式和線性插值算法來(lái)更新八叉樹(shù)中每個(gè)節(jié)點(diǎn)的實(shí)際占用概率，從而構(gòu)建基于八叉樹(shù)的立體占用地圖。實(shí)驗(yàn)結(jié)果表明，所提方法有效克服了Kinect傳感器深度誤差對(duì)地圖精度的影響，降低了地圖的不確定性，其建圖精度較魯棒OctoMap有明顯的提高。

3D地圖；魯棒OctoMap；高斯混合模型；八叉樹(shù)；立體占用地圖

0 引言

自主導(dǎo)航是移動(dòng)機(jī)器人執(zhí)行各項(xiàng)任務(wù)的基礎(chǔ)和前提。移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航需要預(yù)先知道所在環(huán)境的地圖信息，從而可以進(jìn)行自定位和路徑規(guī)劃。機(jī)器人同時(shí)定位與地圖創(chuàng)建(Simultaneous Localization And Mapping, SLAM)技術(shù)完成了移動(dòng)機(jī)器人對(duì)未知環(huán)境的地圖構(gòu)建。然而，傳統(tǒng)的SLAM方法多是使用激光或聲納進(jìn)行二維地圖的創(chuàng)建[1-4]。隨著機(jī)器人移動(dòng)空間的拓展，出現(xiàn)了空中機(jī)器人和水下機(jī)器人，二維地圖已經(jīng)不能滿足這類機(jī)器人導(dǎo)航的需要。即使是地面移動(dòng)機(jī)器人，有些較為高大的機(jī)器人也需要三維地圖來(lái)判斷環(huán)境的空間結(jié)構(gòu)，從而進(jìn)行精確的導(dǎo)航。因此，三維地圖的精確建立引起了廣大學(xué)者的關(guān)注[5-8]。

目前常用的三維地圖表示方法有點(diǎn)云地圖(Point cloud map)[9-10]、高程地圖(Elevation map)[11-12]和立體占用地圖(Volumetric occupancy map)[13-15]等。其中：點(diǎn)云地圖存儲(chǔ)了所有的空間點(diǎn)坐標(biāo)，其對(duì)硬盤(pán)和內(nèi)存的消耗均較大，且對(duì)于機(jī)器人來(lái)說(shuō)不易區(qū)分障礙和空閑的區(qū)域，難以用于機(jī)器人導(dǎo)航。高程地圖只存儲(chǔ)每一柵格的表面高度，有效克服了點(diǎn)云地圖高消耗的缺點(diǎn)，但其無(wú)法表示環(huán)境中的復(fù)雜結(jié)構(gòu)，因此多適用于室外導(dǎo)航。立體占用地圖多是基于八叉樹(shù)構(gòu)建的，其類似于二維地圖中的柵格地圖，使用立方體的狀態(tài)(空閑，占用，未知)來(lái)表示該立方體中是否有障礙。其中，OctoMap[16-17]作為一種基于八叉樹(shù)的地圖表示方法，建立了體素的占用概率模型，提高了地圖的表示精度，且其地圖壓縮方法極大地減少了地圖對(duì)內(nèi)存和硬盤(pán)的需求，代表了當(dāng)前三維地圖表示方法中的較高水準(zhǔn)。Schauwecker等[18]在OctoMap的基礎(chǔ)上提出了一種基于可見(jiàn)性模型和傳感器深度誤差模型的魯棒OctoMap并應(yīng)用于基于雙目立體相機(jī)的SLAM中，有效克服了傳感器深度誤差對(duì)地圖精度的影響，并通過(guò)與OctoMap比較體現(xiàn)了所提方法的有效性; 然而，魯棒OctoMap的可見(jiàn)性模型未考慮相機(jī)和目標(biāo)體素的相對(duì)位置關(guān)系，具有一定的局限性，會(huì)導(dǎo)致建圖精度不高。

本文針對(duì)魯棒OctoMap可見(jiàn)模型的局限性，提出一種基于完整可見(jiàn)性模型的改進(jìn)魯棒OctoMap并將其應(yīng)用于基于Kinect的RGB-D SLAM中。首先，考慮相機(jī)和目標(biāo)體素的相對(duì)位置關(guān)系及地圖分辨率進(jìn)行可連通性判斷，從而獲得滿足可連通性的相鄰體素的個(gè)數(shù)及位置；其次，針對(duì)可連通性的不同情況，分別建立相機(jī)到目標(biāo)體素的可見(jiàn)性模型，從而構(gòu)建普適性更強(qiáng)的完整可見(jiàn)性模型；再次，使用基于高斯混合模型的Kinect深度誤差模型代替文獻(xiàn)[18]中的簡(jiǎn)單深度誤差模型，進(jìn)一步提高地圖精度；最后，結(jié)合貝葉斯公式和線性插值算法來(lái)更新八叉樹(shù)中每個(gè)節(jié)點(diǎn)的實(shí)際占用概率，最終構(gòu)建基于八叉樹(shù)的立體占用地圖。對(duì)比實(shí)驗(yàn)表明，本文方法的建圖精度較魯棒OctoMap有明顯的提高。

1 魯棒OctoMap可見(jiàn)性模型及其局限性

對(duì)于立體雙目攝像頭和Kinect等傳感器，其深度值均具有較大的誤差，對(duì)所建地圖精度影響較大。在基于八叉樹(shù)的地圖中，部分區(qū)域由于遮擋造成不可見(jiàn)，因此這些區(qū)域的所有測(cè)量值均是有誤差的。針對(duì)這兩種情況，Schauwecker等[18]提出了一種基于可見(jiàn)性模型和傳感器深度誤差模型的魯棒OctoMap并應(yīng)用于基于雙目立體相機(jī)的SLAM中，有效克服了傳感器深度誤差對(duì)地圖精度的影響，提高了地圖的精度。

1.1 魯棒OctoMap可見(jiàn)性模型

基于八叉樹(shù)的地圖表示方法就是使用小立方體的狀態(tài)(空閑、占用、未知)來(lái)表示地圖中的障礙物，其中每一個(gè)小立方體稱為一個(gè)體素。假設(shè)體素v與體素a、b、c相鄰，如圖1所示。如果體素a、b、c狀態(tài)均為占用，則體素v可視為局部閉合，稱該事件為Cv。根據(jù)文獻(xiàn)[18]，事件Cv發(fā)生的概率為:

P(Cv)=min{P(Oa),P(Ob),P(Oc)}

(1)

其中:P(Oa)、P(Ob)、P(Oc)分別表示體素a、b、c被占用的概率。

定義從傳感器發(fā)出的射線R能穿過(guò)一系列體素到達(dá)體素vi為事件Vvi，即表示體素vi可見(jiàn)。根據(jù)文獻(xiàn)[18]，事件Vvi發(fā)生的概率取決于事件Cvi和事件Vvi-1，其中vi-1表示沿射線方向靠近傳感器一側(cè)的體素vi的前一個(gè)體素。另外，假設(shè)體素vi-1不可見(jiàn)則vi必然不可見(jiàn)，于是P(Vvi)的計(jì)算公式如下：

(2)

圖1 體素相鄰模型Fig. 1 Neighboring voxel model

1.2 可見(jiàn)性模型的局限性分析

Schauwecker等[18]雖然考慮了體素的可見(jiàn)性對(duì)地圖精度的影響，但其可見(jiàn)性模型并未考慮相機(jī)與體素v的相對(duì)位置關(guān)系，導(dǎo)致計(jì)算出來(lái)的P(Vvi)是不準(zhǔn)確的。下面舉例說(shuō)明該可見(jiàn)性模型的局限性。

如圖2，假設(shè)相機(jī)位置為點(diǎn)o，且P(Oa)=1，P(Ob)=0，P(Oc)=0，即體素a完全占用，體素b和c完全空閑, 則根據(jù)式(2)可以得出P(Vvi)>0，即表示體素v部分可見(jiàn)。而實(shí)際情況如圖2所示，體素a完全占用，因此射線無(wú)法穿過(guò)體素a到達(dá)體素v。而射線從點(diǎn)o發(fā)出，沿直線經(jīng)過(guò)體素b或c根本無(wú)法到達(dá)體素v。因此，實(shí)際情況表示體素v不可見(jiàn)，即P(Vv)=0。

圖2 魯棒OctoMap可見(jiàn)性模型局限性分析示意圖Fig. 2 Sketch diagram of limitations of visibility model of robust OctoMap

通過(guò)分析可以看出，文獻(xiàn)[18]中的可見(jiàn)性模型沒(méi)有考慮相機(jī)與體素v的相對(duì)位置，不能準(zhǔn)確表達(dá)體素v的可見(jiàn)性，因此該模型具有一定的局限性，會(huì)對(duì)地圖精度造成較大的影響。

2 改進(jìn)魯棒OctoMap

本文針對(duì)Schauwecker等[18]的可見(jiàn)性模型的局限性，考慮相機(jī)與體素v的相對(duì)位置關(guān)系及地圖分辨率，提出了一種完整可見(jiàn)性模型，可以準(zhǔn)確描述體素v的可見(jiàn)性。此外，針對(duì)Kinect傳感器的特點(diǎn)，采用基于高斯混合模型的深度誤差模型代替文獻(xiàn)[18]中的深度誤差模型，更準(zhǔn)確地表達(dá)Kinect傳感器的深度值?；谝陨蟽牲c(diǎn)，本文提出了一種基于完整可見(jiàn)模型和Kinect深度誤差模型的改進(jìn)魯棒OctoMap，以克服傳感器深度誤差對(duì)地圖精度的影響，進(jìn)一步提高建圖的精度，算法流程如圖3所示。

2.1 可連通性判斷

完整可見(jiàn)性模型充分考慮了相機(jī)和目標(biāo)體素的相對(duì)位置，有效克服了魯棒OctoMap中可見(jiàn)性模型的局限性，其關(guān)鍵點(diǎn)就是在建立可見(jiàn)性模型之前先對(duì)目標(biāo)體素、相鄰體素以及相機(jī)三者進(jìn)行可連通性判斷。若從相機(jī)發(fā)出的射線能夠穿過(guò)目標(biāo)體素的相鄰體素而到達(dá)目標(biāo)體素，則稱目標(biāo)體素、相鄰體素和相機(jī)三者可連通；反之，則稱三者不可連通。

圖3 算法流程Fig. 3 Algorithm flow

根據(jù)傳感器和目標(biāo)體素的相對(duì)位置并考慮地圖分辨率，可以確定滿足可連通性的相鄰體素的個(gè)數(shù)及其位置，總體上分為三種情況，如圖4所示,其對(duì)應(yīng)的滿足可連通性的相鄰體素的個(gè)數(shù)分別為1，2，3。

圖4 可連通性示意圖Fig. 4 Sketch map of connectivity

假設(shè)目標(biāo)體素的中心坐標(biāo)為(xv,yv,zv)，相機(jī)的坐標(biāo)為(xo,yo,zo)，地圖分辨率為r，則可通過(guò)判斷三者之間的關(guān)系確定相機(jī)和目標(biāo)體素之間的可連通性情況，從而確定滿足可連通性的相鄰體素的個(gè)數(shù)及其位置。

情況1 若(xv,yv,zv)、(xo,yo,zo)和r滿足式(3)～(5)其中之一，則可以判定相機(jī)和目標(biāo)體素之間的可連通性為圖4(a)。

(3)

(4)

(5)

情況2 若(xv,yv,zv)、(xo,yo,zo)和r滿足式(6)～(8)其中之一，則可以判定相機(jī)和目標(biāo)體素之間的可連通性為圖4(b)。

(6)

(7)

(8)

情況3 若(xv,yv,zv)、(xo,yo,zo)和r滿足式(9)，則可以判定相機(jī)和目標(biāo)體素之間的可連通性為圖4(c)。

(9)

判斷完相機(jī)和目標(biāo)體素的可連通性情況之后，其對(duì)應(yīng)的滿足可連通性的相鄰體素的個(gè)數(shù)也就隨之確定了。然后根據(jù)(xv,yv,zv)、(xo,yo,zo)和r的具體關(guān)系可以判斷目標(biāo)體素的位置。

2.2 完整可見(jiàn)性模型

通過(guò)判斷目標(biāo)體素的可連通性可知，目標(biāo)體素的可見(jiàn)性取決于滿足可連通性的相鄰體素的可見(jiàn)性和占用狀態(tài)。當(dāng)滿足可連通性的相鄰體素可見(jiàn)且狀態(tài)為空閑時(shí)，目標(biāo)體素才可見(jiàn)。因此本文根據(jù)可連通性的情況分類，建立了完整可見(jiàn)性模型。

情況1 當(dāng)可連通性滿足圖4(a)時(shí)，只有一個(gè)相鄰體素滿足可連通性條件，體素v可見(jiàn)就意味著體素a可見(jiàn)且狀態(tài)為空閑，因?yàn)轶w素a的可見(jiàn)性與其狀態(tài)是相互獨(dú)立的，所以體素v的可見(jiàn)性概率P(Vv)可通過(guò)式(10)計(jì)算獲得：

(10)

情況2 當(dāng)可連通性滿足圖4(b)時(shí)，有兩個(gè)相鄰體素滿足可連通性條件，體素v可見(jiàn)就意味著體素a、b中至少有一個(gè)可見(jiàn)且狀態(tài)為空閑，因?yàn)轶w素a、b的可見(jiàn)性與其狀態(tài)是相互獨(dú)立的，所以體素v的可見(jiàn)性概率P(Vv)可通過(guò)式(11)計(jì)算獲得：

(11)

情況3 當(dāng)可連通性滿足圖4(c)時(shí)，有三個(gè)相鄰體素滿足可連通性條件，體素v可見(jiàn)就意味著體素a、b、c中至少有一個(gè)可見(jiàn)且狀態(tài)為空閑，因?yàn)轶w素a、b、c的可見(jiàn)性與其狀態(tài)是相互獨(dú)立的，所以體素v的可見(jiàn)性概率P(Vv)可通過(guò)式(12)計(jì)算獲得：

(12)

2.3 基于高斯混合模型的Kinect傳感器深度誤差模型

Schauwecker等[18]建立雙目立體相機(jī)的深度誤差模型，即深度誤差跟深度值的平方成正比，該模型同樣適用于Kinect傳感器。根據(jù)文獻(xiàn)[19]的研究，Kinect深度值的標(biāo)準(zhǔn)差與其期望存在下列關(guān)系：假設(shè)各像素點(diǎn)的深度值z(mì)相互獨(dú)立，則z的不確定性為:

(13)

實(shí)際上，各像素的深度值不一定相互獨(dú)立，因此式(13)具有一定的不合理性。Dryanovski等[20]提出一種基于高斯混合模型的Kinect傳感器深度誤差模型，且在文獻(xiàn)[20]中驗(yàn)證了該模型相對(duì)于式(13)的簡(jiǎn)單深度誤差模型具有更高的可靠性;尤其是在物體邊緣，基于高斯混合模型的深度誤差模型表現(xiàn)出突出的優(yōu)勢(shì)。

因此，本文采用基于高斯混合模型的Kinect傳感器深度誤差模型來(lái)代替文獻(xiàn)[18]中的簡(jiǎn)單深度誤差模型。

假設(shè)1 Kinect深度值z(mì)滿足下列關(guān)系：

2)假設(shè)各像素點(diǎn)深度值與其相鄰像素點(diǎn)深度值有關(guān)。

當(dāng)系統(tǒng)滿足假設(shè)1時(shí)，z的不確定性可以根據(jù)高斯混合模型估計(jì)得到。本文采用如下高斯核

(14)

可以得出Kinect深度圖中像素(m,n)處z的均值和方差為：

(15)

i∈[m-1,m+1],j∈[n-1,n+1]

其中:(m,n)表示像素坐標(biāo)，μ表示均值，σ2表示方差。

2.4 實(shí)際占用概率計(jì)算

要構(gòu)建基于八叉樹(shù)的三維地圖，需要計(jì)算每個(gè)體素的實(shí)際占用概率P(Ov)。文獻(xiàn)[16-17]提出的OctoMap是基于概率表示的八叉樹(shù)，具有可更新性。本文在完整可見(jiàn)性模型和Kinect深度誤差模型的基礎(chǔ)上，結(jié)合貝葉斯公式和線性插值算法來(lái)更新八叉樹(shù)中每個(gè)節(jié)點(diǎn)的實(shí)際占用概率。

對(duì)于Kinect傳感器的一條射線R的反射點(diǎn)q=(x,y,z)，則射線R路過(guò)的體素的實(shí)際占用概率都要更新。對(duì)于遠(yuǎn)離q點(diǎn)的體素和靠近q點(diǎn)的體素分別采用貝葉斯公式和線性插值的方法進(jìn)行更新。

對(duì)于遠(yuǎn)離q點(diǎn)的體素，實(shí)際占用概率更新方法采用貝葉斯公式，即:

(16)

(17)

(18)

(19)

其中，P(Iv)表示點(diǎn)q在障礙物內(nèi)部的概率，由文獻(xiàn)[18]知，根據(jù)q點(diǎn)的深度值及其不確定性可預(yù)先制作一個(gè)P(Iv)的離散表，從而將其作為一個(gè)先驗(yàn)概率。

當(dāng)SLAM過(guò)程完成時(shí)，將通過(guò)式(18)和式(19)計(jì)算的八叉樹(shù)每一節(jié)點(diǎn)的實(shí)際占用概率作為最終的占用概率，從而構(gòu)建立體占用地圖。

3 實(shí)驗(yàn)與分析

本文以一個(gè)模擬室內(nèi)場(chǎng)景為實(shí)際場(chǎng)景，如圖5所示。使用現(xiàn)有RGB-D SLAM算法進(jìn)行建圖，分別使用點(diǎn)云地圖(圖6)、魯棒OctoMap(圖7)和改進(jìn)魯棒OctoMap(圖8)來(lái)表示地圖。其中，本文實(shí)驗(yàn)中所用參數(shù)和魯棒OctoMap所用參數(shù)如表1所示。

圖5 模擬室內(nèi)場(chǎng)景實(shí)物圖Fig. 5 Physical map of simulated indoor scene

圖6 模擬室內(nèi)場(chǎng)景點(diǎn)云圖Fig. 6 Point cloud map of simulated indoor scene

表1中魯棒OctoMap的參數(shù)選取參考文獻(xiàn)[18]，而改進(jìn)魯棒OctoMap的實(shí)驗(yàn)參數(shù)是根據(jù)經(jīng)驗(yàn)調(diào)試獲得的。

表1 實(shí)驗(yàn)參數(shù)表Tab. 1 Table of parameters in the experiments

圖7 模擬室內(nèi)場(chǎng)景魯棒OctoMapFig. 7 Robust OctoMap of simulated indoor scene

圖8 模擬室內(nèi)場(chǎng)景改進(jìn)魯棒OctoMapFig. 8 Improved robust OctoMap of simulated indoor scene

圖7和圖8中，藍(lán)色的點(diǎn)表示占用概率P(Ov)接近0.5，即在該體素地圖的不確定性較大，深色的點(diǎn)表示確定的已占用體素，即P(Ov)接近1。通過(guò)對(duì)比圖7和圖8(a)可以看出，改進(jìn)魯棒OctoMap較魯棒OctoMap淺色的點(diǎn)明顯減少，極大地降低了地圖的不確定性，提高了地圖的精度，驗(yàn)證了高斯混合誤差模型的有效性。其中，淺色的點(diǎn)在物體邊緣的對(duì)比效果更加突出，這與文獻(xiàn)[20]的結(jié)論是一致的。

目前針對(duì)地圖的確定性問(wèn)題，還沒(méi)有公認(rèn)的定量描述形式。為了更精確地描述本文對(duì)地圖確定性的提高，本文提出一個(gè)定量描述地圖確定性的公式：

(20)

其中:P(Ovi)表示體素vi的占用概率，N表示地圖中的體素個(gè)數(shù)，Q表示評(píng)價(jià)結(jié)果，其取值范圍為0～1。Q越接近1，表示地圖的確定性越高。

使用上述評(píng)價(jià)方法對(duì)魯棒OctoMap和圖8(a)中的改進(jìn)魯棒OctoMap的評(píng)價(jià)結(jié)果分別為0.72和0.86，可以看出改進(jìn)魯棒OctoMap較魯棒OctoMap確定性提高了19.4%。

從實(shí)物圖中可以看出，建圖過(guò)程中部分區(qū)域被遮擋(黑色方框中標(biāo)定板背面區(qū)域)，標(biāo)定板后方區(qū)域?yàn)椴豢梢?jiàn)，而圖7和圖8(a)中顯示背后仍有部分區(qū)域可見(jiàn)，足以說(shuō)明該可見(jiàn)性模型的局限性，而圖8(b)中使用完整可見(jiàn)性模型的改進(jìn)魯棒OctoMap標(biāo)定板背后幾乎沒(méi)有可見(jiàn)區(qū)域，表明本文的完整可見(jiàn)性模型更加精確，可適用于任意遮擋情況。

圖8(c)是同時(shí)使用完整可見(jiàn)性模型和高斯混合誤差模型的最終建圖效果，可以看出地圖不確定性較低，且其遮擋問(wèn)題也得到了較好的解決，地圖精度較高，驗(yàn)證了本文方法的可行性和有效性。

本文增加了可連通性判斷，且在建立可見(jiàn)性模型中情況3的計(jì)算量也較改進(jìn)前有所增加。為檢驗(yàn)本文算法的實(shí)時(shí)性，對(duì)魯棒OctoMap和改進(jìn)魯棒OctoMap進(jìn)行耗時(shí)比較。經(jīng)過(guò)測(cè)量，該場(chǎng)景體積為74.8 m3，本文采用地圖分辨率為1 cm3，則共有體素7.48×107個(gè)。本文采用筆記本配置為i3處理器，4 GB內(nèi)存。經(jīng)測(cè)試，改進(jìn)前魯棒OctoMap耗時(shí)52.8 s，改進(jìn)魯棒OctoMap耗時(shí)57.6 s，耗時(shí)稍有增加但對(duì)建圖的實(shí)時(shí)性影響不大。

4 結(jié)語(yǔ)

本文從移動(dòng)機(jī)器人自主導(dǎo)航中對(duì)3D地圖精度的需求出發(fā)，在Schauwecker等[18]魯棒OctoMap的基礎(chǔ)上，提出了一種基于完整可見(jiàn)性模型的改進(jìn)魯棒OctoMap并應(yīng)用于基于Kinect的RGB-D SLAM中。一方面，本文通過(guò)考慮相機(jī)和目標(biāo)體素的相對(duì)位置關(guān)系及地圖分辨率，從而分情況建立了完整可見(jiàn)性模型，克服了魯棒OctoMap中可見(jiàn)性模型的局限性。另一方面，本文采用基于高斯混合模型的Kinect深度誤差模型代替文獻(xiàn)[18]中的簡(jiǎn)單深度誤差模型，從而有效降低了地圖的不確定性。實(shí)驗(yàn)表明，本文的改進(jìn)魯棒OctoMap所建地圖較原有方法具有更高的精度。

References)

[1] KOCH R, MAY S, MURMANN P, et al. Identification of transparent and specular reflective material in laser scans to discriminate affected measurements for faultless robotic SLAM[J]. Robotics and Autonomous Systems, 2016, 87: 296-312.

[2] 宋宇, 李慶玲, 康軼非, 等. 平方根容積 Rao-Blackwillised 粒子濾波 SLAM 算法[J]. 自動(dòng)化學(xué)報(bào), 2014, 40(2): 357-367.(SONG Y, LI Q L, KANG Y F, et al. SLAM with square-root cubature Rao-Blackwillised particle filter[J]. Acta Automatica Sinica, 2014, 40(2): 357-367.)

[3] FOSSEL J D, TUYLS K, STURM J. 2D-SDF-SLAM: a signed distance function based SLAM frontend for laser scanners[C]// Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2015: 1949-1955.

[4] 張文玲, 朱明清, 陳宗海. 基于強(qiáng)跟蹤 UKF 的自適應(yīng) SLAM 算法[J]. 機(jī)器人, 2010, 32(2): 190-195.(ZHANG W L, ZHU M Q, CHEN Z H. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2): 190-195.)

[5] 張毅, 汪龍峰, 余佳航. 基于深度信息的移動(dòng)機(jī)器人室內(nèi)環(huán)境三維地圖創(chuàng)建[J]. 計(jì)算機(jī)應(yīng)用, 2014, 34(12): 3438-3440.(ZHANG Y, WANG L F, YU J H. Depth-image based 3D map reconstruction of indoor environment for mobile robots[J]. Journal of Computer Applications, 2014, 34(12): 3438-3440.)

[6] HENRY P, KRAININ M, HERBST E, et al. RGB-D mapping: using depth cameras for dense 3D modeling of indoor environments[C]// Proceedings of the 12th International Symposium on Experimental Robotics. Berlin: Springer, 2010: 22-25.

[7] 王可, 賈松敏, 徐濤, 等. 基于混合位姿估計(jì)模型的移動(dòng)機(jī)器人三維地圖創(chuàng)建方法[J]. 控制與決策, 2015, 30(8): 1504-1508.(WANG K, JIA S M, XU T, et al. Mobile robot 3D map building based on hybrid pose estimation model[J]. Control and Decision, 2015, 30(8): 1504-1508.)

[8] BRIALES J, GONZLEZ-JIMéNEZ J. Fast global optimality verification in 3D SLAM[EB/OL].[2016-06-20].http://www.riuma.uma.es/xmlui/bitstream/handle/10630/12249/main.pdf?sequence=3.

[9] ENGELHARD N, ENDRES F, HESS J, et al. Real-time 3D visual SLAM with a hand-held RGB-D camera[EB/OL].[2016-06-20].http://vision.in.tum.de/_media/spezial/bib/engelhard11euron.pdf.

[10] FIORAIO N, KONOLIGE K. Real-time visual and point cloud SLAM[EB/OL].[2016-06-20].http://dosen.narotama.ac.id/wp-content/uploads/2015/02/Realtime-visual-and-point-cloud-slam.pdf.

[11] HERBERT M, CAILLAS C, KROTKOV E, et al. Terrain mapping for a roving planetary explorer[C]// Proceedings of the 1989 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 1989: 997-1002.

[12] BAIRD C A. Elevation map-referenced mechanism for updating vehicle navigation system estimates: U.S. Patent 4939663[P]. 1990-07-03.

[13] MARTIN C, THRUN S. Real-time acquisition of compact volumetric 3D maps with mobile robots[C]// Proceedings of the 2002 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2002: 311-316.

[14] LI X, GUO X, WANG H, et al. Harmonic volumetric mapping for solid modeling applications[C]// Proceedings of the 2007 ACM Symposium on Solid and Physical Modeling. New York: ACM, 2007: 109-120.

[15] PAILLé G P, POULIN P. As-conformal-as-possible discrete volumetric mapping[J]. Computers & Graphics, 2012, 36(5): 427-433.

[16] WURM K M, HORNUNG A, BENNEWITZ M, et al. OctoMap: a probabilistic, flexible, and compact 3D map representation for robotic systems[EB/OL].[2016-06-20].http://first-mm.eu/files/wurm10octomap.pdf.

[17] HORNUNG A, WURM K M, BENNEWITZ M, et al. OctoMap: An efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous Robots, 2013, 34(3): 189-206.

[18] SCHAUWECKER K, ZELL A. Robust and efficient volumetric occupancy mapping with an application to stereo vision[C]// Proceedings of the 2014 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2014: 6102-6107.

[19] KHOSHELHAM K, ELBERINK S O. Accuracy and resolution of Kinect depth data for indoor mapping applications[J]. Sensors, 2012, 12(2): 1437-1454.

[20] DRYANOVSKI I, VALENTI R G, XIAO J. Fast visual odometry and mapping from RGB-D data[C]// Proceedings of the 2013 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2013: 2305-2310.

This work is partially suppotted by the National Natural Science Foundation of China (U1404602), the Funded Project of Key Teachers in Colleges and Universities of Henan Province (2011GGJS-198)，the Science and Technology key research project of Education Department of Henan Province (14A520045) .

LIU Jun，born in 1981， M.S., lecturer. His research interests include mobile robot simultaneous localization and mapping.

YUAN Peiyan, born in 1978, Ph.D., associate professor. His research interests include mobile opportunistic network.

LI Yongfeng, born in 1989, M.S. . His research interests include mobile robot simultaneous localization and mapping.

Improved robust OctoMap based on full visibility model

LIU Jun1*, YUAN Peiyan2, LI Yongfeng3

(1.DepartmentofComputerScience&Technology,HenanInstituteofTechnology,XinxiangHenan453002,China;2.CollegeofComputer&InformationEngineering,HenanNormalUniversity,XinxiangHenan453007,China;3.Troops96367,DelinghaQinghai817099,China)

An improved robust OctoMap based on full visibility model was proposed to meet accuracy needs of 3D map for mobile robot autonomous navigation and it was applied to the RGB-D SLAM (Simultaneous Localization And Mapping) based on Kinect. First of all, the connectivity was judged by considering the the relative positional relationship between the camera and the target voxel and the map resolution to get the number and the location of adjacent voxels which met connectivity. Secondly, according to the different connectivity, the visibility model of the target voxel was built respectively to establish the full visibility model which was more universal. The proposed model could effectively overcome the limitations of the robust OctoMap visibility model, and improve the accuracy. Next, the simple depth error model was replaced by the Kinect sensor depth error model based on Gaussian mixture model to overcome the effect of the sensor measurement error on the accuracy of map further and reduce the uncertainty of the map. Finally, the Bayesian formula and linear interpolation algorithm were combined to update the occupancy probability of each node in the octree to build the volumetric occupancy map based on a octree. The experimental results show that the proposed method can effectively overcome the influence of Kinect sensor depth error on map precision and reduce the uncertainty of the map, and the accuracy of map is improved obviously compared with the robust OctoMap.

3D map; robust OctoMap; Gaussian mixture model; octree; volumetric occupancy map

2016-10-13;

2017-01-10。 基金項(xiàng)目:國(guó)家自然科學(xué)基金資助項(xiàng)目(U1404602);河南省高等學(xué)校青年骨干教師計(jì)劃項(xiàng)目(2011GGJS-198);河南省教育廳科學(xué)技術(shù)重點(diǎn)研究項(xiàng)目(14A520045)。

劉晙(1981—),男,湖北天門(mén)人,講師,碩士,主要研究方向:移動(dòng)機(jī)器人視覺(jué)定位與地圖創(chuàng)建； 袁培燕(1978—),男,河南鄧州人,副教授, 博士,CCF會(huì)員,主要研究方向: 移動(dòng)機(jī)會(huì)網(wǎng)絡(luò)； 李永鋒(1989—),男,河南焦作人,碩士,主要研究方向：移動(dòng)機(jī)器人視覺(jué)定位與地圖創(chuàng)建。

1001-9081(2017)05-1445-06

10.11772/j.issn.1001-9081.2017.05.1445

TP242

A