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

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

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

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

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

1地圖融合研究

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

1.1地圖對齊

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

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

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

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

當(dāng)?shù)貓D分辨率不同時，角點(diǎn)響應(yīng)值會發(fā)生變化，因此需要為Harris角點(diǎn)添加尺度不變性。通過建立多尺度空間，將每個位置特征賦予不同尺度的表示，找到能夠使角點(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分別為微分尺度窗口與積分尺度窗口。

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

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

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

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

當(dāng)滿足式（4）時，保留此候選角點(diǎn)，直到找到不同尺度下該位置的最大LoG值，作為該角點(diǎn)最佳尺度。通過Harris角點(diǎn)檢測提取圖像角點(diǎn)，并不是所有角點(diǎn)都可以作為有效信息使用，只需要在各子地圖中都能檢測出來且相似度較高的角點(diǎn)，角點(diǎn)太多影響匹配的效率以及精度，因此需要對角點(diǎn)進(jìn)行篩選。將處于墻角位置的角點(diǎn)定義為內(nèi)角點(diǎn)，將以內(nèi)角點(diǎn)為中心，半徑為r的圓域內(nèi)各柵格狀態(tài)區(qū)域占比作為判斷條件，對角點(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）通過圖像矩找到圖像塊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）

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

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

內(nèi)角點(diǎn)對成功匹配后，設(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)對間的距離比求出；R為旋轉(zhuǎn)矩陣；l為平移矩陣。

1.2信息融合

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

貝葉斯推理用于根據(jù)已知的觀察結(jié)果和先驗(yàn)知識，計算出未知變量的后驗(yàn)概率分布。假設(shè)在狀態(tài)空間中，t時刻的概率為p（xt），觀測數(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ù)語，保證概率密度函數(shù)歸一化；p（zt|xt）表示測量模型的似然函數(shù)；p（xt|Zt-1）表示在t-1時刻的先驗(yàn)概率。設(shè)O表示激光雷達(dá)測得柵格被占據(jù)，表示柵格未被占據(jù)，E為相機(jī)觀測柵格被占據(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）表示觀測模型，p（）=1-p（E），融合后的柵格概率為

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

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

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

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

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

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

2.1仿真驗(yàn)證

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3結(jié)束語

本文提出一種在任務(wù)層面的激光與視覺融合建圖方法。在統(tǒng)一地圖的尺度及分辨率后，采用Harris角點(diǎn)檢測并進(jìn)行篩選，對篩選出的內(nèi)角點(diǎn)添加旋轉(zhuǎn)不變性匹配和轉(zhuǎn)換，最后通過概率柵格融合的方法得到更符合環(huán)境的二維柵格地圖。相比于采用激光和視覺進(jìn)行實(shí)時建圖的數(shù)據(jù)層面融合的方法，本文采用的任務(wù)層面融合的建圖方法具有更高的地圖還原度和精度，驗(yàn)證了融合構(gòu)建地圖方法的有效性。但此算法是針對于類似走廊的環(huán)境，在場景應(yīng)用中有一定的局限性，后續(xù)將對算法應(yīng)用場景和融合后柵格地圖精度展開進(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］張大偉， 蘇帥. 自主移動機(jī)器人視覺 SLAM 技術(shù)研究[J]. 鄭州大學(xué)學(xué)報（理學(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］曾鍵， 夏益民， 蔡錦煒， 等. 一種融合激光與視覺傳感器的柵格地圖構(gòu)建方法[J]. 工業(yè)控制計算機(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)目： 國家自然科學(xué)基金資助項(xiàng)目（51105213）

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

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