劉 雷,柏艷紅,王 銀,孫志毅

(太原科技大學(xué)電子信息工程學(xué)院,山西 太原 030024)

1 引 言

隨著移動(dòng)機(jī)器人技術(shù)快速發(fā)展,機(jī)器人被應(yīng)用到各種復(fù)雜的環(huán)境,比如搶災(zāi)救險(xiǎn)、配送物流、無人測繪等任務(wù)[1-2]。通常在這些環(huán)境下都沒有預(yù)先建立好地圖環(huán)境信息。而隨著SLAM(simultaneous ocalization and mapping)技術(shù)的發(fā)展,為移動(dòng)機(jī)器人在未知環(huán)境中的定位與構(gòu)圖提供了較好的解決方案[3-6]。依靠高精度激光雷達(dá)及多傳感器融合的技術(shù)也越來越成熟,激光點(diǎn)云的配準(zhǔn)技術(shù)在SLAM技術(shù)框架中起著十分重要的作用[7]。研究具有快速性、魯棒性的配準(zhǔn)方法也有著重要的意義。

目前應(yīng)用最廣且最經(jīng)典的點(diǎn)云配準(zhǔn)算法是迭代最近點(diǎn)(ICP)方法,但在兩個(gè)點(diǎn)云的初始位置差別較大、相鄰重復(fù)率較低的情況下達(dá)不到很好的配準(zhǔn)效果[8]。于是后人開始大量研究如何利用初配準(zhǔn)提供較好的點(diǎn)云初始位置。以特征描述為代表的點(diǎn)特征直方圖(PFH)方法、以及改進(jìn)的快速點(diǎn)特征直方圖(FPFH)方法。文獻(xiàn)[9]提出基于法線特征約束的點(diǎn)云精確配準(zhǔn)方法,采用局部表面擬合方法進(jìn)行法線估計(jì),并計(jì)算其快速點(diǎn)特征直方圖。文獻(xiàn)[10]、[11]提出了一種基于內(nèi)部形態(tài)描述子(ISS)及方向直方圖描述子(SHOT)特征的點(diǎn)云配準(zhǔn)算法,采用ISS算法提取特征點(diǎn),并用SHOT對特征點(diǎn)進(jìn)行描述。文獻(xiàn)[12]提出了一種基于二維圖像特征的點(diǎn)云配準(zhǔn)方法,再采用SURF算法提取點(diǎn)云深度圖的特征,并求解其匹配像素點(diǎn)對。

基于特征描述的點(diǎn)云配準(zhǔn)算法思想是構(gòu)建點(diǎn)云局部鄰域描述子,然后通過建立基于描述子的匹配對,最后通過匹配對估計(jì)變換矩陣。針對當(dāng)前基于特征描述的配準(zhǔn)算法存在的問題,本文提出了一種結(jié)合三維尺度不變特征變換關(guān)鍵點(diǎn)和二進(jìn)制方向直方圖描述子匹配的配準(zhǔn)方法。該方法首先利用差分高斯模型檢測尺度空間的SIFT關(guān)鍵點(diǎn),其次在關(guān)鍵點(diǎn)的鄰域構(gòu)建局部坐標(biāo)系,將查詢點(diǎn)處法線和鄰域點(diǎn)法線夾角特征統(tǒng)計(jì)入直方圖,本文將特征向量轉(zhuǎn)化為二進(jìn)制的描述符。然后利用隨機(jī)采樣一致性算法(RANSAC)去除誤匹配的點(diǎn)對,初步估計(jì)點(diǎn)云配準(zhǔn)的初始變換矩陣,最后在精配準(zhǔn)上利用ICP算法估計(jì)最優(yōu)的變換矩陣。

2 關(guān)鍵點(diǎn)提取算法

局部特征描述SIFT(Scale Invariant Feature Transform)被廣泛應(yīng)用于二維圖像的物體辨識上,該局部特征對旋轉(zhuǎn)、尺度縮放、亮度變化保持不變性,對視角變化、仿射變換、噪聲也保持一定程度的穩(wěn)定性。將2D SIFT算法擴(kuò)展到三維空間形成三維尺度不變特征描述符。首先將三維高斯核函數(shù)與點(diǎn)云模型進(jìn)行卷積,得到該模型的尺度空間,計(jì)算公式如下。

