融合激光與視覺點(diǎn)云信息的定位與建圖方法

張偉偉 陳 超 徐 軍

(江蘇科技大學(xué)機(jī)械工程學(xué)院 江蘇 鎮(zhèn)江 212003)

0 引 言

同步定位與建圖(Simultaneous Localization And Mapping，SLAM)主要指機(jī)器人在未知環(huán)境中，通過攜帶的傳感器獲取外界信息以完成對(duì)自身位姿的估計(jì)并構(gòu)建出環(huán)境地圖[1]，是機(jī)器人自主化的重要技術(shù)。

激光雷達(dá)和視覺相機(jī)是目前SLAM常用的兩種傳感器。其中，以二維激光雷達(dá)為基礎(chǔ)的SLAM[2]能快速構(gòu)建出精確的地圖，但其只能對(duì)某一平面進(jìn)行掃描，會(huì)忽略一部分障礙物信息。基于三維視覺傳感器的SLAM[3]能獲取豐富的環(huán)境信息，但探測范圍有限且系統(tǒng)實(shí)時(shí)性較差。為應(yīng)對(duì)使用單一傳感器進(jìn)行SLAM所帶來的局限性、提升機(jī)器人定位精度與地圖質(zhì)量，Chen等[4]通過在機(jī)器人上垂直安裝二維激光雷達(dá)并融合深度相機(jī)采集到的信息實(shí)現(xiàn)了地圖的精確構(gòu)建，但定位精度較低。Ding等[5]提出了混合光束法平差框架，將激光與視覺建立的地圖通過該框架進(jìn)行融合對(duì)齊，有效提高地圖構(gòu)建精度，然而較大的計(jì)算量降低了運(yùn)行效率。文獻(xiàn)[6]利用視覺信息進(jìn)行幀間匹配并以此約束激光點(diǎn)云之間的數(shù)據(jù)關(guān)聯(lián)，通過迭代最近點(diǎn)法實(shí)現(xiàn)了對(duì)機(jī)器人運(yùn)動(dòng)軌跡的準(zhǔn)確估計(jì)卻無法建立精確的地圖。

針對(duì)上述問題，本文在融合激光與視覺傳感器點(diǎn)云數(shù)據(jù)[7]的基礎(chǔ)上，創(chuàng)新性地提出一種分步式點(diǎn)云配準(zhǔn)方法對(duì)點(diǎn)云進(jìn)行匹配以實(shí)現(xiàn)機(jī)器人自定位和同步地圖構(gòu)建；結(jié)合改進(jìn)的雙重閉環(huán)檢測方法可進(jìn)一步提高定位與建圖精度。搭建的移動(dòng)機(jī)器人平臺(tái)在實(shí)際環(huán)境下的實(shí)驗(yàn)結(jié)果驗(yàn)證了本文方法的有效性。

1 數(shù)據(jù)獲取

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

(1)

式中：利用i與傳感器的角度分辨率即可得到θi。本文所用激光雷達(dá)角度分辨率0.36°、探測距離0.06～5.60 m、掃描范圍240°、測量精度±30 mm。

1.2 視覺傳感器數(shù)據(jù)

Kinect相機(jī)水平探測范圍為57°，垂直探測范圍為43°，探測距離為0.8～3.5 m，可同時(shí)獲取環(huán)境的顏色和深度信息[8]。采用文獻(xiàn)[9]中的標(biāo)定方法對(duì)Kinect進(jìn)行標(biāo)定，以此獲得相機(jī)的內(nèi)參數(shù)矩陣，完成校準(zhǔn)后，圖像中任意點(diǎn)的三維坐標(biāo)可根據(jù)對(duì)應(yīng)的深度信息得到，進(jìn)而生成三維點(diǎn)云數(shù)據(jù)。對(duì)于深度圖中的任意像素點(diǎn)(xd,yd)，其三維坐標(biāo)計(jì)算如下：

(2)

式中：depth(xd,yd)代表該像素點(diǎn)所對(duì)應(yīng)的深度值，(cxd,cyd)表示圖像光心坐標(biāo)，由標(biāo)定可得相機(jī)焦距fxd與fyd。

2 數(shù)據(jù)融合

2.1 邊界提取

考慮到機(jī)器人在導(dǎo)航時(shí)只需檢測到環(huán)境中的障礙物，沒必要對(duì)障礙物進(jìn)行詳細(xì)的描述，因此本文只對(duì)物體的幾何輪廓進(jìn)行檢測即提取出邊界點(diǎn)云信息。機(jī)器人搭載的Kinect相機(jī)可直接獲取深度圖像，通過遍歷深度圖像中的每個(gè)點(diǎn)，比較該點(diǎn)與其鄰域點(diǎn)的距離變化信息，根據(jù)評(píng)分判斷該點(diǎn)是否為邊界點(diǎn)。步驟如下：

