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

李明明 張龍

摘 要：為研究移動(dòng)機(jī)器人室內(nèi)傳統(tǒng)單激光雷達(dá)建圖方法對(duì)環(huán)境中鏤空物體存在的漏掃問題，采用一種多傳感器數(shù)據(jù)融合的方法，在輪式移動(dòng)機(jī)器人上集成激光雷達(dá)、深度相機(jī)、輪式里程計(jì)和IMU等傳感器，使用機(jī)器人操作系統(tǒng)開源的深度圖像轉(zhuǎn)激光數(shù)據(jù)功能包對(duì)攝像頭采集的深度圖像數(shù)據(jù)進(jìn)行二維轉(zhuǎn)換，將轉(zhuǎn)換后的數(shù)據(jù)與激光數(shù)據(jù)融合；由于輪式里程計(jì)實(shí)際使用時(shí)輪子會(huì)打滑漂移，影響建圖效果，需要將輪式里程計(jì)和IMU數(shù)據(jù)融合進(jìn)行位姿估計(jì)，搭配融合后的激光數(shù)據(jù)，共同傳給建圖算法完成建圖。結(jié)果表明：優(yōu)化后的融合算法使得室內(nèi)移動(dòng)機(jī)器人對(duì)鏤空物體建圖更加清楚，障礙物識(shí)別更加準(zhǔn)確，機(jī)器人的絕對(duì)位姿誤差約0.15 m。多傳感器融合的建圖方法有效提高了移動(dòng)機(jī)器人在室內(nèi)環(huán)境中識(shí)別鏤空障礙物的能力和機(jī)器人運(yùn)動(dòng)精度，為后續(xù)移動(dòng)機(jī)器人地圖構(gòu)建提供一定參考。

關(guān)鍵詞：傳感器融合；即時(shí)定位與建圖；機(jī)器人操作系統(tǒng)；激光雷達(dá)；深度相機(jī)

中圖分類號(hào)：TP 242.6 文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1672-9315（2023）03-0613-09

DOI：10.13800/j.cnki.xakjdxxb.2023.0319開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

A multi-sensor fusion map construction method based on ROS

LI Mingming，ZHANG Long

（College of Communication and Information? Engineering，Xian University of Science and Technology，Xian 710054，China）

Abstract：A multi-sensor data fusion method was adopted to investigate the problem of the traditional single lidar mapping method for mobile robots in indoor environment.In this method，sensors such as lidar，depth camera，wheeled odometer and IMU are integrated on the wheeled mobile robot.The depth image data collected by camera is converted into two dimensional image data by using the open source depth image to lidar data function pack of the robot operating system，and the converted data is fused with lidar data.Since the wheel odometer will slip and drift when it is actually used，which affects the effect of map construction，the wheel odometer and IMU data are fused for pose estimation，and the fused lidar data is sent to the map building algorithm to complete map construction.The results show that the optimized fusion algorithm makes the indoor mobile robot clearer in the hollow-out object mapping，more accurate in obstacle recognition，and more accurate in robot trajectory，and the absolute pose error of the robot is about 0.15 m，the multi-sensor fusion mapping method effectively improves the mobile robots ability to identify hollowed-out obstacles in indoor environment and the robots motion accuracy，which provides a certain reference for the subsequent mobile robot map construction.

Key words：sensor fusion；simultaneous localization and mapping；robot operating system；lidar；RGB-D camera

0 引 言

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

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

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

1 開源建圖算法

1.1 Gmapping算法原理

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

p（x_1∶t，|z_1∶t，u_1∶t-1）=p（m|x_1∶t，z_1∶t）·

p（x_1∶t|z_1∶t，u_1∶t-1）（1）

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

粒子權(quán)重ω^（i）_t遞推見式（3）。

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

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

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

1.2 Cartographer算法原理

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

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

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

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

2 傳感器原理

2.1 單線激光雷達(dá)

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

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

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

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

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

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

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

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

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

θ=atan（x/y）（4）

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

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

2.3 慣性測(cè)量單元

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

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

2.4 輪式里程計(jì)

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