(1)

L(x,y,z,kσ)=G(x,y,z,kσ)?P(x,y,z)

(2)

式中,尺度空間為L(x,y,z,kσ);高斯核函數(shù)為G(x,y,z,kσ);點(diǎn)云中模型表示為P(x,y,z);σ為空間尺度參數(shù);k為尺度大小調(diào)整參數(shù)。其次計(jì)算相鄰尺度層的高斯差分模型,以保證原始點(diǎn)云具有與之相關(guān)的尺度不變性。定義DoG算子計(jì)算公式如下:

DoGi=Li(x,y,z,kσ)-Li-1(x,y,z,kσ)

(3)

式中,i∈(0,S+2),S為高斯金字塔每組的層數(shù)。最后在DoG函數(shù)空間上尋找極值點(diǎn),該極值點(diǎn)是所有高斯差分模型中的極大值點(diǎn)或極小值點(diǎn)。如圖1所示,中間檢測點(diǎn)與同尺度的26個(gè)近鄰點(diǎn)比較,然后同上下相鄰尺度對應(yīng)的27×2個(gè)點(diǎn)比較,只有該檢測點(diǎn)大于或者小于80個(gè)近鄰點(diǎn)時(shí),該點(diǎn)被檢測為SIFT關(guān)鍵點(diǎn)。

(a)σ/k (b)σ (c)kυ

3 關(guān)鍵點(diǎn)描述算法

為了使描述符具有旋轉(zhuǎn)不變性,加上SIFT原有的局部特征描述方法計(jì)算過于復(fù)雜,為了降低特征描述符匹配的計(jì)算能力和時(shí)間。本文采用二進(jìn)制方向直方圖描述子BSHOT對關(guān)鍵點(diǎn)鄰域特征進(jìn)行描述。首先利用鄰域點(diǎn)云建立局部坐標(biāo)系,計(jì)算鄰域的協(xié)方差矩陣M,計(jì)算公式如下。

(4)

圖2 BSHOT特征空間劃分

本文二進(jìn)制描述符將高維直方圖特征向量轉(zhuǎn)化為352位的二進(jìn)制數(shù)表示,減少了描述子的存儲(chǔ)空間,同時(shí)計(jì)算復(fù)雜度下降?？紤]到一個(gè)SHOT描述符Si,其中i={0,1,2,…,351},并且每個(gè)Si由4個(gè)字節(jié)組成。我們假設(shè)BSHOT描述符Bi,其中i={0,1,2,…,351},Bi為一個(gè)二進(jìn)制數(shù)。將連續(xù)的4個(gè)Si,{S0,S1,S2,S3}編碼為4位二進(jìn)制數(shù),對應(yīng)BSHOT描述符{B0,B1,B2,B3}。

設(shè)S=S0+S1+S2+S3,我們先檢查一個(gè)值Si,Si∈{S0,S1,S2,S3}是否大于S的90 %。如果S1的值大于S的90 %,則編碼{B0,B1,B2,B3}為{0,1,0,0}。然后在檢查兩個(gè)值的和是否大于S的90 %,如果S0+S1大于S的90 %,則編碼{B0,B1,B2,B3}為{1,1,0,0}。以此類推,通過迭代將352維的SHOT描述符編碼為BSHOT描述符。BSHOT描述符并不具有唯一性,但用來估計(jì)粗略的對應(yīng)關(guān)系速度更快,同時(shí)方便修改比值,在有噪聲的環(huán)境下魯棒性更高。

4 對應(yīng)點(diǎn)匹配

