吳 妍,吳 芬,戚國(guó)慶
(南京理工大學(xué) 江蘇 南京 210094)
基于人工標(biāo)志的單目視覺(jué)下無(wú)人機(jī)位姿估計(jì)
吳 妍,吳 芬,戚國(guó)慶
(南京理工大學(xué) 江蘇 南京 210094)
就無(wú)人機(jī)的自定位問(wèn)題,文中提出一種數(shù)字圖像處理結(jié)合2D-3D場(chǎng)景重建的位姿估計(jì)算法。在已知人工標(biāo)志世界坐標(biāo)的前提下,對(duì)于單目視覺(jué)下機(jī)載非線性相機(jī)拍攝的單幀圖像,利用圖像處理技術(shù)為位姿解算提供特征點(diǎn)數(shù)據(jù)。位姿解算提供了一種改進(jìn)的N點(diǎn)透視算法,該算法結(jié)合正交迭代算法,以N點(diǎn)透視算法計(jì)算的旋轉(zhuǎn)矩陣值作為迭代算法初值,迭代得到最終估計(jì)結(jié)果。通過(guò)仿真實(shí)驗(yàn)和實(shí)物實(shí)驗(yàn)驗(yàn)證了算法的可行性。文中提出的算法,較N點(diǎn)透視算法,提高了位姿估計(jì)的精度且收斂速度快,給無(wú)人機(jī)室內(nèi)位姿估計(jì)提供了的一種新的解決思路。
自定位;非線性;人工標(biāo)志;N點(diǎn)透視;正交迭代
無(wú)人機(jī)憑借結(jié)構(gòu)穩(wěn)定、可操作性高、無(wú)人駕駛等特點(diǎn),在航拍勘測(cè)、目標(biāo)定位跟蹤等領(lǐng)域獲得了廣泛應(yīng)用。在利用無(wú)人機(jī)對(duì)地面/海上等目標(biāo)的定位與跟蹤、無(wú)人機(jī)自身位姿控制、無(wú)人機(jī)測(cè)量、無(wú)人機(jī)自主降落等問(wèn)題中,無(wú)人機(jī)自身位姿的確定,成為研究的前提,是不可回避的一大難題。
目前,無(wú)人機(jī)自身的位姿信息主要依靠GPS[1]和IMU[2]獲取。GPS受環(huán)境與氣候的干擾嚴(yán)重,尤其在室內(nèi),無(wú)人機(jī)無(wú)法接收到GPS信號(hào),因此無(wú)法獲取自身的位置信息[3]。而IMU是根據(jù)加速度計(jì)和陀螺儀的姿態(tài)變化積分獲取位姿信息的。實(shí)際應(yīng)用中會(huì)產(chǎn)生累積誤差從而降低位姿估計(jì)的精度。隨著CCD器材等視覺(jué)傳感器的制作工藝日趨精良,利用視覺(jué)傳感器獲取的信息進(jìn)行位姿估計(jì)成為一種有利的替代手段[4]。
基于視覺(jué)的無(wú)人機(jī)位姿估計(jì)[5-7],減少了攜帶的設(shè)備且價(jià)格低;同時(shí),視覺(jué)信號(hào)穩(wěn)定,魯棒性強(qiáng),不存在累積誤差。利用視覺(jué)信息實(shí)現(xiàn)無(wú)人機(jī)的位姿估計(jì),無(wú)疑是實(shí)現(xiàn)無(wú)人機(jī)自主位姿估計(jì)的又一有力技術(shù)手段,尤其是在室內(nèi)無(wú)法獲取GPS信號(hào)的區(qū)域[8-9]。
針對(duì)室內(nèi)無(wú)人機(jī)的位姿估計(jì),文中提出了一種基于數(shù)字圖像處理技術(shù)和2D-3D場(chǎng)景重建的位姿解算方法。事先在實(shí)驗(yàn)室內(nèi)鋪設(shè)信息已知的人工標(biāo)志,利用圖像處理技術(shù)檢測(cè)并提取人工標(biāo)志的特征點(diǎn)信息,在此基礎(chǔ)上,借助坐標(biāo)系轉(zhuǎn)換以及攝像機(jī)成像模型,建立圖像坐標(biāo)系2維平面到3維世界坐標(biāo)下的轉(zhuǎn)換模型,即位姿解算模型,從而解算出無(wú)人機(jī)自身位姿信息。
文中以常見(jiàn)的四旋翼無(wú)人機(jī)作為研究對(duì)象,考慮系統(tǒng)的實(shí)時(shí)性與低復(fù)雜性,選用藍(lán)色雙三角平面標(biāo)志物作為人工標(biāo)志,在無(wú)人機(jī)上固定相機(jī),對(duì)機(jī)載相機(jī)拍攝的圖片進(jìn)行圖像處理,將處理結(jié)果和已知世界坐標(biāo)系下的圖像坐標(biāo)作為位姿解算模型的輸入,解算無(wú)人機(jī)位姿。假設(shè)室內(nèi)無(wú)人機(jī)飛行區(qū)域中都能通過(guò)機(jī)載相機(jī)觀測(cè)到架設(shè)的人工標(biāo)志。位姿估計(jì)示意圖如圖1所示。
圖1 位姿估計(jì)的示意圖
視覺(jué)位姿估計(jì)系統(tǒng)由兩個(gè)模塊構(gòu)成:圖像處理以及位姿解算。圖像處理模塊完成人工標(biāo)志的檢測(cè)與提取。利用顏色特征完成人工標(biāo)志的檢測(cè),借助Harris角點(diǎn)檢測(cè)、Hough直線擬合獲取特征點(diǎn)的提取。圖像處理的流程如圖2所示。
圖2 圖像處理流程圖
位姿解算,實(shí)際就是對(duì)于機(jī)載相機(jī)的外參數(shù)的估計(jì)。在完成攝像機(jī)內(nèi)參數(shù)標(biāo)定的前提下,利用點(diǎn)透視算法求解一個(gè)粗精度的位姿將其作為正交迭代算法[10]的初始值,經(jīng)迭代求解位姿。下文對(duì)位姿解算原理進(jìn)行詳細(xì)介紹。
位姿解算,是對(duì)世界坐標(biāo)系下信息已知的人工標(biāo)志進(jìn)行成像信息進(jìn)行特征分析處理求解。這里對(duì)坐標(biāo)系轉(zhuǎn)換以及攝像機(jī)成像原理進(jìn)行如下說(shuō)明。
2.1 攝像機(jī)模型
攝像機(jī)模型,是對(duì)攝像機(jī)成像過(guò)程的數(shù)學(xué)表達(dá)。物體在圖像坐標(biāo)上的像點(diǎn)與攝像機(jī)內(nèi)部參數(shù)以及物體的空間相對(duì)位置有關(guān)。理想的攝像機(jī)模型為針孔模型,但實(shí)際中,相機(jī)一般由于鏡頭畸變呈非線性。非線性相機(jī)模型為:
式(1)中 f為焦距,xc,yc,zc為攝像機(jī)坐標(biāo)系下坐標(biāo),xI,yI為圖像坐標(biāo)系下坐標(biāo)。 k1,k2為畸變系數(shù),取二階徑向模型。攝像機(jī)的焦距,像元尺寸,光心,偏移量,不垂直度,畸變系數(shù)等內(nèi)部參數(shù)都是未知的,要實(shí)現(xiàn)位姿估計(jì)還需對(duì)其標(biāo)定。
2.2 坐標(biāo)系轉(zhuǎn)換
位姿解算本質(zhì)上是2D-3D的場(chǎng)景重建,在此過(guò)程中,涉及到世界坐標(biāo)系、機(jī)體坐標(biāo)系、攝像機(jī)坐標(biāo)系及圖像坐標(biāo)系的相互轉(zhuǎn)換。
系統(tǒng)將無(wú)人機(jī)與攝像機(jī)固連,只需解算出攝像機(jī)在世界坐標(biāo)系下的位姿,即認(rèn)為獲取無(wú)人機(jī)的位姿。下文的機(jī)體坐標(biāo)系,均以攝像機(jī)坐標(biāo)系取代。就研究涉及的三個(gè)坐標(biāo)系間的轉(zhuǎn)換關(guān)系介紹如下:
1)世界坐標(biāo)系-攝像機(jī)坐標(biāo)系
世界坐標(biāo)系到攝像機(jī)坐標(biāo)系的轉(zhuǎn)換如圖3所示。
圖3 世界坐標(biāo)系-攝像機(jī)坐標(biāo)系
兩不同的右手坐標(biāo)系可通過(guò)旋轉(zhuǎn)平移轉(zhuǎn)換重合。旋轉(zhuǎn)變換可視為三軸以一定順序分別旋轉(zhuǎn)的合運(yùn)動(dòng)。取繞各軸逆時(shí)針旋轉(zhuǎn)的角度為正,按照X-YZ旋轉(zhuǎn)順序,得到符合系統(tǒng)的旋轉(zhuǎn)矩陣:
式(3)中 準(zhǔn),θ,ψ 對(duì)應(yīng)橫滾角、俯仰角、偏航角。下文用r11-r33代替R矩陣的各個(gè)元素。
2)攝像機(jī)坐標(biāo)系-圖像坐標(biāo)系
攝像機(jī)坐標(biāo)系到圖像坐標(biāo)系的轉(zhuǎn)換是基于攝像機(jī)模型的。上一章節(jié)已給出詳細(xì)的闡述
其中 u,v 是圖像坐標(biāo),xc,yc,zc是攝像機(jī)的坐標(biāo),u0,v0為圖像中心,fx,fy為像素單位下的焦距,k1,k2為畸變系數(shù)。
2.3 位姿估計(jì)算法
1)N點(diǎn)透視算法
考慮到無(wú)人機(jī)系統(tǒng)機(jī)動(dòng)性較大,位姿估計(jì)需兼顧實(shí)時(shí)性,不宜取過(guò)多特征點(diǎn)。本系統(tǒng)擬采用平面標(biāo)志物的6個(gè)特征點(diǎn)分析解算。理論上,對(duì)于平面目標(biāo),只需要4個(gè)共面且任意三點(diǎn)不在一條直線上的點(diǎn)集即可確定出無(wú)人機(jī)的位姿[11]。
對(duì)于圖像處理后獲取的點(diǎn)坐標(biāo)進(jìn)行畸變校正。將校正后的坐標(biāo)ui,vi代入模型。從攝像機(jī)坐標(biāo)系到圖像坐標(biāo)系,是一個(gè)3D-2D的轉(zhuǎn)換過(guò)程。轉(zhuǎn)換中,深度信息丟失,但是可以在求解過(guò)程中消去該變量。求解的方程組如下:為歸一化的攝像機(jī)坐標(biāo)。 假設(shè) tz=1,有 8個(gè)未知量,根據(jù)PnP理論和線性代數(shù)知識(shí),需要任意三點(diǎn)不共線的4個(gè)點(diǎn)才能求解。對(duì)于式(5)可記為:Am=b。通過(guò)最小二乘法可以解出 m=(ATA)-1ATb。結(jié)合旋轉(zhuǎn)矩陣的單位性和正交性可以求解出tz、RJS、TJS。
至此,位姿估計(jì)的初始值求解完成。
2)正交迭代算法
對(duì)于N點(diǎn)透視算法,各個(gè)特征點(diǎn)提取都存在著量測(cè)誤差,估計(jì)精度隨之降低。基于此,本文參考了一種正交迭代思想[12-13]來(lái)對(duì)N點(diǎn)透視算法的估計(jì)值進(jìn)行進(jìn)一步優(yōu)化。
已知任意空間點(diǎn)與其成像在同一條過(guò)光心的直線上[14]。利用這個(gè)性質(zhì),以量測(cè)像點(diǎn)到實(shí)際像點(diǎn)的距離構(gòu)造誤差方程。假設(shè)人工標(biāo)志的特征點(diǎn)在世界坐標(biāo)系下的坐標(biāo)為wi,根據(jù)提取的圖像坐標(biāo)解算出投影在歸一化平面上的像點(diǎn)vi?;谖墨I(xiàn)[13]的物空間誤差模型建立目標(biāo)函數(shù)如下:
令E(R,T)值最小的R,T為最優(yōu)估計(jì)。對(duì)于這兩個(gè)參數(shù)的最小平方的估計(jì),文獻(xiàn)[11]給出利用奇異值分解法的求解方案。旋轉(zhuǎn)矩陣的精度與平移向量精度息息相關(guān),利用迭代算法計(jì)算得到旋轉(zhuǎn)矩陣R*。在此基礎(chǔ),計(jì)算關(guān)于R*的T的最優(yōu)解作為平移變量T的初始值:
根據(jù)vi=R*wi+T,更新vi,迭代若干次,計(jì)算使E最小的R和T。其中最優(yōu)R*的求解。采用SVD分解法[11,13-15]進(jìn)行求解。
3)位姿求解
在旋轉(zhuǎn)矩陣和平移向量求解完成后,位姿估計(jì)就十分直觀了。對(duì)于位置的求解,根據(jù)世界坐標(biāo)-攝像機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系以及旋轉(zhuǎn)矩陣的定義,可以得到位置t和姿態(tài)角為:
3.1 仿真實(shí)驗(yàn)
在Matlab2012b平臺(tái)上,對(duì)位姿估計(jì)算法進(jìn)行仿真實(shí)驗(yàn)。通過(guò)給定姿態(tài)角位置和攝像機(jī)模型求解出對(duì)應(yīng)的圖上坐標(biāo),對(duì)求解的圖上坐標(biāo)加不同強(qiáng)度(單位為像素)的隨機(jī)噪聲 q×(rand(1)-0.5),通過(guò)位姿估計(jì)算法,將結(jié)果與參考值對(duì)比。仿真實(shí)驗(yàn)中影響位姿估計(jì)精度的因素,可以歸納為:量測(cè)噪聲強(qiáng)度、人工標(biāo)志距離光軸的遠(yuǎn)近、人工標(biāo)志的大小/距攝像機(jī)距離以及人工標(biāo)志的特征點(diǎn)數(shù)。下文就以上因素給出仿真結(jié)果。
1)量測(cè)噪聲強(qiáng)度——取6個(gè)特征點(diǎn)作為人工標(biāo)志的輸入,保持位置一定,對(duì)于噪聲強(qiáng)度從0-10,以0.5為間隔,每個(gè)強(qiáng)度取1 000次估計(jì)結(jié)果的平均值。在不同強(qiáng)度的噪聲下,解算出來(lái)的位姿誤差如如圖4所示,隨著噪聲的增強(qiáng),姿態(tài)角和位置估計(jì)的誤差逐漸增大。在噪聲強(qiáng)度為10像素之內(nèi),角度偏差最大為2°。當(dāng)無(wú)人機(jī)距人工標(biāo)志物的距離在1 600 mm左右,對(duì)于強(qiáng)度為10像素的隨機(jī)噪聲會(huì)造成最大x方向2cm的偏差,y方向6 cm的偏差,z方向4 cm的偏差。
圖4 不同噪聲強(qiáng)度下位姿估計(jì)誤差
圖5 距光軸遠(yuǎn)近不同對(duì)位姿估計(jì)的影響
2)人工標(biāo)志距離光軸的遠(yuǎn)近——仿真取相同噪聲條件下,初始狀態(tài)為[-pi,0,0]時(shí),可視為距光軸為0,橫滾角以0.1 rad遞增,距離逐漸變大,位姿解算的誤差如圖5所示。人工標(biāo)志距光軸的距離隨著roll角的減小而增大,位姿估計(jì)誤差隨著光軸的距離的增大而減小。
圖6 人工標(biāo)志大小/相機(jī)距離對(duì)位姿估計(jì)的影響
圖7 特征點(diǎn)個(gè)數(shù)對(duì)位姿估計(jì)的影響
3)人工標(biāo)志的大小/距攝像機(jī)距離——簡(jiǎn)單地,取相機(jī)位于目標(biāo)的正上方,在相同的噪聲強(qiáng)度下,同樣大小的人工標(biāo)志,取不同的高度,即對(duì)應(yīng)不同的得到的仿真結(jié)果如圖6所示。結(jié)果表明,距離越大位姿估計(jì)精度越低。
4)平面特征點(diǎn)的個(gè)數(shù)——平面特征點(diǎn)至少要4個(gè),考慮到這里要保證其他因素不變,取標(biāo)志物上的4/5/6個(gè)特征點(diǎn)觀察一下趨勢(shì),仿真結(jié)果如圖7所示。特征點(diǎn)給的個(gè)數(shù)越多,位姿估計(jì)精度越高,但實(shí)際系統(tǒng)中太多的特征點(diǎn)在圖像處理中會(huì)耗時(shí)多,無(wú)法滿足實(shí)時(shí)性的要求。
5)除此以外,仿真實(shí)驗(yàn)比較了本文算法和單獨(dú)的
N點(diǎn)透視算法的估計(jì)精度。對(duì)于不同噪聲強(qiáng)度下,?。?/p>
作為衡量位置和角度與標(biāo)準(zhǔn)值的平均偏差,仿真結(jié)果如圖8。
實(shí)驗(yàn)結(jié)果表明,加入正交迭代,對(duì)于位姿估計(jì)的精度有明顯提升,尤其是在噪聲強(qiáng)度大的時(shí)候,效果更明顯。
圖8 兩種算法的位姿誤差
3.2 實(shí)物實(shí)驗(yàn)
由于無(wú)人機(jī)在室內(nèi)無(wú)法獲取GPS信號(hào),因此利用VICON紅外三維運(yùn)動(dòng)捕捉系統(tǒng),來(lái)獲取無(wú)人機(jī)的位置姿態(tài)信息,以此作為無(wú)人機(jī)位姿的標(biāo)準(zhǔn)值。實(shí)物實(shí)驗(yàn)選取的機(jī)載相機(jī)型號(hào)為Gopro hero4 silver。采用加州理工大學(xué)開(kāi)發(fā)的camera calibration toolbox在MATLAB2012b下進(jìn)行棋盤(pán)標(biāo)定,標(biāo)定結(jié)果如表1所示。
圖9 圖像處理過(guò)程
表1 攝像機(jī)參數(shù)標(biāo)定結(jié)果
實(shí)物實(shí)驗(yàn)使用的人工標(biāo)志物為藍(lán)色的挖空的三角形圖像,對(duì)機(jī)載相機(jī)拍攝的圖像做離線的圖像處理,由于篇幅限制,截取的部分圖像處理的過(guò)程如圖9所示。
人工標(biāo)志固定,選取無(wú)人機(jī)任意位置拍攝的兩組圖片進(jìn)行位姿解算驗(yàn)證算法。相機(jī)拍攝的兩組位姿圖像如圖10所示。解算結(jié)果如表2所示。
圖10 兩組位姿圖像
表2 位姿解算結(jié)果
實(shí)驗(yàn)表明,該位姿估計(jì)算法能實(shí)現(xiàn)無(wú)人機(jī)的位姿估計(jì),并達(dá)到一定的精度。在實(shí)物實(shí)驗(yàn)中,出現(xiàn)了上文例子2中,偏航角的解算出復(fù)數(shù)形式,即死鎖問(wèn)題,這是由于歐拉角表示的旋轉(zhuǎn)矩陣的局限,這一問(wèn)題可通過(guò)四元數(shù)法表示的旋轉(zhuǎn)矩陣進(jìn)行修正。
文中對(duì)無(wú)人機(jī)室內(nèi)飛行時(shí)的位姿實(shí)時(shí)獲取提供了一種由粗到精的位姿估計(jì)方法。根據(jù)不同位姿下機(jī)載相機(jī)下的人工標(biāo)志不同的成像信息,在世界坐標(biāo)系和圖像坐標(biāo)系下建立轉(zhuǎn)換關(guān)系,利用點(diǎn)透視算法解位姿初值代入正交迭代算法,最終實(shí)現(xiàn)位姿的精確解算。經(jīng)實(shí)驗(yàn)驗(yàn)證,在噪聲影響較小的情況下,本算法能實(shí)現(xiàn)室內(nèi)無(wú)人機(jī)位姿的實(shí)時(shí)高精度估計(jì)。但該算法是基于點(diǎn)特征,受噪聲影響較大,一旦噪聲過(guò)大,則估計(jì)不準(zhǔn)確,下一步人工目標(biāo)的特征設(shè)計(jì)可朝著曲線特征[16]以及與IMU信息融合的方向改進(jìn)。
[1]孫罡.低成本微小型無(wú)人機(jī)慣性組合導(dǎo)航技術(shù)研究[D].南京:南京理工大學(xué),2014.
[2]沈浩.無(wú)人機(jī)MIMU/GPS組合導(dǎo)航系統(tǒng)研究[D].哈爾濱:哈爾濱工程大學(xué),2012.
[3]張勇.基于合作目標(biāo)的無(wú)人機(jī)位姿估計(jì)算法研究[D].南京:南京航空航天大學(xué),2010.
[4]竇川川.基于慣性傳感和視覺(jué)方法的無(wú)人直升機(jī)位姿估計(jì)比較研究[D].哈爾濱:哈爾濱工業(yè)大學(xué),2013.
[5]Li J H,Xie X Z.Study of Monocular Measuring Technique Based on Homography Matrix[M].Berlin Heidelberg:Springer 2011:316-324.
[6]Jayatilleke L,Zhang N.Landmark-based localization for Unmanned Aerial Vehicles[C].//Systems Conference (SysCon), 2013 IEEE International IEEE,2013:448-451.
[7]Zhu L, Fan B,Du Y, et al.A tracking and locating method for UAVs Vision System[C].//Information and Automation (ICIA), 2010 IEEE International Conference onIEEE,2010:1953-1957.
[8]Wang T,Zhang Y,Wang C,et al.Indoor visual navigation system based on paired-landmark for smallUAVs [C].Robotics and Biomimetics(ROBIO), 2014 IEEE International Conference onIEEE,2014:1703-1708.
[9]楊世保.無(wú)人機(jī)著陸位姿參數(shù)視覺(jué)估計(jì)研究[D].南京:南京航空航天大學(xué),2012.
[10]李鑫,龍古燦,劉進(jìn)博,等.相機(jī)位姿估計(jì)的加速正交迭代算法[J].光學(xué)學(xué)報(bào),2015(1):266-273.
[11]王鵬,孫長(zhǎng)庫(kù),張子淼.單目視覺(jué)位姿測(cè)量的線性求解[J].儀器儀表學(xué)報(bào), 2011,32(5):1126-1131.
[12]Morbidi F,Mariottini G L.Active Target Tracking and Cooperative Localization for Teams of Aerial Vehicles[J].IEEE Transactions on Control Systems Technology, 2013,21(5):1694-1707.
[13]Mirota D J, Ishii M, Hager G D.Vision-based navigation in image-guided interventions.[J].Annual Review of Biomedical Engineering,2011,13(13):297-319.
[14]李飛,唐大全,畢波,等.基于點(diǎn)線對(duì)應(yīng)的無(wú)人機(jī)位姿估計(jì)算法 [J].計(jì)算機(jī)測(cè)量與控制,2011,19(12):3108-3110.
[15]許允喜,蔣云良,陳方,等.基于點(diǎn)線對(duì)應(yīng)的多攝像機(jī)全局位姿估計(jì)迭代算法[J].光子學(xué)報(bào),2010,39(10):1881-1888.
[16]尚曉紅.一種基于圓或平行線特征的位姿估計(jì)方法[D].昆明:云南大學(xué),2012.
Pose estimation of UAV based on artificial marks with monocular vision
WU Yan, WU Fen, QI Guo-qing
(Nanjing University of Science and Technology, Nanjing 210094, China)
This article proposes a pose estimation algorithm with a combination of digital image processing and 2D-3D scene reconstruction to deal with UAV self localization.Under the premise of the known world coordinates of the artificial mark,we obtain the information from the images taken by a nonlinear onboard camera and take some methods of digital image processing to provide feature point datas for subsequent calculation.Pose estimation provides a improved N-point perspective algorithm which combines orthogonal iteration.N-point perspective calculates attitude angles matrix as the initial values of orthogonal iteration.The feasibility of the algorithm is verified by simulation experiments and real experiments.Compared with N-point perspective algorithm,this method improves the pose estimation accuracy and fast convergence and provide a new idea for the UAV indoor pose estimation.
self localization; nonlinear; artificial marks; PnP; orthogonal iteration
TN29
A
1674-6236(2017)12-0143-06
2016-08-04稿件編號(hào):201608037
國(guó)家自然科學(xué)基金項(xiàng)目(61104186;61273076)
吳妍(1992—),女,江蘇南通人,碩士研究生。研究方向:四旋翼無(wú)人機(jī)的定位與導(dǎo)航、圖像處理等。