基于視覺標(biāo)志檢測的旋翼無人機姿態(tài)估計方法

齊詠生 孟學(xué)斌 高學(xué)金 張麗杰

(1.內(nèi)蒙古工業(yè)大學(xué)電力學(xué)院， 呼和浩特 010080； 2.北京工業(yè)大學(xué)信息學(xué)部， 北京 100124)

0 引言

四旋翼無人機涉及到導(dǎo)航、計算機視覺、控制理論、嵌入式電子等多方面的技術(shù)，具有體積小、成本低、機動性好、可垂直起降等特點[1]，不僅被應(yīng)用于醫(yī)療救援、環(huán)境監(jiān)控、災(zāi)情監(jiān)測、遠程遙感等領(lǐng)域，而且也越來越多地應(yīng)用于農(nóng)業(yè)感知和電力檢修等特殊領(lǐng)域[2-3]。這些應(yīng)用中，無人機自主著陸是重要技術(shù)之一，要實現(xiàn)無人機的自主著陸，就要實現(xiàn)對無人機姿態(tài)的精確估計。

隨著計算機視覺技術(shù)的發(fā)展，利用機載攝像頭捕獲的圖像信息估計無人機的姿態(tài)受到國內(nèi)外學(xué)者的廣泛關(guān)注。LEE等[4]提出了一種識別國際標(biāo)準(zhǔn)直升機停機坪的視覺算法，通過外部圓環(huán)和內(nèi)部H形標(biāo)志的邊緣分布函數(shù)，可實現(xiàn)著陸標(biāo)志的識別，但并未進行無人機的姿態(tài)估計。OLIVEIRA等[5]提出兩種依靠機器學(xué)習(xí)的方法來自動檢測由傳統(tǒng)的圓環(huán)和H、X形字符組成的兩類著陸標(biāo)志。PRAKASH等[6]采用加速魯棒特征(Speeded up robust features, SURF)算法提取H形著陸標(biāo)志的特征，并根據(jù)這些特征，將預(yù)裝載的著陸標(biāo)志模板和無人機拍攝的圖像進行匹配，該方法同樣只進行了標(biāo)志檢測與匹配。SARIPALLI等[7-9]提出了一種基于幾何特征的形狀識別方法來檢測H形視覺標(biāo)志，并實現(xiàn)了無人機姿態(tài)的估計。該方法只給出了無人機相對于標(biāo)志的x、y坐標(biāo)和方向，并未得到6自由度的姿態(tài)。SANCHEZ-LOPEZ等[10-11]也提出了類似的方法，將主成分分析法(Principal component analysis, PCA)應(yīng)用于圖像中每一個連通域的前7個Hu不變矩，并將PCA處理獲得的信息作為人工神經(jīng)網(wǎng)絡(luò)的輸入，利用得到的H形標(biāo)志角點計算無人機的姿態(tài)，然而神經(jīng)網(wǎng)絡(luò)的應(yīng)用增加了算法的復(fù)雜度和計算量，不利于嵌入式系統(tǒng)移植。SILVA等[12]提出了一種全局搜索方法，能夠在視野中找出可能包含著陸標(biāo)志的最佳地點，無人機對每一個觀測點進行基于視覺的局部標(biāo)志搜索。LIN等[13]提出基于單目視覺的實時目標(biāo)識別與跟蹤方法，對直升機停機坪的輪廓進行特征點匹配和聚類，能夠在雜亂的環(huán)境中識別出著陸標(biāo)志，并得到4自由度的姿態(tài)信息。PREMACHANDRA等[14]提出了另一種檢測無人機著陸標(biāo)志的方法，通過樹莓派處理由攝像頭捕獲的圖像數(shù)據(jù)和由超聲波傳感器獲得的無人機飛行高度數(shù)據(jù)，并利用Hough變換識別H形標(biāo)志。SUDEVAN等[15]采用SURF算法提取H形標(biāo)志的特征點，并采用基于快速近似最鄰近(Fast library for approximate nearest neighbors, FLANN)的描述符匹配方法將捕獲的圖像與模板圖像進行匹配，從而計算出無人機與H形標(biāo)志中心的位置偏差。

基于視覺的無人機姿態(tài)估計仍然是當(dāng)前的研究熱點問題，文獻[10-11]雖可以較精確地估計出無人機的6自由度姿態(tài)數(shù)據(jù)，但算法中應(yīng)用了計算量較大的多層感知器(Multi-layer perceptron, MLP)，對控制器的實時處理能力要求較高；文獻[12，14]需要GPS、慣性測量單元(Inertial measurement unit, IMU)、超聲波等輔助傳感器，增大了算法的復(fù)雜性和成本。

為此，本文提出一種僅由單目視覺實現(xiàn)無人機姿態(tài)估計的方法，旨在保證準(zhǔn)確性的前提下滿足無人機控制器的快速性和實時性要求。充分利用圖像幾何特征易于快速提取的優(yōu)勢，采用五步標(biāo)志提取方法獲取視覺標(biāo)志，提出一種基于距離三點法的角點檢測算法得到視覺標(biāo)志的角點，實現(xiàn)視覺標(biāo)志的快速提取。并對算法的有效性和時間性能進行驗證。