(1) 對(duì)于圖像中每個(gè)點(diǎn)pi，構(gòu)建以其為中心邊長S=5的鄰域，假設(shè)至少有Ν=((S+1)/2)2=9個(gè)點(diǎn)與pi共面。

(3) 計(jì)算點(diǎn)pi與其頂部、左側(cè)、右側(cè)以及底部鄰域內(nèi)點(diǎn)的歐式距離。以右側(cè)為例，px,y是圖像中坐標(biāo)為(x,y)的一點(diǎn)，其右側(cè)鄰域內(nèi)點(diǎn)的平均位置為：

(3)

(4)

(5)

式中：Sshadow表示潛在陰影邊界的評(píng)分。

2.2 聯(lián)合標(biāo)定

在對(duì)兩傳感器進(jìn)行數(shù)據(jù)融合之前，要先解決二者之間的標(biāo)定問題，即建立二維激光數(shù)據(jù)點(diǎn)坐標(biāo)與三維視覺數(shù)據(jù)點(diǎn)坐標(biāo)之間的變換關(guān)系。本文采用文獻(xiàn)[10]方法對(duì)兩不同類型的傳感器進(jìn)行標(biāo)定以此獲得兩不同坐標(biāo)系間的旋轉(zhuǎn)矩陣R和平移矩陣t。若一空間點(diǎn)在激光坐標(biāo)系為PL、相機(jī)坐標(biāo)系下為PC，則坐標(biāo)間的變換關(guān)系為：

PC=RPL+t

(6)

2.3 點(diǎn)云融合

利用聯(lián)合標(biāo)定得到的兩傳感器間的旋轉(zhuǎn)平移矩陣，可通過將兩者獲取的點(diǎn)云轉(zhuǎn)換到同一坐標(biāo)系下來進(jìn)行數(shù)據(jù)的融合，具體步驟如下：

(1) 擬合平面方程：將激光坐標(biāo)系下二維點(diǎn)云利用式(6)轉(zhuǎn)換到相機(jī)坐標(biāo)系并通過平面擬合構(gòu)建包含相機(jī)坐標(biāo)系原點(diǎn)的平面方程：ax+by+cz=d。

(2) 去除地面點(diǎn)云：相機(jī)采集到的點(diǎn)云數(shù)據(jù)中亦包含地面點(diǎn)云，地面點(diǎn)云不屬于機(jī)器人導(dǎo)航時(shí)的障礙物信息，因此要將該部分點(diǎn)云去除。

(3) 三維點(diǎn)云投影：通過忽略相機(jī)獲取的描述空間障礙物的三維點(diǎn)云的Z軸信息，只保留X軸和Y軸信息，可將三維點(diǎn)云投影至步驟(1)中的平面方程，以實(shí)現(xiàn)點(diǎn)云的融合。

3 點(diǎn)云配準(zhǔn)

機(jī)器人利用配準(zhǔn)相鄰幀點(diǎn)云得到的相對(duì)變換關(guān)系估計(jì)自身位姿，從而實(shí)現(xiàn)在環(huán)境中的定位。為提高配準(zhǔn)效率、降低誤差，本文結(jié)合點(diǎn)對(duì)去除策略提出一種分步式配準(zhǔn)方法,具體步驟如下：

(1) 點(diǎn)云初配準(zhǔn)。對(duì)點(diǎn)云模型進(jìn)行均勻網(wǎng)格化處理，計(jì)算格中點(diǎn)xk(n為點(diǎn)的個(gè)數(shù))的均值向量與協(xié)方差矩陣，分別用q、C表示。

(7)

(8)

利用N(q,C)對(duì)點(diǎn)xk進(jìn)行正態(tài)分布化處理，則其概率密度函數(shù)為：

(9)

用一組正態(tài)分布{N(μ,Σ)}表示點(diǎn)云模型并將待配準(zhǔn)點(diǎn)云按轉(zhuǎn)移矩陣T進(jìn)行變換。利用Hessian矩陣對(duì)兩點(diǎn)云模型對(duì)應(yīng)的正態(tài)分布{N(μ,Σ)}的概率密度值進(jìn)行優(yōu)化即得相對(duì)變換矩陣，記為Tini。

(2) 點(diǎn)對(duì)去除。經(jīng)初始配準(zhǔn)后的兩點(diǎn)云中存在誤匹配點(diǎn)對(duì)，在進(jìn)行ICP精配準(zhǔn)之前要將該部分點(diǎn)對(duì)去除。對(duì)于源點(diǎn)云中的點(diǎn)xi，其在目標(biāo)點(diǎn)云中的對(duì)應(yīng)點(diǎn)為yi：

