一種基于ROS的多傳感器融合地圖構(gòu)建方法

李明明，張 龍

（西安科技大學(xué) 通信與信息工程學(xué)院，陜西 西安 710054）

0 引言

從20世紀(jì)的50，60年代開始，科學(xué)家開始對機(jī)器人技術(shù)進(jìn)行研究。王天然院士指出，本世紀(jì)以來機(jī)器人的應(yīng)用越來越廣泛，對生產(chǎn)力的推動作用越來越突出，機(jī)器人領(lǐng)域的各項(xiàng)技術(shù)也在快速進(jìn)步，發(fā)展形式多樣化［1］。

工業(yè)級機(jī)器人大多是采用固定式機(jī)器人為主，在處理簡單事件時(shí)有很高的效率，楊琨等在其研究中提出當(dāng)前機(jī)器人自身的缺陷使其應(yīng)用范圍具有一定的局限性［2］。在對機(jī)器人進(jìn)一步研究之后，李成進(jìn)等也提出移動機(jī)器人存在的一些重要缺陷，由于它的分層結(jié)構(gòu)設(shè)計(jì)，其分序需要很長的時(shí)間，導(dǎo)致反應(yīng)速度變慢［3］。與此同時(shí)，由于機(jī)器人技術(shù)發(fā)展緩慢與周圍環(huán)境復(fù)雜，其在執(zhí)行任務(wù)中難免會遇到各種各樣的問題。隨著移動機(jī)器人技術(shù)迅速發(fā)展，其復(fù)雜多變的使用場景也越來越多，故提高移動機(jī)器人在面對未知環(huán)境時(shí)處理遇到的障礙物的能力顯得尤為重要。TAHERI等在其研究中提出使用基于同時(shí)定位與建圖（Simulta-neous Localization and Mapping，SLAM）的靜態(tài)地圖構(gòu)建方法，其作用是先控制機(jī)器人對周圍靜態(tài)環(huán)境感知，將周圍的障礙物等信息掃描識別［4］，然后標(biāo)注到楊蘊(yùn)秀等所構(gòu)建的二維柵格地圖里面［5］，其好處是機(jī)器人在特定環(huán)境的場景中進(jìn)行作業(yè)更加方便，可以提高后續(xù)路徑規(guī)劃與導(dǎo)航的效率。但由于機(jī)器人周圍環(huán)境障礙物結(jié)構(gòu)復(fù)雜，使用傳統(tǒng)方法在室內(nèi)建圖經(jīng)常會漏掃一些特征不明顯，形狀多樣的障礙物，如桌椅等鏤空障礙物。這些鏤空障礙物體積較大，在室內(nèi)環(huán)境中經(jīng)常出現(xiàn)。同時(shí)，沈斯杰等在其研究中指出，傳統(tǒng)激光雷達(dá)建圖受制于激光雷達(dá)掃描單一平面的缺點(diǎn)，在掃到鏤空障礙物時(shí)，識別的激光數(shù)據(jù)較少而無視此處的障礙物［6］。這樣的錯誤顯然是很致命的，因?yàn)槿绻ê玫牡貓D中未標(biāo)出障礙物，而實(shí)際環(huán)境中存在障礙物，這對移動機(jī)器人后續(xù)的自動避障作業(yè)是很麻煩的。

因此，為了解決漏掃鏤空障礙物的問題，文中在靜態(tài)建圖的條件下，給移動機(jī)器人安裝多傳感器，融合激光雷達(dá)數(shù)據(jù)和深度相機(jī)數(shù)據(jù)，彌補(bǔ)單激光雷達(dá)建圖漏掃的缺點(diǎn)；為糾正輪式里程計(jì)實(shí)際使用時(shí)輪子打滑導(dǎo)致建圖不準(zhǔn)確的問題，將輪式里程計(jì)和慣性測量單元（Inertial Measurement Unit，IMU）融合提供位姿估計(jì)的定位數(shù)據(jù)，搭配融合后的激光數(shù)據(jù)，共同傳給建圖算法完成建圖。利用本方法在固定障礙物的場景下能有效識別鏤空障礙物，實(shí)現(xiàn)準(zhǔn)確建圖，幫助移動機(jī)器人完成之后的作業(yè)與任務(wù)。

