基于矢量約束的隨機特征點選取算法

馬　旭, 程詠梅, 郝　帥

(1. 西北工業大學自動化學院, 陜西 西安 710129;2. 西安科技大學電氣與控制工程學院, 陜西 西安 710054)

?

基于矢量約束的隨機特征點選取算法

馬旭1,2, 程詠梅1, 郝帥2

(1. 西北工業大學自動化學院, 陜西 西安 710129;2. 西安科技大學電氣與控制工程學院, 陜西 西安 710054)

無人機利用視覺在未知區域自主著降時,提取的特征點具有數量多、隨機性強等特點。針對利用隨機特征點進行位姿估計精度低、穩定性差的問題,提出一種基于矢量約束的隨機特征點選取算法。首先通過分析位姿估計方程可知，特征點地理坐標是影響方程組求解精度的重要因素;然后在引入矢量角均分度、矢量模值均值及矢量模值最大值三項約束指標基礎上，制定了一種基于矢量約束的特征點選取策略;最后利用正交迭代算法對所選取的特征點進行位姿估計精度驗證。實驗結果表明，提出的算法相比于隨機選取的特征點進行相對位姿估計精度更高,魯棒性更強。

隨機特征點; 相對位姿估計; 矢量約束; 正交迭代; 未知區域

0　引　言

無人機在執行營救、搜索等任務時,面臨著降區地形未知、復雜且無地面輔助導航設備的緊急著陸問題[1]。由于慣性導航存在累計誤差、GPS易受干擾,使人機在復雜未知環境中著陸時存在安全隱患。視覺相對導航具有設備簡單、信息量大、隱蔽性強、自主性好等優點,在無人機自主著陸/著艦、空中加油及航天器交匯對接[2]等領域都有廣泛應用。利用計算機視覺技術引導無人機在未知環境自主著陸是目前的一個研究熱點和難點[3-4]。視覺引導無人機自主著陸主要包括以下5個步驟:著降區圖像分割、搜索、地質識別、平坦度估計以及無人機與著降區的相對位姿估計[5-7]。其中，如何準確估計出無人機與著降區的相對位姿參數,是保證無人機能夠最終安全著陸的關鍵步驟。

基于合作目標的無人機自主著陸相對位姿估計是通過在無人機降落地點事先布置結構參數已知且地理坐標已標定好的合作目標。然后通過提取合作目標特征點的像素點坐標,在計算機視覺成像原理基礎上解決透視N點定位問題。通過求解位姿估計方程來估計出無人機與著降區的相對位姿參數[8-9]。但是，在未知環境中無法提取固定目標的特征點(如Harris角點、尺度不變特征變換(scale-invariantfeaturetransform,SIFT)特征或是其他特征),而目前主要通過兩種方式來獲得形式上所謂的固定特征點。第1種是基于連續幀的方式,通過幀間特征點匹配,并結合無人機當前高度、飛行距離,實現特征點的標定(特征點的地理坐標),具體可見文獻[10];第2種是通過雙目立體視覺的方法,可直接重構出著降區的三維坐標信息,實現特征點的標定[11]。然而,在未知區域中提取的特征點存在數量眾多、隨機性強以及特征點提取誤差等因素,因此當利用所有隨機特征點進行相對位姿估計時不僅實時性差、而且易造成估計精度不高的問題。文獻[12]以星敏感器為研究對象,通過實驗指出在估計相對位姿參數時,選取特征點的地理坐標位置對于位姿參數估計精度影響較大,但是文中并未給出特征點應如何選取。針對上述問題,本文提出一種基于矢量約束的隨機特征點選取算法,在此基礎上利用正交迭代(orthogonaliteration,OI)算法在仿真及物理實驗條件下驗證所提出算法的有效性。

1　相對位姿估計模型

基于視覺的相對位姿估計主要是通過圖像像素坐標系、圖像物理坐標系、攝像機坐標系以及一組空間關系已知的特征點坐標系之間的轉換關系,推導出相機在合作目標坐標系中的位置和姿態,從而進一步得到相機載體在合作目標坐標系中的位置和姿態[13-14],具體有

(1)

式中,(XL,YL,ZL)、(Xc,Yc,Zc)和(u,v)分別為特征點在世界坐標系下、相機坐標系下以及像素坐標系下的坐標；(u0,v0)為圖像物理坐標系原點在圖像像素坐標系中的坐標;fx=f/dx,fy=f/dy,f為焦距;dx和dy為一個像素點在x軸和y軸方向的長度;0T=(0,0,0),t=(tx,ty,tz)T是三維平移向量;R是3×3的旋轉矩陣,它包含3個獨立變量記為滾轉角α、航向角γ以及俯仰角β,3個姿態角定義可文獻[15],表達式為