1 算法框架

本文提出的基于視覺標(biāo)志檢測的旋翼無人機姿態(tài)估計算法的總體框圖如圖1所示。首先對由機載攝像頭捕獲到的包含視覺標(biāo)志在內(nèi)的圖像進行預(yù)處理，消除圖像噪聲并突出視覺標(biāo)志；然后根據(jù)視覺標(biāo)志的幾何特征，通過五步法提取出H形標(biāo)志和圓環(huán)組成的視覺標(biāo)志；采用距離三點法檢測H形標(biāo)志的12個角點并與預(yù)設(shè)參考圖像中的對應(yīng)角點進行匹配；最后，根據(jù)匹配的角點對計算單應(yīng)矩陣，采用直接線性變換(DLT)方法分解單應(yīng)矩陣，解算出無人機相對視覺標(biāo)志的姿態(tài)角，并根據(jù)相機成像構(gòu)成的相似三角形原理計算出無人機相對視覺標(biāo)志的位置數(shù)據(jù)。

2 無人機視覺姿態(tài)估計算法

無人機姿態(tài)估計算法的目的是通過無人機采集到包含視覺標(biāo)志的圖像，快速獲得無人機相對于視覺標(biāo)志的姿態(tài)?？紤]到算法的可比較性和通用性，本文算法仍采用由圓環(huán)和H形標(biāo)志組成的標(biāo)準(zhǔn)直升機停機坪作為視覺標(biāo)志，如圖2所示。

圖1 基于視覺的旋翼無人機姿態(tài)估計算法總體框圖Fig.1 Overall framework of pose estimation algorithm of rotor UAV based on vision

圖2 視覺識別標(biāo)志Fig.2 Visual identification mark

2.1 圖像預(yù)處理

將機載攝像頭采集到的彩色圖像轉(zhuǎn)換為灰度圖像，以消除算法對圖像色彩的依賴[16]，減少計算量并消除色調(diào)和飽和度信息而只保留亮度信息，提高標(biāo)志提取的準(zhǔn)確度，轉(zhuǎn)換過程為

Y=0.299r+0.596g+0.211b

(1)

式中Y——轉(zhuǎn)換后的灰度圖像

r、g、b——彩色圖像中的紅色、綠色和藍色分量值

轉(zhuǎn)換結(jié)果如圖3a所示。

考慮到無人機的振動或外界天氣條件的影響，由機載攝像頭采集到的圖像可能會變模糊[6]，因此，采用維納濾波器[17]對灰度圖進行去模糊處理，消除圖像模糊的對比結(jié)果如圖3b所示，維納濾波器表達式為

(2)

其中

|H(u,v)|2=H*(u,v)H(u,v)

H(u,v)——退化函數(shù)

H*(u,v)——退化函數(shù)的復(fù)數(shù)共軛

Sη(u,v)——噪聲功率譜

Sf(u,v)——信號功率譜

G(u,v)——噪聲圖像的傅里葉變換函數(shù)

圖3 圖像預(yù)處理結(jié)果Fig.3 Image preprocessing results

由于視覺標(biāo)志中背景和標(biāo)志具有較大的亮度差，為了明顯區(qū)分高亮度的視覺標(biāo)志和低亮度的背景，對維納濾波后的圖像進行自適應(yīng)閾值化處理[18]，得到黑色背景和白色標(biāo)志，如圖3c所示。從圖3c中可看到，在對圖像進行閾值化處理的過程中，會引入脈沖噪聲和椒鹽噪聲?？紤]到標(biāo)準(zhǔn)中值濾波對所有像素采用統(tǒng)一的處理方法，在濾除噪聲的同時，也可能改變真正信號點的值，造成圖像失真。因此，利用極值中值濾波器(EM)[19]對閾值化結(jié)果做進一步的處理，由于EM濾波器只對噪聲點進行中值處理而對信號點保留原像素值，可以有效消除圖像的脈沖噪聲和椒鹽噪聲，同時保留圖像的邊緣信息，濾波結(jié)果如圖3d所示，EM原理式為

(3)

式中yij——輸出圖像(i、j表示各像素點的位置)

med(W[xij])——對窗口W[xij]內(nèi)的所有點取中值函數(shù)

min(W[xij])——對窗口W[xij]內(nèi)的所有點取最小值函數(shù)

max(W[xij])——對窗口W[xij]內(nèi)的所有點取最大值函數(shù)

xij——輸入圖像

2.2 標(biāo)志提取

經(jīng)圖像預(yù)處理得到二值圖像后，就可以對圖像中的視覺標(biāo)志進行提取。本文提出的標(biāo)志提取方法分為5個步驟，包括連通域檢測、連通域剔除、連通域分類、連通域匹配與視覺標(biāo)志判定，稱之為五步法。其核心思想就是利用視覺標(biāo)志中圓環(huán)和H形標(biāo)志固有的幾何特征，對得到的二值圖像進行快速標(biāo)志提取。

