吳耀威，吳自然,2，趙宇博，繆宏圣

（1.溫州大學(xué)電氣與電子工程學(xué)院，浙江 溫州 325035；2.溫州大學(xué)樂清工業(yè)研究院，浙江 溫州 325035）

0 引言

目前的同步定位和地圖構(gòu)建（SLAM）方案大致分為使用激光雷達(dá)傳感器的激光SLAM 與使用視覺傳感器的視覺SLAM。激光SLAM 與視覺SLAM 經(jīng)過了近30年的研究，目前已經(jīng)有成熟并且可以落地的方案出現(xiàn)。其中基于激光雷達(dá)的SLAM 方案有GMapping[1]、Cartographer[2]、Hector[3]等?；谙鄼C(jī)的SLAM 方案有基于單目相機(jī)的ORB-SLAM 系列[4-6]、使用深度相機(jī)的RGB-D SLAM[7]。雖然上述方案基本解決了SLAM 問題，但使用單一傳感器的SLAM 方案仍然存在著許多不足。如使用單目相機(jī)的SLAM 方案，通過提取關(guān)鍵幀圖像上的特征點(diǎn)[8]來估計(jì)機(jī)器人的位姿增量，然而單目相機(jī)所構(gòu)建的關(guān)鍵幀圖像質(zhì)量嚴(yán)重依賴環(huán)境中的光照條件。通常在強(qiáng)光或者黑暗的條件下由于RGB 圖像缺乏環(huán)境中的紋理信息，將導(dǎo)致SLAM 系統(tǒng)無法提取圖像中的特征點(diǎn)來構(gòu)建特征地圖，因此基于單目的SLAM 在室外強(qiáng)光或者黑夜環(huán)境中極易失效；使用深度相機(jī)的RGB-D SLAM，利用RGB-D 相機(jī)的成像特點(diǎn)，通過深度圖獲得每一幀圖像中的像素深度，彌補(bǔ)了單目SLAM 需要消耗大量計(jì)算性能用于估計(jì)像素深度的缺點(diǎn)，但是RGB-D 相機(jī)往往精度與量程有限，因此只能用于室內(nèi)建圖。相較于受環(huán)境限制的視覺SLAM，激光SLAM 方案的應(yīng)用場(chǎng)景要更加廣泛，因此被常用于移動(dòng)機(jī)器人的建圖與導(dǎo)航任務(wù)[9]。雖然激光雷達(dá)具有精度高、抗干擾能力強(qiáng)等特點(diǎn)，但是仍有其局限性。例如二維激光雷達(dá)只能掃描單一平面上的2D信息，無法檢測(cè)到掃描平面之外的障礙物，使用多線激光雷達(dá)[10]或者添加多組處在不同平面的二維激光雷達(dá)[11]可以解決上述問題，但是這樣的方案造價(jià)高昂無法普及。由于使用單一傳感器的SLAM 技術(shù)存在上述缺點(diǎn)，目前研究者開始探索融合激光雷達(dá)與深度相機(jī)等多傳感器信息融合的SLAM方案。Ali Yeon[12]分析了融合深度相機(jī)與2D激光雷達(dá)信息的可行性，但是并未進(jìn)行實(shí)驗(yàn)驗(yàn)證。Federico Nardi[13]提出了一種利用深度圖模擬2D 激光雷達(dá)的方法，利用深度相機(jī)發(fā)布的深度圖像提取出偽激光雷達(dá)數(shù)據(jù)，進(jìn)而用于構(gòu)建占用柵格地圖，但是所提出的方法精度與效率較低。陳鵬[14]提出了一種利用貝葉斯估計(jì)理論融合視覺局部地圖與激光局部地圖得到全局地圖的方案。鐘敏[15]利用深度相機(jī)進(jìn)行運(yùn)動(dòng)估計(jì)矯正激光SLAM 過程中的位姿累積誤差，鄭麗麗[16]通過擴(kuò)展卡爾曼濾波框架融合IMU、輪式里程計(jì)與視覺里程計(jì)，從而提高機(jī)器人位姿估計(jì)精度，并沒有考慮地圖構(gòu)建任務(wù)。

針對(duì)上述問題本文提出了一種深度相機(jī)與激光雷達(dá)在數(shù)據(jù)層面上的融合方法，融合后的傳感器數(shù)據(jù)使用GMapping算法進(jìn)行建圖，最后利用Gazebo仿真軟件搭建移動(dòng)機(jī)器人仿真環(huán)境，驗(yàn)證本文所提出的傳感器融合方法的可行性。

