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

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

0 引言

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

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

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

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

1 算法框架

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

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

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

圖1 基于視覺的旋翼無人機(jī)姿態(tài)估計(jì)算法總體框圖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ù)處理

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

Y=0.299r+0.596g+0.211b

(1)

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

r、g、b——彩色圖像中的紅色、綠色和藍(lán)色分量值

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

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

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

(3)

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

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

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

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

xij——輸入圖像

2.2 標(biāo)志提取

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

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

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

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

(1)連通域檢測

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

(2)連通域剔除

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

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

(3)連通域分類

根據(jù)H形標(biāo)志和圓環(huán)的幾何特征對更新后的候選連通域進(jìn)行分類，分為H形連通域和圓環(huán)連通域。根據(jù)H形標(biāo)志和圓環(huán)自身面積-周長比的尺度不變性和旋轉(zhuǎn)不變性，對候選連通域進(jì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)系，通過計(jì)算兩類連通域質(zhì)心間的歐氏距離，對H形候選連通域BH[j]和圓環(huán)候選連通域BCir[k]進(jìn)行匹配，進(jìn)而得到候選視覺標(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(·)——計(jì)算兩點(diǎn)間歐氏距離函數(shù)

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

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

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

如果第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)直升機(jī)停機(jī)坪所包含的圓環(huán)及其內(nèi)部的H形標(biāo)志，進(jìn)而可以得到視覺標(biāo)志的邊緣輪廓，如圖4e所示。

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

此外，圖5給出了標(biāo)志提取過程可能存在的兩種干擾情況。圖5a給出由于標(biāo)志表面雜質(zhì)或光線反射的影響，在H形和圓環(huán)連通域之間出現(xiàn)面積較大的白色斑點(diǎ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 角點(diǎn)檢測

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

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

(8)

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

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

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

圖6 角點(diǎn)檢測原理Fig.6 Corner detection principle

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

Lstep=αl

(9)

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

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

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

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

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

δCornerDet=βl

(10)

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

篩選判定條件式為

(11)

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

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

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

2.4 角點(diǎn)匹配

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

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

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

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

圖9 角點(diǎn)匹配Fig.9 Corner matching

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

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

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

圖11 角點(diǎn)匹配測試Fig.11 Corner matching test

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

(12)

其中

式中s——尺度因子

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

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

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

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

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

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

(13)

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

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

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

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

(14)

式中f——相機(jī)焦距

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

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

(15)

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

dpixel——相機(jī)的像元尺寸

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

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

(16)

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

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

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

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

(17)

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

3 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

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

3.1 實(shí)驗(yàn)平臺及參數(shù)選擇

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

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

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

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

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

3.2 標(biāo)志提取與角點(diǎn)檢測算法驗(yàn)證

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 姿態(tài)估計(jì)驗(yàn)證

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

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

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

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

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

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

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

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

3.4 時間性能測試

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

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

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

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

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

4 結(jié)束語

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