基于多傳感器信息融合的未知環(huán)境下移動(dòng)機(jī)器人的地圖創(chuàng)建

李 艷, 唐達(dá)明, 戴慶瑜

(1.陜西科技大學(xué) 電氣與控制工程學(xué)院， 陜西 西安 710021； 2.陜西農(nóng)產(chǎn)品加工技術(shù)研究院, 陜西 西安 710021)

0 引言

未知環(huán)境中，機(jī)器人的地圖創(chuàng)建與自主定位密切相關(guān)，SLAM(Simultaneous Localization and Mapping)將二者相結(jié)合，通過(guò)已經(jīng)獲得的環(huán)境地圖信息完成自主定位，并根據(jù)定位結(jié)果完成對(duì)環(huán)境地圖的更新[1].

SLAM問(wèn)題是指機(jī)器人在未知的環(huán)境下，僅依靠自身傳感器的觀測(cè)數(shù)據(jù)zt和控制輸入量ut的情況下確定機(jī)器人的大致位置，并且通過(guò)觀測(cè)信息構(gòu)建出大致的環(huán)境地圖，同時(shí)用構(gòu)建出來(lái)的地圖繼續(xù)對(duì)機(jī)器人的位置進(jìn)行估計(jì).如圖1所示，在機(jī)器人的移動(dòng)過(guò)程中，通過(guò)一系列的控制輸入量u1∶t={u1,u2,…,ut}和觀測(cè)數(shù)據(jù)z1∶t={z1,z2,…,zt}，對(duì)自身的運(yùn)動(dòng)軌跡x1∶t={x1,x2,…,xt}及環(huán)境地圖m進(jìn)行準(zhǔn)確的估計(jì).

圖1 SLAM問(wèn)題示意圖

SLAM問(wèn)題用數(shù)學(xué)語(yǔ)言描述為求解如下概率分布：

Bel(xt)=p(xt,m|u1∶t,z1∶t)

或Bel(x0∶t)=p(x0∶t,m|u1∶t,z1∶t)

(1)

當(dāng)前，求解SLAM問(wèn)題的方法可大致分為兩大類：一類是基于濾波的方法，另一類則是基于圖優(yōu)化的方法.其中，基于濾波器的方法由于其算法簡(jiǎn)單而被大量應(yīng)用于SLAM中[2-4]，但隨著研究的不斷深入，SLAM的應(yīng)用范圍也越來(lái)越大，當(dāng)使用基于濾波的方法時(shí)，機(jī)器人每次移動(dòng)只考慮當(dāng)前狀態(tài)，在建圖時(shí)誤差會(huì)不斷累積，導(dǎo)致其最后構(gòu)建出的地圖與實(shí)際環(huán)境有差別，使其難以應(yīng)用于較大環(huán)境下的地圖創(chuàng)建.而基于圖優(yōu)化的方法在估計(jì)機(jī)器人運(yùn)動(dòng)軌跡時(shí)，會(huì)用到傳感器的所有觀測(cè)信息，在建圖的過(guò)程中把對(duì)地圖特征的描述轉(zhuǎn)化為對(duì)運(yùn)動(dòng)軌跡的約束，提高了建圖的效率與準(zhǔn)確性，使其適用于較大環(huán)境下的地圖創(chuàng)建.

目前，對(duì)基于圖優(yōu)化的建圖算法研究越來(lái)越多，文獻(xiàn)[5]使用激光雷達(dá)完成地圖的構(gòu)建，將基于圖優(yōu)化的方法應(yīng)用到地圖構(gòu)建中提高了建圖的效率；文獻(xiàn)[6]使用雙目相機(jī)完成地圖的構(gòu)建，將多層次松弛的方法應(yīng)用在圖優(yōu)化的地圖構(gòu)建中，很好地解決了因算法形成閉環(huán)而無(wú)法更新地圖的問(wèn)題；文獻(xiàn)[7]使用深度相機(jī)完成地圖的構(gòu)建，并在建圖時(shí)應(yīng)用梯度下降方法，提高了SLAM的魯棒性.目前，對(duì)于SLAM的研究主要集中在對(duì)地圖構(gòu)建算法的研究[2-7]，而對(duì)于傳感器數(shù)據(jù)處理方面的研究較少，并且以上方法都是基于單一的距離傳感器或視覺(jué)傳感器，都存在著很多缺陷，例如，二維激光雷達(dá)所獲取的信息會(huì)因?yàn)榘惭b位置不同而受到影響，視覺(jué)傳感器獲取的信息會(huì)因?yàn)榄h(huán)境的變化而受到影響，因此導(dǎo)致實(shí)驗(yàn)效果仍然不佳.