1 傳感器模型

本文提出的傳感器數(shù)據(jù)融合方法依賴于單線激光雷達(dá)與深度相機(jī)這兩種傳感器，單線激光雷達(dá)作為移動(dòng)機(jī)器人環(huán)境感知的核心功能傳感器，具有測(cè)距精準(zhǔn)、感知范圍廣、抗干擾、運(yùn)行穩(wěn)定可靠等特點(diǎn)，在傳感器融合過程中主要用于環(huán)境中的平面障礙物信息測(cè)量。深度相機(jī)是一種能夠獲取場(chǎng)景中物體與攝像頭之間物理距離的相機(jī)，主流的深度相機(jī)通常由多種鏡頭、光學(xué)傳感器組成，通常使用深度相機(jī)獲取三維空間中的障礙物信息。本文主要研究的側(cè)重點(diǎn)在于融合兩種傳感器的數(shù)據(jù)。

2 傳感器數(shù)據(jù)融合

2.1 傳感器數(shù)據(jù)融合框架

傳統(tǒng)的SLAM 研究通常集中在單一傳感器上，然而不管是基于視覺傳感器的SLAM 還是基于激光雷達(dá)的SLAM 都無法準(zhǔn)確的描述出復(fù)雜的環(huán)境，因而需要用到傳感器融合技術(shù)，使用多個(gè)傳感器能夠提供豐富的環(huán)境信息，減少測(cè)量誤差，提高移動(dòng)機(jī)器人的運(yùn)行魯棒性。本文使用深度相機(jī)與單線激光雷達(dá)傳感器，通過數(shù)據(jù)融合的形式實(shí)現(xiàn)傳感器之間的優(yōu)勢(shì)互補(bǔ)。下面將從傳感器外參聯(lián)合標(biāo)定、深度相機(jī)點(diǎn)云處理、傳感器數(shù)據(jù)融合3個(gè)方面進(jìn)行研究，圖1所示為傳感器數(shù)據(jù)融合總體框圖。

圖1 傳感器數(shù)據(jù)融合總體框圖

2.2 傳感器外參標(biāo)定

移動(dòng)機(jī)器人搭載多種傳感器，不同傳感器通常位于移動(dòng)機(jī)器人載體的不同位置，因此傳感器之間是存在外參變換的。然而傳感器融合技術(shù)需要將不同傳感器之間的數(shù)據(jù)轉(zhuǎn)換到同一坐標(biāo)系下進(jìn)行處理。為了求出傳感器之間的外參矩陣，需要進(jìn)行傳感器外參聯(lián)合標(biāo)定實(shí)驗(yàn)，該步驟是傳感器融合技術(shù)的必要步驟。

本文需要融合深度相機(jī)與激光雷達(dá)的數(shù)據(jù)，采用文獻(xiàn)[17]的方法進(jìn)行深度相機(jī)與激光雷達(dá)的聯(lián)合標(biāo)定。圖2所示為深度相機(jī)與激光雷達(dá)聯(lián)合標(biāo)定實(shí)驗(yàn)示意圖，由圖可知深度相機(jī)通過場(chǎng)景中的棋盤格標(biāo)定內(nèi)參，激光雷達(dá)對(duì)場(chǎng)景中的棋盤格進(jìn)行測(cè)量形成線狀特征，通過構(gòu)建三維激光點(diǎn)與棋盤格三維坐標(biāo)之間的約束關(guān)系，進(jìn)而求解深度相機(jī)與激光雷達(dá)的外參。其中傳感器外參求解的本質(zhì)就是估計(jì)激光雷達(dá)坐標(biāo)系與相機(jī)坐標(biāo)系之間的坐標(biāo)變換。

圖2 聯(lián)合標(biāo)定實(shí)驗(yàn)示意圖

為了描述外參標(biāo)定的過程，引入如圖3所示4個(gè)坐標(biāo)系：激光雷達(dá)坐標(biāo)系Ol-XlYlZl，基準(zhǔn)坐標(biāo)系Ow-XwYwZw，相機(jī)坐標(biāo)系Oc-XcYcZc以及圖像坐標(biāo)系O-uv，其中基準(zhǔn)坐標(biāo)系為世界坐標(biāo)系，為空間中的每一個(gè)點(diǎn)賦予坐標(biāo)信息。激光雷達(dá)與相機(jī)坐標(biāo)系為傳感器中的每一個(gè)觀測(cè)點(diǎn)賦予坐標(biāo)信息。雷達(dá)外參的求解可以由式（1）實(shí)現(xiàn)。