1 開源建圖算法

1.1 Gmapping算法原理

Gmapping［7］算法是一種基于粒子濾波（Rao Blackwellized Particle Filter，RBPF）［8］框架的開源算法，能夠即時(shí)構(gòu)建周圍環(huán)境的柵格地圖，在小場景內(nèi)具有計(jì)算量小，精度高的優(yōu)點(diǎn)。其對RBPF算法進(jìn)行了提議分布改進(jìn)，主要是將里程計(jì)數(shù)據(jù)同激光數(shù)據(jù)聯(lián)合起來提議，校準(zhǔn)后的提議分布與真實(shí)分布更為相似。因此，將二者聯(lián)合起來，則RB-PF算法［9］改進(jìn)提議分布見式（1）。

式中 x1∶t為位姿信息；m為地圖信息；u1∶t－1為里程計(jì)數(shù)據(jù)；z1∶t為觀測數(shù)據(jù)。使用最近一次觀測數(shù)據(jù)改進(jìn)后，提議分布見式（2）。

最后對權(quán)重粒子進(jìn)行改進(jìn)提議分布計(jì)算。

通常目標(biāo)分布是機(jī)器人在t時(shí)刻位置的真實(shí)分布，只能使用提議分布來描述該分布。由于改進(jìn)的提議分布發(fā)現(xiàn)激光具有單峰值特性，在其附近采樣就能減少其他噪聲的數(shù)據(jù)［10］。因此，在激光峰值附近采集K個樣本數(shù)據(jù)后，結(jié)合高斯函數(shù)模擬提議分布。模擬好提議分布后就可以獲取其位姿信息，再結(jié)合激光信息構(gòu)建局部地圖信息，最后生成二維柵格地圖。

在實(shí)際環(huán)境中，單激光雷達(dá)建圖的魯棒性較差，地圖噪聲較多，與實(shí)際場景中的障礙物位置情況不符合。激光雷達(dá)建立的二維柵格地圖，更重要的應(yīng)用是為了移動機(jī)器人在建好的地圖中自主移動并且提高它在靜態(tài)環(huán)境中的避障成功率。

1.2 Cartographer算法原理

基于粒子濾波算法的SLAM的局限性就在于濾波所用的粒子在經(jīng)過一次計(jì)算后就會被丟棄，下次計(jì)算再次重新選取粒子，導(dǎo)致SLAM的計(jì)算效率低下。THRUN等針對此問題，在2006年提出了Graph-SLAM（圖優(yōu)化），試圖將激光雷達(dá)或攝像頭采集到的數(shù)據(jù)以位姿變化的方式呈現(xiàn)，并將稠密點(diǎn)云稀疏化以降低計(jì)算消耗，根據(jù)位姿間的約束關(guān)系繪制工作環(huán)境地圖，并最終將定位問題通過降維方法簡化為最小二乘問題［11］。

從圖1可以看出，圖優(yōu)化問題可以用圖的方式表示，節(jié)點(diǎn)P是需要優(yōu)化的變量，邊是優(yōu)化變量之間的約束。X節(jié)點(diǎn)可以是位姿，也可以是路標(biāo)；邊可以是位姿之間的關(guān)系也可以是觀測量；圖優(yōu)化是把位姿和空間點(diǎn)一起，進(jìn)行優(yōu)化。隨著軌跡增多，特征點(diǎn)的增長也很快。因此位姿圖優(yōu)化的意義是再優(yōu)化幾次以后將特征點(diǎn)固定住不再優(yōu)化，只當(dāng)作位姿估計(jì)的約束，之后主要優(yōu)化位姿。