待配準(zhǔn)的兩片點(diǎn)云經(jīng)過下采樣、SIFT關(guān)鍵點(diǎn)提取及SHOT描述子計(jì)算后得到每個(gè)特征點(diǎn)的特征向量。一般來說,不同點(diǎn)云中同一個(gè)點(diǎn)的特征描述子應(yīng)該近似相同,傳統(tǒng)的算法通常利用兩個(gè)特征向量間的歐氏距離作為衡量標(biāo)準(zhǔn)。本文將SHOT描述符轉(zhuǎn)化為二進(jìn)制描述符BSHOT,利用漢明距離進(jìn)行匹配。漢明距離近似則為同一個(gè)點(diǎn),但是由于點(diǎn)云拓?fù)浣Y(jié)構(gòu)、稀疏性、噪聲的影響,難免會(huì)產(chǎn)生錯(cuò)誤的匹配對,直接影響配準(zhǔn)的結(jié)果。本文采用隨機(jī)采樣一致性(RANSAC)去除點(diǎn)云中錯(cuò)誤的匹配對。首先RANSAC假設(shè)一個(gè)含有3個(gè)“內(nèi)點(diǎn)”以上的模型作為初始狀態(tài)。假設(shè)“內(nèi)點(diǎn)”在n個(gè)數(shù)據(jù)中的占比為t:

(5)

其中,ninliners為“內(nèi)點(diǎn)”,noutliners為“外點(diǎn)”,在k次迭代的情況下,(1-t)k計(jì)算k次迭代計(jì)算模型都至少采樣到一個(gè)外點(diǎn)的概率,那么能正確采樣到內(nèi)點(diǎn)的概率為P=1-(1-t)k。P接近1時(shí)表示此時(shí)模型已經(jīng)具有較多的內(nèi)點(diǎn),迭代完成后估計(jì)出最優(yōu)的模型參數(shù)。RANSAC是通過反復(fù)選擇數(shù)據(jù)集去估計(jì)出模型,一直迭代到估計(jì)出認(rèn)為比較好的模型。

5 點(diǎn)云的配準(zhǔn)過程

點(diǎn)云配準(zhǔn)的一般過程分為粗配準(zhǔn)、精配準(zhǔn)兩個(gè)階段。粗配準(zhǔn)是在點(diǎn)云相對位姿完全未知的情況下對點(diǎn)云進(jìn)行配準(zhǔn),可以為精配準(zhǔn)提供良好的初始值。精配準(zhǔn)的目的是在粗配準(zhǔn)的基礎(chǔ)上讓點(diǎn)云之間的空間位置差別最小化。

本文采樣特征描述子匹配的點(diǎn)云配準(zhǔn)方法,首先對輸入點(diǎn)云進(jìn)行下采樣,得到較為稀疏的點(diǎn)云,然后利用k近鄰搜索查詢點(diǎn)附近的鄰域空間來提取SIFT關(guān)鍵點(diǎn),其次在每個(gè)關(guān)鍵點(diǎn)處構(gòu)建BSHOT描述子,最后通過漢明距離建立點(diǎn)云匹配對,RANSAC利用點(diǎn)云匹配對估計(jì)出初始的變換矩陣。為了進(jìn)一步提高配準(zhǔn)的精度,利用粗配準(zhǔn)提供的初始位置,本文采用ICP估計(jì)兩個(gè)點(diǎn)云的最優(yōu)的變換關(guān)系。本文配準(zhǔn)過程如圖3所示。

圖3 本文的配準(zhǔn)算法

6 實(shí)驗(yàn)結(jié)果與分析

本文的實(shí)驗(yàn)數(shù)據(jù)采用的是斯坦福大學(xué)不同視角下的bunny數(shù)據(jù)和dragon數(shù)據(jù),作為點(diǎn)云配準(zhǔn)的源與目標(biāo)點(diǎn)云。實(shí)驗(yàn)平臺(tái)為Intel(R)Core(TM)i7-7500U CPU @2.7GHz處理器,8G運(yùn)行內(nèi)存,操作系統(tǒng)為Windows10 64位操作系統(tǒng),編譯環(huán)境為cmake-gui和Visual Studio 2017,結(jié)合第三方點(diǎn)云處理庫PCL1.81。

