王 君，徐曉鳳，董明利，孫 鵬，陳 敏

(1.北京信息科技大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京100192；2.長春理工大學(xué) 光電工程學(xué)院，吉林 長春 130022；3.北京信息科技大學(xué) 機電系統(tǒng)測控北京市重點實驗室，北京100192)

引言

同步定位與地圖構(gòu)建SLAM[1-2](simultaneous localization and mapping)使得移動機器人能在沒有先驗知識的未知環(huán)境中或自身位置不確定的條件下，借助傳感器增量式構(gòu)建或更新環(huán)境地圖，同時利用地圖進行自主定位、導(dǎo)航、避障和規(guī)劃路徑等。

SLAM系統(tǒng)主要依賴視覺傳感器與激光雷達兩種傳感器對周圍環(huán)境進行感知[3-4]。相比激光雷達，視覺傳感器以其價格便宜、體積小并且能夠獲取更加豐富的信息等特點，展現(xiàn)出了在小型機器人上的強大優(yōu)勢[5-6]。因此，基于視覺的SLAM系統(tǒng)構(gòu)建成為了機器人自主定位和導(dǎo)航的重要方法。對于單目移動機器人，通過相鄰兩幅或多幅關(guān)鍵幀圖像恢復(fù)相機相對位姿成為視覺SLAM系統(tǒng)中建立視覺里程計的核心問題。

傳統(tǒng)的位姿估計方法通常是通過五點法、八點法等算法獲得本質(zhì)矩陣[7-8]，然后分解本質(zhì)矩陣得到相機相對位姿參數(shù)。機器人在運動過程中需以較快的速度采集圖像信息，以滿足實時更新地圖與定位的需求。因此，相鄰關(guān)鍵幀圖像之間基線一般較短，常規(guī)方法求解本質(zhì)矩陣存在困難[9]。

本文結(jié)合單目機器人定位需求，提出一種利用特定參照物或運動信息獲取本質(zhì)矩陣并進一步分解相對位姿的估算方法，解決傳統(tǒng)方法中由于基線過小無法恢復(fù)相機相對位姿的問題。該方法首先利用已知特定參照信息，初始化本質(zhì)矩陣的估計值；然后，通過相鄰關(guān)鍵幀間匹配的特征點組和本質(zhì)矩陣估計值迭代優(yōu)化本質(zhì)矩陣；最后通過本質(zhì)矩陣的約束關(guān)系求得平移向量估計值，并建立目標(biāo)函數(shù)，利用基于李群的姿態(tài)表示得到旋轉(zhuǎn)矩陣估計值，優(yōu)化出唯一解。

1 本質(zhì)矩陣估計

1.1 本質(zhì)矩陣

(1)

(2)

(3)

而本質(zhì)矩陣E由旋轉(zhuǎn)平移矩陣R、t得:

E=t×R=[t]×R

(4)

所以，得到本質(zhì)矩陣的重要約束條件:

(5)

1.2 單幅圖像位參估計

對于已知的特定參照物，利用其空間坐標(biāo)以及在單幅圖像中的像面坐標(biāo)進行空間后方交會，獲取外參數(shù)旋轉(zhuǎn)角度(φ、ω、κ)和平移量(Xs、Ys、Zs)。

共線條件方程[11]有:

(6)

[ΔXsΔYsΔZsΔκΔωΔφ]T+

(7)

其中x0、y0是將外參數(shù)近似值帶入共線方程得到的x、y。

由共線方程的線性形式可列出誤差方程：

[ΔXsΔYsΔZsΔκΔωΔφ]T-

(8)

式中：x、y為特征點的像面坐標(biāo)；vx、vy為像面坐標(biāo)的改正數(shù)；ΔXs、ΔYs、ΔZs、Δκ、Δω，Δφ為外參數(shù)修正值。(7)～(8)式兩組方程為一個控制點列出，如果有n個控制點，則可以列出2n個方程，故n≥3且不共線時即可求出外參數(shù)。

利用所得外參數(shù)計算出單幅圖像位姿參數(shù)Rφωκ和Tφωκ：