針對(duì)移動(dòng)機(jī)器人在未知環(huán)境下利用單一傳感器進(jìn)行地圖構(gòu)建時(shí)所出現(xiàn)的問(wèn)題，本文提出利用基于圖優(yōu)化的算法構(gòu)建地圖，并在建圖時(shí)將移動(dòng)機(jī)器人的激光與視覺(jué)信息進(jìn)行融合，使得構(gòu)建出的地圖與實(shí)際環(huán)境更加接近.

1 傳感器數(shù)據(jù)處理

1.1 激光雷達(dá)數(shù)據(jù)處理

激光雷達(dá)通過(guò)旋轉(zhuǎn)的方式掃描外界物體獲得信息[8]，并以極坐標(biāo)的形式表示為Zl=[dl,θl]T，其中，θl為激光雷達(dá)掃描時(shí)，目標(biāo)點(diǎn)和激光雷達(dá)掃描起始位置之間的夾角，并且0 °<θl<360 °，dl是目標(biāo)到激光雷達(dá)的直線距離，將原始掃描數(shù)據(jù)轉(zhuǎn)換至直角坐標(biāo)，則表示形式為Zl=[x,y]T，如圖2所示.

圖2 激光雷達(dá)掃描測(cè)量示意圖

利用式(2)，可將掃描得到的數(shù)據(jù)點(diǎn)轉(zhuǎn)換至直角坐標(biāo)系中.

(2)

處理激光雷達(dá)數(shù)據(jù)時(shí)，可以將周圍環(huán)境的信息抽象的表示為點(diǎn)、線等特征[8].在掃描過(guò)的環(huán)境信息中可以得到線性與非線性的數(shù)據(jù)，若是線性的數(shù)據(jù)則直接化為直線進(jìn)行處理；若是非線性的數(shù)據(jù)則需要將這些數(shù)據(jù)變換為折線特征(即直線加角點(diǎn))，同時(shí)采用迭代的方法，對(duì)非線性區(qū)域中的角點(diǎn)進(jìn)行尋找，直到掃描區(qū)域內(nèi)不存在角點(diǎn)，由公式(3)可以計(jì)算出角點(diǎn)之間的距離.這個(gè)過(guò)程不斷往復(fù)循環(huán)，直到最后可以形成2D環(huán)境地圖.

db=|(xi-x1)sinδ+(yi-y1)cosδ|

(3)

式(3)中：db為角點(diǎn)間的距離，(x1,y1)為機(jī)器人當(dāng)前的位置坐標(biāo)，(xi,yi)為掃描到的目標(biāo)點(diǎn)的坐標(biāo)，δ為角點(diǎn)之間形成的連線與X軸的夾角.

1.2 深度相機(jī)數(shù)據(jù)處理

深度相機(jī)的數(shù)據(jù)處理主要是將深度相機(jī)獲取到的圖像數(shù)據(jù)轉(zhuǎn)換為模擬激光數(shù)據(jù)[9].深度相機(jī)能夠通過(guò)相機(jī)模型將獲取到的深度圖像數(shù)據(jù)轉(zhuǎn)換成為模擬的二維激光雷達(dá)數(shù)據(jù)，實(shí)現(xiàn)將三維數(shù)據(jù)壓縮為二維數(shù)據(jù)的功能，從而完成傳感器之間的數(shù)據(jù)融合.

深度圖像數(shù)據(jù)轉(zhuǎn)換模擬激光數(shù)據(jù)的步驟如下：

(1)計(jì)算圖像坐標(biāo)系下的點(diǎn)m(u,v)在世界坐標(biāo)系下的位置坐標(biāo)M(XW,YW,ZW) .深度數(shù)據(jù)轉(zhuǎn)激光數(shù)據(jù)的原理如圖3所示.

圖3 深度轉(zhuǎn)激光原理圖