圖3 各坐標(biāo)軸關(guān)系

同理對(duì)于相機(jī)坐標(biāo)系與基準(zhǔn)坐標(biāo)系之間的外參標(biāo)定，由式（2）描述：

根據(jù)針孔相機(jī)模型，如圖4 所示。對(duì)于在相機(jī)坐標(biāo)系下的三維空間點(diǎn)P，有齊次坐標(biāo)P=[xc yc zc1]T，點(diǎn)P在成像平面上的坐標(biāo)為P′=[u v1]T，點(diǎn)P與P’存在如下轉(zhuǎn)換關(guān)系：

圖4 針孔相機(jī)模型

式中：[xw yw zw]T為世界坐標(biāo)系中的坐標(biāo)；fx和fy分別為相機(jī)坐標(biāo)為相機(jī)在x方向與y方向上的等效焦距；Cx和Cy分別為像素中心點(diǎn)在成像平面上坐標(biāo)。

在聯(lián)合標(biāo)定過程中，激光雷達(dá)會(huì)在棋盤格標(biāo)定板上生成掃描點(diǎn)，每一個(gè)掃描點(diǎn)都會(huì)在相機(jī)的圖像空間中存在唯一的一個(gè)像素點(diǎn)與之對(duì)應(yīng)。因此二維激光雷達(dá)與相機(jī)之間的外參標(biāo)定問題即為激光雷達(dá)數(shù)據(jù)點(diǎn)與二維圖像像素點(diǎn)之間的映射矩陣求解問題，聯(lián)立式（1）、（2）、（3）可得外參標(biāo)定問題方程組為：

式中：[xl yl1]T為激光雷達(dá)數(shù)據(jù)點(diǎn)的齊次坐標(biāo)；[u v1]T為激光點(diǎn)對(duì)應(yīng)的像素齊次坐標(biāo)；Rt和tt分別為激光雷達(dá)坐標(biāo)到相機(jī)坐標(biāo)的旋轉(zhuǎn)矩陣與平移向量。

2.3 深度相機(jī)點(diǎn)云預(yù)處理

根據(jù)第1 節(jié)描述的傳感器模型，深度相機(jī)能夠發(fā)布環(huán)境點(diǎn)云，但是發(fā)布的環(huán)境點(diǎn)云中包含大量的冗余點(diǎn)云。如圖5 所示為深度相機(jī)發(fā)布的點(diǎn)云，由圖可知，過高的墻體與地面點(diǎn)云不屬于機(jī)器人建圖時(shí)的障礙物信息，因此需要將這些點(diǎn)云進(jìn)行剔除。本文采用VoxelGrid 體素化網(wǎng)格濾波器進(jìn)行點(diǎn)云降采樣，之后在使用隨機(jī)抽樣一致性算法RANSAC 提取地面點(diǎn)云并剔除，對(duì)于過高的墻體點(diǎn)云可以通過設(shè)置點(diǎn)云高度閾值進(jìn)行剔除，具體步驟如下。

圖5 深度相機(jī)發(fā)布的原始點(diǎn)云

（1）點(diǎn)云降采樣處理：將深度相機(jī)發(fā)布的點(diǎn)云轉(zhuǎn)換為能被PCL 庫(kù)處理的點(diǎn)云格式，利用PCL 中的體素濾波器進(jìn)行濾波處理，濾波后的點(diǎn)云如圖6所示。通過與圖5所示的原始點(diǎn)云進(jìn)行對(duì)比可以看出，濾波后的點(diǎn)云數(shù)量明顯減少，但是點(diǎn)云的形狀特征并沒有丟失。

圖6 濾波后的點(diǎn)云

（2）去除過高點(diǎn)云：由于本文采用的機(jī)器人為地面移動(dòng)機(jī)器人，因此過高的點(diǎn)云將不被視為障礙物。本文所用的機(jī)器人總高度為0.15 m，以0.30 m 為閾值，遍歷所有點(diǎn)云的z軸數(shù)值，當(dāng)點(diǎn)云的z軸數(shù)值小于0.30 時(shí)，即可保留該點(diǎn)云，圖7所示為剔除過高點(diǎn)云之后的點(diǎn)云圖。從圖中可以看到使用該方法有效地去除了過高的點(diǎn)云，并且保留了地面上的障礙物點(diǎn)云。