通過檢測圖像中的所有連通域，得到每一個連通域的質(zhì)心坐標(biāo)、面積以及周長等特征。由于標(biāo)志所處環(huán)境的光照、陰影等因素的影響，得到的連通域中包含一些小面積區(qū)域，因此，為了減少候選連通域的數(shù)量，首先刪除候選連通域中的小面積區(qū)域。然后，分別根據(jù)圓環(huán)和H形標(biāo)志自身的幾何特征對剩余連通域進行篩選和分類，得到圓環(huán)和H形候選連通域。

圖4 標(biāo)志提取結(jié)果Fig.4 Mark extraction results

最后，根據(jù)H形連通域位于圓環(huán)連通域內(nèi)部并且其質(zhì)心位置重合的條件，通過判斷圓環(huán)連通域與H形連通域的質(zhì)心間的歐氏距離，分別對兩類候選連通域進行篩選。進一步，根據(jù)H形與圓環(huán)連通域的面積比和周長比，對剩余連通域進行判定，完成視覺標(biāo)志提取。具體提取步驟如下：

(1)連通域檢測

對二值圖像進行連通域檢測，得到包含視覺標(biāo)志的所有連通域B[i](i=1,2,…,n，n為連通域數(shù)量)及其幾何特征，如圖4a所示?？梢钥吹?，共檢測到9個候選連通域，分別以不同顏色表示，黑色表示背景，候選連通域中有許多錯誤的連通域，其中包含一些面積很小的區(qū)域，即像素很少的連通域。

(2)連通域剔除

此步驟主要刪除上述連通域檢測過程中得到的一些小面積區(qū)域。

如果第i個連通域的面積B[i]Area小于閾值TArea，那么從候選連通域刪除該連通域，更新候選連通域，如圖4b所示?？梢钥吹?，經(jīng)連通域剔除后，面積較小的連通域5、6、8被刪除，減少了候選連通域的數(shù)量。

(3)連通域分類

根據(jù)H形標(biāo)志和圓環(huán)的幾何特征對更新后的候選連通域進行分類，分為H形連通域和圓環(huán)連通域。根據(jù)H形標(biāo)志和圓環(huán)自身面積-周長比的尺度不變性和旋轉(zhuǎn)不變性，對候選連通域進行分類。如果第i個連通域的面積-周長比與H形連通域的面積-周長比的期望值相差很小，即滿足

(4)

式中B[i]Perimeter——第i個連通域的周長

εA-P——連通域面積-周長比閾值

則將該連通域標(biāo)記為H形候選連通域BH[j](j=1,2,…,m1，m1為H形候選連通域的數(shù)量)。

如果第i個連通域的面積-周長比與圓環(huán)連通域的面積-周長比的期望值相差很小，即滿足

(5)

則將該連通域標(biāo)記為圓環(huán)候選連通域BCir[k](k=1,2,…,m2，m2為圓環(huán)候選連通域的數(shù)量)；否則，直接從候選連通域中刪除。

連通域分類結(jié)果如圖4c所示，可以看出，H形候選連通域有2個，對應(yīng)連通域3、9，圓環(huán)候選連通域有1個，對應(yīng)連通域2。

(4)連通域匹配

根據(jù)H形連通域與圓環(huán)連通域質(zhì)心位置重合的關(guān)系，通過計算兩類連通域質(zhì)心間的歐氏距離，對H形候選連通域BH[j]和圓環(huán)候選連通域BCir[k]進行匹配，進而得到候選視覺標(biāo)志VMarks[i](i=1,2, …,N，N為候選視覺標(biāo)志的數(shù)量)，即H形連通域與圓環(huán)連通域的對應(yīng)表。

如果第i個H形連通域與第j個圓環(huán)連通域質(zhì)心間的歐氏距離小于設(shè)定閾值εDis，則將對應(yīng)的H形連通域和圓環(huán)連通域標(biāo)記為候選視覺標(biāo)志VMarks[i]，即更新對應(yīng)表，判定條件式為

EucliDist(BH[i]Centroid,BCir[j]Centroid)≤εDis

(6)

式中BH[i]Centroid——第i個H形連通域的質(zhì)心坐標(biāo)

BCir[j]Centroid——第j個圓環(huán)連通域的質(zhì)心坐標(biāo)

EucliDist(·)——計算兩點間歐氏距離函數(shù)

連通域匹配結(jié)果如圖4d所示，可以看出，實驗圖像中只得到一個匹配的視覺標(biāo)志，即期望的H形標(biāo)志和圓環(huán)。但是，如果采集到的圖像中存在干擾信息，將得到不止一個視覺標(biāo)志，因此，需對得到的視覺標(biāo)志做進一步的判定。

(5)視覺標(biāo)志判定

利用H形連通域和圓環(huán)連通域的面積比和周長比，對連通域匹配得到的候選視覺標(biāo)志做進一步的判定和確認。