由于bunny原始數(shù)據(jù)和dragon原始數(shù)據(jù)均為高密度點(diǎn)云數(shù)據(jù),數(shù)據(jù)點(diǎn)數(shù)高達(dá)4萬。本文采用體素濾波器進(jìn)行下采樣,體素采樣半徑設(shè)置為0.001 m。在采樣后數(shù)據(jù)中分別提取SIFT關(guān)鍵點(diǎn),然后在每個(gè)關(guān)鍵點(diǎn)處計(jì)算BSHOT描述子。將BSHOT描述子存儲(chǔ)為一個(gè)352位二進(jìn)制的數(shù)。兩個(gè)相同描述子的漢明距離應(yīng)該近似相同,在源和目標(biāo)點(diǎn)云上提取出對應(yīng)的點(diǎn),結(jié)果如圖4所示。

圖4 對應(yīng)點(diǎn)匹配結(jié)果

對bunny00、bunny45數(shù)據(jù)和dragon00、dragon24數(shù)據(jù)進(jìn)行粗配準(zhǔn),RANSAC通過對應(yīng)點(diǎn)的關(guān)系估計(jì)出較為準(zhǔn)確的變換矩陣,并與文獻(xiàn)[11]的粗配準(zhǔn)結(jié)果對比,不同算法粗配準(zhǔn)結(jié)果如圖5所示,其中圖5(a)、(d)為配準(zhǔn)前原始點(diǎn)云,圖5(b)、(e)為文獻(xiàn)[11]粗配準(zhǔn)結(jié)果，圖5(c)、(f)為本文粗配準(zhǔn)結(jié)果,本文配準(zhǔn)效果通過計(jì)算MSE(均方誤差)來衡量,該結(jié)果反映兩個(gè)點(diǎn)云的接近程度。由圖5和表1分析可知,文獻(xiàn)[11]的粗配準(zhǔn)方法最小均方誤差達(dá)到11.91 μm,本文粗配準(zhǔn)最小均方誤差達(dá)到8.08 μm。本文方法在粗配準(zhǔn)效率和配準(zhǔn)精度上有所提高。

完成粗配準(zhǔn)后,利用圖5的粗配準(zhǔn)結(jié)果進(jìn)行精配準(zhǔn)。為了確保粗配準(zhǔn)算法的可比性,文獻(xiàn)[11]和本文方法精配準(zhǔn)均采用ICP方法,ICP方法迭代次數(shù)均為20。最終精配準(zhǔn)結(jié)果誤差和運(yùn)行時(shí)間如表1所示,精配準(zhǔn)效果如圖6所示,其中圖6(a)、(d)為ICP配準(zhǔn)結(jié)果,圖6(b)、(e)為文獻(xiàn)[11]精配準(zhǔn)結(jié)果,圖6(c)、(f)為本文精配準(zhǔn)結(jié)果。由圖6和表1分析可知,本文配準(zhǔn)方法為3種方法里面用時(shí)最少,和文獻(xiàn)[11]的方法對比,時(shí)間效率平均提升了67.2 %。配準(zhǔn)精度上略低于文獻(xiàn)[11]方法,但較傳統(tǒng)ICP方法,配準(zhǔn)精度有所提高。

圖5 不同方法粗配準(zhǔn)結(jié)果

圖6 不同方法精配準(zhǔn)結(jié)果

表1 不同算法配準(zhǔn)效果評估

7 結(jié) 語

本文受二維圖像中檢測尺度不變特征關(guān)鍵點(diǎn)SIFT思路的啟發(fā),提出了結(jié)合三維空間的尺度不變特征關(guān)鍵點(diǎn)和二進(jìn)制方向直方圖描述子特征匹配的配準(zhǔn)方法。該方法對空間中兩個(gè)重疊率較低的點(diǎn)云完成快速精確配準(zhǔn)。在實(shí)際應(yīng)用中,有較高的使用價(jià)值。在如今多傳感器數(shù)據(jù)融合SLAM的大趨勢下,可以利用其他傳感器為點(diǎn)云配準(zhǔn)算法提供初始參數(shù),以進(jìn)一步提高配準(zhǔn)效率。