基于圓形塊匹配的無人機航向角解算算法*

李天松， 張浩強， 楊溢凡， 連 濤

(桂林電子科技大學(xué) 信息與通信學(xué)院, 廣西 桂林 541000)

0 引 言

無人機(unmanned aerial vehicle,UAV)由于其成本低、功耗低、靈活性高等優(yōu)點已經(jīng)越來越多地應(yīng)用于生產(chǎn)生活當(dāng)中。由于捷聯(lián)式慣性導(dǎo)航技術(shù)的快速發(fā)展與微機電系統(tǒng)(MEMS)慣性器件的日趨成熟，無人機的實時姿態(tài)解算與控制問題已經(jīng)成功解決。但是，使用傳統(tǒng)的六軸傳感器，只能將加速度計與陀螺儀所得數(shù)據(jù)進(jìn)行卡爾曼濾波[1]或者互補濾波[2]來修正橫滾角與俯仰角，無法對無人機的航向角漂移進(jìn)行補償修正，航向角精度會隨著時間的增長而不斷降低[3]。目前，比較常用的航向角檢測方法是使用地磁檢測傳感器，這種方式有效解決了航向角漂移問題，但地球磁場是極其微弱的，無人機的地磁傳感器又工作在復(fù)雜電流、電機旋轉(zhuǎn)等因素引起的電磁場復(fù)雜的環(huán)境中，極易對地磁傳感器的檢測造成干擾。目前，視覺運動感知系統(tǒng)，即光流傳感器已經(jīng)成功應(yīng)用于無人機的懸停定點[4]。光流是由相機和場景相對運動產(chǎn)生的，光流場是指圖像灰度模式的表面運動。Horn和Schunck在灰度圖像分析中引入了二維速度場的概念[5]，并建立了光流基本約束方程，為早期的光流計算發(fā)展起到了奠基性作用。由Lucas與Kanade發(fā)明的Lucas-Kanade算法[6]可以計算出連續(xù)拍攝序列圖像序列上的光流信息，得到了廣泛的應(yīng)用。

塊匹配算法是最通用的光流計算方法，本文將塊匹配光流算法進(jìn)行了改進(jìn)，通過圓形塊匹配方式改善了匹配效果，并將其應(yīng)用于無人機的航向角檢測。經(jīng)過對連續(xù)的拍攝畫面進(jìn)行逐幀檢測，證明該方法可以準(zhǔn)確地解算出無人機的航向角，并且相較于陀螺儀有效抑制了漂移，相較磁力計沒有電磁干擾帶來的影響。

1 塊匹配光流算法原理

1.1 匹配過程

在攝像頭與被拍攝物體存在相對運動時，所拍攝的不同幀之間會有邊界遮擋的現(xiàn)象，所以難以檢測整幅圖像的運動狀態(tài)。而且，在連續(xù)的視頻中檢測全局圖像運動情況會有極大的運算量。在此情況下提出了塊匹配光流算法。塊匹配技術(shù)的前提是：所拍攝的畫面在運動量很小、時間間隔很小的情況下默認(rèn)兩幅圖像中物體在畫面中所呈現(xiàn)的灰度沒有引起變化；在足夠小的區(qū)域內(nèi)，物體的運動情況是相同的[7]。在此前提下，將視頻圖像序列分為很多不重疊的小塊，假設(shè)塊內(nèi)的所有像素點的運動狀態(tài)相同，且只發(fā)生平移運動，然后在目標(biāo)圖像中按照一定的匹配準(zhǔn)則、通過一定的搜索算法尋找與此小塊最相似的塊，計算出該小塊在目標(biāo)圖像中相對位置的變化即為運動矢量。如圖1所示。

圖1 塊匹配光流法示意

圖1為第k-1幀圖像與k第幀圖像進(jìn)行匹配的示意圖。圖1中M，N分別為所設(shè)置匹配塊的長和寬，一般取M與N相等。這里，當(dāng)M與N的取值過大時，則上述塊內(nèi)所有像素運動狀態(tài)相同的前提條件就會不成立，而M與N的取值過小會使塊內(nèi)包含的信息量過小導(dǎo)致匹配精度下降。在目前的H.26x與MPEG[8]等視頻壓縮標(biāo)準(zhǔn)中，均按式(1)取

M=N=16

(1)

這個取值已經(jīng)被實踐證明過能取得較好的匹配效果。um與vm為匹配塊的最大橫向位移與縱向位移，則長寬分別為M+2um,N+2vm的矩形區(qū)域則為在第k幀圖像中搜索范圍的大小。經(jīng)過一定的搜索算法得到最佳匹配塊，則向量w為匹配塊在k-1幀到k幀間的運動矢量。