利用相機(jī)模型能夠完成世界坐標(biāo)系與圖像坐標(biāo)系之間的轉(zhuǎn)換[10]，如式(4)所示.

(4)

式(4)中：(u,v)為圖像坐標(biāo)系中的某一坐標(biāo)點(diǎn)，(u0,v0)為圖像坐標(biāo)系中心點(diǎn)，(xw,yw,zw)表示該點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，其中zc表示目標(biāo)到相機(jī)的距離.R、T為攝像機(jī)的外參數(shù)，用來(lái)表示旋轉(zhuǎn)向量和平移向量；f為內(nèi)參數(shù)，通過(guò)攝像機(jī)標(biāo)定得到.

由于世界坐標(biāo)系的原點(diǎn)與相機(jī)的原點(diǎn)重合，因此沒(méi)有平移和旋轉(zhuǎn)向量，所以外參數(shù)矩陣R、T可以設(shè)置為

(5)

由于在相機(jī)坐標(biāo)系和世界坐標(biāo)系下對(duì)于同一個(gè)目標(biāo)的深度值相同，即ZC=ZW.因此公式可進(jìn)一步簡(jiǎn)化為：

(6)

三維空間中任意一點(diǎn)的坐標(biāo)，都能夠從以上的變換矩陣公式推導(dǎo)得出，因此，圖像點(diǎn)[u,v]T到世界坐標(biāo)點(diǎn)[XW,YW,ZW]T的變換公式可以表示為：

(7)

(2)計(jì)算直線AO和DO的夾角AOD，計(jì)算公式如式(8)所示.

θ=arctan(x/z)

(8)

(3)將角AOD投影到對(duì)應(yīng)的激光掃描范圍內(nèi).已知激光雷達(dá)的掃描范圍為[α,β]，將該范圍內(nèi)的光束分為N份，則可用數(shù)組laser[N]來(lái)表示[α,β]內(nèi)的所有激光數(shù)據(jù).因此點(diǎn)M投影到數(shù)組laser中的索引值n可用式(9)計(jì)算.

n=(θ-α)/((β-α)/N)=N(θ-α)/(β-α)

(9)

laser[n]的值為M在x軸上投影的點(diǎn)D到相機(jī)光心O的距離r，即

(10)

因此，以激光雷達(dá)數(shù)據(jù)格式為標(biāo)準(zhǔn)對(duì)深度數(shù)據(jù)進(jìn)行轉(zhuǎn)換時(shí)，通過(guò)將深度圖像在水平范圍內(nèi)取整數(shù)角度上的最小深度值作為激光雷達(dá)距離數(shù)據(jù)，相當(dāng)于取三維深度數(shù)據(jù)的水平投影，并加上對(duì)應(yīng)角度值，這樣就完成了深度數(shù)據(jù)轉(zhuǎn)換為激光雷達(dá)數(shù)據(jù)的過(guò)程，再將數(shù)據(jù)保存到激光傳感器獲取的數(shù)組中，完成了深度相機(jī)關(guān)于前方障礙物距離信息的獲取.Ranges[]數(shù)組體現(xiàn)了橫向每個(gè)障礙點(diǎn)到相機(jī)的距離，完成數(shù)據(jù)獲取后，將Ranges[]數(shù)組中關(guān)于同一角度的較大的數(shù)據(jù)都用小數(shù)據(jù)來(lái)替換，這樣就完成了對(duì)于障礙物在高度上的壓縮，形成了二維平面地圖.

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

當(dāng)單獨(dú)使用激光雷達(dá)或深度相機(jī)進(jìn)行地圖構(gòu)建時(shí)都會(huì)存在一定的誤差，因此本文先采用擴(kuò)展卡爾曼濾波將激光雷達(dá)的數(shù)據(jù)與深度相機(jī)的數(shù)據(jù)進(jìn)行融合，再利用貝葉斯估計(jì)的方法將深度相機(jī)與激光雷達(dá)分別生成的地圖進(jìn)行二次融合，來(lái)提高建圖的準(zhǔn)確性.

2.1 基于擴(kuò)展卡爾曼濾波的數(shù)據(jù)融合