(10)

式中：超過距離閾值dk的點(diǎn)對(duì)(xi,yi)需被去除且距離閾值隨迭代過程以步長δd逐漸下降：

(11)

dk=dk-1-δd

(12)

式(11)中：Tk為第k次迭代所得的變換矩陣。

(13)

(14)

(3) 點(diǎn)云精確配準(zhǔn)。為避免ICP算法陷入局部極值，以初始匹配得到的變換矩陣作為ICP精確配準(zhǔn)的初值。目標(biāo)誤差函數(shù)為：

(15)

式中：(xi,yi)表示滿足條件的關(guān)聯(lián)點(diǎn)對(duì)；n為點(diǎn)對(duì)個(gè)數(shù)；Tk是第k迭代次時(shí)的變換矩陣。令T0=Tini并通過四元數(shù)法計(jì)算使目標(biāo)函數(shù)E最小的變換參數(shù)T。

(4) 將T作用于兩待配準(zhǔn)點(diǎn)云，求取轉(zhuǎn)換誤差并判斷是否小于誤差閾值，若小于則終止迭代；否則返回步驟(2)繼續(xù)迭代。

4 閉環(huán)與優(yōu)化

4.1 閉環(huán)檢測

機(jī)器人不斷運(yùn)動(dòng)會(huì)使其自身位姿的累計(jì)誤差不斷增大，因此需進(jìn)行閉環(huán)檢測以判斷機(jī)器人是否處于之前到過的位置，為全局位姿優(yōu)化提供約束。本文結(jié)合關(guān)鍵幀機(jī)制提出一種雙重閉環(huán)檢測方法，具體步驟如下：

(1) 將傳感器采集的第一幀作為關(guān)鍵幀，設(shè)置一定的時(shí)間間隔。若當(dāng)前幀獲取時(shí)間為tc，前一關(guān)鍵幀的獲取時(shí)間為tp，則要滿足tc-tp≤εt，令εt=0.5 s。

(2) 通過正態(tài)分布點(diǎn)云匹配算法可得每一關(guān)鍵幀的位姿。計(jì)算當(dāng)前幀位姿與歷史幀位姿的距離，將距離小于d0的幀作為候選幀。

(16)

(3) 將當(dāng)前幀與在候選幀中隨機(jī)挑選的4～6幀進(jìn)行ICP匹配。

(17)

4.2 圖優(yōu)化

以每一關(guān)鍵幀位姿為節(jié)點(diǎn)、幀間變換為邊構(gòu)建位姿圖，通過SLAM++優(yōu)化庫[11]進(jìn)行優(yōu)化可得全局一致的機(jī)器人運(yùn)動(dòng)軌跡；將每一幀點(diǎn)云進(jìn)行拼接并映射到2D柵格可構(gòu)建二維環(huán)境地圖。

5 實(shí) 驗(yàn)

如圖1所示，實(shí)驗(yàn)以TurtleBot2移動(dòng)機(jī)器人為平臺(tái)，其上搭載激光雷達(dá)和Kinect傳感器。使用筆記本電腦進(jìn)行控制和數(shù)據(jù)處理，算法運(yùn)行在基于Ubuntu14.04的ROS[12]操作系統(tǒng)下并通過RVIZ顯示建圖與導(dǎo)航過程。實(shí)驗(yàn)場地長5.1 m，寬2.4 m，其中設(shè)置有大小不一、形狀各異的四個(gè)物體(已編號(hào))，用以模擬復(fù)雜環(huán)境。

圖1 實(shí)驗(yàn)平臺(tái)及環(huán)境

5.1 環(huán)境建圖

對(duì)于外形較為復(fù)雜的椅子D，只需提取出其邊界點(diǎn)云信息即可，如圖2所示，利用深度點(diǎn)云圖得到的椅子輪廓更為簡化。

圖2 邊界提取

圖3為局部場景的點(diǎn)云數(shù)據(jù)融合結(jié)果，該場景中僅有椅子D一個(gè)物體。Kinect采集到的數(shù)據(jù)通過邊界點(diǎn)云提取與點(diǎn)云投影得到較為準(zhǔn)確的椅子邊緣輪廓并構(gòu)建出場景的二維地圖；激光雷達(dá)雖能構(gòu)建更加精確的環(huán)境地圖，但無法對(duì)椅子D進(jìn)行準(zhǔn)確描述。將兩者數(shù)據(jù)進(jìn)行融合，在得到精確地圖的同時(shí),也能準(zhǔn)確探測出場景中的復(fù)雜物體。

圖3 點(diǎn)云數(shù)據(jù)融合

