文章編號(hào)： 1006-9798（2024）03-0001-06； DOI： 10.13306/j.1006-9798.2024.03.001

摘要： 針對(duì)單一傳感器建圖時(shí)還原度較低，易受環(huán)境干擾及可靠性不足等問(wèn)題，提出一種任務(wù)層面融合激光雷達(dá)與深度相機(jī)的建圖方法。采用一種針對(duì)走廊環(huán)境的組合式對(duì)齊算法對(duì)地圖進(jìn)行對(duì)齊，利用基于貝葉斯推理的方法進(jìn)行信息融合。為驗(yàn)證融合建圖效果，通過(guò)搭建多樣化虛擬場(chǎng)景進(jìn)行仿真和對(duì)真實(shí)走廊環(huán)境進(jìn)行地圖融合實(shí)驗(yàn)。結(jié)果表明，經(jīng)過(guò)融合的柵格地圖比單一傳感器創(chuàng)建的柵格地圖還原度更高，有效提高了地圖的精度，得到了更能反映真實(shí)物理環(huán)境信息的柵格地圖，為后續(xù)的路徑規(guī)劃提供了準(zhǔn)確可靠的環(huán)境地圖。

關(guān)鍵詞： 融合建圖； 柵格地圖建立； 激光雷達(dá)； 深度相機(jī)

中圖分類號(hào)： TP391文獻(xiàn)標(biāo)識(shí)碼： A

同步定位與地圖構(gòu)建技術(shù)（Simultaneous Localization and Mapping，SLAM）[1]是利用移動(dòng)機(jī)器人自身攜帶的傳感器在陌生環(huán)境中同時(shí)定位與構(gòu)建周圍環(huán)境地圖[2-3]。目前，激光SLAM[4]與視覺(jué)SLAM[5]研究均取得較大進(jìn)展，然而2種方法仍存在各自的局限性和缺點(diǎn)，因此多傳感器融合的SLAM系統(tǒng)成為移動(dòng)機(jī)器人的研究熱點(diǎn)。激光與視覺(jué)融合建圖按照融合層面可分為數(shù)據(jù)層面融合和任務(wù)層面融合。數(shù)據(jù)層面融合主要是將激光雷達(dá)掃描點(diǎn)云與相機(jī)點(diǎn)云通過(guò)數(shù)據(jù)處理、特征提取等方式進(jìn)行匹配和對(duì)齊，達(dá)到充分利用兩種傳感器信息。任務(wù)層面融合過(guò)程為激光雷達(dá)和相機(jī)分別構(gòu)建出場(chǎng)景的點(diǎn)云地圖或柵格地圖，匹配對(duì)齊后對(duì)兩者構(gòu)建的地圖進(jìn)行融合。基于自適應(yīng)蒙特卡羅定位（Adaptive Monte Carlo Localization，AMCL）的方法對(duì)齊由異構(gòu)傳感器構(gòu)建的占用柵格地圖[6]；激光雷達(dá)與深度相機(jī)融合建圖方法，通過(guò)貝葉斯公式更新融合地圖中柵格的概率融合兩者地圖信息[7]。任務(wù)層面的融合在決策過(guò)程中有效的利用來(lái)自不同傳感器的信息，可以在導(dǎo)航任務(wù)中更好的考慮環(huán)境的復(fù)雜性，并可減少算法對(duì)重復(fù)信息的處理，因此本文在以上學(xué)者的基礎(chǔ)上主要研究任務(wù)層面上的激光與視覺(jué)融合建圖。在地圖對(duì)齊中使用改進(jìn)的BRIEF特征作為內(nèi)角點(diǎn)描述子，并為其添加旋轉(zhuǎn)不變性。在信息融合中充分考慮兩種傳感器的建圖特點(diǎn)制定新的融合規(guī)律進(jìn)行地圖融合，從而生成最終的真實(shí)場(chǎng)景地圖。利用仿真和實(shí)驗(yàn)驗(yàn)證多傳感器融合建圖方法的可靠性和準(zhǔn)確性。

1地圖融合研究