(2)

當解算特征點在同一個平面上時,可假設世界坐標ZL=0,將Zc可以看作為比例因子ρ,則式(1)可以改寫為

(3)

令

式中,i=1,2,…,N,N為解算特征點個數。式(3)可改寫為

ρV=KR1M

(4)

由于K為相機內參構成的矩陣,所以是一個非奇異矩陣,將式(4)兩端乘以K-1,化簡得

(5)

進一步對式(5)兩端乘以MT,并對其兩端同時轉置,最終得

(6)

2　特征點分布對位姿估計影響分析

2.1位姿估計方程組條件數定義

從位姿估計方程式(6)可以看出,該方程是典型的線性方程組Ax=b形式,其中

(7)

由式(7)可以看出,系數矩陣A是由特征點的地理坐標構成的;b矩陣中包含了攝像機內參、特征點的像素坐標和地理坐標;x為包含所求位姿參數的矩陣。對于線性方程組Ax=b,系數矩陣A的條件數表示為

(8)

其物理意義表示方程組的解對測量數據誤差的敏感程度。針對本文的研究問題,假設b中存在誤差δb(包括相機參數標定誤差,特征點像素提取誤差等),得到的解記為x+δx,δx為誤差,則線性方程組可表示為

(9)

由式(9)可得

(10)

進一步可推出

(11)

而由方程組Ax=b可知

(12)

因此,根據式(11)和式(12)可得

(13)

通過式(13)可知,如果cond(A)大,則b的微小擾動就能引起解x的較大改變,數值穩定性差。反之,如果cond(A)小,則方程的數值穩定性好,即當有外界擾動時得到的位姿參數精度高且數據穩定性好。

2.2評價指標

(14)

(15)

為了分析特征點分布與相對位姿參數估計精度及穩定性間的關系,引入了3個特征點分布評價指標,即矢量角均分度、矢量模值均值以及最大矢量模值。首先將N個特征點的矢量角按從大到小排列ω={ω1,ω2,…,ωN},對應的矢量模長d={d1,d2,…,dN},3個評價指標分別定義如下。

(1) 矢量角均分度?

(16)

該指標用于描述特征點在平移坐標系中矢量角的均分程度,當N個相鄰特征點之間的矢量夾角越接近,其矢量均分度越小。

(17)

該指標用于描述特征點在平移坐標系中矢量模值的平均值。

(3) 矢量模值最大值dmax

(18)

該指標用于描述特征點在平移坐標系中矢量模值中的最大值。

此外,為了整體評價位姿估計精度,引入位置誤差εp和角度誤差εr,其定義分別如式(19)和式(20)所示。

(19)

(20)

式中,xt，yt和zt分別表示X,Y,Z3個方向上的位置真值;Δx，Δy和Δz分別為3個方向的位置估計誤差; Δα，Δβ和Δγ分別表示3個姿態角的誤差。

2.3數值分析

假設相機的等效焦距為821,3個姿態角(α,β,γ)分別為10°、20°、-15°,固定平移向量在x、y方向上的分量tx=ty=25,tz=300,在特征點坐標中隨機加入不同信噪比噪聲來模擬實際提取特征點時的誤差,并取信噪比為60 dB。針對以下3種情況,進行了500次的蒙特卡羅仿真相對位姿估計實驗,其中位姿估計算法使用的是OI算法[16]。

(1)矢量角均分度對位姿估計精度的影響

以6個特征點為例,解算特征點幾種典型分布情況如圖1所示。

圖1　6個特征點的4種典型分布Fig.1　Four kinds of typical distribution of six feature points

通過計算各評價指標,其結果如表1所示。

表1　6個特征點隨機分布時各指標評價結果

從表1可以看出,當條件數小時,位姿估計點誤差也小;當矢量模值均值和最大矢量模值一樣或相差不大時,矢量角均分度越小的,對應條件數也越小,位姿估計誤差也越小。

(2)矢量模值最大值和矢量模值均值對位姿估計精度的影響

隨機生成模值相等的6個特征點,如圖2(a)所示,保持6個點的矢量角均分度不變,改變其中幾個特征點的模值分別如圖2(b)～圖2(e)所示。

圖2　6個特征點的5種典型分布Fig.2　Five kinds of typical distribution of six feature points

計算圖2中的各評價指標,其結果如表2所示。

表2　矢量均分度不變情況下各指標評價結果