圖1 圖優(yōu)化方法Fig.1 Method of graph optimization

Google基于機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）開源Cartographer算法就是基于圖優(yōu)化方式進(jìn)行建圖的［12］。優(yōu)點(diǎn)在于基于圖優(yōu)化的方法，在建圖中加入了回環(huán)檢測與閉環(huán)糾正改進(jìn)，能夠很好地完善全局建圖［13］，對于室內(nèi)鏤空障礙物建圖具有很好的校正作用。缺點(diǎn)是對計(jì)算機(jī)硬件性能要求較高，低配平臺會降低回環(huán)與閉環(huán)的運(yùn)行效率。

在實(shí)際仿真與建圖中，基于圖優(yōu)化的算法相比于粒子濾波算法，由于加入了閉環(huán)檢測和回環(huán)糾正，得到的地圖更加完整清楚。后將會在實(shí)驗(yàn)部分進(jìn)行具體的建圖效果對比。

2 傳感器原理

2.1 單線激光雷達(dá)

目前，多線激光雷達(dá)價(jià)格昂貴，均在萬元級別以上，一般企業(yè)難以承受如此高昂的價(jià)格。相比來說，單線激光雷達(dá)［14］成本就低得多，可以幫助機(jī)器人規(guī)避障礙物，掃描速度快、分辨率強(qiáng)、可靠性高。相比多線激光雷達(dá)，單線激光雷達(dá)在角頻率及靈敏度上反應(yīng)更快捷，所以，在測試周圍障礙物的距離和精度上更加精準(zhǔn)。但單線雷達(dá)只能平面式掃描，不能測量物體高度，由于激光數(shù)據(jù)的特性導(dǎo)致它需要大量的掃描數(shù)據(jù)，所建地圖無法進(jìn)行大范圍閉環(huán)檢測［15］。

研究使用的杉川3irobotix Delta 3A激光雷達(dá)是一種三角測距原理的單線激光雷達(dá)，原理和TOF激光雷達(dá)不同。TOF激光雷達(dá)［16］是根據(jù)測量光的飛行時(shí)間來計(jì)算距離的，而三角測距激光雷達(dá)是通過攝像頭的光斑成像位置來解三角形的。

從圖2可以看出，O1是激光發(fā)射器，射出去的激光由虛線表示，A，B，C是3個反射點(diǎn)。O2是攝像頭光心軸，三角形代表用來捕捉反射光斑的相機(jī)模型。A，B，C的成像點(diǎn)分別是A′，B′，C′。由于激光發(fā)射器和相機(jī)安裝的相對位置是已知的，也就是說相機(jī)的光心軸和激光（線）的角度已知，線段O1到O2長度已知，角（O1，O2，A）也已知，根據(jù)角邊角定理，三角形有唯一解，于是（O1，A）的長度是可以算出來的。同理B，C這2點(diǎn)距離O1的距離也是可解的。這樣就獲取到了障礙物到激光雷達(dá)點(diǎn)的測量距離。

圖2 激光雷達(dá)三角測距法Fig.2 Lidar triangulation of distance

在ROS系統(tǒng)中，將激光雷達(dá)數(shù)據(jù)封裝為sensor＿msg／LaserScan的自定義雷達(dá)專用消息包，方便各個廠家在制作自己的激光雷達(dá)時(shí)，能夠有統(tǒng)一的格式。

2.2 深度相機(jī)數(shù)據(jù)轉(zhuǎn)激光數(shù)據(jù)原理