地圖融合的關(guān)鍵步驟是地圖對(duì)齊和信息融合[8]。地圖對(duì)齊的目的是使2幅地圖具有相同的地圖格式、尺度以及分辨率[9]，通過(guò)一些對(duì)齊算法[10-11]得出子地圖之間的坐標(biāo)轉(zhuǎn)換，為之后的信息關(guān)聯(lián)奠定基礎(chǔ)。信息關(guān)聯(lián)是在子地圖對(duì)齊后，將地圖之間的信息進(jìn)行合并的過(guò)程，生成最終的場(chǎng)景地圖。地圖融合流程如圖1所示。

1.1地圖對(duì)齊

本文提出一種針對(duì)走廊環(huán)境的組合式對(duì)齊算法，輸入子地圖后，統(tǒng)一尺度及分辨率，提取Harris角點(diǎn)并篩選出內(nèi)角點(diǎn)，為角點(diǎn)添加主方向使之具有旋轉(zhuǎn)不變性，并添加描述子，通過(guò)描述子以及點(diǎn)對(duì)距離匹配子地圖之間的角點(diǎn)，匹配成功后計(jì)算出子地圖之間的變換矩陣。

Harris角點(diǎn)檢測(cè)是一種點(diǎn)特征提取算法，核心思想是利用局部窗口在圖像上移動(dòng)來(lái)觀察灰度值的變化，如果梯度圖上的窗口內(nèi)灰度變化較大，則窗口區(qū)域就存在角點(diǎn)。對(duì)于每個(gè)窗口，根據(jù)式（1）計(jì)算其對(duì)應(yīng)的角點(diǎn)響應(yīng)函數(shù)R，如果R大于設(shè)定閾值，則表示該窗口對(duì)應(yīng)一個(gè)角點(diǎn)特征。

R=det（M）-k（trace（M））2（1）

其中，det（M）為矩陣M的行列式的值；k是調(diào)節(jié)響應(yīng)函數(shù)敏感度的常數(shù)；trace（dyWgZILhp4zT6UhKmJgl0w==M）為矩陣對(duì)角線的和。

當(dāng)?shù)貓D分辨率不同時(shí)，角點(diǎn)響應(yīng)值會(huì)發(fā)生變化，因此需要為Harris角點(diǎn)添加尺度不變性。通過(guò)建立多尺度空間，將每個(gè)位置特征賦予不同尺度的表示，找到能夠使角點(diǎn)響應(yīng)最顯著的尺度。添加尺度不變性后的Harris二階矩為

M=μ（x，y，σD，σI）=σ2Dg（σI）I2x（x，y，σD）IxIy（x，y，σD）IxIy（x，y，σD）I2y（x，y，σD）（2）

其中，Ix，Iy是圖像在x，y方向的梯度圖；σD和σI分別為微分尺度窗口與積分尺度窗口。

通過(guò)建立多尺度空間σn=σI，kσI，…knσI，在每個(gè)尺度空間下計(jì)算角點(diǎn)響應(yīng)值，并在大于閾值的候選角點(diǎn)8領(lǐng)域內(nèi)進(jìn)行非極大值抑制，在不同尺度的該候選角點(diǎn)對(duì)應(yīng)位置下進(jìn)行拉普拉斯響應(yīng)LoG的計(jì)算

|LoG（x，σn）|=σ2n|Ixx（x，σn）+Iyy（x，σn）|（3）

在相鄰尺度空間內(nèi)對(duì)比LoG值[12]

|LoG（x，σn）|>|LoG（x，σi）|， i∈｛n-1，n+1｝（4）

當(dāng)滿足式（4）時(shí)，保留此候選角點(diǎn)，直到找到不同尺度下該位置的最大LoG值，作為該角點(diǎn)最佳尺度。通過(guò)Harris角點(diǎn)檢測(cè)提取圖像角點(diǎn)，并不是所有角點(diǎn)都可以作為有效信息使用，只需要在各子地圖中都能檢測(cè)出來(lái)且相似度較高的角點(diǎn)，角點(diǎn)太多影響匹配的效率以及精度，因此需要對(duì)角點(diǎn)進(jìn)行篩選。將處于墻角位置的角點(diǎn)定義為內(nèi)角點(diǎn)，將以內(nèi)角點(diǎn)為中心，半徑為r的圓域內(nèi)各柵格狀態(tài)區(qū)域占比作為判斷條件，對(duì)角點(diǎn)進(jìn)行篩選。使用灰度質(zhì)心法為內(nèi)角點(diǎn)添加主方向，使之具有旋轉(zhuǎn)不變性[13]。

