薛淵，王志勝，徐滔宏

(南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京 211100)

0 引言

四旋翼無人機(jī)憑借其垂直起降、機(jī)動(dòng)性好、可懸停等優(yōu)點(diǎn)，現(xiàn)已被用于許多行業(yè)中。四旋翼在飛行過程中利用慣性傳感器(inertial measurement unit, IMU)采集姿態(tài)信息，通過反饋調(diào)節(jié)姿態(tài)使其達(dá)到預(yù)期位置。但在實(shí)際飛行中僅依靠IMU無法精確估計(jì)無人機(jī)姿態(tài)，通常采用其他傳感器對(duì)IMU數(shù)據(jù)進(jìn)行修正，使其估計(jì)值更接近真實(shí)值。在室外，可利用全球定位系統(tǒng)(global position system, GPS)進(jìn)行輔助定位；在室內(nèi)或者GPS受干擾的地方，則需要用其他傳感器進(jìn)行輔助定位。相比于GPS，目前四旋翼室內(nèi)定位還沒有一套成熟的解決方案[1]。

目前適用于四旋翼無人機(jī)室內(nèi)定位的解決方案有UWB無線電通信、激光雷達(dá)、光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)和視覺等。尤洪祥將光流與UWB技術(shù)結(jié)合，提高了定位精度[2]。美國麻省理工的ANDREW Gregory等人使用慣性傳感器、激光雷達(dá)、攝像頭和超聲波傳感器搭建了室內(nèi)四旋翼自主導(dǎo)航平臺(tái)，并在2010年無人機(jī)大賽中，利用該平臺(tái)實(shí)現(xiàn)指定的自主飛行任務(wù)[3-4]。2010年，美國賓夕法尼亞大學(xué)的教授利用三維紅外運(yùn)動(dòng)捕捉系統(tǒng)實(shí)現(xiàn)四旋翼定位飛行[5]，由于其精度高且穩(wěn)定性好，受到國內(nèi)外許多研究者的廣泛關(guān)注。光流法定位屬于視覺定位，可用于輔助四旋翼懸停。光流指的是空間運(yùn)動(dòng)目標(biāo)在成像面上像素運(yùn)動(dòng)的瞬時(shí)速度，它表示圖像亮度變化與場(chǎng)景中目標(biāo)運(yùn)動(dòng)的關(guān)系，根據(jù)像素亮度時(shí)域變化和相關(guān)性確定各個(gè)像素的運(yùn)動(dòng)信息[6]，實(shí)現(xiàn)了光流位置估計(jì)，通過對(duì)光流值積分估計(jì)其位置信息。文獻(xiàn)[7-8]中D. Honegger等人利用光流法對(duì)速度實(shí)現(xiàn)實(shí)時(shí)估計(jì)，并在FPGA硬件平臺(tái)上進(jìn)行實(shí)時(shí)光流計(jì)算。目前光流法被廣泛應(yīng)用于四旋翼定位與避障中，其中LK光流法為常用算法。LK光流法是LUCAS和KANADE在1981年論文中提出的，屬于稀疏光流的經(jīng)典算法[9]。雖然光流在四旋翼定位中被廣泛使用，但其精度并不高，而且隨著時(shí)間的積累，其誤差會(huì)越來越大。

本文針對(duì)傳統(tǒng)LK光流法在四旋翼定位中精度不高的缺點(diǎn)，采用了一種改進(jìn)的LK光流法，利用該算法得出的速度信息來修正IMU的姿態(tài)數(shù)據(jù)，最后通過實(shí)際飛行實(shí)驗(yàn)驗(yàn)證了改進(jìn)后算法的可行性和穩(wěn)定性。

1 LK光流模型建立

假設(shè)第t幀圖像存在點(diǎn)(x1,y1)，且其周圍n×n領(lǐng)域內(nèi)所有點(diǎn)的運(yùn)動(dòng)狀態(tài)相同，在t+1幀上存在與(x1,y1)亮度相同的點(diǎn)(x2,y2)，根據(jù)光流約束條件和上述假設(shè)條件可建立方程組如下：

(1)

將式(1)簡化成如下形式，即：