由表2可看出,當矢量角均分度和矢量模值最大值保持不變時,矢量模值均值小的對應較小的條件數,估計結果誤差越小;對比圖2(c)和圖2(d)可知,當矢量模值平均值也一樣時,矢量模值最小模值小的,條件數較小;對比圖2(e),當矢量角均分度和矢量模值均值相等時,矢量模值最大值小的,條件數較小。

(3)特征點數對位姿估計精度的影響

在第(1)種情況下所示的4種特征點分布基礎上添加同樣一個解算點,如圖3所示,統計結果如表3所示。

圖3　7個特征點的4種典型分布Fig.3　Four kinds of typical distribution of seven feature points

由表3分析可知,當整體分布不變的情況下,添加一個解算點可以使得條件數和解算精度有所改善,但程度有限。此外當特征點個數分別為5～8個、特征點均成正多邊形分布且矢量模值最大值相等的情況下,進行了500次蒙特卡羅仿真實驗,記錄不同特征點個數對解算時間和精度的影響,其結果如表4所示。

表4　實時性比較

從表4中可以看出,隨著參與相對位姿估計的特征點數量增加,位姿估計精度也會提高,但是實時性會下降。

通過上述3種情況對相對位姿估計精度的影響分析可知,當矢量角均分度、矢量模值均值及矢量模值最大值越小時,相對位姿估計精度越高。此外,將實時性和位姿估計精度綜合考慮,本文選取6個特征點進行相對位姿估計。

3　矢量約束搜索策略

3.1特征點組合生成

本文主要通過對最大矢量模值和矢量角均分度進行約束來提取滿足要求的特征點組合。

根據第2節的結論可知,當其他評價指標一定時,最大矢量模值越小時,相對位姿估計精度越高。此外隨著無人機的降落,靠近平移坐標系中心點的特征點也不易出視野,所以本文首先對特征點的最大矢量模值進行約束。

其最大模值約束策略如下:選取滿足集合

的m個特征點為初選點,其中為著降區半徑在圖像坐標系中所對應的像素數。由于矢量角均分度對相對位姿估計精度有較大影響,為保證矢量角均分度盡量小,本文對其進一步約束。

最后分別以m個特征點為第一個解算點進行遍歷,可生成m組位姿參數估計特征點組合,并計算每個組合的矢量角均分度和矢量模值均值。

3.2矢量約束歸一化與最優值計算

在得到用于相對位姿估計的特征點組后,對m組特征點按照如下3個步驟進行求解。

(1) 矢量均分度歸一化

將第j組的矢量均分度用?j(j=1,2,…,m；i=1,2,…,6)表示,其中最大值為?max,最小值為?min，并對其做線性歸一化轉化。

(21)

式中,0≤?(j)≤1。

(2)矢量模值均值歸一化

(22)

(3) 優先值計算

根據選取策略,設定第j組的優先值為

(23)

式中,j=1,2,…,m。將W(j)從小到大按順序排列,選取優先值最小的第j組特征點作為解算特征點,來估計無人機的位置與姿態。由于無人機在降落過程中,隨著圖像尺度變化參與解算的特征點可能存在出視野或是點丟失的情況,同時著降區內也可能出現新的特征點。此時,需要及時更新初選點集合,確保完成相對位姿估計。

3.3算法流程

本文算法計算流程如圖4所示。

圖4　算法流程圖Fig.4　Algorithm flowchart

4　實驗結果及分析

4.1仿真實驗驗證

假設在著降區內有29個特征點,如圖5所示。

圖5　初始特征點Fig.5　Initial feature points

根據本文所提出的矢量約束選點策略,選取用于相對位姿估計的特征點如圖6中用藍色框選出的解算特征點組合。

圖6　矢量約束結果Fig.6　Vector constraint results

為了驗證所選取特征點的解算精度,在圖5中分別隨機選取了2組特征點進行對比解算。設定3個旋轉角(α,β,γ)分別為15°、30°、-15°,固定平移向量在x、y方向上的分量tx=ty=25,令tz為變量(相機距離地面的高度),假設信噪比為70dB,對3組特征點分別進行測試,結果如圖7所示。

圖7　誤差結果Fig.7　Error results

由圖7可知,在相同信噪比情況下,用矢量約束策略選出的點組合與隨機選取點組合相比,位姿解算結果的相對誤差更小,尤其在遠距離解算時,本文選取的解算點組合在位置解算中獲得了更高的精度,具有更好的抗噪性。

4.2物理實驗驗證

主要的實驗設備有:北京博創興盛科技有限公司生產的UP-VoyagerIIA自主移動機器人;CCD傳感器采用羅技C310攝像頭(分辨率為640×480,等效焦距為821);實驗使用70cm×50cm的坐標紙,代表無人機選定的著降區,記為模擬著降區;并在坐標紙上隨機黏貼39個藍色方格和20個紅色方格,方格尺寸為1cm×1cm,同時記錄這59個方格的中心坐標,代表已經被標注好的隨機特征點,其中黑色原點代表著降區坐標系的原點,完成特征點的標定,相關設備如圖8所示。

