李 艷, 唐達(dá)明, 戴慶瑜
(1.陜西科技大學(xué) 電氣與控制工程學(xué)院, 陜西 西安 710021; 2.陜西農(nóng)產(chǎn)品加工技術(shù)研究院, 陜西 西安 710021)
未知環(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)境更加接近.
激光雷達(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軸的夾角.
深度相機(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ì)于障礙物在高度上的壓縮,形成了二維平面地圖.
當(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)確性.
在本文設(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)
在通過(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)建地圖流程
完成對(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ì)比圖
本文利用一臺(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ì)比
本文提出了一種將激光雷達(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í)的魯棒性.