1.2 匹配準(zhǔn)則

光流的塊匹配準(zhǔn)則主要有均方差準(zhǔn)則(MSE)、歸一化相關(guān)函數(shù)(NCCF)、平均絕對誤差準(zhǔn)則(MAD)、絕對誤差和準(zhǔn)則(SAD)等。其中,MAD與SAD容易實現(xiàn)，是比較常用的兩種匹配準(zhǔn)則，這里對其進(jìn)行介紹。

平均絕對誤差準(zhǔn)則是指待匹配塊與搜索塊對應(yīng)像素點灰度值之差的絕對值的平均值

(2)

式中M與N為所選取塊的長寬，fk與fk-1為第k幀與第k-1幀的像素點灰度值，(i,j)為像素點在匹配塊中的坐標(biāo)，u與v為搜索塊相對于待匹配塊的橫向與縱向位置偏移。當(dāng)MAD最小時，則認(rèn)為向量(u,v)為第k-1幀待匹配塊在第k幀的運動位移。由于平均絕對誤差準(zhǔn)則不需要進(jìn)行平方運算，運算的復(fù)雜度較低，所以經(jīng)常被使用。

為了進(jìn)一步降低運算的時間復(fù)雜度與空間復(fù)雜度，提出了絕對誤差和準(zhǔn)則。由于M與N的值在匹配過程中是確定的，所以,對上述所得的像素灰度絕對值不取平均值則為絕對誤差和準(zhǔn)則

(3)

同樣的，當(dāng)SAD最小時，向量 (u,v)為第k-1幀待匹配塊在第k幀的運動位移。

2 圓形塊匹配法

2.1 方形匹配塊存在的問題

現(xiàn)有的塊匹配光流算法中，選取16×16的正方形匹配塊，并且默認(rèn)匹配塊只做平移運動而沒有旋轉(zhuǎn)、縮放等運動。但是，搭載光流傳感器的設(shè)備尤其是無人機，做航向調(diào)整、拉升及降落是常用到的動作。例如，飛機在旋轉(zhuǎn)速度很快時，相鄰的兩幀圖像就會有過大的相對旋轉(zhuǎn)角度，后面一幀搜索到的匹配塊與待匹配塊的相似性就會大打折扣。圖2(a)為在某圖片中選取的16×16的方形待匹配塊灰度的平面分布圖，圖2(b)為此圖片旋轉(zhuǎn)過一定角度后對待匹配塊進(jìn)行匹配所得塊的灰度平面分布圖。

圖2 正方形塊灰度平面分布

如圖2所示，匹配結(jié)果灰度值分布中，“黑影”部分相對于整塊的位置相較待匹配塊“黑影”部分相對于整塊的位置有了一定的變化。由于圖像轉(zhuǎn)過了一定的角度，而匹配塊依然使用相對于原圖像沒有旋轉(zhuǎn)的正方形塊，所以導(dǎo)致了匹配質(zhì)量一定程度的下降。

2.2 圓形匹配塊匹配法

由2.1節(jié)知，方形匹配塊在圖像發(fā)生旋轉(zhuǎn)時會有一定的缺陷。由此本文提出一種新的匹配塊：圓形匹配塊。圓形是沒有方向的，所以如果使用圓形塊作為匹配塊，即使圖形發(fā)生旋轉(zhuǎn)，理論上也可以準(zhǔn)確地得到匹配結(jié)果。圓形匹配塊的生成如圖3所示。

圖3 圓形匹配塊的生成

邊長為16，具有256個像素點的方形匹配塊已經(jīng)被證明具有較好的匹配效果，所以，此圓形匹配塊依然保留256個像素點。以9個像素點的長度為半徑畫圓，圓的邊界處大于1/2的像素點保留，小于1/2的像素點去掉，得到的匹配塊即為包含256個像素點的圓形匹配塊。

圖4(a)為與圖2(a)同一位置選取的圓形待匹配塊灰度的二維分布圖，圖4(b)為在圖像旋轉(zhuǎn)過后對圓形待匹配塊匹配所得塊的灰度二維分布，圖4(c)為將匹配所得塊逆時針旋轉(zhuǎn)一定角度所得塊的灰度二維分布圖。

圖4 圓形匹配塊灰度分布

如4圖(a),(b)所示，在相鄰兩幀圖像旋轉(zhuǎn)過一定角度后，匹配所得塊中的“黑影”確實發(fā)生了旋轉(zhuǎn)，但由于圓形匹配塊的方向性表現(xiàn)不強，如圖4(c)將匹配所得塊再逆時針旋轉(zhuǎn)一定角度后，依然可以得到與待匹配塊較相似的塊。