1）以角點(diǎn)位置為幾何中心O的圖像塊B中，定義圖像塊的矩

mpq=∑x，y∈BxpyqI（x，y）p，q={0，1}（5）

2）通過(guò)圖像矩找到圖像塊B的質(zhì)心

C=（cx，cy）=m10m00，m01m00（6）

3）連接角點(diǎn)位置O與質(zhì)心C，得到方向向量OC，角點(diǎn)主方向定義為

θ=arctan 2（cy，cx）=arctan 2（m01，m10）（7）

通過(guò)以上步驟，內(nèi)角點(diǎn)便具有了旋轉(zhuǎn)的描述。對(duì)每個(gè)點(diǎn)計(jì)算描述子，本文使用改進(jìn)的BRIEF特征作為內(nèi)角點(diǎn)描述子。改進(jìn)的BRIEF利用特征點(diǎn)主方向信息使描述子具有較好的旋轉(zhuǎn)不變性。

在角點(diǎn)對(duì)匹配階段，對(duì)子圖內(nèi)部的各內(nèi)角點(diǎn)進(jìn)行兩兩組合并計(jì)算之間的歐氏距離，然后取子圖間組合點(diǎn)對(duì)距離差距最小的兩個(gè)點(diǎn)對(duì)，計(jì)算其點(diǎn)對(duì)間的描述子距離，本文使用二進(jìn)制描述子，因此采用漢明距離（Hamming Distance）作為度量，由于計(jì)算的是點(diǎn)對(duì)與點(diǎn)對(duì)的描述子距離，兩點(diǎn)之間的先后順序可能導(dǎo)致距離變化，因此對(duì)兩點(diǎn)之間交換順序后再計(jì)算一次漢明距離，即對(duì)每?jī)蓚€(gè)點(diǎn)對(duì)計(jì)算2次漢明距離，取其中最小值為最終描述子距離。如果點(diǎn)對(duì)歐式距離以及漢明距離差值在一定范圍內(nèi)，即視為點(diǎn)對(duì)匹配成功；否則將重新篩選內(nèi)角點(diǎn)。

內(nèi)角點(diǎn)對(duì)成功匹配后，設(shè)子圖a和子圖b中匹配內(nèi)角點(diǎn)位置分別為ma=[xa，ya，1]T，mb=[xa，ya，1]T，則兩子圖之間的映射關(guān)系為[14]

ma=λRmb+l（8）

其中，λ為尺度變化，可由匹配點(diǎn)對(duì)間的距離比求出；R為旋轉(zhuǎn)矩陣；l為平移矩陣。

1.2信息融合

地圖對(duì)齊后得出子圖之間的映射關(guān)系，需要依靠信息融合部分將子圖間的數(shù)據(jù)進(jìn)行結(jié)合。本文提出一種基于貝葉斯推理來(lái)融合柵格信息的方法，使融合后的地圖更反映真實(shí)情況。

貝葉斯推理用于根據(jù)已知的觀察結(jié)果和先驗(yàn)知識(shí)，計(jì)算出未知變量的后驗(yàn)概率分布。假設(shè)在狀態(tài)空間中，t時(shí)刻的概率為p（xt），觀測(cè)數(shù)據(jù)為Zt={z1，z2，…，zt}，則后驗(yàn)概率為

p（xt|Zt）=p（zt|xt）p（xt|Zt-1）p（Zt|Zt-1）（9）

其中，p（Zt|Zt-1）是一種規(guī)格化術(shù)語(yǔ)，保證概率密度函數(shù)歸一化；p（zt|xt）表示測(cè)量模型的似然函數(shù)；p（xt|Zt-1）表示在t-1時(shí)刻的先驗(yàn)概率。設(shè)O表示激光雷達(dá)測(cè)得柵格被占據(jù)，表示柵格未被占據(jù)，E為相機(jī)觀測(cè)柵格被占據(jù)，表示未被占據(jù)。根據(jù)貝葉斯定理得出后驗(yàn)概率為