圖7 剔除過高點(diǎn)云

（3）去除地面點(diǎn)云：本文使用隨機(jī)抽樣一致性算法RANSAC去除地面點(diǎn)云，首先通過計(jì)算地面點(diǎn)云法向量，利用地面法向量計(jì)算地面擬合方程式如下：

通過RANSAC 算法隨機(jī)選擇數(shù)據(jù)點(diǎn)并計(jì)算該點(diǎn)所在平面方程π(A,B,C,D)，計(jì)算點(diǎn)云數(shù)據(jù)中每一個(gè)點(diǎn)到該平面的距離，利用距離閾值dmax統(tǒng)計(jì)出該閾值內(nèi)的點(diǎn)數(shù)Nin，重復(fù)上述過程，最終選擇點(diǎn)數(shù)最多的平面方程，該平面方程所構(gòu)成的點(diǎn)集為{P1,…,PN}，利用該集合最小化距離函數(shù)式（6），求得最優(yōu)地面擬合方程如下：

其中Dk表示集合{P1,…,PN}中的第k個(gè)點(diǎn)到地面平面的距離。滿足最優(yōu)地面擬合方程的點(diǎn)云即為地面點(diǎn)云，去除地面點(diǎn)云后的深度相機(jī)點(diǎn)云數(shù)據(jù)如圖8所示。

圖8 剔除過高點(diǎn)云與地面點(diǎn)云后

（4）點(diǎn)云轉(zhuǎn)換偽激光雷達(dá)數(shù)據(jù)：去除地面與過高點(diǎn)云之后的數(shù)據(jù)仍然具有冗余的信息，本文采取點(diǎn)云投影的方法，將第三步所得點(diǎn)云轉(zhuǎn)換為偽激光雷達(dá)數(shù)據(jù)，從而實(shí)現(xiàn)深度相機(jī)點(diǎn)云對(duì)激光雷達(dá)數(shù)據(jù)的模擬，轉(zhuǎn)換原理如圖9所示。

設(shè)障礙物點(diǎn)云為Mi(xi,yi,zi)，地面投影點(diǎn)云為mi(θi,di)，兩者之間的轉(zhuǎn)換關(guān)系由式（7）～（8）描述：

對(duì)于給定的深度相機(jī)，已知相機(jī)水平FOV 范圍為(βmin,βmax)，一幀激光束共有N個(gè)點(diǎn)，假設(shè)點(diǎn)mi(θi,di)為OC射線上距離光心O最近的點(diǎn)，則可以計(jì)算點(diǎn)mi(θi,di)在模擬激光數(shù)據(jù)中的索引值為：

重復(fù)上述步驟，即可得到模擬激光數(shù)據(jù)，如圖10 所示為模擬激光數(shù)據(jù)。

2.4 傳感器數(shù)據(jù)融合

本文采用點(diǎn)云融合的方式將深度相機(jī)模擬激光數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)進(jìn)行融合，先將激光雷達(dá)數(shù)據(jù)轉(zhuǎn)換成點(diǎn)云格式，之后利用傳感器外參矩陣將模擬激光點(diǎn)云通過坐標(biāo)變換，轉(zhuǎn)移到激光雷達(dá)坐標(biāo)系下，最后在同一坐標(biāo)系下將兩種傳感器點(diǎn)云進(jìn)行融合，融合后的點(diǎn)云數(shù)據(jù)通過程序轉(zhuǎn)換為激光雷達(dá)數(shù)據(jù)，為后文的SLAM 算法提供數(shù)據(jù)來源。圖11所示為融合后的激光雷達(dá)數(shù)據(jù)，可以看出融合后的數(shù)據(jù)保留了深度相機(jī)點(diǎn)云的障礙物特征。

圖11 融合后的激光雷達(dá)數(shù)據(jù)

3 GMapping算法研究

3.1 算法概述

GMapping 是一種基于Rao-Blackwellised 粒子濾波器（RBPF）的SLAM 算法，GMapping 在RBPF 的框架改進(jìn)了提議分布，使用里程計(jì)與激光雷達(dá)數(shù)據(jù)作為提議分布。其次GMapping算法改進(jìn)了重采樣策略，使用自適應(yīng)重采樣來緩解粒子匱乏問題。因此本文第二章提出的傳感器數(shù)據(jù)融合方法生成的激光掃描數(shù)據(jù)能夠直接輸入GMapping算法用于地圖構(gòu)建。