(9)

Tφωκ=[XsYsZs]T

(10)

式中用sκ、cκ等表示對應(yīng)旋轉(zhuǎn)角度的正弦和余弦。

1.3 本質(zhì)矩陣估計

得到單幅圖像的位姿參數(shù)后，可利用兩幅圖像的旋轉(zhuǎn)平移關(guān)系計算出相對位移，即：

(11)

由(11)式得

(12)

(13)

(14)

2 相機相對位姿解算

由于最終的本質(zhì)矩陣需由特征點組參與迭代優(yōu)化，所以對特征點組的匹配精確度有較高要求。為了得到有效的匹配特征點組，首先利用SURF (speed up robust features)算法[12]進行特征點的提取和相鄰關(guān)鍵幀之間的匹配，獲得初始特征點組；然后，通過RANSAC(random sample consensus)算法[13]去除特征點組中的誤匹配；最后，利用(5)式的約束條件對特征點組做進一步篩取，獲得精確匹配的特征點組。

2.1 本質(zhì)矩陣優(yōu)化

任何本質(zhì)矩陣均有奇異值分解[14]：

(15)

(16)

其中：Rx、Ry、Rz分別為3個軸方向的位移向量；Rκ、Rω和Rφ分別為繞x、y、z軸旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，有：

式中：sκ、cκ是對應(yīng)旋轉(zhuǎn)角度的正弦和余弦；Rx和Ry的形式與Rκ和Rω相同；θ=(θκ,θω,θφ,θx,θy)中5個旋轉(zhuǎn)參數(shù)均為構(gòu)成本質(zhì)矩陣E的基本參數(shù)。本文將利用這些本質(zhì)矩陣的局部參數(shù)對其進行迭代優(yōu)化。

(17)

當(dāng)θ=0時，對參數(shù)向量θ=(θκ,θω,θφ,θx,θy)進行求導(dǎo)，可得：

(18)

矩陣J是通過對應(yīng)Ji|0的每i行進行疊加所得。

3) 計算向量誤差ε；

(19)

6) 重復(fù)2～5步，直到收斂。

2.2 相對位姿計算

利用ETT=0估計出平移向量T的兩個解：

(20)

矩陣E和矩陣[T]×R滿足關(guān)系式E-[T]×R=03×3。建立目標(biāo)函數(shù)，對目標(biāo)函數(shù)尋優(yōu)來確定旋轉(zhuǎn)矩陣：

R=arg minR‖E-[T]×R‖2

(21)

對R有，R=R0R(ξ)，其中：R0為初值；姿態(tài)角度假設(shè)為0；R為單位陣。R(ξ)利用李群方法可表示為

(22)

式中ξ=(ξ1,ξ2,ξ3)為運動增量，對應(yīng)于相機圖像間角速度?；仃嘒j如下所示：

對d(ξ)=E-[T]×R0R(ξ)進行線性處理：

d(ξ)≈d(0)+d·ξ

(23)

令雅可比矩陣J=d，則

(24)

令Q1=[T]×R0G1，Q2=[T]×R0G2，Q3=[T]×R0G3，則J可以表示為

得：

(25)

圖1 匹配點的幾何關(guān)系

同一世界坐標(biāo)系中，m與m′對應(yīng)的相機坐標(biāo)分別為

(26)

(27)

3 實驗結(jié)果

布置已知三維關(guān)系的特定點群(圖2)(3 m×3 m)作為特定參照物用來檢驗位姿，只要機器人視野范圍內(nèi)識別到其中5個點即可得到相機相對位姿，從而減小了設(shè)置特定點群帶來的局限性。

圖2 特定點群

為了檢驗實驗結(jié)果的可靠性和有效性，將本文算法得到的結(jié)果與V-Stars靜態(tài)攝影測量系統(tǒng)處理的結(jié)果進行比較。V-Stars系統(tǒng)是美國GSI公司研制的工業(yè)數(shù)字近景攝影測量系統(tǒng)[15]，可快速、高精度(相對精度可達1/200 000)地計算出相鄰關(guān)鍵幀之間的相對位姿轉(zhuǎn)換參數(shù)。

