馬　旭, 程詠梅, 郝　帥

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

?

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

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

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

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

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

0　引　言

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

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

1　相對位姿估計模型

基于視覺的相對位姿估計主要是通過圖像像素坐標系、圖像物理坐標系、攝像機坐標系以及一組空間關(guān)系已知的特征點坐標系之間的轉(zhuǎn)換關(guān)系,推導(dǎo)出相機在合作目標坐標系中的位置和姿態(tài),從而進一步得到相機載體在合作目標坐標系中的位置和姿態(tài)[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的旋轉(zhuǎn)矩陣,它包含3個獨立變量記為滾轉(zhuǎn)角α、航向角γ以及俯仰角β,3個姿態(tài)角定義可文獻[15],表達式為

(2)

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

(3)

令

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

ρV=KR1M

(4)

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

(5)

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

(6)

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

2.1位姿估計方程組條件數(shù)定義

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

(7)

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

(8)

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

(9)

由式(9)可得

(10)

進一步可推出

(11)

而由方程組Ax=b可知

(12)

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

(13)

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

2.2評價指標

(14)

(15)

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

(1) 矢量角均分度?

(16)

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

(17)

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

(3) 矢量模值最大值dmax

(18)

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

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

(19)

(20)

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

2.3數(shù)值分析

假設(shè)相機的等效焦距為821,3個姿態(tài)角(α,β,γ)分別為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

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

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

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

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

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

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

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

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

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

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

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

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

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

表4　實時性比較

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

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

3　矢量約束搜索策略

3.1特征點組合生成

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

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

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

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

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

3.2矢量約束歸一化與最優(yōu)值計算

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

(1) 矢量均分度歸一化

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

(21)

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

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

(22)

(3) 優(yōu)先值計算

根據(jù)選取策略,設(shè)定第j組的優(yōu)先值為

(23)

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

3.3算法流程

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

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

4　實驗結(jié)果及分析

4.1仿真實驗驗證

假設(shè)在著降區(qū)內(nèi)有29個特征點,如圖5所示。

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

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

圖6　矢量約束結(jié)果Fig.6　Vector constraint results

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

圖7　誤差結(jié)果Fig.7　Error results

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

4.2物理實驗驗證

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

圖8　實驗中的相關(guān)設(shè)備Fig.8　Related equipment in experiments

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

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

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

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

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

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

(24)

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

表5　位置相對誤差

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

5　結(jié)　論

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

[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.(馬旭,程詠梅,郝帥,等.面向未知區(qū)域深度測量的單目序列圖像稠密點特征生成算法[J].航空學(xué)報,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.(張磊,何昕,魏仲慧,等.星圖分布對星敏感器最小二乘姿態(tài)精度的影響[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.(郝帥, 程詠梅, 馬旭, 等. 無人機視覺著艦導(dǎo)航魯棒角點精確檢測算法[J].系統(tǒng)工程與電子技術(shù), 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；網(wǎng)絡(luò)優(yōu)先出版日期：2016-06-19。

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

TP217.3

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

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

E-mail:maxucat@gmail.com

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

E-mail:chengym@nwpu.edu.cn

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

E-mail:hsh000@163.com

網(wǎng)絡(luò)優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160619.0954.002.html