圖8　實驗中的相關設備Fig.8　Related equipment in experiments

實驗中將相機固定在移動機器人上,使移動機器人距離模擬著降區1.5m處開始不斷靠近待選著降點,模擬無人機在著降區域的下降過程。

當移動機器人距離模擬著降區110cm時,攝像機在模擬著降區坐標系下的坐標(單位為cm)記為(x,y,z)=(14.5,6,110),拍攝結果如圖9所示。

圖9　t3=110 cmFig.9　t3=110 cm

用本文方法對圖9中的59個隨機特征點(即59個方格的中心坐標)進行計算,得到圖10中用藍色方框標注的解算特征點組合。另外隨機選取了2組特征點組合,分別用綠色和紅色方框標注,進行后續相對位姿估計精度對比實驗。

圖10　特征點選取Fig.10　Feature selection

在距離待選特征點60～150cm的范圍內,通過改變相機與待選特征點x、y和z 3個方向的偏移量來進行實驗測試,圖像中特征點的質心像素坐標通過手動標注(運動平臺與待選特征點越近,成像的像素分辨率越高,手動標定的像素坐標精度也越高)。將本文方法選取的解算點組合與上述隨機選取的2組解算點進行相對位姿估計誤差對比分析,相對誤差ε定義為

(24)

位置相對誤差實驗結果如表5所示。

表5　位置相對誤差

從表5可以看出,基于矢量約束的隨機特征點選取算法,可以自動有效選取特征點組合,且數值穩定性高于隨機選取的特征點組合,隨著運動平臺與待選特征點越近,解算精度越高。而對于其他兩組隨機選取的特征點由于其中一些特征點出視場,造成無法進行相對位姿估計,所以表5中用“-”來表示。此外需要說明的是，由于移動機器人姿態角控制精度不高,所以實驗中未對姿態角誤差進行比較。

5　結　論

提出一種基于矢量約束的特征點選取算法,為在未知環境下利用視覺進行相對位姿參數準確、魯棒估計提供一種有效的解決思路。通過大量仿真實驗及物理實驗,結果表明，本文提出的算法相比于隨機選取特征點進行位姿估計的方法精度更高,魯棒性更強。

[1]FitzgeraldD,WalkerR,CampbellD.AvisionbasedforcedlandingsiteselectionsystemforanautonomousUAV[C]∥Proc. of the IEEE International Conference on Intelligent Sensors, Sensor Networks and Information Processing, 2005: 397-402.

[2]YangS,SchererSA,ZellA.Anonboardmonocularvisionsystemforautonomoustakeoff,hoveringandlandingofamicroaerialvehicle[J].Journal of Intelligent & Robotic Systems, 2013, 69(1): 499-515.

[3]SchererS,ChamberlainL,SinghS.Autonomouslandingatunpreparedsitesbyafull-scalehelicopter[J].Robotics and Autonomous Systems, 2012, 60(12): 1545-1562.

[4]TakahashiMD,AbershitzA,RubinetsR,etal.Evaluationofsafelandingareadeterminationalgorithmsforautonomousrotorcraftusingsitebenchmarking[J].Journal of the American Helicopter Society, 2013, 58(3): 1-13.

[5]ShenYF,RahmanZ,KrusienskiD,etal.Avision-basedautomaticsafelanding-sitedetectionsystem[J].IEEE Trans. on Aerospace and Electronic Systems, 2013, 49(1): 294-311.

[6]CoombesM,ChenWH,RenderP.ReachabilityanalysisoflandingsitesforforcedlandingofaUAS[J].Journal of Intelligent & Robotic Systems, 2014, 73(1/4): 635-653.

[7]MejiasL,EngP.Controlledemergencylandingofanunpoweredunmannedaerialsystem[J].Journal of Intelligent & Robotic Systems, 2013, 70(1/4): 421-435.

[8]GholamiMR,GeziciS,StromEG.ImprovedpositionestimationusinghybridTW-TOAandTDOAincooperativenetworks[J].IEEE Trans. on Signal Processing, 2012, 60(7): 3770-3785.

[9]ZhangH,JiangZ,ElgammalA.Vision-basedposeestimationforcooperativespaceobjects[J].Acta Astronautica,2013,91(10):115-122.