目前的差分兩輪輪式里程計(jì)^[20]普遍使用的是光電碼盤，經(jīng)過光電管后，每次碼盤轉(zhuǎn)動(dòng)都會(huì)產(chǎn)生一次脈沖，微控制單元（Microcontroller Unit，MCU）可以通過中斷記錄2次脈沖之間的時(shí)間t，已知這個(gè)時(shí)間，已知2個(gè)碼盤間的距離，就可以算出來這段時(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)勢(shì)結(jié)合互補(bǔ)，能夠很大地優(yōu)化移動(dòng)機(jī)器人在實(shí)際環(huán)境中的建圖效果，使得其掃描到的障礙物信息更加完善，建立完整的可用于導(dǎo)航的靜態(tài)二維柵格地圖。

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

另一種方法^[22]是在采集后端對(duì)SLAM生成的局部地圖數(shù)據(jù)進(jìn)行融合。這種方法需要反復(fù)對(duì)同一場(chǎng)景進(jìn)行移動(dòng)建圖才能達(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ī)的初始激光幀。

L_i={x_i|x＞0，i∈N^*}

C_i={y_i|y＞0，i∈N^*}（7）

式中 L_i和C_i分別為獲取的激光數(shù)據(jù)；x_i和y_i為該點(diǎn)角度上的距離值。

2）對(duì)激光與相機(jī)數(shù)據(jù)融合，若選取范圍內(nèi)的激光雷達(dá)數(shù)據(jù)中存在無效值，則替換為該處相機(jī)對(duì)應(yīng)的值；若該處激光雷達(dá)存在有效值，則對(duì)比該處激光與相機(jī)有效值的誤差，大于閾值則替換為該處相機(jī)對(duì)應(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ù)測(cè)模型，初始化傳感器狀態(tài)矩陣與誤差協(xié)方差矩陣。

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

K′=P_kH^T_k（H_kP_kH^T_k+R_k）^-1（9）

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