如果第i個視覺標(biāo)志中對應(yīng)的H形連通域與圓環(huán)連通域的面積比和周長比與期望的面積比和周長比相差很小，則標(biāo)記為最終的視覺標(biāo)志；否則，將該視覺標(biāo)志從候選視覺標(biāo)志中刪除，判定條件式為

(7)

式中VMarks[i]HArea——第i個視覺標(biāo)志中H形連通域的面積

VMarks[i]CirArea——第i個視覺標(biāo)志中圓環(huán)連通域的面積

VMarks[i]HPerimeter——第i個視覺標(biāo)志中H形連通域的周長

VMarks[i]CirPerimeter——第i個視覺標(biāo)志中圓環(huán)連通域的周長

εArea——面積比閾值

εPerimeter——周長比閾值

經(jīng)過上述連通域檢測和篩選，可以提取出唯一期望的視覺標(biāo)志，即標(biāo)準(zhǔn)直升機停機坪所包含的圓環(huán)及其內(nèi)部的H形標(biāo)志，進而可以得到視覺標(biāo)志的邊緣輪廓，如圖4e所示。

在角點檢測、角點匹配及無人機姿態(tài)估計過程中，只需要H形標(biāo)志的12個角點信息，而不需要圓環(huán)的相關(guān)信息。因此，將主要提取H形連通域的輪廓，如圖4f所示。將H形連通域的輪廓坐標(biāo)順序排列在一個列向量中，作為角點檢測的輸入。

此外，圖5給出了標(biāo)志提取過程可能存在的兩種干擾情況。圖5a給出由于標(biāo)志表面雜質(zhì)或光線反射的影響，在H形和圓環(huán)連通域之間出現(xiàn)面積較大的白色斑點，本文提出方法仍可準(zhǔn)確地完成兩類標(biāo)志的分類；圖5b給出在連通域匹配過程中得到兩個候選視覺標(biāo)志，通過視覺標(biāo)志判定最終得到了期望的視覺標(biāo)志。可見本文算法具有一定的魯棒性。

圖5 干擾情況下的標(biāo)志提取結(jié)果Fig.5 Mark extraction results under disturbed condition

2.3 角點檢測

角點檢測的目標(biāo)是找到H形標(biāo)志的12個角點位置，目前存在許多角點檢測算法，主要分為基于灰度圖像的角點檢測、基于二值圖像的角點檢測和基于輪廓曲線的角點檢測[20-21]。但上述方法都會出現(xiàn)錯檢或漏檢的情況，并且計算量較大，很難滿足無人機姿態(tài)估計的實時性要求。因此，本文提出一種基于距離三點法的幾何方法來檢測角點，可有效提升運算速度。

基于距離三點法的角點檢測方法根據(jù)上述標(biāo)志提取得到的H形連通域輪廓，對目標(biāo)輪廓的每一個像素點進行檢測。假設(shè)點S是一個待檢測的像素種子點，根據(jù)一定的步長Lstep沿H形輪廓在種子點S的左右兩側(cè)分別取P1、P2兩個相鄰點，并得到通過點P1、P2的直線方程，從而可以計算種子點S到直線的距離dS，如圖6所示。從圖中可以看到，一個理想的角點對應(yīng)一個較大的距離，而距離越小表示種子點與相鄰點越接近一條直線，當(dāng)距離為0時，三點共線。利用這一關(guān)系，通過判斷種子點S對應(yīng)的距離dS，就可以檢測到所有可能的候選角點，計算如下

(8)

式中f(S)——種子點的檢測結(jié)果

TDis——根據(jù)步長Lstep得到的距離閾值

f(S)=1表示S為可能的候選角點，f(S)=0表示S為非角點。

圖6 角點檢測原理Fig.6 Corner detection principle

在角點檢測過程中，種子點S沿H形輪廓遍歷每一個像素點，步長Lstep根據(jù)H形輪廓的邊長值確定，邊長越長，步長越大，邊長越小，步長越小，關(guān)系式為

Lstep=αl

(9)

式中α——小于1的步長因子

l——H形輪廓最短邊的長度

這樣，由于無人機高度不同而引起的H形輪廓邊長的變化不會影響角點檢測的準(zhǔn)確性，使得角點檢測算法的魯棒性更好。

為了排除上述步驟得到的H形輪廓所有候選角點中的離群點，即假角點，利用H形標(biāo)志的尺寸關(guān)系對所有候選角點進行篩選。具體步驟如下：

(2)計算所有候選角點到H形標(biāo)志質(zhì)心的歐氏距離，并與3個期望的距離進行比較，如果與3個期望距離的任意一個的差值小于閾值δCornerDet，則標(biāo)記為候選角點，否則認為是離群點，從候選角點中刪除，其中δCornerDet根據(jù)H形輪廓的最短邊計算得到，公式為

δCornerDet=βl

(10)

式中β——小于1的比例因子

篩選判定條件式為

(11)