設(shè)圓形匹配塊偏移矩陣為block，偏移矩陣block為256行2列的矩陣，每一行為以18×18方塊左上角像素點為起點的偏移坐標(biāo)。用block(i)表示block矩陣中第i行所表示的坐標(biāo)，則

(4)

式中L為圓形匹配塊包含的像素點數(shù)，即L=256。同樣的，當(dāng)SAD最小時，(u+v)為當(dāng)前匹配塊的位移向量。

2.3 圓形塊匹配光流算法實驗

為了檢驗用圓形匹配塊的匹配效果，分別用傳統(tǒng)的方形塊匹配與圓形塊匹配光流算法檢測如圖5兩幅畫面的光流情況，其中圖5(b)圖像相對于圖5(a)發(fā)生了一定角度的旋轉(zhuǎn)，兩幅均為同一相機采集的像素為300×400 的圖像。為了方便展示與計算快捷，均采用稀疏光流檢測[9]，且為了消除光流檢測的邊界遮擋帶來的影響，只取圖像中心位置7×7的方陣，共49塊匹配塊來進(jìn)行匹配，且兩次檢測的匹配塊偏移的起點均相同。

圖5 光流檢測圖片樣例

首先采用方形匹配塊進(jìn)行匹配檢測，匹配效果如圖6(a)，然后采用圓形匹配塊進(jìn)行匹配檢測，匹配效果如圖6(b)。在圖6(a)與圖6(b)中，用白色的線條表示所檢測到的匹配塊的位移向量。觀察圖6(a)與圖6(b)可以發(fā)現(xiàn)，在圖像發(fā)生一定角度旋轉(zhuǎn)后，采用正方形匹配塊檢測得出的光流信息已經(jīng)發(fā)生了失真的情況，而采用圓形匹配塊檢測的光流信息則沒有出現(xiàn)失真情況。在這里，提出用兩次檢測所有匹配塊匹配的SAD值之差來定量比較匹配效果

圖6 光流檢測效果

(5)

式中SADD為兩次檢測所有匹配SAD之差，SAD(i)為對第i塊圓形匹配塊進(jìn)行檢測的SAD值，SAD2(j)為對第j塊方形匹配塊進(jìn)行檢測的SAD值，p為選取的匹配塊個數(shù)，即p=49。

由于SAD值越小，認(rèn)為搜索到的匹配塊與待匹配塊越相似，那么可以得出當(dāng)SADD值小于0時，則可以認(rèn)為圓形匹配塊的匹配效果總體上優(yōu)于方形匹配塊的匹配效果，對以上兩次檢測計算SADD，得出結(jié)果為-1 120。

光流檢測經(jīng)常是一個連續(xù)的過程，接下來連續(xù)的畫面進(jìn)行基于圓形匹配塊與方形匹配塊的光流檢測，對比其檢測效果。截取分辨率為300×400 的30幀的視頻進(jìn)行連續(xù)的光流檢測，并對兩次檢測逐幀計算 ，得出結(jié)果如圖7所示。

圖7 連續(xù)光流檢測SADD對比

如圖7，實線表示兩次檢測的SADD值，虛線表示圖像旋轉(zhuǎn)過的角度。圖7顯示：有73.3%的SADD檢測結(jié)果小于0；關(guān)鍵的，那些大于0的SADD值除去個別點外幾乎都處于0的附近，此時對應(yīng)的旋轉(zhuǎn)角度也幾乎為0；旋轉(zhuǎn)角度超過0.5°的19個點中有16個點的SADD小于0，占84.2%；旋轉(zhuǎn)超過1.5°的15個點中，有14個點的SADD小于0，占93.3 %。因此可以得出結(jié)論，當(dāng)畫面發(fā)生一定旋轉(zhuǎn)時，圓形匹配塊的匹配效果總體上優(yōu)于方形匹配塊的匹配效果。

3 基于圓形塊匹配的航向角檢測

3.1 光流航向角檢測算法

通過第2章得出結(jié)論，由于圓形匹配塊的方向性較弱，當(dāng)圖像發(fā)生旋轉(zhuǎn)時，圓形塊的匹配效果優(yōu)于方形塊，所以本小節(jié)針對搭載光流傳感器的無人機，提出基于圓形塊匹配的航向角檢測算法。

