視覺與慣導融合的煤礦移動機器人定位方法

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

(1.西安科技大學 電氣與控制工程學院， 陜西 西安 710054；2.西安科技大學 機械工程學院， 陜西 西安 710054；3.陜西省礦山機電裝備智能監測重點實驗室， 陜西 西安 710054)

0 引言

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

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

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

1 特征點提取與匹配算法

1.1 ORB算法原理

ORB算法主要包括FAST關鍵點提取算法與BRIEF特征子描述2個方面。其中，BRIEF特征子主要負責對特征點進行描述，影響單目視覺定位精度的主要是FAST關鍵點提取算法。關鍵點是指圖像中與周圍像素點有明顯灰度值差異的點。FAST關鍵點提取算法原理如圖1所示，通過比較一定鄰域內像素點的灰度值與中心點的灰度值大小，實現對特征點的判斷。

圖1 FAST關鍵點提取算法原理

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

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

(1)

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

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

1.2 ORB算法改進

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

(2)

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

FAST關鍵點提取算法中閾值是固定的，但是，光線條件不同，相鄰像素點的灰度差值也不同。在光線較差的環境下，中心像素點與鄰域像素點的灰度差值很小，若閾值過大，則提取出的關鍵點較少；若單純地減小閾值，則可能導致在光照較好的環境下提取到多余或不穩定的關鍵點。因此，本文在FAST關鍵點提取算法中增加自適應閾值調節過程，根據前一幀檢測到的關鍵點數確定當前幀的閾值，從而保證檢測到的關鍵點的數量和質量在合理范圍內。關鍵點數為300～600時定位結果較精確，因此，以該區間為基準，設定初始閾值εth為20。當前幀閾值與前一幀關鍵點數的關系見表1。

1.3 RANSAC算法原理

完成特征點提取后，需要對2個關鍵幀之間的特征點進行匹配，然后才能對圖像數據進行解算。傳統的特征點匹配方法在進行大量特征點匹配時往往會產生誤匹配現象，導致視覺定位精度下降。本文選擇RANSAC算法進行特征點匹配。

表1 當前幀閾值與前一幀關鍵點數的關系

RANSAC算法原理：在2個包含特征點的關鍵幀中隨機選擇4對點作為內點，其他特征點作為外點;根據內點求解出描述2個幀間位置關系的單應性矩陣，利用單應性矩陣對所有外點進行測試;將所有滿足單應性矩陣的外點歸為新的內點，再次利用新的內點更新單應性矩陣，對剩余的外點進行測試。

設W為本次測試中特征點都是內點的概率，則1-W為本次測試中特征點都是外點的概率，(1-W)y表示y次測試中特征點都是外點的概率，進行y次測試后,至少有1次選取的點數都是內點的概率z為

z=1-(1-W)y

(3)

(4)

進行y次測試后的剩余點對即為匹配好的點對。RANSAC算法能夠根據大量特征點準確建立起2個關鍵幀之間的最佳匹配模型，減少誤匹配的點，達到提高定位精度的目的。

2 視覺與慣導融合算法

多傳感器耦合主要包括緊耦合和松耦合2種方法。松耦合是指各部分直接解算后再融合，因為慣導的誤差不能完全消除，且解算過程中需要經過多次積分，會導致累計誤差越來越大，所以本文采用緊耦合方法，即將慣導數據與單目視覺數據分別不完全解算后再進行聯合優化。

緊耦合原理如圖2所示。對慣導數據進行一次預積分，得到速度、旋轉矩陣與平移向量；對單目視覺數據進行特征點提取與匹配，得到圖像的特征數據；將圖像特征與慣導預積分后的數據融合成一個待優化變量，構建優化函數；對函數進行求解與優化，從而估計出機器人位姿。

慣導與單目相機的頻率不同，數據類型不匹配，因此需要分別對數據進行處理。慣導數據與視覺數據的關系如圖3所示，其中r1—r3為地圖中的路標點，c1—c3為相機的中心，d1—d3為關鍵幀。在2個關鍵幀之間存在許多慣導數據，因此需要將若干慣導數據積分成一點，才可進行后續數據融合。

圖2 緊耦合原理

圖3 慣導數據與視覺數據的關系

對慣導數據進行預積分[16]，得

(5)

(6)

(7)

通過對極幾何法[17]將單目相機檢測到的關鍵幀數據表示成如下形式：

(8)

G=K-TEK-1

(9)

(10)

將經過處理的慣導數據與單目相機數據融合成一個待優化的量：

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

(11)

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

采用圖優化[18]方法進行數據融合，將IMU的測量殘差和視覺觀測殘差結合在一起構成代價函數，對代價函數進行優化，從而獲得機器人的位姿信息。需要優化的代價函數為

(12)

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

對代價函數進行展開、合并，可得

(13)

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

對函數F(χ)進行一階泰勒展開，可得

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

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

(14)

代價函數的改變量為

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

(15)

為了使增量為極小值，令ΔF對Δχ的導數為0，可得

(16)

代價函數優化問題被轉換成線性方程求解問題，使用Schur消元法[19]求解線性方程，即可得到下一時刻機器人的位姿信息。

3 實驗驗證

3.1 同類算法對比

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

(a) Harris算法

表2 4種算法的特征點提取結果對比

分析圖4和表2可知，在弱光照環境下，特征點提取數量最多的為SIFT算法，但因為其存在無用特征點數，且實時性不好，所以不適合用于弱光照條件下的特征點提取。SURF算法與Harris算法雖然提取的特征點數較多且耗時較短，但其提取的特征點具有聚集性，不能完整描述空間物體的特征，也不能用于弱光照條件下的特征點提取。ORB算法雖然提取的特征點數較少，但耗時最短，且特征點分布均勻，可以準確描述物體特征。

3.2 ORB算法改進效果

由于ORB算法提取的特征點數較少，通過自適應閾值和極大值抑制等方法對其進行改進。改進ORB算法與原ORB算法的特征點提取結果對比見表3。由表3可知，改進ORB算法雖然提取時間有一定增加，但提取的可用特征點數也大大增加了。ORB算法改進前后特征點提取效率對比如圖5所示。

表3 ORB算法改進前后特征點提取結果對比

(a) 提取時間對比

3.3 特征點匹配算法對比

設定特征點提取數為400，對同一圖像進行特征點提取，并使用傳統的暴力(Brute-Force，BF)匹配算法與RANSAC特征點匹配算法進行實驗比對，特征點匹配效果如圖6所示，通過對比發現，BF算法存在誤匹配現象，而RANSAC算法剔除了誤匹配點，提高了特征點匹配的準確性，從而可提高單目視覺定位精度。

3.4 視覺與慣導融合定位效果

使用煤礦移動機器人(圖7)作為載體，ZED雙目相機中的左攝像頭作為視覺相機，其采樣頻率為100 Hz；使用SYD TransducerM型慣導，其采樣頻率為1 000 Hz，在Linux環境下運行視覺與慣導融合定位算法，進行實驗驗證。

(a) BF算法

圖7 煤礦移動機器人

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

4 結論

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

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

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

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

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