Av=b

(2)

將式(2)兩邊同乘以AT，得到如下形式：

ATAv=ATb

(3)

利用最小二乘法求解上述方程，假設(shè)矩陣ATA可逆，將等式(3)兩邊同乘以(ATA)-1，可以得到v的解為：

v=(ATA)-1ATb

(4)

式(4)即為LK光流基本模型。在四旋翼定位實(shí)際應(yīng)用中，因?yàn)槠浯嬖谝欢ň窒扌?，造成?jì)算精度不高，所以需要對(duì)其進(jìn)行改進(jìn)和優(yōu)化。

2 改進(jìn)的LK光流模型

2.1 Shi-Tomas角點(diǎn)檢測(cè)算法

角點(diǎn)檢測(cè)(corner detection)是檢測(cè)圖像特征的一種方法，主要用于對(duì)圖像關(guān)鍵點(diǎn)的檢測(cè)，角點(diǎn)檢測(cè)主要利用圖像灰度信息，比如兩條線之間的交點(diǎn)等。Shi-Tomasi角點(diǎn)檢測(cè)是Harris方法的改進(jìn)，其選取滿足條件的窗口提取圖像特征點(diǎn)，設(shè)點(diǎn)(x,y)的灰度值為I(x,y)，以該點(diǎn)為中心點(diǎn)建立窗口W，將該窗口平移Δx和Δy，得到如下公式：

(5)

將式(5)中I(x+Δx,y+Δy)進(jìn)行一階泰勒級(jí)數(shù)展開，然后將展開式轉(zhuǎn)化為矩陣的形式，表示為如下形式：

(6)

式中：Ix、Iy表示圖像在x，y方向上的偏導(dǎo)數(shù)；T表示一個(gè)2×2的海森矩陣；W表示窗口大小。Shi-Tomasi角點(diǎn)檢測(cè)算法需要求解T矩陣，將行列式特征值中最小值與預(yù)先設(shè)定閾值進(jìn)行比較，如果大于設(shè)定閾值，則表明該點(diǎn)可做為特征點(diǎn)。

2.2 光流金字塔模型建立

在利用LK光流法對(duì)特征點(diǎn)進(jìn)行跟蹤時(shí)需滿足小位移約束條件。因?yàn)樗男碓陲w行時(shí)會(huì)產(chǎn)生位置偏差過大的問題，所以需建立高斯金字塔對(duì)LK光流法進(jìn)行改進(jìn)[9]。光流金字塔是對(duì)大位移運(yùn)動(dòng)縮放幾倍之后，使其近似轉(zhuǎn)化為小位移運(yùn)動(dòng)，從而滿足約束條件。光流金字塔在其最上層(低分辨率圖像)利用LK光流法求出圖像該層光流值，由金字塔自上而下對(duì)光流進(jìn)行逐步細(xì)化，在細(xì)化過程中不斷對(duì)上層結(jié)果進(jìn)行修正，得到更為精確的值，最后得到相應(yīng)光流值。

光流金字塔通過計(jì)算圖像變換矩陣以及光流值，將該值作為初始量傳遞到下一層。下一層圖像計(jì)算得到新的變換矩陣和光流值，以此類推向下傳遞，直到傳到最后一層，即原始圖像層，最后計(jì)算得到最終的變換矩陣和光流值。具體流程如圖1所示。

圖1 金字塔光流計(jì)算過程

2.3 FB誤差分析及補(bǔ)償

1) FB誤差分析

引入FB誤差有利于提高特征點(diǎn)跟蹤的精確度。如圖2所示，利用LK算法對(duì)圖像t時(shí)刻特征點(diǎn)points_prev進(jìn)行跟蹤，可得t+1幀該點(diǎn)位置points_tracked，同樣也可通過t+1幀中點(diǎn)point_tracked反過來跟蹤t幀的位置points_prev_eva。理論來說，points_prev和points_prev_eva兩個(gè)位置應(yīng)該重合，但是由于光流計(jì)算存在誤差，得到結(jié)果會(huì)存在一定的偏差，這個(gè)偏差即為FB誤差。圖2中doffset_prev和doffset_back分別表示前向和反向估計(jì)的偏移量。