在本文設(shè)計(jì)的方案中，利用擴(kuò)展卡爾曼濾波器融合傳感器的數(shù)據(jù)時(shí)，因?yàn)榧す饫走_(dá)的測(cè)量精度相較于深度相機(jī)更高，所以將激光雷達(dá)的測(cè)量值作為觀測(cè)值，將深度相機(jī)的測(cè)量值作為預(yù)測(cè)值，這樣能保證融合效果更好.令Xt=[xt,yt,θt]T作為系統(tǒng)的狀態(tài)變量，擴(kuò)展卡爾曼濾波數(shù)據(jù)融合的過(guò)程如下：

(1)預(yù)測(cè)階段

(11)

(12)

(2)觀測(cè)階段

將激光雷達(dá)的測(cè)量值ZS作為系統(tǒng)測(cè)量值，

ZS=Ht+1Xt+1

(13)

式(13)中：Ht+1為激光雷達(dá)的觀測(cè)矩陣.

(3)觀測(cè)的預(yù)測(cè)階段

將深度相機(jī)的測(cè)量值ZV作為預(yù)測(cè)的觀測(cè)值，

(14)

式(14)中：Ht+1為深度相機(jī)觀測(cè)矩陣，且與激光雷達(dá)的觀測(cè)矩陣相同.

(4)數(shù)據(jù)關(guān)聯(lián)階段

在對(duì)激光雷達(dá)和深度相機(jī)進(jìn)行數(shù)據(jù)融合之前，還需要解決兩個(gè)傳感器的數(shù)據(jù)關(guān)聯(lián)問(wèn)題，目的是辨別兩個(gè)傳感器獲取的信息是不是源自于外界環(huán)境中的同一個(gè)物體[11].這是數(shù)據(jù)融合的前提，如果數(shù)據(jù)關(guān)聯(lián)出錯(cuò)會(huì)導(dǎo)致對(duì)于環(huán)境目標(biāo)的獲取出現(xiàn)錯(cuò)誤，最后無(wú)法構(gòu)建出完整的地圖.

根據(jù)激光雷達(dá)與深度相機(jī)獲取的觀測(cè)信息ZS，ZV之間的Mahalanobis距離[12]，來(lái)判斷兩個(gè)傳感器獲取到的數(shù)據(jù)的關(guān)聯(lián)性，當(dāng)距離小于設(shè)定閾值χ2時(shí)，即則說(shuō)明關(guān)聯(lián)性較高.

(15)

τt=ZS-ZV

(16)

式(15)、(16)中：d=rank(τij)，α為可信度，τt為測(cè)量的位置誤差.當(dāng)ZS，ZV中有多個(gè)特征相匹配時(shí)，則選取距離最近的兩個(gè)特征作為最佳匹配，當(dāng)完成了數(shù)據(jù)關(guān)聯(lián)后就能夠完成狀態(tài)的更新.

(5)更新階段

首先計(jì)算卡爾曼增益Kg，利用多傳感器的測(cè)量值修正之前的系統(tǒng)預(yù)測(cè)值，完成激光雷達(dá)采集的數(shù)據(jù)和深度相機(jī)采集的數(shù)據(jù)之間的融合，

(17)

再利用卡爾曼增益計(jì)算傳感器數(shù)據(jù)融合的最優(yōu)估計(jì)值，

(18)

最后利用卡爾曼增益完成對(duì)協(xié)方差矩陣的更新，

(19)

2.2 基于貝葉斯估計(jì)的地圖融合

在通過(guò)擴(kuò)展卡爾曼濾波完成對(duì)于傳感器的數(shù)據(jù)融合后，如果激光雷達(dá)與深度相機(jī)觀測(cè)到的信息沒(méi)有完成數(shù)據(jù)關(guān)聯(lián)，那么系統(tǒng)狀態(tài)不會(huì)完成更新，之后兩個(gè)傳感器的數(shù)據(jù)也不會(huì)進(jìn)行融合，環(huán)境中的某些障礙物就不會(huì)被檢測(cè)出來(lái)，進(jìn)而導(dǎo)致地圖構(gòu)建出現(xiàn)誤差.因此在對(duì)兩個(gè)傳感器獲取的數(shù)據(jù)完成一次融合后，還應(yīng)該在分別生成局部地圖之后，將兩個(gè)傳感器分別生成的局部地圖進(jìn)行二次融合，以保證構(gòu)建的地圖的準(zhǔn)確性.

