單目移動(dòng)機(jī)器人相對(duì)位姿估計(jì)方法

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

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

引言

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

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

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

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

1 本質(zhì)矩陣估計(jì)

1.1 本質(zhì)矩陣

(1)

(2)

(3)

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

E=t×R=[t]×R

(4)

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

(5)

1.2 單幅圖像位參估計(jì)

對(duì)于已知的特定參照物，利用其空間坐標(biāo)以及在單幅圖像中的像面坐標(biāo)進(jìn)行空間后方交會(huì)，獲取外參數(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為特征點(diǎn)的像面坐標(biāo)；vx、vy為像面坐標(biāo)的改正數(shù)；ΔXs、ΔYs、ΔZs、Δκ、Δω，Δφ為外參數(shù)修正值。(7)～(8)式兩組方程為一個(gè)控制點(diǎn)列出，如果有n個(gè)控制點(diǎn)，則可以列出2n個(gè)方程，故n≥3且不共線時(shí)即可求出外參數(shù)。

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

(9)

Tφωκ=[XsYsZs]T

(10)

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

1.3 本質(zhì)矩陣估計(jì)

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

(11)

由(11)式得

(12)

(13)

(14)

2 相機(jī)相對(duì)位姿解算

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

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

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

(15)

(16)

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

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

(17)

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

(18)

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

3) 計(jì)算向量誤差ε；

(19)

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

2.2 相對(duì)位姿計(jì)算

利用ETT=0估計(jì)出平移向量T的兩個(gè)解：

(20)

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

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

(21)

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

(22)

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

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

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

(23)

令雅可比矩陣J=d，則

(24)

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

得：

(25)

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

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

(26)

(27)

3 實(shí)驗(yàn)結(jié)果

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

圖2 特定點(diǎn)群

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

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

圖3 實(shí)驗(yàn)現(xiàn)場

3.1 特征點(diǎn)提取與匹配

圖4 匹配結(jié)果

3.2 相機(jī)相對(duì)位移精度評(píng)估

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

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

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

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

圖6 X軸方向上的位移

圖7 Y軸方向上的位移

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

表1 X軸方向相機(jī)相對(duì)位移估計(jì)誤差 m

表2 Y軸方向相機(jī)相對(duì)位姿估計(jì)誤差 m

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

3.3 算法可靠性分析

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

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

4 結(jié)論

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