3.2 提議分布優(yōu)化

傳統(tǒng)RBPF框架中，通常使用移動(dòng)機(jī)器人里程計(jì)數(shù)據(jù)作為控制量計(jì)算提議分布。此時(shí)粒子的權(quán)重由式（10）計(jì)算：

3.3 重采樣策略

傳統(tǒng)RBPF 濾波器中采用的重采樣策略為：在重采樣過程中使用權(quán)重大的粒子替代權(quán)重小的粒子，這種做法容易丟棄一些性質(zhì)較好的低權(quán)重粒子，造成粒子匱乏使得最后的樣本近似的分布與目標(biāo)分布相差甚遠(yuǎn)。GMapping 采用自適應(yīng)重采樣的策略，該策略使用式（12）計(jì)算指標(biāo)Neff，通過設(shè)定的閾值來判斷是否需要進(jìn)行重采樣操作。

式（12）中分母的意義在于衡量粒子樣本權(quán)值的差異，其核心思想：如果一組樣本是對(duì)目標(biāo)分布的良好采樣，那么根據(jù)重要性權(quán)值的定義，各個(gè)樣本的權(quán)重應(yīng)當(dāng)基本一致。所以權(quán)重的差異越大，意味著樣本的分布與目標(biāo)分布之間存在著較大的差異，需要進(jìn)行重采樣來修正。理論上樣本的分布越靠近目標(biāo)分布則各個(gè)粒子之間的權(quán)重應(yīng)該越接近。

4 仿真實(shí)驗(yàn)驗(yàn)證和結(jié)果分析

4.1 仿真實(shí)驗(yàn)設(shè)計(jì)

為驗(yàn)證本文所提出的傳感器數(shù)據(jù)融合方法的有效性與可行性，利用Gazebo仿真軟件搭建3.5 m×3.5 m大小的實(shí)驗(yàn)環(huán)境，在實(shí)驗(yàn)環(huán)境中導(dǎo)入了2個(gè)邊長(zhǎng)為10 cm的正立方體，3 個(gè)邊長(zhǎng)7.5 cm 的正立方體，以及2 個(gè)邊長(zhǎng)為2.5 cm 的正立方體，仿真環(huán)境如圖12（a）所示。實(shí)驗(yàn)環(huán)境中導(dǎo)入開源機(jī)器人模型turtlebot3 模擬真實(shí)移動(dòng)機(jī)器人，turtlebot3 最高處為15 cm，其中激光雷達(dá)處在13 cm 處，深度相機(jī)處在11 cm 處。圖13 所示為機(jī)器人仿真模型與障礙物對(duì)比。為了體現(xiàn)傳感器數(shù)據(jù)融合對(duì)建圖效果的影響，組別1僅使用激光雷達(dá)進(jìn)行建圖；組別2僅使用深度相機(jī)建圖；組別3 使用深度相機(jī)與激光雷達(dá)數(shù)據(jù)融合進(jìn)行建圖。3 組實(shí)驗(yàn)皆為使用手動(dòng)建圖模式，其行駛路徑節(jié)點(diǎn)順序?yàn)?-2-3-4-1-5，并且在每個(gè)節(jié)點(diǎn)處移動(dòng)機(jī)器人都順時(shí)針旋轉(zhuǎn)360°，目的是為了讓傳感器更好地感知周圍的環(huán)境，行駛路徑如圖12（b）所示。

圖12 仿真環(huán)境及行駛路徑

圖13 機(jī)器人仿真模型與障礙物對(duì)比

4.2 實(shí)驗(yàn)結(jié)果分析

實(shí)驗(yàn)按照所使用的傳感器類型分為僅使用激光雷達(dá)、僅使用深度相機(jī)以及使用深度相機(jī)與激光雷達(dá)融合3 種方案，3種方案生成的傳感器數(shù)據(jù)如圖14所示。

圖14 3種方案?jìng)鞲衅鲾?shù)據(jù)