由于激光雷達(dá)價(jià)格不菲，而攝像頭價(jià)格相對便宜。使用ROS官網(wǎng)開源的depthimage＿to＿laser-scan功能包能夠?qū)⑸疃葦z像頭和多線程激光雷達(dá)采集的三維圖像信息轉(zhuǎn)換為單線激光雷達(dá)的二維點(diǎn)云信息，并且發(fā)布／scan話題消息供建圖算法使用［17］。該方法存在的問題是，雷達(dá)數(shù)據(jù)是二維的、平面的，深度圖像數(shù)據(jù)是三維的，如果將三維的數(shù)據(jù)轉(zhuǎn)換成二維數(shù)據(jù)，只需要取深度圖的某一層即可。由于攝像頭對光線感知較為明顯，在室內(nèi)環(huán)境下，可以對攝像頭采集到的圖像進(jìn)行處理，將障礙物的缺陷點(diǎn)提取出來再轉(zhuǎn)換，可以減小轉(zhuǎn)換計(jì)算量。因此，深度相機(jī)較之于激光雷達(dá)無論是檢測范圍還是精度都有不小的差距。

如圖3所示，其主要轉(zhuǎn)換步驟如下。

圖3 深度圖轉(zhuǎn)激光原理Fig.3 Principle of converting depth map to laser

步驟1：將深度圖像的點(diǎn)p（u，v，z）轉(zhuǎn)換到深度相機(jī)坐標(biāo)下的點(diǎn)P（x，y，z）。

步驟2：計(jì)算夾角AOC見式（4）。

步驟3：將AOC角投影到對應(yīng)的激光數(shù)據(jù)槽中，已知激光的范圍［α，β］，激光束細(xì)分為N份，則可用laser［n］表示激光數(shù)據(jù)。點(diǎn)P投影到數(shù)組la-ser中的索引值n，見式（5）。

laser［n］的值為P在x軸上投影點(diǎn)C到相機(jī)光心O的距離r，見式（6）。

2.3 慣性測量單元

IMU傳感器［18］是加速度計(jì)和陀螺儀傳感器的組合。它被用來檢測加速度和角速度以表示運(yùn)動速度和運(yùn)動強(qiáng)度。陀螺儀是用于測量或維護(hù)方位和角速度的設(shè)備。它是一個旋轉(zhuǎn)的輪子或圓盤，其中旋轉(zhuǎn)軸可以不受影響地設(shè)定在任何方向。當(dāng)旋轉(zhuǎn)發(fā)生時(shí)，根據(jù)角動量守恒定律，該軸的方向不受支架傾斜或旋轉(zhuǎn)的影響。

實(shí)驗(yàn)平臺所使用的是MPU6050傳感器［19］，它是一個6軸運(yùn)動處理傳感器，集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)以及一個可擴(kuò)展的數(shù)字運(yùn)動處理器（Digital Motion Processor，DMP），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過其I2C接口輸出一個6軸信號。DMP通過調(diào)用函數(shù)對獲取的數(shù)據(jù)進(jìn)行四元數(shù)求解，即可得到所需姿態(tài)角。MPU6050對陀螺儀和加速度計(jì)分別用了3個16位ADC，將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動，傳感器的測量范圍都是用戶可控的。

2.4 輪式里程計(jì)

機(jī)器人可以基于碼盤數(shù)據(jù)和底盤運(yùn)動學(xué)模型進(jìn)行航跡推演，從而得到機(jī)器人的軌跡。對于移動機(jī)器人來說，假定機(jī)器人的初始位置已知，在一個行駛維度為一維的環(huán)境，機(jī)器人可以根據(jù)輪子的圈數(shù)實(shí)時(shí)估計(jì)自己在這個一維行駛環(huán)境的何處。在二維平面情況下，通過左右輪的轉(zhuǎn)速測量單元可以推知左右輪的轉(zhuǎn)圈數(shù)，顯然，當(dāng)左右輪的轉(zhuǎn)圈數(shù)不同時(shí)，移動機(jī)器人會向左或者向右轉(zhuǎn)。

目前的差分兩輪輪式里程計(jì)［20］普遍使用的是光電碼盤，經(jīng)過光電管后，每次碼盤轉(zhuǎn)動都會產(chǎn)生一次脈沖，微控制單元（Microcontroller Unit，MCU）可以通過中斷記錄2次脈沖之間的時(shí)間t，已知這個時(shí)間，已知2個碼盤間的距離，就可以算出來這段時(shí)間的真實(shí)速度。實(shí)際使用中，輪式里程計(jì)的輪子容易受環(huán)境因素影響出現(xiàn)打滑和漂移的問題，出現(xiàn)累計(jì)誤差增大從而影響定位效果。