p（E|O）=p（O|E）p（E）p（O|E）p（E）+p（O|）p（）（10）

p（E|）=p（|E）p（E）p（|E）p（E）+p（|）p（）（11）

其中，P（E）表示先驗(yàn)概率；p（O|E）表示觀測(cè)模型，p（）=1-p（E），融合后的柵格概率為

p=papbpapb+（1-pa）（1-pb）（12）

其中，pa，pb分別表示激光雷達(dá)與深度相機(jī)所建地圖中柵格處于當(dāng)前狀態(tài)的先驗(yàn)概率；p表示融合后的柵格概率。通過(guò)貝葉斯推理融合子圖數(shù)據(jù)，會(huì)產(chǎn)生除pfree、pocc、pun之外其它的概率柵格，這些額外的概率柵格可能會(huì)造成地圖信息表達(dá)不準(zhǔn)確，為了消除這種影響，本文將額外的概率柵格以最接近的狀態(tài)表示出來(lái)，保證融合后的柵格種類與融合前的相同，使融合后的地圖表達(dá)的更為清晰準(zhǔn)確

pf=argp′ minp′-pp′，p′∈{pfree，pocc，pun}（13）

其中，pf為最終的柵格概率;p為通過(guò)貝葉斯推理融合后的柵格概率。

考慮到2種傳感器的建圖特點(diǎn)，本文制定了一種融合規(guī)則，見(jiàn)表1，其中大部分規(guī)則與改進(jìn)后的貝葉斯推理融合結(jié)果相同，個(gè)別規(guī)則（如激光雷達(dá)柵格概率為未知時(shí)，融合結(jié)果為未知）通過(guò)融合結(jié)果與真實(shí)場(chǎng)景對(duì)比后進(jìn)行的修正，使其符合實(shí)際情況。

2地圖融合仿真與實(shí)驗(yàn)驗(yàn)證

2.1仿真驗(yàn)證

為了檢驗(yàn)地圖融合方法的有效性和準(zhǔn)確性，針對(duì)室內(nèi)走廊環(huán)境，分別對(duì)Y型和Z型走廊做了仿真場(chǎng)景，如圖2所示，每種場(chǎng)景分別構(gòu)建了激光柵格地圖與視覺(jué)柵格地圖，仿真結(jié)果如圖3和圖4所示，其中圖3和圖4中a中左側(cè)為視覺(jué)柵格地圖，右側(cè)為激光柵格地圖。

由如圖3可知，仿真環(huán)境中，右下部分設(shè)置了一個(gè)激光雷達(dá)掃描平面范圍外的障礙物，從圖3 a中可以看到單線激光雷達(dá)無(wú)法觀測(cè)到掃描平面外的障礙物信息，而在視覺(jué)柵格地圖，相機(jī)視野范圍內(nèi)不同平面的障礙物都可以通過(guò)八叉樹(shù)投影成柵格地圖信息，因此垂直地面方向的障礙物建立比較完善。在匹配融合后的地圖中，如圖3 b，場(chǎng)景中不同高度的障礙物都被完善起來(lái)，更好提高了地圖還原度。

Z型走廊仿真如圖4a所示，當(dāng)激光柵格地圖與視覺(jué)柵格地圖存在著角度差時(shí)，通過(guò)為BRIEF描述子添加旋轉(zhuǎn)不變性之后，使內(nèi)角點(diǎn)仍有較好的匹配效果，融合后的地圖完善了各處細(xì)節(jié)。

2.2實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證本文的融合建圖效果，搭建移動(dòng)機(jī)器實(shí)驗(yàn)平臺(tái)并部署相關(guān)軟件，選用辦公樓一處走廊為實(shí)驗(yàn)場(chǎng)景，構(gòu)建出激光柵格地圖與視覺(jué)柵格地圖進(jìn)行地圖融合驗(yàn)證。

2.2.1實(shí)驗(yàn)平臺(tái)及環(huán)境

實(shí)驗(yàn)選用一維弦的Tyran移動(dòng)機(jī)器人作為硬件平臺(tái)，選用RPLIDAR A2作為使用的激光雷達(dá)傳感器、RealSense D435深度相機(jī)作為移動(dòng)機(jī)器人的視覺(jué)傳感器。上位機(jī)操作系統(tǒng)為Ubuntu 1604 LTS，機(jī)器人操作系統(tǒng)ROS為Kinect版本。