圖7 H形標(biāo)志的尺寸關(guān)系Fig.7 Dimension of H shaped mark

圖8 角點檢測結(jié)果Fig.8 Corner detection results

經(jīng)過上述角點檢測，可以得到12個角點群，如圖8a所示。通過計算一個小鄰域內(nèi)角點群的質(zhì)心，可以得到12個角點群的質(zhì)心，將其作為最終檢測到的H形標(biāo)志角點位置，如圖8b所示。

2.4 角點匹配

角點匹配是無人機姿態(tài)估計的另一個關(guān)鍵環(huán)節(jié)，本文算法將當(dāng)前圖像中的角點與參考圖像中的角點進行匹配，從而準(zhǔn)確估計出無人機的姿態(tài)信息。角點匹配步驟如下：

(1) 根據(jù)2.3節(jié)角點檢測得到的H形標(biāo)志12個角點，首先確定H形標(biāo)志的主方向。由于H形標(biāo)志的輪廓坐標(biāo)按順序排列進行存儲，因此，由角點檢測方法得到的12個角點同樣沿H形輪廓順序存儲。通過順序計算相鄰兩個角點的距離，可以得到角點間的12個距離，即H形標(biāo)志的12條邊長，如圖9a所示。

(2)比較得到的12個距離，取最大的距離所對應(yīng)的兩個角點所在的直線斜率作為H形標(biāo)志的主方向，即H形標(biāo)志的垂直方向，如圖9b、9c中紅色線段所示。與主方向垂直的方向定義為H形標(biāo)志的水平方向，如圖9c中綠色線段所示。這樣，H形標(biāo)志被劃分為4個象限，分別為第Ⅰ象限、第Ⅱ象限、第Ⅲ象限和第Ⅳ象限。

(3)根據(jù)角點到質(zhì)心歐氏距離的3種情況，將每個象限中的3個角點分類并進行編號，分為外點、中點和內(nèi)點，如圖9c所示。這樣，就可以與參考圖像中定義的角點完成匹配，如圖10所示，從而得到12個匹配的角點對，將其作為無人機姿態(tài)估計的輸入?yún)?shù)。

圖9 角點匹配Fig.9 Corner matching

圖10 當(dāng)前圖像與參考圖像角點匹配結(jié)果(左邊為 參考圖像，右邊為當(dāng)前圖像)Fig.10 Corner matching results between current image and reference image (Left picture was reference, and right one was current image)

根據(jù)上述角點匹配算法，分別對不同位移和旋轉(zhuǎn)情況下所得到的圖像與參考圖像進行角點匹配測試，匹配結(jié)果如圖11所示。從圖中可以看出，該算法對無人機的平移(圖11a)、旋轉(zhuǎn)(圖11b)甚至無人機產(chǎn)生劇烈傾斜的極端情況(圖11c)都能實現(xiàn)精確的匹配。

2.5 無人機姿態(tài)估計

無人機的姿態(tài)估計，即根據(jù)上述得到的12個匹配的角點對，估計出無人機相對于視覺標(biāo)志的三維姿態(tài)。為了消除攝像頭畸變對無人機姿態(tài)估計精度的影響，首先對攝像頭進行標(biāo)定，獲得攝像頭的內(nèi)參矩陣。

圖11 角點匹配測試Fig.11 Corner matching test

二維像素坐標(biāo)與三維世界坐標(biāo)的對應(yīng)關(guān)系式為

(12)

其中

式中s——尺度因子

[uv1]T——像素坐標(biāo)系中的齊次坐標(biāo)

K——相機內(nèi)部參數(shù)矩陣

[R|t]——相機外部參數(shù)矩陣

R——相機旋轉(zhuǎn)矩陣t——相機平移向量

P——世界坐標(biāo)系中的齊次坐標(biāo)

其次，利用匹配的12個角點對，根據(jù)直接線性變換(DLT)可以計算出相應(yīng)的單應(yīng)矩陣[22]，利用非奇異值分解(Singular value decomposition, SVD)對單應(yīng)矩陣進行分解[23]，可以得到旋轉(zhuǎn)矩陣R和平移向量t，從而根據(jù)旋轉(zhuǎn)矩陣解算出無人機的姿態(tài)角

(13)

式中θx——無人機繞x軸旋轉(zhuǎn)角，即橫滾角

θy——無人機繞y軸旋轉(zhuǎn)角，即俯仰角

θz——無人機繞z軸旋轉(zhuǎn)角，即偏航角

在位置估計中，由于單目相機在三維重建過程中存在尺度不確定的問題，本文利用針孔相機成像原理，通過三角形相似關(guān)系對當(dāng)前圖像中H形標(biāo)志質(zhì)心的深度進行估計。相機成像原理如圖12所示，空間中任意一點在相機坐標(biāo)系下的坐標(biāo)M(X,Y,Z)與成像后圖像中對應(yīng)坐標(biāo)m(x,y)的關(guān)系可根據(jù)三角形相似性得出，即

(14)

式中f——相機焦距