本文利用貝葉斯估計(jì)的方法將激光雷達(dá)和深度相機(jī)獲得的局部地圖完成二次融合.貝葉斯估計(jì)通過(guò)將傳感器采集的環(huán)境信息轉(zhuǎn)換成后驗(yàn)概率分布函數(shù)[13]，并通過(guò)貝葉斯條件概率公式更新后驗(yàn)概率分布值，從而實(shí)現(xiàn)對(duì)多傳感器的信息融合[14].

貝葉斯估計(jì)可以描述為：通過(guò)已知的向量Z,估計(jì)未知的m維狀態(tài)向量X并完成對(duì)于后驗(yàn)概率的計(jì)算，已知t時(shí)刻的概率為p=(xt)，且k組觀測(cè)數(shù)據(jù)為Zk=(Z1,…,Zk)，先驗(yàn)分布公式如下：

(20)

式(20)中：p(zt|xt)是基于傳感器觀測(cè)模型的似然函數(shù)；p(xt|zt-1)是基于系統(tǒng)模型得到的前一時(shí)刻的先驗(yàn)分布；p(zt|zt-1)表示當(dāng)前時(shí)刻與上一時(shí)刻之間的狀態(tài)變化.

在建圖的初始階段，由于環(huán)境信息未知，因此地圖中存在障礙物和無(wú)障礙物的先驗(yàn)概率初始化均為0.5.隨著建圖的進(jìn)行，利用傳感器不斷掃描周圍環(huán)境，通過(guò)多傳感器獲得的環(huán)境信息并利用貝葉斯估計(jì)得到傳感器信息融合的公式如下所示：

(21)

具體融合規(guī)則如表1所示，柵格地圖屬性分為三類：占據(jù)(O)、未占據(jù)(E)以及未知(U).

表1 激光和視覺(jué)傳感器融合規(guī)則

通過(guò)本文提出的方法得到的傳感器數(shù)據(jù)融合構(gòu)建地圖的流程如圖4所示.

圖4 數(shù)據(jù)融合構(gòu)建地圖流程

3 移動(dòng)機(jī)器人的地圖構(gòu)建

完成對(duì)傳感器數(shù)據(jù)處理之后，可以利用融合后的傳感器數(shù)據(jù)，采用基于圖優(yōu)化的算法完成地圖的構(gòu)建.基于圖優(yōu)化的地圖構(gòu)建算法由圖構(gòu)建和圖優(yōu)化構(gòu)成，如圖5所示.其中圖構(gòu)建包括相鄰兩幀傳感器數(shù)據(jù)之間的匹配與回環(huán)檢測(cè)，圖優(yōu)化包括對(duì)于運(yùn)動(dòng)姿態(tài)的優(yōu)化[15].

圖5 基于圖優(yōu)化的SLAM算法框架

如圖6所示，基于圖優(yōu)化的方法通過(guò)對(duì)機(jī)器人的位姿和地圖節(jié)點(diǎn)之間的最小均方誤差的求解來(lái)完成對(duì)SLAM問(wèn)題的求解：將各個(gè)時(shí)刻的位姿xt及地圖m中的路標(biāo)點(diǎn)(P1,P2,…,Pn) 看成一個(gè)個(gè)待優(yōu)化的節(jié)點(diǎn)，將相鄰位姿節(jié)點(diǎn)間的邊作為一個(gè)約束條件，將位姿節(jié)點(diǎn)和路標(biāo)節(jié)點(diǎn)間的邊作為另一個(gè)約束條件[16]，因此，建圖的過(guò)程主要是通過(guò)傳感器對(duì)環(huán)境的掃描匹配來(lái)建立位姿節(jié)點(diǎn)和路標(biāo)節(jié)點(diǎn)之間的約束關(guān)系，當(dāng)滿足這個(gè)約束關(guān)系時(shí)，找到最佳的位姿節(jié)點(diǎn)xt和地圖m值，同時(shí)讓總體的誤差平方和最小，以此來(lái)完成地圖的構(gòu)建[17].

圖6 基于圖優(yōu)化模型的SLAM