式中_k為狀態(tài)向量;為雅可比矩陣；F_k為控制矩陣;u_k為控制向量;P_k為系統(tǒng)誤差協(xié)方差矩陣;Q_k為系統(tǒng)噪聲協(xié)方差矩陣;R_k為傳感器噪聲協(xié)方差矩陣;Z_k為傳感器觀測(cè)矩陣;H_k為測(cè)量值的轉(zhuǎn)換矩陣。最后進(jìn)行聯(lián)合位姿估計(jì)，輸出新定位數(shù)據(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或Cartographer開源SLAM算法，構(gòu)建完整的二維柵格地圖。多傳感器融合框架如圖4所示。

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

實(shí)驗(yàn)搭建的實(shí)物平臺(tái)如圖5所示，為長(zhǎng)0.42 m，寬0.42 m，高0.88 m的輪式機(jī)器人，圖中標(biāo)注出各個(gè)傳感器相對(duì)位置，圖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底層控制板，移動(dòng)底盤采用輪式差速電機(jī)底盤。上位機(jī)使用基于Ubuntu16.04的ROS-kinetic的機(jī)器人操作系統(tǒng)，使用C++語言編寫融合代碼，在室內(nèi)條件下進(jìn)行實(shí)驗(yàn)。

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

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

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

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

5 結(jié) 論

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

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

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

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

參考文獻(xiàn)（References）：

[1] 王天然.機(jī)器人技術(shù)的發(fā)展[J].機(jī)器人，2017，39（4）：385-386.

WANG Tianran.Development of robot technology[J].Robot，2017，39（4）：385-386.

[2]楊琨，夏信凱，馬環(huán)洲，等.工業(yè)機(jī)器人主動(dòng)安全控制技術(shù)研究現(xiàn)狀與展望[J].機(jī)器人技術(shù)與應(yīng)用，2021，201（3）：9-19.

YANG Kun，XIA Xinkai，MA Huanzhou，et al.Research status and prospect of industrial robot active safety control technology[J].Robot Technique and Application，2021，201（3）：9-19.

[3]李成進(jìn)，王芳.智能移動(dòng)機(jī)器人導(dǎo)航控制技術(shù)綜述[J].導(dǎo)航定位與授時(shí)，2016，3（5）：22-26.

LI Chengjin，WANG Fang.Review of navigation control technology of intelligent mobile robot[J].Navigation Positioning and Timing，2016，3（5）：22-26.

[4]TAHERI H，ZHAO C X.SLAM；Definition and evolution[J/OL].Engineering Applications of Artificial Intelligence，97，104032.Https：//doi.org/10.1016/j.engappai.2020.104032.

[5]楊蘊(yùn)秀，韓寶玲，羅霄.移動(dòng)機(jī)器人同時(shí)定位與建圖室內(nèi)導(dǎo)航地圖研究[J].科學(xué)技術(shù)與工程，2021，21（18）：7597-7603.

YANG Yunxiu，HAN Baoling，LUO Xiao.Research on simultaneous localization and mapping of indoor navigation map for mobile robot[J].Science Technology and Engineering，2021，21（18）：7597-7603.

[6]沈斯杰，田昕，魏國(guó)亮，等.基于2D激光雷達(dá)的SLAM算法研究綜述[J].計(jì)算機(jī)技術(shù)與發(fā)展，2022，32（1）：13-18，46.

SHEN Sijie，TIAN Xin，WEI Guoliang，et al.Review of SLAM algorithm based on 2D lidar[J].Computer Technology and Development，2022，32（1）：13-18，46.

[7]MONTEMERLO M，THRUN S.Simultaneous localization and mapping with unknown data association using FastSLAM[C]//IEEE International Conference on Robotics and Automation.TaiPei，Taiwan，Sep.14-19，2003：1985-1991.

[8]LUO Y，SU Q，ZHANG Y，et al.Improved rao-blackwellized H∞ filter based mobile robot SLAM[J].The Jounnal of China Universities of Postsand Telecommunications，2016，23（5）：47-55.

[9]孫弋，張笑笑.結(jié)合退火優(yōu)化和遺傳重采樣的RBPF算法[J].西安科技大學(xué)學(xué)報(bào)，2020，40（2）：349-355.

SUN Yi，ZHANG Xiaoxiao.RBPF algorithm combining annealing optimization and genetic resampling[J].Journal of Xian University of Science and Technology，2020，40（2）：349-355.

[10]王險(xiǎn)峰，邱祖澤，丁子琳，等.基于改進(jìn)RBPF算法的輪式機(jī)器人SLAM導(dǎo)航系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2022，30（4）：172-176.

WANG Xianfeng，QIU Zuze，DING Zilin，et al.Design of wheeled robot SLAM navigation system based on improved RBPF algorithm[J].Computer Measurement & Control，2022，30（4）：172-176.

[11]THRUN S，MONTEMERLO M.The graph SLAM algorithm with applications to large-scale mapping of urban structures[J].International Journal of Robotics Research，2006，25（5-6）：403-429.

[12]HESS W，KOHLER D，RAPP H，et al.Real-time loop closure in 2D lidar SLAM[C]//IEEE Internationl Conference on Robotics and Automation.Stockholm，Sweden，May.16-21，2016：1271-1278.

[13]張亮，劉智宇，曹晶瑛，等.掃地機(jī)器人增強(qiáng)位姿融合的Cartographer算法及系統(tǒng)實(shí)現(xiàn)[J].軟件學(xué)報(bào)，2020，31（9）：2678-2690.

ZHANG Liang，LIU Zhiyu，CAO Jingying，et al.Cartographer algorithm and system implementation based on enhanced pose fusion of sweeping robot[J].Journal of Software，2020，31（9）：2678-2690.

[14]師瑞卓，張小俊，孫凌宇，等.基于單線激光雷達(dá)的三維形貌重建方法研究[J].激光與紅外，2022，52（2）：188-195.

SHI Ruizhuo，ZHANG Xiaojun，SUN Lingyu，et al.Research on 3D shape reconstruction method based on single-line lidar[J].Laser & Infrared，2022，52（2）：188-195.

[15]尹磊，彭建盛，江國(guó)來，等.低成本激光和視覺相結(jié)合的同步定位與建圖研究[J].集成技術(shù)，2019，8（2）：11-22.

YIN Lei，PENG Jiansheng，JIANG Guolai，et al.Research on simultaneous localization and mapping fusion with low-cost laser sensor and vision[J].Journal of Integration Technology，2019，8（2）：11-22.

[16]趙捷，趙野，童紀(jì)昀，等.一種用于TOF激光雷達(dá)的多通道時(shí)數(shù)轉(zhuǎn)換芯片設(shè)計(jì)[J].半導(dǎo)體光電，2021，42（3）：342-347.

ZHAO Jie，ZHAO Ye，TONG Jiyun，et al.A design of multi-channel time-to-digital converter for TOF lidar[J].Semiconductor Optoelectronics，2021，42（3）：342-347.

[17]KAMARUDIN K，MAMDUH S M，SHAKAFF A Y M，et al.Method to convert Kinects 3D depth data to a 2D map for indoor SLAM[C]//IEEE 9th International Colloquium on Signal Processing and Its Applications.Kuala Lumpur，Malaysia，Mac.8-10，2013：247-251.

[18]姚星，李海英.基于微慣性測(cè)量單元的礦用定向鉆孔軌跡繪制[J].傳感器與微系統(tǒng)，2022，41（8）：112-115.

YAO Xing，LI Haiying.Directional drilling track drawing for mine based on micro inertial measurement unit[J].Transducer and Microsystem Technologies，2022，41（8）：112-115.

[19]卓從彬，楊龍頻，周林，等.基于MPU6050加速度傳感器的跌倒檢測(cè)與報(bào)警系統(tǒng)設(shè)計(jì)[J].電子器件，2015，38（4）：821-825.

ZHUO Congbin，YANG Longpin，ZHOU Lin，et al.Design of fall detection system based on MPU6050 acceleration transducer[J].Chinese Journal of Electron Devices，2015，38（4）：821-825.

[20]張書亮，譚向全，吳清文.基于多傳感器融合技術(shù)的室內(nèi)移動(dòng)機(jī)器人定位研究[J].傳感器與微系統(tǒng)，2021，40（8）：53-56.

ZHANG Shuliang，TAN Xiangquan，WU Qingwen.Indoor mobile robot localization based on multi-sensor fusion technology[J].Transducer and Microsystem Technologies，2021，40（8）：53-56.

[21]陳凱.基于多傳感器融合的同時(shí)定位和地圖構(gòu)建研究[D].淮南：安徽理工大學(xué)，2021.

CHEN Kai.Research on simultaneous localization and map construction based on multi-sensor fusion[D].Huainan：Anhui University of Science and Technology，2021.

[22]張粵.基于激光和深度相機(jī)的移動(dòng)機(jī)器人建圖導(dǎo)航與定位的研究和實(shí)現(xiàn)[D].廣州：華南理工大學(xué)，2020.

ZHANG Yue.Research and implementation of mapping navigation and positioning of mobile robot based on laser and RGB-D camera[D].Guangzhou：South China University of Technology，2020.

[23]NAIR P，CHAUDHURY K N.Fast high-dimensional bilateral and nonlocal means filtering[J].IEEE Transactions on Image Processing，2019，28（3）：1470-1481.

[24]高沛林，高赟.移動(dòng)機(jī)器人多信標(biāo)SLAM技術(shù)[J].西安科技大學(xué)學(xué)報(bào)，2019，39（5）：905-911.

GAO Peilin，GAO Yun.SLAM navigation of mobilerobots based on multi-beacons[J].Journal of Xian University of Science and Technology，2019，39（5）：905-911.

[25]薛旭升，張旭輝，毛清華，等.基于雙目視覺的掘進(jìn)機(jī)器人定位定向方法研究[J].西安科技大學(xué)學(xué)報(bào)，2020，40（5）：781-789.

XUE Xusheng，ZHANG Xuhui，MAO Qinghua，et al.Localization and orientation method of roadheader robot based on binocular vision[J].Journal of Xian University of Science and Technology，2020，40（5）：781-789.

（責(zé)任編輯：劉潔）