圖14（a）所示為僅使用深度相機(jī)模擬激光雷達(dá)時(shí)發(fā)布的偽激光雷達(dá)掃描點(diǎn)，由圖可知利用深度相機(jī)能夠感知三維空間中的物體信息的特點(diǎn)，機(jī)器人感知到前方的5個(gè)障礙物，但是受限于相機(jī)視場(chǎng)角FOV 的限制，并不能得到360°方位的障礙物信息，因此僅使用深度相機(jī)模擬激光雷達(dá)數(shù)據(jù)是不完備的。圖14（b）所示為僅使用激光雷達(dá)傳感器時(shí)發(fā)布的激光掃描點(diǎn)，由圖可知激光雷達(dá)很好地感知了周圍墻體的信息，但是由于激光雷達(dá)掃描平面高于障礙物所在平面，因此僅使用激光雷達(dá)數(shù)據(jù)也是不完備的。如圖14（c）所示為兩種傳感器融合后發(fā)布的激光掃描點(diǎn)數(shù)據(jù)，可以看出融合后的數(shù)據(jù)將深度相機(jī)與激光雷達(dá)的優(yōu)點(diǎn)有效結(jié)合，因此本文提出的傳感器融合策略是有效的。

通過將上述傳感器數(shù)據(jù)輸入GMapping算法分別構(gòu)建地圖，最終建圖結(jié)果如圖15所示。

圖15 建圖結(jié)果對(duì)比

從僅使用深度相機(jī)建圖結(jié)果圖15（a）來看，僅使用深度相機(jī)構(gòu)建的地圖邊緣輪廓粗糙，這是由于在深度相機(jī)點(diǎn)云模擬激光雷達(dá)掃描的時(shí)候傳感器噪聲帶來的影響，雖然地圖輪廓粗糙，但是使用深度相機(jī)仍然觀測(cè)到了環(huán)境中的7 個(gè)障礙物。其次地圖發(fā)生了較大的位移，這是由于深度相機(jī)的FOV過小，引起GMapping算法中掃描匹配模塊失敗，具體原因：當(dāng)前時(shí)刻機(jī)器人通過轉(zhuǎn)換深度相機(jī)點(diǎn)云至偽激光雷達(dá)數(shù)據(jù)，但是由于相機(jī)的FOV限制，該偽激光雷達(dá)數(shù)據(jù)過短，從而無法使用掃描匹配中的迭代最近點(diǎn)算法（ICP）估算出移動(dòng)機(jī)器人此時(shí)的位姿增量，因?yàn)镮CP 算法是通過迭代的方式求解最優(yōu)位姿變換，所以采集到點(diǎn)越多意味著可迭代次數(shù)越多，求解出的位姿增量精度更高，同理數(shù)據(jù)越少ICP 效果越差。從圖15（b）來看，由于單線激光雷達(dá)的掃描平面高于障礙物平面，因此在僅使用激光雷達(dá)建圖時(shí)，地圖中并沒有構(gòu)建出7個(gè)障礙物的位置。從圖15（c）來看使用傳感器融合的方法構(gòu)建出的地圖成功顯示了7 個(gè)障礙物的位置，地圖輪廓清晰但地圖邊緣略微有點(diǎn)加粗，這是由于兩種傳感器數(shù)據(jù)中的噪聲所引起的。

為了定量分析本文所提出的方法，分別對(duì)上述3 種類別傳感器進(jìn)行多次建圖實(shí)驗(yàn)，通過計(jì)算建圖時(shí)間和障礙物檢測(cè)率體現(xiàn)所提出方法的優(yōu)點(diǎn)。

（1）建圖時(shí)間。對(duì)3種類型的傳感器分別進(jìn)行5次實(shí)驗(yàn)，記錄每次實(shí)驗(yàn)的建圖耗時(shí)，通過觀察實(shí)時(shí)建圖結(jié)果，當(dāng)機(jī)器人所構(gòu)建的地圖面積不再有明顯增加時(shí)停止建圖，通過計(jì)算每組實(shí)驗(yàn)平均用時(shí)衡量3種建圖方法的效率。