3 激光與攝像頭融合建圖方法

3.1 激光雷達(dá)數(shù)據(jù)與深度相機(jī)數(shù)據(jù)融合

基于激光雷達(dá)與深度相機(jī)各自建圖的優(yōu)缺點(diǎn)，文中提出激光雷達(dá)與深度相機(jī)融合建圖方法，通過均值濾波算法將二者優(yōu)勢結(jié)合互補(bǔ)，能夠很大地優(yōu)化移動機(jī)器人在實(shí)際環(huán)境中的建圖效果，使得其掃描到的障礙物信息更加完善，建立完整的可用于導(dǎo)航的靜態(tài)二維柵格地圖。

目前，二者融合的方案有許多，大致分為2種。一種是［21］在采集前端直接對傳感器數(shù)據(jù)融合，主要對傳感器中的障礙物信息加以濾波，選取確定的障礙物信息。這種方法能夠?qū)?個傳感器識別的障礙物都儲存起來，與融合前的相比，可識別的障礙物信息增加。

另一種方法［22］是在采集后端對SLAM生成的局部地圖數(shù)據(jù)進(jìn)行融合。這種方法需要反復(fù)對同一場景進(jìn)行移動建圖才能達(dá)到比較好的效果，比較繁雜。

文中采用前端傳感器數(shù)據(jù)融合方法。由于深度相機(jī)輸出的是三維深度數(shù)據(jù)；最后建圖算法需要的是二維激光數(shù)據(jù)，因此，融合算法首先利用ROS官網(wǎng)的三維深度相機(jī)數(shù)據(jù)轉(zhuǎn)二維激光數(shù)據(jù)功能包，將三維深度相機(jī)數(shù)據(jù)轉(zhuǎn)成二維激光數(shù)據(jù)，然后再利用均值濾波算法［23］進(jìn)行激光和相機(jī)數(shù)據(jù)融合。均值濾波融合算法描述如下。

1）分別選取激光與相機(jī)的初始激光幀。

式中 Li和Ci分別為獲取的激光數(shù)據(jù)；xi和yi為該點(diǎn)角度上的距離值。

2）對激光與相機(jī)數(shù)據(jù)融合，若選取范圍內(nèi)的激光雷達(dá)數(shù)據(jù)中存在無效值，則替換為該處相機(jī)對應(yīng)的值；若該處激光雷達(dá)存在有效值，則對比該處激光與相機(jī)有效值的誤差，大于閾值則替換為該處相機(jī)對應(yīng)的有效值。

3）檢查最后固定范圍內(nèi)的激光數(shù)據(jù)，并作為新的激光幀進(jìn)行發(fā)布。

通過把二者激光幀的障礙物信息通過均值濾波算法融合，所產(chǎn)生的新激光數(shù)據(jù)中就擁有周圍環(huán)境真實(shí)的障礙物信息。

3.2 輪式里程計(jì)與IMU數(shù)據(jù)融合

為了獲得更準(zhǔn)確的建圖效果，需要將里程計(jì)和IMU通過擴(kuò)展卡爾曼濾波［24］融合輸出定位［25］信息，融合過程如下。

1）構(gòu)建系統(tǒng)預(yù)測模型，初始化傳感器狀態(tài)矩陣與誤差協(xié)方差矩陣。

2）然后獲取傳感器數(shù)據(jù)和對應(yīng)的協(xié)方差矩陣，并計(jì)算系統(tǒng)的卡爾曼增益K′。

3）更新系統(tǒng)狀態(tài)矩陣和誤差協(xié)方差矩陣，完成迭代，并輸出融合后的位姿估計(jì)信息。