機器人采用USB接口的GY200工業(yè)相機作為視覺傳感器，其有效像素為44萬。機器人在地面移動軌跡約3.12 m，對實驗現(xiàn)場(圖3)采集圖像，并選取16幅關(guān)鍵幀。機器人在移動過程中進行同步定位與地圖構(gòu)建，相對位移誤差是影響定位與地圖構(gòu)建精度的主要因素。因此，實驗主要通過機器人相機相對位移誤差評價位姿估計結(jié)果。

圖3 實驗現(xiàn)場

3.1 特征點提取與匹配

圖4 匹配結(jié)果

3.2 相機相對位移精度評估

將V-Stars系統(tǒng)處理得到的相機相對位姿作為真實值，控制匹配點數(shù)，統(tǒng)計不同匹配點數(shù)下傳統(tǒng)五點法與本文算法獲得相機相對位移的估計精度，結(jié)果如圖5所示。

實驗結(jié)果表明，隨著特征點對數(shù)量的增加，相機相對位移估計的誤差逐漸減小，并在特征點數(shù)大于200后誤差變化逐漸趨于穩(wěn)定。

將傳統(tǒng)五點法與本文算法優(yōu)化前、后估計得到的相機運動軌跡和相機運動的真實路徑進行比較，從而進一步檢驗本文算法對相機位移估計的精度與魯棒性。圖6、7展示了移動機器人在地面上X方向和Y方向上的位移數(shù)據(jù)比較。

圖5 特征點數(shù)與相機相對位移估計誤差的關(guān)系

圖6 X軸方向上的位移

圖7 Y軸方向上的位移

利用相機相對位移估計絕對誤差的均方根值(RMSE)、中值(Median)和最大值(Max)3個指標(biāo)進行比較評估，實驗結(jié)果如表1和表2所示。

表1 X軸方向相機相對位移估計誤差 m

表2 Y軸方向相機相對位姿估計誤差 m

實驗結(jié)果表明，本文算法優(yōu)化后的相機相對位移估計絕對誤差RMSE、Median值均小于0.03 m，優(yōu)于傳統(tǒng)五點法。

3.3 算法可靠性分析

從算法本身來講，具有較強適應(yīng)性，不僅適用于非共面參照物，還適用于參照物特征共面的退化情況，僅對于參照物特征共線的退化情況不適用。而且，算法對于參照物的分布數(shù)量、密集程度沒有特殊要求，理論上只需要匹配5個特征點就可以實現(xiàn)定向和位姿估計。

但是，通過大量實驗發(fā)現(xiàn)，影響算法可靠性的關(guān)鍵并不在于本質(zhì)矩陣優(yōu)化本身，因為優(yōu)化目標(biāo)函數(shù)是單峰函數(shù)，局部最小值就是全局最小值，算法本身的優(yōu)化結(jié)果是可靠的。影響最終結(jié)果的關(guān)鍵在于本質(zhì)矩陣初值估計，而影響初值估計的主要問題在于特征識別和匹配，一旦存在誤匹配，即使數(shù)量很少，也會對本質(zhì)矩陣估計產(chǎn)生較大影響，造成后續(xù)優(yōu)化失敗。因此，如何確保用于初值估計的特征匹配結(jié)果準(zhǔn)確率是算法可靠性的關(guān)鍵。

4 結(jié)論

本文提出了一種利用已知特定參照物迭代優(yōu)化本質(zhì)矩陣，快速分解機器人相對位移參數(shù)的方法，該方法不受限于機器人視覺關(guān)鍵幀圖像之間基線過短等問題，大大提高了機器人單目SLAM系統(tǒng)在選取關(guān)鍵幀的靈活性，使其只需考慮圖像重疊使用率。同時，該方法也保證了獲取的相機相對位移參數(shù)具有較高的精度。而如何結(jié)合魯棒性算法(M-估計法、最小中值法、最大后驗概率法等)提高特征匹配的準(zhǔn)確率，并提高算法可靠性等是進一步研究的方向。