圖4是在整個(gè)實(shí)驗(yàn)場地中分別使用單一傳感器方式、本文融合方式和文獻(xiàn)[6]方式建立的二維地圖。由于掃描高度的限制，激光雷達(dá)無法探測到箱子C且只能得到椅子的腿部信息(如圖5所示)；而Kinect則可以檢測到環(huán)境中的所有物體，但構(gòu)建的地圖精度較低。本文通過融合兩傳感器，可對(duì)環(huán)境中物體進(jìn)行完整全面的探測并且構(gòu)建的地圖相較文獻(xiàn)[6]更為精確。

圖4 四種方式地圖構(gòu)建

圖5 激光雷達(dá)與物體位置

5.2 定位分析

本文利用點(diǎn)云配準(zhǔn)法來實(shí)現(xiàn)機(jī)器人位姿估計(jì)，而文獻(xiàn)[5]采用光束平差法來估計(jì)位姿。為驗(yàn)證本文方法的有效性，分別在機(jī)器人移動(dòng)2、4和6 m時(shí)將兩種方法進(jìn)行比較并得到如表1所示數(shù)據(jù)。

表1 位姿估計(jì)方法比較

相較于文獻(xiàn)[5]方法，本文方法在處理速度上提高約18%且位姿估計(jì)誤差降低約24%。點(diǎn)對(duì)去除策略降低了后續(xù)匹配點(diǎn)對(duì)數(shù)量，使得效率更高，同時(shí)由于初始配準(zhǔn)為后續(xù)精確匹配提供了迭代初值，誤差也大幅下降。

圖6為三種不同方式的定位誤差曲線。僅使用激光雷達(dá)時(shí)，由于其只能獲取特定平面的點(diǎn)云，使得用于配準(zhǔn)的點(diǎn)云量較少而導(dǎo)致位姿估計(jì)出現(xiàn)較大偏差。Kinect能獲取豐富的點(diǎn)云信息，但冗余量大且探測精度較差，降低了位姿估計(jì)的效率。本文將傳感器融合后，在降低數(shù)據(jù)冗余的同時(shí)提高了點(diǎn)云配準(zhǔn)效率，再結(jié)合改進(jìn)的閉環(huán)檢測與優(yōu)化方法，使得定位精度較激光雷達(dá)方式提高約85%、比Kinect方式提高約64%。

圖6 定位誤差曲線

圖7為使用文獻(xiàn)[4]方法和本文方法時(shí)機(jī)器人的運(yùn)動(dòng)軌跡。在進(jìn)行長時(shí)間運(yùn)動(dòng)時(shí)，本文方法依然能保持較高的定位精度，相較于文獻(xiàn)[4]方法優(yōu)勢(shì)明顯。

(a)

(b)圖7 兩種方法的運(yùn)動(dòng)軌跡

5.3 自主導(dǎo)航

利用已構(gòu)建的二維地圖進(jìn)行導(dǎo)航實(shí)驗(yàn)，1為起始點(diǎn)、2為目標(biāo)點(diǎn)，結(jié)果如圖8所示，黑線表示機(jī)器人自主規(guī)劃的路徑。

圖8 不同地圖下的導(dǎo)航

在激光雷達(dá)構(gòu)圖下的導(dǎo)航，由于無法探測到箱子C并準(zhǔn)確識(shí)別椅子D,機(jī)器人在移動(dòng)時(shí)會(huì)與它們發(fā)生碰撞。而使用Kinect地圖進(jìn)行導(dǎo)航時(shí)，機(jī)器人能有效避開場景中所有物體，但由于地圖精度較低使得到達(dá)目標(biāo)點(diǎn)的路徑并非最優(yōu)。本文融合方法構(gòu)圖下的導(dǎo)航，機(jī)器人不僅能準(zhǔn)確規(guī)避障礙物，同時(shí)路徑規(guī)劃也更高效。

6 結(jié) 語

針對(duì)單一傳感器在機(jī)器人SLAM中存在的不足，本文通過將兩種不同傳感器進(jìn)行融合并結(jié)合提出的分步式點(diǎn)云配準(zhǔn)策略與雙重閉環(huán)檢測方法，以實(shí)現(xiàn)機(jī)器人對(duì)自身的定位，同時(shí)構(gòu)建出二維環(huán)境地圖。實(shí)驗(yàn)結(jié)果表明，本文方法能有效提高定位精度且構(gòu)建的地圖更精確、完整，提高了自主導(dǎo)航的可靠性與效率。今后將開展在室外大場景下的SLAM研究,具體改進(jìn)如下：(1) 引入動(dòng)態(tài)物體識(shí)別技術(shù)以探測移動(dòng)物體；(2) 利用多線程技術(shù)提高系統(tǒng)的處理速度；(3) 結(jié)合慣性傳感器進(jìn)一步提高定位精度。