圖12 針孔相機成像原理Fig.12 Imaging principle of pinhole camera

本文分別通過面積和周長對標(biāo)志質(zhì)心深度進行估計，取其平均值作為最終的標(biāo)志質(zhì)心深度值，并由此計算出H形標(biāo)志質(zhì)心相對于相機光心的x坐標(biāo)和y坐標(biāo)，實驗表明，該方法可以較精確地估計出標(biāo)志質(zhì)心的深度。

(15)

式中Spixel——H形標(biāo)志的像素面積

dpixel——相機的像元尺寸

Sactual——H形標(biāo)志的真實面積

DArea——由面積相似求得的標(biāo)志質(zhì)心深度

(16)

式中Ppixel——H形標(biāo)志的像素周長

Pactual——H形標(biāo)志的真實周長

DPerimeter——由周長相似求得的標(biāo)志質(zhì)心深度

最終得到H形標(biāo)志質(zhì)心相對于相機光心的坐標(biāo)(xH,yH,zH)為

(17)

式中xerror、yerror——圖像中H形標(biāo)志質(zhì)心與圖像中心在x方向和y方向的像素偏差

3 實驗驗證與結(jié)果分析

為驗證本文所提算法的有效性和準(zhǔn)確性，并考慮到直接通過無人機采集圖像難以準(zhǔn)確獲得圖像對應(yīng)的無人機姿態(tài)信息，不利于算法的驗證，因此，在實驗室搭建了視覺實驗平臺來模擬無人機的運動并獲取圖像數(shù)據(jù)，將圖像輸入本文算法，通過比較算法輸出的估計值和測量真實值之間的誤差，對算法有效性和準(zhǔn)確性進行驗證，并測算所提出算法的時間性能。

3.1 實驗平臺及參數(shù)選擇

搭建的視覺實驗平臺如圖13所示，分別由沿x、y、z方向移動的平移單元和繞x、y、z軸旋轉(zhuǎn)的旋轉(zhuǎn)單元組成，以模擬無人機飛行過程中的6個自由度。

圖13 視覺實驗平臺Fig.13 Visual experiment platform1.視覺標(biāo)志 2.控制和驅(qū)動單元 3.攝像頭

通過3組電動絲杠模擬無人機沿x、y、z方向的平移運動，其中y方向移動單元安裝在x方向移動單元上，如圖13a所示。3個旋轉(zhuǎn)臺用于模擬無人機繞x、y、z軸的旋轉(zhuǎn)運動，如圖13b所示。其中橫滾和俯仰旋轉(zhuǎn)臺安裝在y方向移動單元上，在俯仰旋轉(zhuǎn)臺上方固定有視覺標(biāo)志，偏航旋轉(zhuǎn)臺通過一個連桿安裝在z方向移動單元上，偏航旋轉(zhuǎn)臺下方安裝垂直向下的攝像頭，用于獲取包含視覺標(biāo)志的圖像。通過控制和驅(qū)動單元調(diào)節(jié)平移運動單元和旋轉(zhuǎn)運動單元，就可以模擬無人機相對于視覺標(biāo)志的位置和姿態(tài)角，并通過計算機采集攝像頭捕獲的圖像數(shù)據(jù)。

本文所使用的攝像頭是一臺分辨率為640像素×480像素的USB接口單目相機，視覺標(biāo)志檢測及姿態(tài)估計算法運行在計算機上。本文所提出算法中的具體參數(shù)選擇如表1所示。

表1 算法參數(shù)Tab.1 Algorithm parameters

3.2 標(biāo)志提取與角點檢測算法驗證

為了驗證本文標(biāo)志提取及角點檢測算法的有效性，分別采用視覺實驗平臺和四旋翼無人機共拍攝800幅包含視覺標(biāo)志的測試圖像，并對其進行標(biāo)志提取和角點檢測，通過評價檢測結(jié)果的準(zhǔn)確率，對標(biāo)志提取和角點檢測算法的準(zhǔn)確性和有效性進行驗證，其中，視覺平臺所拍攝的圖像包含了攝像頭與標(biāo)志間0.5～1.5 m的距離范圍，并且加入了其余5個自由度的運動，無人機所拍攝的圖像包含了1.5～7.5 m的距離范圍。實驗中所用的四旋翼無人機如圖14所示，在無人機下方中心位置垂直向下安裝單目相機，并搭載了圖像處理單元、飛控模塊、慣性測量單元IMU和GPS模塊。其中，單目相機用于采集圖像，圖像處理單元搭載本文視覺姿態(tài)估計算法用于進行圖像處理并給出無人機的控制命令，飛控模塊、慣性測量單元和GPS模塊用于根據(jù)控制命令調(diào)節(jié)無人機的位置。

圖14 四旋翼無人機Fig.14 Quadrotor UAV platform1.飛控模塊和IMU 2.GPS 3.圖像處理單元 4.單目相機

圖15 視覺實驗平臺上標(biāo)志提取與角點檢測效果Fig.15 Results of mark extraction and corner detection in visual experiment platform

