視覺(jué)與慣導(dǎo)融合的煤礦移動(dòng)機(jī)器人定位方法

張羽飛， 馬宏偉， 毛清華， 華洪濤， 石金龍

(1.西安科技大學(xué) 電氣與控制工程學(xué)院， 陜西 西安 710054；2.西安科技大學(xué) 機(jī)械工程學(xué)院， 陜西 西安 710054；3.陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室， 陜西 西安 710054)

0 引言

隨著煤礦智能化的不斷發(fā)展，移動(dòng)機(jī)器人在煤礦井下的應(yīng)用逐步增多[1]。目前常用的井下機(jī)器人定位方法有超聲定位、激光定位和超寬帶(Ultra Wide Band,UWB)定位等[2-4]。其中，超聲定位性價(jià)比高，但易受幻影干擾等因素影響；激光定位精度高，但成本很高，適用于無(wú)人環(huán)境；UWB定位精度高、速度快，但只在小范圍內(nèi)表現(xiàn)出色[5-7]。

近年來(lái)，基于視覺(jué)傳感器的定位方法成為研究熱點(diǎn)。單目視覺(jué)定位是指利用1臺(tái)攝像機(jī)完成定位工作，具有結(jié)構(gòu)簡(jiǎn)單、標(biāo)定步驟少等優(yōu)點(diǎn)。單目視覺(jué)定位算法主要包括特征點(diǎn)法和直接法。特征點(diǎn)法是指提取能夠描述圖像信息的特征點(diǎn)進(jìn)行定位[8]，由于其穩(wěn)定、對(duì)動(dòng)態(tài)物體不敏感的優(yōu)勢(shì)，一直是目前主流的解算方法[9-10]。在計(jì)算機(jī)視覺(jué)中，圖像信息一般都是以灰度值矩陣的方式儲(chǔ)存[11]。特征點(diǎn)是圖像的一個(gè)重要局部特征，也是圖像信息的數(shù)字表現(xiàn)形式。很多特征點(diǎn)提取算法被提出。D. G. Lowe[12]提出了基于尺度不變特征變換(Scale-Invariant Feature Transform，SIFT)的算法，該算法精度高、魯棒性好，但計(jì)算量極大。H. Bay等[13]提出了加速穩(wěn)健特征(Speeded Up Robust Features，SURF)算法，該算法減少了提取時(shí)間，提高了魯棒性，但實(shí)時(shí)性仍較差。E. Rublee等[14]提出了快速特征點(diǎn)提取和描述(Oriented FAST and Rotated BRIEF，ORB) 算法，該算法實(shí)時(shí)性、魯棒性較好，但在光照變化和弱光照區(qū)域表現(xiàn)較差，無(wú)法應(yīng)用于煤礦井下光照較暗的場(chǎng)景。

本文在ORB算法的基礎(chǔ)上進(jìn)行改進(jìn)，采用隨機(jī)抽樣一致性(Random Sample Consensus，RANSAC)算法[15]進(jìn)行特征點(diǎn)匹配，采用對(duì)極幾何法進(jìn)行視覺(jué)解算，提高了煤礦井下弱光照區(qū)域的特征點(diǎn)提取效率。但單目視覺(jué)定位仍存在尺度問(wèn)題，即觀測(cè)一個(gè)物體時(shí)，僅靠單目視覺(jué)定位無(wú)法確定機(jī)器人與物體的距離及物體的大小，因此，本文將單目視覺(jué)定位數(shù)據(jù)與慣導(dǎo)數(shù)據(jù)進(jìn)行融合，以恢復(fù)視覺(jué)相機(jī)的尺度，提高定位精度。

1 特征點(diǎn)提取與匹配算法

1.1 ORB算法原理

ORB算法主要包括FAST關(guān)鍵點(diǎn)提取算法與BRIEF特征子描述2個(gè)方面。其中，BRIEF特征子主要負(fù)責(zé)對(duì)特征點(diǎn)進(jìn)行描述，影響單目視覺(jué)定位精度的主要是FAST關(guān)鍵點(diǎn)提取算法。關(guān)鍵點(diǎn)是指圖像中與周圍像素點(diǎn)有明顯灰度值差異的點(diǎn)。FAST關(guān)鍵點(diǎn)提取算法原理如圖1所示，通過(guò)比較一定鄰域內(nèi)像素點(diǎn)的灰度值與中心點(diǎn)的灰度值大小，實(shí)現(xiàn)對(duì)特征點(diǎn)的判斷。