在機(jī)器人仿真軟件 中搭建環(huán)境模型，并分別對(duì)基于濾波和基于圖優(yōu)化的地圖構(gòu)建算法進(jìn)行仿真.其中，如圖7(a)所示為仿真環(huán)境圖，圖7(b)為濾波算法創(chuàng)建的環(huán)境地圖，圖7(c)為圖優(yōu)化算法創(chuàng)建的環(huán)境地圖.通過(guò)對(duì)比圖7(b)和圖7(c)可以發(fā)現(xiàn)，基于濾波的建圖算法在構(gòu)建地圖的過(guò)程中，由于數(shù)據(jù)不能及時(shí)更新，導(dǎo)致誤差不斷被累積[18]，因此構(gòu)建出的地圖與實(shí)際地圖相差較大.而基于圖優(yōu)化算法構(gòu)建的地圖，通過(guò)對(duì)傳感器的連續(xù)幀數(shù)據(jù)的提取，完成對(duì)位姿軌跡的確定[19]，并構(gòu)建出位姿圖，同時(shí)將得到的位姿圖用回環(huán)檢測(cè)進(jìn)行校正，最后通過(guò)位姿優(yōu)化消除建圖中產(chǎn)生的誤差，得到與仿真環(huán)境基本一致的地圖.

(a) GAZEBO仿真環(huán)境圖

(b) 濾波算法創(chuàng)建的地圖

(c) 圖優(yōu)化算法創(chuàng)建的地圖圖7 實(shí)際地圖及算法仿真對(duì)比圖

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

本文利用一臺(tái)履帶式的移動(dòng)機(jī)器人進(jìn)行實(shí)驗(yàn)，如圖8所示.系統(tǒng)硬件框圖如圖9所示，系統(tǒng)由上位機(jī)和下位機(jī)兩部分構(gòu)成，其中下位機(jī)系統(tǒng)由STM32、電機(jī)驅(qū)動(dòng)電路、慣性測(cè)量單元組成，上位機(jī)系統(tǒng)由樹莓派、深度相機(jī)和激光雷達(dá)組成，通過(guò)樹莓派的WIFI模塊可以遠(yuǎn)程連接PC端，并且通過(guò)PC端完成對(duì)機(jī)器人的遠(yuǎn)程控制，最后，環(huán)境地圖的構(gòu)建結(jié)果將在PC端的Rviz中顯示[20].

實(shí)驗(yàn)環(huán)境如圖10(a)和圖11(a)所示，分別是大小為6.0 mm×0.8 m的簡(jiǎn)單走廊環(huán)境和大小為6.0 m×5.0 m的較復(fù)雜室內(nèi)環(huán)境，其中室內(nèi)環(huán)境包含桌椅、箱子等障礙物.在實(shí)際應(yīng)用時(shí)，由于二維激光雷達(dá)獲取的信息會(huì)受到安裝位置的影響，深度相機(jī)獲取的信息會(huì)受到環(huán)境光線的影響，因此都會(huì)出現(xiàn)因?yàn)槁z障礙物而導(dǎo)致構(gòu)建的環(huán)境地圖精度不高的情況，所以通過(guò)設(shè)計(jì)這兩種環(huán)境進(jìn)行地圖構(gòu)建實(shí)驗(yàn)，將兩個(gè)傳感器獲取的信息相融合，來(lái)驗(yàn)證本文提出的方法的可靠性.

圖8 移動(dòng)機(jī)器人模型

圖9 系統(tǒng)硬件框圖

圖10(b)和圖11(b)為僅使用激光雷達(dá)對(duì)環(huán)境進(jìn)行建圖.可以看出，當(dāng)只使用激光雷達(dá)進(jìn)行建圖時(shí)，隨著時(shí)間的推移，建圖誤差越來(lái)越大，構(gòu)建出的地圖與實(shí)際環(huán)境不一致，并且由于受激光雷達(dá)安裝位置的影響，有些障礙物如椅子(B、C、D、E)的某些部分沒(méi)有在掃描平面內(nèi)，因此導(dǎo)致掃描獲得的地圖信息不夠全面，造成建圖精度低、噪點(diǎn)多的問(wèn)題.