在測試過程中，視覺實驗平臺所拍圖像的預(yù)處理過程將維納濾波器關(guān)閉，而無人機所拍圖像的預(yù)處理過程將維納濾波器打開，因為無人機在飛行過程中可能會發(fā)生抖動。

視覺實驗平臺拍攝的部分圖像及其檢測結(jié)果如圖15所示，四旋翼無人機拍攝的部分圖像及其檢測結(jié)果如圖16所示，圖中從左到右分別為原始圖像、圖像預(yù)處理結(jié)果、標(biāo)志提取結(jié)果和角點檢測結(jié)果。

從圖15中可以看到，當(dāng)攝像頭與標(biāo)志間的距離在0.5～1.5 m范圍內(nèi)時，本文算法可以準(zhǔn)確提取出圖像中的視覺標(biāo)志并得到H形標(biāo)志的12個角點。

圖16 室外環(huán)境下標(biāo)志提取與角點檢測效果Fig.16 Results of mark extraction and corner detection in outdoor

從圖16中可以看到，當(dāng)無人機高度不大于5.0 m時，可以準(zhǔn)確提取出視覺標(biāo)志并完成角點檢測，但隨著無人機飛行高度的繼續(xù)上升，圖像中的視覺標(biāo)志變小，標(biāo)志提取和角點檢測會出現(xiàn)錯誤。當(dāng)高度大于5.0 m時，角點檢測出現(xiàn)漏檢現(xiàn)象，如圖16b所示，當(dāng)高度大于7.0 m時，已經(jīng)無法完整、準(zhǔn)確地提取出視覺標(biāo)志，如圖16a所示。無人機飛行高度在0～5.0 m時，本文算法可以精確地提取出視覺標(biāo)志并得到12個角點位置，可以滿足無人機自主降落的要求。

此外，由于天氣原因，無人機會發(fā)生小幅度的抖動，所拍攝的圖像受到一定的影響，如圖16e所示。如果關(guān)閉維納濾波器，會出現(xiàn)標(biāo)志提取錯誤的情況，實際上，丟失了重要標(biāo)志H形字符，如圖16e第3個圖像中左下角紅色方框內(nèi)的圖像；而經(jīng)過維納濾波、極值中值濾波等圖像預(yù)處理后，可以得到較好的二值圖像，并可以提取出正確的視覺標(biāo)志，完成角點檢測。

由于目前的研究還沒有完全實現(xiàn)視覺算法與飛控的結(jié)合，并正在進一步研究，以上實驗過程只通過單目相機和圖像處理單元采集無人機航拍圖像，然后由計算機進行仿真實驗。

為了進一步驗證本文所提算法的有效性和可行性，將部分視覺算法移植到樹莓派，并在脫機狀態(tài)下對采集的圖像進行處理，樹莓派處理單元運行效果如圖17所示。

圖17 樹莓派運行效果Fig.17 Raspberry Pi running effect1.液晶顯示屏 2.樹莓派處理單元

未來進一步的研究將重點完成視覺算法的移植及其與飛控的結(jié)合，并在四旋翼無人機上對算法進行更進一步的實驗驗證。視覺算法與飛控的結(jié)合框圖如圖18所示。

從圖18中可以看出，視覺算法與飛控模塊獨立運行。視覺算法對采集到的圖像進行標(biāo)志提取、角點檢測以及姿態(tài)估計等處理，從而計算出無人機與標(biāo)志的相對姿態(tài)并給出無人機姿態(tài)調(diào)整指令；飛控模塊接收到視覺算法的控制指令并進行解析，得到無人機姿態(tài)調(diào)整的具體參數(shù)，控制無人機完成姿態(tài)調(diào)整并下降一定高度，之后進入旋停狀態(tài)，等待下一次控制指令的輸入，經(jīng)過多次圖像處理與姿態(tài)調(diào)整，最終完成無人機的自主著陸。

圖18 視覺算法與飛控結(jié)合框圖Fig.18 Block diagram of combination of visual algorithms and flight control

3.3 姿態(tài)估計驗證

為驗證本文提出的無人機姿態(tài)估計算法的有效性，分別對無人機相對于標(biāo)志的姿態(tài)角估計和位置估計進行了實驗驗證。

首先，對每一個旋轉(zhuǎn)單元以4°的步長進行旋轉(zhuǎn)，鎖定其他平移運動單元和旋轉(zhuǎn)單元以獲得不同姿態(tài)角下的圖像數(shù)據(jù)，將其輸入提出的算法，分別對視覺標(biāo)志的橫滾角、俯仰角和偏航角進行估計，并計算估計值與真實值之間的絕對誤差和相對誤差，實驗結(jié)果如表2所示。