圖2 FB誤差示意圖

2) FB誤差補(bǔ)償方法

圖3 偏移量構(gòu)造

(7)

(8)

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

3.1 硬件平臺(tái)介紹

本文采用四旋翼硬件平臺(tái)進(jìn)行飛行實(shí)驗(yàn)，圖4為實(shí)驗(yàn)平臺(tái)。該平臺(tái)為自主設(shè)計(jì)的四旋翼無人機(jī)，其具有足夠的保護(hù)措施，而且結(jié)構(gòu)強(qiáng)度高并易于維修。

圖4 無人機(jī)實(shí)驗(yàn)平臺(tái)

表1為實(shí)驗(yàn)平臺(tái)具體性能參數(shù)。

表1 無人機(jī)實(shí)驗(yàn)平臺(tái)重要參數(shù)

圖5為光流傳感器，實(shí)驗(yàn)時(shí)將其安裝在四旋翼底部且平行于地面。傳感器通過攝像頭和超聲波模塊采集數(shù)據(jù)，然后將采集的數(shù)據(jù)傳入STM32F407處理器中進(jìn)行光流計(jì)算。

圖5 光流傳感器

3.2 實(shí)驗(yàn)數(shù)據(jù)分析

本小節(jié)根據(jù)實(shí)際飛行效果驗(yàn)證算法可行性和穩(wěn)定性。實(shí)驗(yàn)對(duì)兩組數(shù)據(jù)進(jìn)行了分析，第一組為光流傳感器采集的x、y方向上的光流值，第二組為運(yùn)動(dòng)捕捉系統(tǒng)采集四旋翼懸停過程中姿態(tài)和位置信息。

圖6為四旋翼利用傳統(tǒng)LK光流法進(jìn)行室內(nèi)定點(diǎn)飛行時(shí)x、y方向的光流數(shù)據(jù)，圖7為改進(jìn)后LK光流法進(jìn)行室內(nèi)飛行時(shí)x、y方向光流數(shù)據(jù)。從圖中可以看出，改進(jìn)的LK光流法相比于傳統(tǒng)LK光流法，其光流值較密集，而傳統(tǒng)LK光流法輸出光流值稀疏，反映了改進(jìn)的LK算法在精度上要優(yōu)于傳統(tǒng)LK光流法。

圖6 傳統(tǒng)光流法懸停光流效果圖

圖7 改進(jìn)后算法的光流效果圖

圖8 無人機(jī)懸停時(shí)俯仰角信息

圖8表示采用改進(jìn)的LK光流法定點(diǎn)飛行時(shí)俯仰角變化情況，從圖中可以看出飛機(jī)俯仰角誤差在3°～4°之間，其定點(diǎn)懸停效果良好。

圖9反映四旋翼懸停時(shí)位置變化。根據(jù)圖9(a)中三維運(yùn)動(dòng)軌跡可以看出四旋翼在飛行高度達(dá)到1m時(shí)，可以穩(wěn)定懸停。圖9(b)反映了xy和xz的位置變化，可以看出懸停時(shí)xy方向的誤差在±0.5m范圍之內(nèi)，高度z的誤差在±0.1m之間，其懸停精度大約為半徑40cm的圓形。

圖9 無人機(jī)懸停時(shí)位置信息

4 結(jié)語

本文針對(duì)傳統(tǒng)LK光流法在四旋翼懸停時(shí)存在精度不高的缺點(diǎn)，采用了一種改進(jìn)的LK光流法。該方法采用了Shi-Tomasi角點(diǎn)檢測(cè)算法和雙線性差值法提高特征點(diǎn)檢測(cè)的精確度。在進(jìn)行光流計(jì)算時(shí)，通過建立光流金字塔解決四旋翼運(yùn)動(dòng)產(chǎn)生的大位移問題，而后對(duì)圖像FB誤差分析和補(bǔ)償，提高光流計(jì)算的精確度。最后通過實(shí)際飛行驗(yàn)證了改進(jìn)后的LK算法相較于傳統(tǒng)的LK光流法精確度更高且可以實(shí)現(xiàn)穩(wěn)定飛行。