圖1 FAST關(guān)鍵點(diǎn)提取算法原理

以關(guān)鍵幀的灰度圖像中的一個(gè)像素點(diǎn)p為圓心、固定半徑畫(huà)圓，從圓上選取16個(gè)像素點(diǎn)xn(n=1,2,…,16)，將各像素點(diǎn)的灰度值與中心點(diǎn)p的灰度值作差，并取絕對(duì)值，得

m=|I(xn)-I(p)|

(1)

式中I(xn)，I(p)分別為像素點(diǎn)xn，p的灰度值。

FAST-9是常用的FAST關(guān)鍵點(diǎn)提取算法，其原理如下：設(shè)定閾值ε作為判定關(guān)鍵點(diǎn)的標(biāo)準(zhǔn)，將像素點(diǎn)x1—x16代入式(1)進(jìn)行計(jì)算。若存在連續(xù)9個(gè)以上的鄰域點(diǎn)與中心點(diǎn)的灰度差滿足m>ε，則說(shuō)明像素點(diǎn)p為關(guān)鍵點(diǎn)，否則舍棄該點(diǎn)。

1.2 ORB算法改進(jìn)

FAST-9算法需要大量的計(jì)算來(lái)確定關(guān)鍵點(diǎn)，且容易包含錯(cuò)誤點(diǎn)。因此，本文將符合標(biāo)準(zhǔn)的鄰域點(diǎn)數(shù)提高到12，并選取像素點(diǎn)x1,x5,x9,x13代入式(1)進(jìn)行計(jì)算。如果上述4個(gè)點(diǎn)中存在2個(gè)以上的點(diǎn)不滿足條件，則舍棄點(diǎn)p，反之則將點(diǎn)p確定為候選點(diǎn)。對(duì)候選點(diǎn)進(jìn)行非極大值抑制處理：檢查候選點(diǎn)的一個(gè)鄰域內(nèi)是否存在多個(gè)候選點(diǎn)，若鄰域內(nèi)無(wú)其他候選點(diǎn)，則該候選點(diǎn)為準(zhǔn)確的關(guān)鍵點(diǎn)；若鄰域內(nèi)存在多個(gè)候選點(diǎn)，則計(jì)算每個(gè)候選點(diǎn)的強(qiáng)度響應(yīng)值s(式(2))。若點(diǎn)p是鄰域內(nèi)所有候選點(diǎn)中s值最大的點(diǎn)，則選定點(diǎn)p為關(guān)鍵點(diǎn)，并舍棄其他候選點(diǎn)；否則舍棄點(diǎn)p，選擇s值最大的點(diǎn)作為關(guān)鍵點(diǎn)。

(2)

對(duì)FAST-9算法進(jìn)行改進(jìn)后，不但減小了計(jì)算量，而且提高了特征點(diǎn)提取精度和效率。

FAST關(guān)鍵點(diǎn)提取算法中閾值是固定的，但是，光線條件不同，相鄰像素點(diǎn)的灰度差值也不同。在光線較差的環(huán)境下，中心像素點(diǎn)與鄰域像素點(diǎn)的灰度差值很小，若閾值過(guò)大，則提取出的關(guān)鍵點(diǎn)較少；若單純地減小閾值，則可能導(dǎo)致在光照較好的環(huán)境下提取到多余或不穩(wěn)定的關(guān)鍵點(diǎn)。因此，本文在FAST關(guān)鍵點(diǎn)提取算法中增加自適應(yīng)閾值調(diào)節(jié)過(guò)程，根據(jù)前一幀檢測(cè)到的關(guān)鍵點(diǎn)數(shù)確定當(dāng)前幀的閾值，從而保證檢測(cè)到的關(guān)鍵點(diǎn)的數(shù)量和質(zhì)量在合理范圍內(nèi)。關(guān)鍵點(diǎn)數(shù)為300～600時(shí)定位結(jié)果較精確，因此，以該區(qū)間為基準(zhǔn)，設(shè)定初始閾值εth為20。當(dāng)前幀閾值與前一幀關(guān)鍵點(diǎn)數(shù)的關(guān)系見(jiàn)表1。