2.2.2地圖構(gòu)建實(shí)驗(yàn)

為驗(yàn)證激光與視覺(jué)融合建圖在實(shí)際場(chǎng)景中的效果，選擇樓宇內(nèi)一段走廊作為實(shí)驗(yàn)場(chǎng)景，實(shí)驗(yàn)走廊環(huán)境如圖5，圖5b在走廊起始的拐角處放置學(xué)習(xí)桌作為障礙物，移動(dòng)機(jī)器人無(wú)法從中間穿過(guò)。

VSLAM系統(tǒng)在走廊環(huán)境中所構(gòu)建的稠密地圖和八叉樹(shù)地圖如圖6所示。從圖6a可以看出，稠密點(diǎn)云構(gòu)建了走廊兩側(cè)的門(mén)、地面環(huán)境等周圍的物體信息，圖6b為構(gòu)建的實(shí)驗(yàn)場(chǎng)景八叉樹(shù)地圖，反映了三維空間中的占據(jù)狀態(tài)。

將八叉樹(shù)地圖投影得到二維柵格地圖如圖7所示。由圖7可以看出，視覺(jué)成功提取到學(xué)習(xí)桌的特征，在地圖中的位置與在真實(shí)場(chǎng)景中的位置基本吻合，回環(huán)效果較好。由于相機(jī)視角范圍限制，移動(dòng)機(jī)器人在起始位置，即圖中走廊末端部分的點(diǎn)云信息沒(méi)有充分獲取，導(dǎo)致地圖部分缺失。激光柵格地圖構(gòu)建效果如圖8所示。由于兩桌腿之間的連接梁低于雷達(dá)掃描平面，無(wú)法獲取桌腿連接梁的點(diǎn)云信息，導(dǎo)致學(xué)習(xí)桌的占據(jù)信息未在地圖中體現(xiàn)。

2.2.3場(chǎng)景地圖融合實(shí)驗(yàn)

單獨(dú)傳感器所構(gòu)建的柵格地圖雖然能大致反映出真實(shí)場(chǎng)景中障礙物的占據(jù)關(guān)系，但仍然存在一些細(xì)節(jié)缺失。將2張柵格地圖信息通過(guò)本文融合方法進(jìn)行融合，結(jié)果如圖9所示。

由圖9可以看出，學(xué)習(xí)桌及右側(cè)走廊盡頭都體現(xiàn)在地圖中，通過(guò)融合彌補(bǔ)了各自的不足，使柵格地圖更符合場(chǎng)景的實(shí)際情況。

3結(jié)束語(yǔ)

本文提出一種在任務(wù)層面的激光與視覺(jué)融合建圖方法。在統(tǒng)一地圖的尺度及分辨率后，采用Harris角點(diǎn)檢測(cè)并進(jìn)行篩選，對(duì)篩選出的內(nèi)角點(diǎn)添加旋轉(zhuǎn)不變性匹配和轉(zhuǎn)換，最后通過(guò)概率柵格融合的方法得到更符合環(huán)境的二維柵格地圖。相比于采用激光和視覺(jué)進(jìn)行實(shí)時(shí)建圖的數(shù)據(jù)層面融合的方法，本文采用的任務(wù)層面融合的建圖方法具有更高的地圖還原度和精度，驗(yàn)證了融合構(gòu)建地圖方法的有效性。但此算法是針對(duì)于類似走廊的環(huán)境，在場(chǎng)景應(yīng)用中有一定的局限性，后續(xù)將對(duì)算法應(yīng)用場(chǎng)景和融合后柵格地圖精度展開(kāi)進(jìn)一步優(yōu)化。

參考文獻(xiàn)：

［1］LI J， ZHANG X， LI J， et al. Building and optimization of 3D semantic map based on Lidar and camera fusion[J]. Neurocomputing， 2020， 409： 394-407.

［2］ZHANG T， ZHANG H， LI Y， et al. Flowfusion： Dynamic dense rgbd slam based on optical flow[C]∥2020 IEEE international conference on robotics and automation （ICRA）. Paris： IEEE， 2020： 7322-7328.

