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

李 艷, 唐達明, 戴慶瑜

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

0 引言

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

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

圖1 SLAM問題示意圖

SLAM問題用數(shù)學語言描述為求解如下概率分布：

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

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

(1)

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

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

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

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

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

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

圖2 激光雷達掃描測量示意圖

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

(2)

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

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

(3)

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

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

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

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

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

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

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

(4)

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

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

(5)

由于在相機坐標系和世界坐標系下對于同一個目標的深度值相同，即ZC=ZW.因此公式可進一步簡化為：

(6)

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

(7)

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

θ=arctan(x/z)

(8)

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

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

(9)

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

(10)

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

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

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

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

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

(1)預測階段

(11)

(12)

(2)觀測階段

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

ZS=Ht+1Xt+1

(13)

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

(3)觀測的預測階段

將深度相機的測量值ZV作為預測的觀測值，

(14)

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

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

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

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

(15)

τt=ZS-ZV

(16)

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

(5)更新階段

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

(17)

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

(18)

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

(19)

2.2 基于貝葉斯估計的地圖融合

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

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

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

(20)

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

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

(21)

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

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

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

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

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

完成對傳感器數(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)檢測，圖優(yōu)化包括對于運動姿態(tài)的優(yōu)化[15].

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

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

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

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

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

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

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

4 實驗結(jié)果及分析

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

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

圖8 移動機器人模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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