1.3 RANSAC算法原理

完成特征點(diǎn)提取后，需要對(duì)2個(gè)關(guān)鍵幀之間的特征點(diǎn)進(jìn)行匹配，然后才能對(duì)圖像數(shù)據(jù)進(jìn)行解算。傳統(tǒng)的特征點(diǎn)匹配方法在進(jìn)行大量特征點(diǎn)匹配時(shí)往往會(huì)產(chǎn)生誤匹配現(xiàn)象，導(dǎo)致視覺(jué)定位精度下降。本文選擇RANSAC算法進(jìn)行特征點(diǎn)匹配。

表1 當(dāng)前幀閾值與前一幀關(guān)鍵點(diǎn)數(shù)的關(guān)系

RANSAC算法原理：在2個(gè)包含特征點(diǎn)的關(guān)鍵幀中隨機(jī)選擇4對(duì)點(diǎn)作為內(nèi)點(diǎn)，其他特征點(diǎn)作為外點(diǎn);根據(jù)內(nèi)點(diǎn)求解出描述2個(gè)幀間位置關(guān)系的單應(yīng)性矩陣，利用單應(yīng)性矩陣對(duì)所有外點(diǎn)進(jìn)行測(cè)試;將所有滿足單應(yīng)性矩陣的外點(diǎn)歸為新的內(nèi)點(diǎn)，再次利用新的內(nèi)點(diǎn)更新單應(yīng)性矩陣，對(duì)剩余的外點(diǎn)進(jìn)行測(cè)試。

設(shè)W為本次測(cè)試中特征點(diǎn)都是內(nèi)點(diǎn)的概率，則1-W為本次測(cè)試中特征點(diǎn)都是外點(diǎn)的概率，(1-W)y表示y次測(cè)試中特征點(diǎn)都是外點(diǎn)的概率，進(jìn)行y次測(cè)試后,至少有1次選取的點(diǎn)數(shù)都是內(nèi)點(diǎn)的概率z為

z=1-(1-W)y

(3)

(4)

進(jìn)行y次測(cè)試后的剩余點(diǎn)對(duì)即為匹配好的點(diǎn)對(duì)。RANSAC算法能夠根據(jù)大量特征點(diǎn)準(zhǔn)確建立起2個(gè)關(guān)鍵幀之間的最佳匹配模型，減少誤匹配的點(diǎn)，達(dá)到提高定位精度的目的。

2 視覺(jué)與慣導(dǎo)融合算法

多傳感器耦合主要包括緊耦合和松耦合2種方法。松耦合是指各部分直接解算后再融合，因?yàn)閼T導(dǎo)的誤差不能完全消除，且解算過(guò)程中需要經(jīng)過(guò)多次積分，會(huì)導(dǎo)致累計(jì)誤差越來(lái)越大，所以本文采用緊耦合方法，即將慣導(dǎo)數(shù)據(jù)與單目視覺(jué)數(shù)據(jù)分別不完全解算后再進(jìn)行聯(lián)合優(yōu)化。

緊耦合原理如圖2所示。對(duì)慣導(dǎo)數(shù)據(jù)進(jìn)行一次預(yù)積分，得到速度、旋轉(zhuǎn)矩陣與平移向量；對(duì)單目視覺(jué)數(shù)據(jù)進(jìn)行特征點(diǎn)提取與匹配，得到圖像的特征數(shù)據(jù)；將圖像特征與慣導(dǎo)預(yù)積分后的數(shù)據(jù)融合成一個(gè)待優(yōu)化變量，構(gòu)建優(yōu)化函數(shù)；對(duì)函數(shù)進(jìn)行求解與優(yōu)化，從而估計(jì)出機(jī)器人位姿。

慣導(dǎo)與單目相機(jī)的頻率不同，數(shù)據(jù)類型不匹配，因此需要分別對(duì)數(shù)據(jù)進(jìn)行處理。慣導(dǎo)數(shù)據(jù)與視覺(jué)數(shù)據(jù)的關(guān)系如圖3所示，其中r1—r3為地圖中的路標(biāo)點(diǎn)，c1—c3為相機(jī)的中心，d1—d3為關(guān)鍵幀。在2個(gè)關(guān)鍵幀之間存在許多慣導(dǎo)數(shù)據(jù)，因此需要將若干慣導(dǎo)數(shù)據(jù)積分成一點(diǎn)，才可進(jìn)行后續(xù)數(shù)據(jù)融合。