［3］TAKETOMI T， UCHIYAMA H， IKEDA S. Visual SLAM algorithms： A survey from 2010 to 2016[J]. IPSJ transactions on computer vision and applications， 2017， 9： 1-11.

［4］LI H， ZHOU Y， DONG Y， et al. Research on navigation algorithm of ros robot based on laser SLAM[J]. World Scientific Research Journal， 2022， 8（5）： 581-584.

［5］張大偉， 蘇帥. 自主移動(dòng)機(jī)器人視覺(jué) SLAM 技術(shù)研究[J]. 鄭州大學(xué)學(xué)報(bào)（理學(xué)版）， 2021， 53（1）： 1-8.

［6］ZHANG B， LIU J， CHEN H. Amcl based map fusion for multirobot slam with heterogenous sensors[C]∥2013 IEEE International Conference on Information and Automation （ICIA）. Yinchuan： IEEE， 2013： 822-827.

［7］曾鍵， 夏益民， 蔡錦煒， 等. 一種融合激光與視覺(jué)傳感器的柵格地圖構(gòu)建方法[J]. 工業(yè)控制計(jì)算機(jī)， 2020， 33（9）： 92-94，111.

［8］BOSSE M， ZLOT R. Map matching and data association for largescale twodimensional laser scanbased slam[J]. The International Journal of Robotics Research， 2008， 27（6）： 667-691.

［9］SUNIL S， MOZAFFARI S， SINGH R， et al. FeatureBased Occupancy MapMerging for Collaborative SLAM[J]. Sensors， 2023， 23（6）： 3114-3135.

［10］KAKUMA D， TSUICHIHARA S， RICARDEZ G A G， et al. Alignment of occupancy grid and floor maps using graph matching[C]∥2017 IEEE 11th international conference on semantic computing （ICSC）. San Diego： IEEE， 2017： 57-60.

［11］MA L， ZHU J， ZHU L， et al. Merging grid maps of different resolutions by scaling registration[J]. Robotica， 2016， 34（11）： 2516-2531.

［12］MIKOLAJCZYK K， SCHMID C. Scale & affine invariant interest point detectors[J]. International journal of computer vision， 2004， 60： 63-86.

［13］ROSIN P L. Measuring corner properties[J]. Computer Vision and Image Understanding， 1999， 73（2）： 291-307.

［14］YU S， FU C， GOSTAR A K， et al. A review on mapmerging methods for typical map types in multiplegroundrobot SLAM solutions[J]. Sensors， 2020， 20（23）： 6988-7008.

A Map Fusion Construction Method for Heterogeneous Sensors

MENG Xiaocheng， WANG Xianlun， CAO Tongkun， ZHOU Yusong

（College of Mechanical and Electrical Engineering， Qingdao University of Science and Technology， Qingdao 266061， China）

Abstract：

A task level mapping method that integrates Laser radar and depth camera is proposed to address the issues of low fidelity， susceptibility to environmental interference， and insufficient reliability when using a single sensor for mapping. Firstly， a combination based algorithm for corridor environment is adopted to align the map， and then a Bayesian inference based method is used for information fusion. To verify the effectiveness of fusion mapping， simulations are conducted by building diverse virtual scenes and map fusion experiments are carried out on real corridor environments. The results show that the fused grid map has a higher degree of restoration than the grid map created by a single sensor， effectively improving the accuracy of the map and obtaining a grid map that better reflects real physical environment information. This provides accurate and reliable environmental maps for subsequent path planning.

Keywords：integrated mapping; raster map establishment; laser radar; depth camera

收稿日期： 2024-06-27； 修回日期： 2024-08-29

基金項(xiàng)目： 國(guó)家自然科學(xué)基金資助項(xiàng)目（51105213）

第一作者： 孟小程（2000-），男，碩士，主要研究方向?yàn)闄C(jī)器人及智能制造技術(shù)。

通信作者： 王憲倫（1978-），男，博士，副教授，主要研究方向?yàn)闄C(jī)器人及智能制造技術(shù)、機(jī)械系統(tǒng)智能化設(shè)計(jì)及虛擬設(shè)計(jì)、機(jī)械加工過(guò)程的計(jì)算機(jī)控制。Email： xlwang@126.com