實(shí)驗(yàn)結(jié)果如表1所示。由表中數(shù)據(jù)可知，3種建圖實(shí)驗(yàn)平均用時(shí)如下：深度相機(jī)建圖實(shí)驗(yàn)平均用時(shí)為40.31 s，激光雷達(dá)建圖平均用時(shí)為16.91 s，傳感器融合建圖平均用時(shí)為30.72 s。3 種類別的使用中激光雷達(dá)建圖用時(shí)最短，因?yàn)榧す饫走_(dá)返回的每一幀數(shù)據(jù)占用內(nèi)存小，輸入建圖算法后處理過程快，所以得到最短的建圖用時(shí)。使用深度相機(jī)建圖，需要先將點(diǎn)云轉(zhuǎn)換為激光雷達(dá)，這一過程需要將深度相機(jī)發(fā)布的大量點(diǎn)云數(shù)據(jù)進(jìn)行提取并轉(zhuǎn)換成對(duì)應(yīng)格式，因此深度相機(jī)用時(shí)最長(zhǎng)。使用傳感器融合進(jìn)行建圖平均用時(shí)比深度相機(jī)建圖平均用時(shí)減少31.22%，這是因?yàn)槌绦驅(qū)⑸疃认鄼C(jī)發(fā)布的點(diǎn)云數(shù)據(jù)進(jìn)行濾波和剪裁處理，使得融合時(shí)所用的數(shù)據(jù)量減??；又因?yàn)槿诤线^程需要將點(diǎn)云格式進(jìn)行轉(zhuǎn)換，因此平均用時(shí)會(huì)比激光雷達(dá)建圖增多44.95%。

表1 建圖時(shí)間數(shù)據(jù)s

（2）障礙物檢測(cè)率：本文使用障礙物檢測(cè)率來衡量建圖效果，統(tǒng)計(jì)由傳感器構(gòu)建的二維柵格地圖中障礙物的總邊長(zhǎng)S1，真實(shí)環(huán)境中的障礙物總邊長(zhǎng)S2，通過式（13）計(jì)算障礙物檢測(cè)率：

式（13）的原理簡(jiǎn)單表述：SLAM 構(gòu)建的占用柵格地圖都保存為.pgm 格式的灰度圖，這種格式的圖片每一個(gè)像素都保存為灰度值。已知所有的障礙物灰度值都是黑色也就是255，因此只要統(tǒng)計(jì)圖像中所有黑色像素的個(gè)數(shù)，乘以每個(gè)像素的分辨率（算法規(guī)定的每一個(gè)像素柵格代表現(xiàn)實(shí)中多少米，本文使用的分辨率為0.05 m/柵格），即可得到障礙物總邊長(zhǎng)，再通過與真實(shí)環(huán)境中計(jì)算的總障礙物邊長(zhǎng)求比值，即可得到障礙物檢測(cè)率。

每次實(shí)驗(yàn)的計(jì)算結(jié)果如表2 所示。由表2 數(shù)據(jù)可知，使用傳感器數(shù)據(jù)融合方法得到最好的障礙物檢測(cè)率為98.66%，比使用激光雷達(dá)的建圖方法提高了13.24%，比使用深度相機(jī)建圖方法提高了8.21%。

表2 障礙物檢測(cè)率數(shù)據(jù)%

綜上所述本文提出的傳感器融合方法具有一定的可行性與有效性。

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

本文提出了一種多傳感器數(shù)據(jù)融合的方法，該方法首先將深度相機(jī)點(diǎn)云數(shù)據(jù)進(jìn)行濾波降采樣處理，然后去除點(diǎn)云中的無關(guān)部分，再將深度相機(jī)點(diǎn)云轉(zhuǎn)換成偽激光雷達(dá)數(shù)據(jù)，最后融合偽激光雷達(dá)與激光雷達(dá)數(shù)據(jù)，將融合后的激光雷達(dá)數(shù)據(jù)發(fā)布，利用GMapping算法構(gòu)建二維占用柵格地圖。通過實(shí)驗(yàn)對(duì)比分析建圖效果，實(shí)驗(yàn)結(jié)果表明，本文所提出的傳感器融合建圖方法在建圖時(shí)間上比使用深度相機(jī)平均建圖用時(shí)減少31.22%。本文方法在障礙物檢測(cè)率方面比激光雷達(dá)建圖提高13.24%，比深度相機(jī)建圖提高8.21%。但本文所提出的方法也存在不足，一是由于深度相機(jī)發(fā)布的點(diǎn)云數(shù)據(jù)在轉(zhuǎn)換成偽激光雷達(dá)數(shù)據(jù)時(shí)計(jì)算量較大，影響融合程序性能。二是由于兩種傳感器的數(shù)據(jù)發(fā)布頻率不同，導(dǎo)致建圖時(shí)間增加。下一步的工作將會(huì)利用多線程方法改進(jìn)融合程序，減少數(shù)據(jù)處理時(shí)間。