圖2 緊耦合原理

圖3 慣導(dǎo)數(shù)據(jù)與視覺(jué)數(shù)據(jù)的關(guān)系

對(duì)慣導(dǎo)數(shù)據(jù)進(jìn)行預(yù)積分[16]，得

(5)

(6)

(7)

通過(guò)對(duì)極幾何法[17]將單目相機(jī)檢測(cè)到的關(guān)鍵幀數(shù)據(jù)表示成如下形式：

(8)

G=K-TEK-1

(9)

(10)

將經(jīng)過(guò)處理的慣導(dǎo)數(shù)據(jù)與單目相機(jī)數(shù)據(jù)融合成一個(gè)待優(yōu)化的量：

χ=[Rj,RC,Pj,PC,vj,ba,bg]

(11)

式中ba，bg分別為IMU加速度計(jì)和陀螺儀的誤差。

采用圖優(yōu)化[18]方法進(jìn)行數(shù)據(jù)融合，將IMU的測(cè)量殘差和視覺(jué)觀測(cè)殘差結(jié)合在一起構(gòu)成代價(jià)函數(shù)，對(duì)代價(jià)函數(shù)進(jìn)行優(yōu)化，從而獲得機(jī)器人的位姿信息。需要優(yōu)化的代價(jià)函數(shù)為

(12)

式中：eB(χ)為IMU的測(cè)量誤差方程;eC(χ)為單目相機(jī)的測(cè)量誤差方程。

對(duì)代價(jià)函數(shù)進(jìn)行展開(kāi)、合并，可得

(13)

式中：q為階數(shù);ek(χk)為包含IMU測(cè)量誤差和單目相機(jī)測(cè)量誤差的誤差方程；Ωk為信息矩陣，為協(xié)方差矩陣的逆。

對(duì)函數(shù)F(χ)進(jìn)行一階泰勒展開(kāi)，可得

(ek+JkΔχ)TΩk(ek+JkΔχ)=

Mk+2NkΔχ+ΔχTHkΔχ

(14)

代價(jià)函數(shù)的改變量為

ΔFk=2NkΔχ+ΔχTHkΔχ

(15)

為了使增量為極小值，令ΔF對(duì)Δχ的導(dǎo)數(shù)為0，可得

(16)

代價(jià)函數(shù)優(yōu)化問(wèn)題被轉(zhuǎn)換成線性方程求解問(wèn)題，使用Schur消元法[19]求解線性方程，即可得到下一時(shí)刻機(jī)器人的位姿信息。

3 實(shí)驗(yàn)驗(yàn)證

3.1 同類算法對(duì)比

在弱光照條件下對(duì)本文算法進(jìn)行實(shí)驗(yàn)驗(yàn)證。在光線很暗的樓道里，采用ZED雙目相機(jī)對(duì)廊燈進(jìn)行拍照采樣，并使用ORB算法、SURF算法、SIFT算法、Harris算法分別對(duì)圖像進(jìn)行特征點(diǎn)提取。設(shè)置4種算法的最大特征點(diǎn)提取數(shù)均為400，特征點(diǎn)提取效果如圖4所示，特征點(diǎn)提取點(diǎn)數(shù)、耗時(shí)及無(wú)用特征點(diǎn)數(shù)見(jiàn)表2。

(a) Harris算法

表2 4種算法的特征點(diǎn)提取結(jié)果對(duì)比

分析圖4和表2可知，在弱光照環(huán)境下，特征點(diǎn)提取數(shù)量最多的為SIFT算法，但因?yàn)槠浯嬖跓o(wú)用特征點(diǎn)數(shù)，且實(shí)時(shí)性不好，所以不適合用于弱光照條件下的特征點(diǎn)提取。SURF算法與Harris算法雖然提取的特征點(diǎn)數(shù)較多且耗時(shí)較短，但其提取的特征點(diǎn)具有聚集性，不能完整描述空間物體的特征，也不能用于弱光照條件下的特征點(diǎn)提取。ORB算法雖然提取的特征點(diǎn)數(shù)較少，但耗時(shí)最短，且特征點(diǎn)分布均勻，可以準(zhǔn)確描述物體特征。