激光通過圖2方式采集周圍平面中的障礙物信息，深度相機(jī)通過采集三維環(huán)境中的障礙物數(shù)據(jù)，并通過式（6）將深度圖像三維投影轉(zhuǎn)換為二維激光數(shù)據(jù)，然后通過式（7）均值濾波算法將二者所采集數(shù)據(jù)融合，輸出包含二者障礙物信息的新激光數(shù)據(jù)，最后結(jié)合里程計(jì)與IMU傳感器通過式（10）融合定位數(shù)據(jù)，與第1章的Gmapping或Car-tographer開源SLAM算法，構(gòu)建完整的二維柵格地圖。多傳感器融合框架如圖4所示。

圖4 多傳感器融合框架Fig.4 Frame of multi-sensor fusion

4 融合建圖實(shí)驗(yàn)

實(shí)驗(yàn)搭建的實(shí)物平臺如圖5所示，為長0.42 m，寬0.42 m，高0.88 m的輪式機(jī)器人，圖中標(biāo)注出各個傳感器相對位置，圖5（b）為機(jī)器人硬件框架圖。機(jī)器人使用的主要傳感器有杉川3irobotix Delta 3A激光雷達(dá)、樂視Letv Pro Xtion體感攝像頭和輪式里程計(jì)，工控機(jī)使用英特爾酷睿i7－4500U中央處理器，擁有1.80 GHz主頻，8 GB內(nèi)存的工業(yè)控制機(jī)，下位機(jī)使用意法半導(dǎo)體STM32F407VG底層控制板，移動底盤采用輪式差速電機(jī)底盤。上位機(jī)使用基于Ubuntu16.04的ROS-kinetic的機(jī)器人操作系統(tǒng)，使用C＋＋語言編寫融合代碼，在室內(nèi)條件下進(jìn)行實(shí)驗(yàn)。

圖5 移動平臺與硬件框架Fig.5 Frame of mobile platform and hardware

由于激光雷達(dá)與深度攝像頭分別建圖時(shí)，其采集到的激光數(shù)據(jù)處在不同圖層，在不做任何處理時(shí)，建圖算法只能接收到頻率高的激光數(shù)據(jù)。因此，該方法對二者的激光數(shù)據(jù)進(jìn)行C＋＋代碼重構(gòu)，使用時(shí)間同步函數(shù)同時(shí)訂閱二者／scan話題，使用回調(diào)函數(shù)對二者／scan話題數(shù)據(jù)進(jìn)行處理并發(fā)布一個新的／scan話題。新話題成功解決了二者建圖數(shù)據(jù)不在同一圖層的問題，在實(shí)驗(yàn)中也取得了良好的建圖效果。

圖6為Gazebo仿真環(huán)境，有2塊鏤空桌子分別用A、B表示鏤空障礙物。融合前后對比如圖7所示，圖7（a）、圖7（b）是Gmapping建圖算法使用數(shù)據(jù)融合前后對比建圖，圖中的紅框處并不能識別出鏤空障礙物；融合深度相機(jī)后，能識別出仿真環(huán)境中的障礙物A、B。圖7（c）、圖7（d）是Cartog-rapher算法融合深度相機(jī)前后的對比圖，之前識別不出來的桌子障礙物，在融合深度相機(jī)后，二者均能識別出鏤空障礙物A，B的具體位置。

圖6 Gazebo仿真環(huán)境Fig.6 Gazebo simulation environment

圖7 不同方法仿真環(huán)境下融合前后建圖對比Fig.7 Comparison of different methods of mapping before and after fusion in simulation environment