首先，如圖8(a)所示，以原圖像第一個像素點為原點，水平像素點方向為x軸，豎直像素點方向為y軸，在原圖像中選取兩塊相距一定距離的圓形待匹配塊A和待匹配塊B，在原圖像中的坐標(biāo)分別為(xa0,ya0),(xb0,yb0)計算塊A與塊B的連線與X軸的夾角α0。則有

(6)

圖8 圓形匹配塊航向角檢測示意

其次，如圖8(b)在旋轉(zhuǎn)后的圖像中搜索塊A與塊B，得到目標(biāo)塊A1，B1，其坐標(biāo)分別為(xa1,ya1),(xb1,yb1) ，則A1與B1的連線與x軸的夾角為α1

(7)

則搭載此光流傳感器的無人機在這一幀的航向角變化量Δyaw1為

Δyaw1=α1-α0

(8)

連續(xù)進(jìn)行航向角檢測，無人機的航向角即為Δyaw的累加

(9)

將式(8)代入式(9)得

(10)

3.2 航向角檢測實驗驗證

拍攝一段對著固定物體在夾角60°內(nèi)左右搖晃的視頻，截取其中5 s，視頻幀速率為28.08幀/s，幀寬度為300 P，幀高度為400 P。對此視頻的前130幀圖像逐幀進(jìn)行基于圓形塊匹配的航向角檢測，輸出結(jié)果如圖9所示。

圖9 連續(xù)視頻航向角檢測結(jié)果

如圖9所示，3.1節(jié)中提出的基于圓形塊匹配的航向角解算算法可以有效地檢測出攝像頭正對的畫面旋轉(zhuǎn)情況，即無人機的航向角。

傳統(tǒng)的航向角檢測方法中，對于陀螺儀檢測，由于重力加速度與無人機z軸共線，所以加速度計無法修正航向角的漂移，致使檢測結(jié)果會有積分累積誤差；對于磁力計檢測，雖然可以解決漂移的問題，但由于地球磁場極其微弱，一般情況下只有微弱的0.5 Gs，而一個普通的手機喇叭當(dāng)相距2 cm時仍會有大約4 Gs的磁場，所以磁力計檢測所得的航向角極易受到干擾。圖10為分別使用陀螺儀采用四元數(shù)法[10]、磁力計、圓形塊匹配光流法進(jìn)行30 s的靜態(tài)檢測結(jié)果。

圖10 陀螺儀、磁力計、光流所得航向角對比

由圖10可以看出，陀螺儀積分所得的航向角在30 s產(chǎn)生了5°左右的漂移，磁力計數(shù)據(jù)沒有漂移但是噪聲很大。使用圓形塊匹配光流法解算所得的的航向角靜態(tài)數(shù)據(jù)既沒有產(chǎn)生漂移，也幾乎沒有受到干擾。因此可以得出結(jié)論，采用圓形塊匹配航向角解算算法所得的航向角靜態(tài)數(shù)據(jù)具有幾乎無漂移、不易受干擾的優(yōu)點。

但是，值得提出的是：實際上上文所提出的圓形塊匹配光流航向角解算算法也是采取了累積的方法，在進(jìn)行長時間動態(tài)檢測時，同樣會產(chǎn)生累積誤差。此外，圖像處理算法的空間與時間復(fù)雜度較高，圖像數(shù)據(jù)采集周期較長，所以，基于光流法的航向角檢測數(shù)據(jù)輸出頻率以及所需要的硬件資源遠(yuǎn)遠(yuǎn)高于陀螺儀與磁力計。因此，基于圓形塊匹配光流檢測所得的航向角可以用來定期修正航向角，而不可以用來直接進(jìn)行姿態(tài)控制。

4 結(jié)束語

本文分析了傳統(tǒng)塊匹配光流算法的位置解算思路，并發(fā)現(xiàn)在檢測畫面發(fā)生旋轉(zhuǎn)時傳統(tǒng)正方形塊匹配算法存在的缺陷，在此基礎(chǔ)上，提出了圓形塊匹配光流算法。通過實驗計算兩次檢測所有匹配塊匹配的SAD值之差得出：圓形塊匹配算法在檢測畫面發(fā)生旋轉(zhuǎn)時相較于方形塊匹配算法有較好的檢測效果。由于圓形塊對方向的不敏感性，本文基于圓形塊匹配光流法進(jìn)一步提出基于圓形塊匹配的航向角解算算法。實驗證明：此算法可以有效檢測所拍攝畫面旋轉(zhuǎn)角度的變化，并相較于陀螺儀、磁力計檢測具有靜態(tài)無漂移、不易受環(huán)境干擾的優(yōu)點。