3.2 ORB算法改進(jìn)效果

由于ORB算法提取的特征點(diǎn)數(shù)較少，通過(guò)自適應(yīng)閾值和極大值抑制等方法對(duì)其進(jìn)行改進(jìn)。改進(jìn)ORB算法與原ORB算法的特征點(diǎn)提取結(jié)果對(duì)比見(jiàn)表3。由表3可知，改進(jìn)ORB算法雖然提取時(shí)間有一定增加，但提取的可用特征點(diǎn)數(shù)也大大增加了。ORB算法改進(jìn)前后特征點(diǎn)提取效率對(duì)比如圖5所示。

表3 ORB算法改進(jìn)前后特征點(diǎn)提取結(jié)果對(duì)比

(a) 提取時(shí)間對(duì)比

3.3 特征點(diǎn)匹配算法對(duì)比

設(shè)定特征點(diǎn)提取數(shù)為400，對(duì)同一圖像進(jìn)行特征點(diǎn)提取，并使用傳統(tǒng)的暴力(Brute-Force，BF)匹配算法與RANSAC特征點(diǎn)匹配算法進(jìn)行實(shí)驗(yàn)比對(duì)，特征點(diǎn)匹配效果如圖6所示，通過(guò)對(duì)比發(fā)現(xiàn)，BF算法存在誤匹配現(xiàn)象，而RANSAC算法剔除了誤匹配點(diǎn)，提高了特征點(diǎn)匹配的準(zhǔn)確性，從而可提高單目視覺(jué)定位精度。

3.4 視覺(jué)與慣導(dǎo)融合定位效果

使用煤礦移動(dòng)機(jī)器人(圖7)作為載體，ZED雙目相機(jī)中的左攝像頭作為視覺(jué)相機(jī)，其采樣頻率為100 Hz；使用SYD TransducerM型慣導(dǎo)，其采樣頻率為1 000 Hz，在Linux環(huán)境下運(yùn)行視覺(jué)與慣導(dǎo)融合定位算法，進(jìn)行實(shí)驗(yàn)驗(yàn)證。

(a) BF算法

圖7 煤礦移動(dòng)機(jī)器人

使移動(dòng)機(jī)器人按照固定路線移動(dòng)，用全站儀測(cè)量其移動(dòng)軌跡，分別計(jì)算ORB算法改進(jìn)前后與慣導(dǎo)融合定位的測(cè)量誤差，結(jié)果如圖8所示。從圖8可看出，改進(jìn)后融合定位方法精度有了很大提升，相對(duì)誤差由原來(lái)的0.6 m降低到0.4 m以下，平均誤差由0.20 m減小到0.15 m，均方根誤差由0.24 m減小到0.18 m。實(shí)驗(yàn)結(jié)果證明，改進(jìn)ORB算法與慣導(dǎo)融合定位方法能夠有效提高定位精度。

4 結(jié)論

(1) 與同類算法相比，ORB算法雖然提取的特征點(diǎn)數(shù)較少，但耗時(shí)最短，且特征點(diǎn)分布均勻，可以準(zhǔn)確描述物體特征。

(2) 改進(jìn)ORB算法與原ORB算法相比，雖然提取時(shí)間有了一定的增加，但提取的可用特征點(diǎn)數(shù)也大大增加了。

(3) 特征點(diǎn)匹配算法對(duì)比結(jié)果表明，與BF算法相比，RANSAC算法剔除了誤匹配點(diǎn)，增加了特征點(diǎn)匹配的準(zhǔn)確性，從而提高了單目視覺(jué)定位精度。

(a) 原融合定位方法的誤差

(4) 分別用改進(jìn)前后的ORB算法與慣導(dǎo)進(jìn)行融合定位，對(duì)比結(jié)果表明，改進(jìn)后融合定位方法精度有了很大提升，相對(duì)誤差由原來(lái)的0.6 m降低到0.4 m以下，平均誤差由0.20 m減小到0.15 m，均方根誤差由0.24 m減小到0.18 m。