[10]MaX,ChengYM,HaoS,etal.Densepointfeaturegenerationalgorithmbasedonmonocularsequenceimagesfordepthmeasurementofunknownzone[J].Acta Aeronautica et Astronautica Sinica,2015,36(2):596-604.(馬旭,程詠梅,郝帥,等.面向未知區域深度測量的單目序列圖像稠密點特征生成算法[J].航空學報,2015,36(2):596-604.)

[11]TheodoreCT,TischlerMB.Precisionautonomouslandingadaptivecontrolexperiment(PALACE),ADM002075[R].California:NationalAeronauticsandSpaceAdministrationAmesResearchCenterMoffettField, 2006.

[12]ZhangL,HeX,WeiZH,etal.Impactanalysisofstardistributiononleastsquareattitudemeasuringprecisioninstarsensor[J].Infrared and Laser Engineering, 2014, 43(6): 1836-1841.(張磊,何昕,魏仲慧,等.星圖分布對星敏感器最小二乘姿態精度的影響[J].紅外與激光工程,2014,43(6):1836-1841.)

[13]ZhuX,RamananD.Facedetection,poseestimation,andlandmarklocalizationinthewild[C]∥Proc. of the IEEE International Conference on Computer Vision and Pattern Recognition, 2012: 2879-2886.

[14]PauwelsK,RubioL,DiazJ,etal.Real-timemodel-basedrigidobjectposeestimationandtrackingcombiningdenseandsparsevisualcues[C]∥Proc. of the IEEE International Conference on Computer Vision and Pattern Recognition, 2013: 2347-2354.

[15]HaoS,ChengYM,MaX,etal.RobustcornerprecisedetectionalgorithmforvisuallandingnavigationofUAV[J].Systems Engineering and Electronics, 2013, 35(6): 1262-1267.(郝帥, 程詠梅, 馬旭, 等. 無人機視覺著艦導航魯棒角點精確檢測算法[J].系統工程與電子技術, 2013, 35(6): 1262-1267.)

[16]LuCP,HagerGD,MjolsnessE.Fastandgloballyconvergentposeestimationfromvideoimages[J].IEEE Trans. on Pattern Analysis and Machine Intelligence, 2000, 23(2):610-622.

Selection algorithm of random feature points based on vector constraints

MA Xu1,2, CHENG Yong-mei1, HAO Shuai2

(1.College of Automation, Northwestern Polytechnical University, Xi’an 710129, China; 2. School of Electrical and Control Engineering, Xi’an University of Science and Technology, Xi’an 710054, China)

Thefeaturepointsoftheextractedimagearecharacterizedbyalargenumberandstrongrandomnesswhenunmannedaerialvehicle(UAV)landingautonomouslyatanunknownzonebyusingvision.Inordertoovercometheproblemsthatrandomlyselectingfeaturepointsforrelativepositionandangleestimationleadstolowprecisionestimationandpoorstability,aselectionalgorithmofrandomfeaturepointsbaseonvectorconstraintsisproposed.Firstly,geographiccoordinatesofthefeaturepointsareconsideredasanimportantfactorwhichaffectstheequationprecisionthroughanalyzingthepositionandattitudeestimationequation.Secondly,thevectorangleaveragedegree,themeanofvectormodulusandthemaximumvalueofvectormodulus,threekindsofconstraintfunctionsareintroduced.Andaselectionstrategyofrandomfeaturepointsbasedonvectorconstraintsisdeveloped.Finaly,theorthogonaliterativealgorithmisusedtoevaluatethepositionandattitudeestimationaccuracyfortheselectedfeaturepoints.Theexperimentalresultsshowthattheproposedalgorithmhashigheraccuracyandstrongerrobustnesscomparedtothemethodofrandomlyselectingfeaturepoints.

randomfeaturepoints;relativepositionandattitudeestimation;vectorconstraints;orthogonaliteration;unknownzone

2015-11-03；

2016-04-27；網絡優先出版日期：2016-06-19。

國家自然科學基金(61135001)；西安科技大學博士啟動金(2015QDJ007)；西安科技大學培育基金(2014015)；信息融合技術教育部重點實驗室開放基金(LIFT2015-G-1)資助課題

TP217.3

ADOI:10.3969/j.issn.1001-506X.2016.10.21

馬旭(1985-),女,博士研究生,主要研究方向為視覺導航、模式識別和圖像處理。

E-mail:maxucat@gmail.com

程詠梅(1960-),女,教授,博士,主要研究方向為信息融合、目標跟蹤、視覺導航。

E-mail:chengym@nwpu.edu.cn

郝帥(1986-),男,講師,博士,主要研究方向為視覺導航、模式識別和圖像處理。

E-mail:hsh000@163.com

網絡優先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160619.0954.002.html