從表2可以看出，橫滾角和俯仰角的最大絕對誤差在0.5°左右，分別為0.504 7°和0.588 5°，最大相對誤差為2.76%和2.50%，而偏航角的最大絕對誤差為0.207 7°，最大相對誤差為1.44%，小于橫滾角和俯仰角的誤差。分析可能是由于在角點檢測過程中，將角點群的質(zhì)心作為H形標(biāo)志的最終角點位置會引入一定的誤差，在視覺標(biāo)志發(fā)生傾斜時這一誤差大于視覺標(biāo)志旋轉(zhuǎn)時的誤差，因此，造成了橫滾角和俯仰角估計的誤差大于偏航角估計的誤差。

實驗中得到較小的均方根誤差可以表明本文算法對姿態(tài)角估計的準(zhǔn)確性和有效性。從表2中可以得出，本文算法對姿態(tài)角估計的平均均方根誤差為0.223°，明顯優(yōu)于LIN等[13]提出的方法，其平均均方根誤差為1.786°。

表2 姿態(tài)角估計結(jié)果Tab.2 Attitude angle estimation result

其次，用相同的實驗方法對算法的位置估計進行驗證。分別將x、y、z平移單元以0.05 m的步長移動，對視覺標(biāo)志與攝像頭的相對位置進行估計，實驗結(jié)果如表3所示。

表3 位置估計結(jié)果Tab.3 Position estimation result

從表3可以看出，在視覺標(biāo)志與攝像頭成像平面平行的情況下，位置估計的精度較高，x、y、z方向的最大絕對誤差分別為0.003 3、0.003 7、0.015 9 m，最大相對誤差分別為1.40%、1.40%、1.06%。可以看到，x、y方向的最大相對誤差和均方根誤差基本相同，因為z方向(即視覺標(biāo)志與攝像頭之間的距離)估計的誤差會由位置估計算法等量地引入x方向和y方向。LIN等[13]提出的方法中，x、y、z方向位置估計的平均均方根誤差分別為0.032、0.025、0.030 m，而本文算法在x、y、z方向的最大均方根誤差為0.006 2 m，因此，本文算法具有更精確的位置估計。

3.4 時間性能測試

對本文提出算法的時間性能進行測試。為了與其他文獻中的方法進行比較，分別在2種不同測試平臺上運行本文所提算法，測試平臺1是臺式計算機(PC1)，硬件配置為Intel Core i3處理器，主頻2.40 GHz，4.0 GB內(nèi)存，安裝64位Windows 7操作系統(tǒng)，軟件環(huán)境為Matlab R2015b(配置與文獻[5]算法的運行環(huán)境相同)；測試平臺2是一臺便攜式計算機(PC2)，硬件配置為Intel Core i7處理器，主頻2.40 GHz，4.0 GB內(nèi)存，安裝64位Ubuntu 16.04 LTS操作系統(tǒng)，軟件環(huán)境為Matlab R2016b(配置與文獻[6]算法的運行環(huán)境基本相同)。

從表4中可以看出，本文算法在PC2上的時間性能更好，平均標(biāo)志檢測時間為89.7 ms，平均姿態(tài)估計時間為217.5 ms，整個基于視覺的無人機姿態(tài)估計算法運行時間為307.2 ms，優(yōu)于PC1上的性能。這是由于PC2擁有更好的配置，因此，算法在PC1上的時間性能低于PC2。

此外，本文算法在視覺標(biāo)志檢測過程中所需的時間略高于LEE等[4]提出的方法，但LEE等算法的運行環(huán)境為專用的板載處理器，并且其所有算法均進行了優(yōu)化，可以得到較好的運行效果。相反，本文算法的運行時間明顯小于文獻[5-6]提出的方法。

可以看出，在相同運行環(huán)境的情況下，本文算法分別比文獻[5-6]的算法節(jié)省了117.1 ms和194.6 ms，因此，本文算法具有更好的時間性能，可以滿足無人機自主降落的實時性要求。

表4 算法時間性能Tab.4 Time performance of algorithm ms

4 結(jié)束語

提出了一種基于單目視覺的無人機姿態(tài)估計及自主著陸算法。利用視覺標(biāo)志的幾何特征，采用標(biāo)志五步提取法實現(xiàn)H形標(biāo)志提??；提出一種基于距離三點法的角點檢測方法，得到H形標(biāo)志的12個角點，并與參考圖像對應(yīng)角點匹配；由單應(yīng)矩陣實現(xiàn)姿態(tài)角的估計，根據(jù)視覺標(biāo)志質(zhì)心與圖像中心的位置關(guān)系及相機成像原理，實現(xiàn)視覺標(biāo)志的位置估計。搭建驗證算法有效性和準(zhǔn)確性的視覺實驗平臺，對本文算法進行了實驗驗證。實驗結(jié)果顯示，本文算法對視覺標(biāo)志姿態(tài)角和位置估計具有較高的精度，其相對誤差分別低于3%和2%。本文算法對視覺標(biāo)志的平均檢測時間為89.7 ms，姿態(tài)估計平均用時為217.5 ms。雖然標(biāo)志檢測時間略高于文獻[4]方法，但相比其他文獻明顯縮短，綜合性能指標(biāo)更優(yōu)，具有較好的實用性。