圖8為移動平臺真實(shí)室內(nèi)環(huán)境，室內(nèi)場景為7.6 m×3.8 m（長×寬），有2張0.48 m×0.41 m×0.76 m（長×寬×高）的鏤空椅子充當(dāng)鏤空障礙物。融合前后的機(jī)器人建圖如圖9所示，圖9為二維柵格地圖俯視圖，地圖分辨率為0.05 m，圖9（a）、圖9（c）是未融合的2種算法建圖，圖中紅色方框并不能識別機(jī)器人前方的鏤空障礙物，且圓形紅框也顯示出2種算法都有不同程度的漫反射現(xiàn)象；圖9（b）、圖9（d）是融合深度相機(jī)后的2種算法建圖，之前識別不出來的椅子障礙物，也能成功識別出來，并標(biāo)識出鏤空障礙物A、B的具體位置。深度相機(jī)采集的是環(huán)境深度信息，由于移動機(jī)器人尺寸較高，講臺高0.22 m，激光雷達(dá)距地0 28 m，相機(jī)距地面0.88 m，激光雷達(dá)只能識別同位置水平平面的物體，深度相機(jī)對不在同一平面的特征點(diǎn)識別較少，且物體特征點(diǎn)集中，與物體顏色關(guān)系不大，因此其主要特征點(diǎn)在鏤空障礙物上，所以未識別出特征點(diǎn)單一的講臺。圓形紅框處的激光雷達(dá)漫反射也得到改善，建圖更加準(zhǔn)確，達(dá)到了該方法識別鏤空障礙物的目的，提高了對周圍鏤空障礙物的識別能力。

圖8 移動平臺室內(nèi)環(huán)境Fig.8 Indoor environment of mobile platform

圖9 不同方法室內(nèi)環(huán)境下融合前后建圖對比Fig.9 Comparison of different methods of mapping before and after fusion in indoor environment

移動平臺的建圖精度可以轉(zhuǎn)換為其在環(huán)境中運(yùn)動軌跡的精度測試。平臺在室內(nèi)環(huán)境下的建圖移動軌跡對比如圖10（a）所示，為數(shù)據(jù)融合前后移動平臺運(yùn)動軌跡對比圖；從圖10（b）可以看出，數(shù)據(jù)融合后的移動機(jī)器人運(yùn)動軌跡優(yōu)于融合之前，室內(nèi)環(huán)境下融合前比融合后平均存在15 cm左右的軌跡誤差，且融合后的軌跡更加平滑和穩(wěn)定，表明了數(shù)據(jù)融合后的地圖更符合真實(shí)環(huán)境，從而驗(yàn)證了融合方法的有效性。

圖10 室內(nèi)環(huán)境中運(yùn)動軌跡與誤差對比Fig.10 Comparison of motion trajectories and errors in indoor environment

5 結(jié)論

1）通過仿真實(shí)驗(yàn)驗(yàn)證，相比于單激光掃描，數(shù)據(jù)融合方法能夠在仿真環(huán)境下識別桌子等鏤空障礙物，在建立的二維柵格地圖中有清晰的障礙物輪廓，邊緣平滑，建圖魯棒性有明顯改善。

2）在真實(shí)環(huán)境中，通過深度相機(jī)和激光雷達(dá)的數(shù)據(jù)融合方式，成功解決了傳統(tǒng)單激光雷達(dá)室內(nèi)環(huán)境建圖漏掃鏤空障礙物的問題。多傳感器融合的移動機(jī)器人實(shí)驗(yàn)平臺，能夠在真實(shí)環(huán)境中進(jìn)行更加準(zhǔn)確的定位，更好地識別周圍環(huán)境障礙物，也互補(bǔ)了不同傳感器之間的優(yōu)點(diǎn)。

3）優(yōu)化后的融合算法相比于單激光雷達(dá)SLAM，機(jī)器人運(yùn)動軌跡更準(zhǔn)確，障礙物識別精度明顯改善，可以為后續(xù)移動機(jī)器人自主導(dǎo)航與避障提供一定參考。

4）目前隨著傳感器性能的發(fā)展和SLAM系統(tǒng)實(shí)時(shí)性的要求，使用神經(jīng)網(wǎng)絡(luò)等融合算法也在SLAM未來發(fā)展中有了一定的嘗試。