圖10(c)和圖11(c)為僅使用深度相機(jī)對(duì)環(huán)境進(jìn)行建圖.當(dāng)只有深度相機(jī)完成地圖構(gòu)建時(shí)，雖然深度相機(jī)相對(duì)于激光傳感器能夠獲取到環(huán)境的三維信息，對(duì)于障礙物的檢測(cè)更加準(zhǔn)確，但是深度相機(jī)的拍攝精度容易受到環(huán)境光線的影響，因此在建圖的準(zhǔn)確性上較差.

將深度相機(jī)與激光雷達(dá)的數(shù)據(jù)融合以后，構(gòu)建的地圖如圖10(d)和圖11(d)所示.通過(guò)對(duì)比可以發(fā)現(xiàn)，將兩個(gè)傳感器獲取的信息融合之后，有效地彌補(bǔ)了單一傳感器掃描環(huán)境的局限性，克服了復(fù)雜環(huán)境的干擾，使構(gòu)建的地圖更加完整，構(gòu)建的地圖與真實(shí)環(huán)境更接近，同時(shí)地圖上的噪點(diǎn)也更少.

(a)走廊環(huán)境

(b)激光雷達(dá)構(gòu)建地圖

(c)深度相機(jī)構(gòu)建地圖

(d)數(shù)據(jù)融合后構(gòu)建地圖圖10 走廊環(huán)境下構(gòu)建的地圖

(c)深度相機(jī)構(gòu)建地圖

(d)數(shù)據(jù)融合后構(gòu)建地圖圖11 室內(nèi)環(huán)境下構(gòu)建的地圖

移動(dòng)機(jī)器人對(duì)于環(huán)境地圖的構(gòu)建精度測(cè)試可以轉(zhuǎn)換為機(jī)器人在環(huán)境中移動(dòng)時(shí)對(duì)于運(yùn)動(dòng)軌跡的精度測(cè)試[21].機(jī)器人在不同環(huán)境下的運(yùn)動(dòng)軌跡的對(duì)比圖，如圖12(a)和圖13(a)所示.圖12(b)和圖13(b)所示為數(shù)據(jù)融合前后機(jī)器人運(yùn)動(dòng)軌跡的均方根誤差對(duì)比，可以看出，數(shù)據(jù)融合后的機(jī)器人運(yùn)動(dòng)軌跡明顯優(yōu)于融合之前，并且走廊環(huán)境下的均方根誤差在3 cm左右，室內(nèi)環(huán)境下的均方根誤差在8 cm左右，均小于數(shù)據(jù)融合之前的誤差，同時(shí)曲線波動(dòng)幅度更小且更穩(wěn)定，表明了數(shù)據(jù)融合之后構(gòu)建的地圖更精確，更符合真實(shí)環(huán)境，從而驗(yàn)證了本文算法的準(zhǔn)確性.

(a)走廊環(huán)境下不同軌跡對(duì)比

(b)數(shù)據(jù)融合前后的均方根誤差圖12 走廊環(huán)境下運(yùn)動(dòng)軌跡和誤差對(duì)比

(a) 室內(nèi)環(huán)境下不同軌跡對(duì)比

(b) 數(shù)據(jù)融合前后的均方根誤差圖13 室內(nèi)環(huán)境下運(yùn)動(dòng)軌跡和誤差對(duì)比

5 結(jié)論

本文提出了一種將激光雷達(dá)和深度相機(jī)信息融合的地圖構(gòu)建方法，先利用擴(kuò)展卡爾曼濾波器將移動(dòng)機(jī)器人獲取的深度視覺(jué)數(shù)據(jù)與激光雷達(dá)的數(shù)據(jù)進(jìn)行融合，再利用貝葉斯估計(jì)的方法將激光雷達(dá)與深度相機(jī)分別建立的局部地圖進(jìn)行二次融合，并在構(gòu)建地圖時(shí)采用基于圖優(yōu)化的地圖構(gòu)建算法，在完全未知的環(huán)境中，完成對(duì)環(huán)境地圖的準(zhǔn)確構(gòu)建.實(shí)驗(yàn)結(jié)果表明，利用本文提出的多傳感器信息融合的方法，獲得的環(huán)境信息更加全面，提高了地圖構(gòu)建的可靠性和準(zhǔn)確性，并且有效提高了移動(dòng)機(jī)器人在SLAM時(shí)的魯棒性.