改進(jìn)ORB特征光流的飛行器室內(nèi)導(dǎo)航方法*

郝志洋，張虹波，車 進(jìn)，趙子君，葛聰樂

(1 寧夏大學(xué)物理與電子電氣工程學(xué)院， 銀川 750021；2 長春工業(yè)大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院， 長春 130012)

0 引言

目前的飛行器應(yīng)用主要集中在室外GPS環(huán)境下的定位與導(dǎo)航，在室內(nèi)GPS缺失的環(huán)境下，飛行器的定位與導(dǎo)航正是近年來的研究熱點(diǎn)。傳統(tǒng)的視覺導(dǎo)航是飛行器采集圖像，通過圖傳模塊或者wifi等設(shè)備將采集的照片發(fā)送到地面終端，地面終端進(jìn)行處理后再反饋給飛行器來完成導(dǎo)航。此類方法在無干擾或者干擾程度小的環(huán)境中使用，且反饋回的視頻信息將壓縮，缺失了實(shí)時性和魯棒性。

針對于傳統(tǒng)視覺導(dǎo)航方法的不足，文中提出一種基于改進(jìn)ORB特征光流算法，對于傳統(tǒng)光流算法不能獲得完整光流信息和ORB算法自身缺失尺度不變性的缺點(diǎn)，將具有尺度不變性特點(diǎn)的SURF算法與ORB算法進(jìn)行結(jié)合，并且通過PROSAC算法來剔除誤匹配點(diǎn)，然后將所提取的高精度特征點(diǎn)的光流信息與慣導(dǎo)信息融合，來提高對飛行器速度的精確估計(jì)。

1 改進(jìn)ORB光流的速度估計(jì)

1.1 ORB算法的特征點(diǎn)提取與描述

ORB是一種實(shí)時性較高的特征提取與描述算法，是將FAST特征點(diǎn)的檢測方法與BRIEF特征描述子結(jié)合起來，并在其基礎(chǔ)上做的改良。具體實(shí)現(xiàn)為：

1)建立圖像高斯金字塔，對每一層圖像提取FAST特征點(diǎn)。

2)為特征點(diǎn)增加方向因子。定義像素點(diǎn)到特征點(diǎn)的矩為：

3)取一個S*S的像素塊P，平滑處理后，利用二進(jìn)制碼串得到二值測試定義為：

式中I(p,x)是像素塊P的中點(diǎn)x=(u,v)T的灰度值。生成的特征描述子即為n維二進(jìn)制碼串[3]，如下：

用變換矩陣Rθ對S矩陣進(jìn)行旋轉(zhuǎn)，即可以得到新的描述矩陣SQ:

只要θ確定，結(jié)合S可以得到變換后的描述子：

gn(p,θ)=fn(p)|(xi,yi)∈Sθ

1.2 改進(jìn)ORB算法

ORB算法是一種快速高效的特征點(diǎn)提取算法。當(dāng)前較為成熟的關(guān)于特征點(diǎn)提取的算法主要有SIFT和SURF，了解到ORB算法提取特征點(diǎn)的速度是SIFT的兩倍，SURF的一倍多[4]。而且，ORB算法本質(zhì)上對于特征點(diǎn)提取采用的是FAST算法，而對于特征點(diǎn)的描述采用的是BRIEF算法，ORB算法是先將兩者結(jié)合起來，然后在兩者的基礎(chǔ)上做了一些優(yōu)化。

SURF算法是在SIFT算法的基礎(chǔ)上提出的改進(jìn)，提高了算法的執(zhí)行效率，但其最大的優(yōu)勢是保持了SIFT算法的基于尺度空間的，對圖像縮放、旋轉(zhuǎn)、甚至仿射變換保持不變性的特點(diǎn)[5]。

SURF算法為了剔除重復(fù)特征點(diǎn)，構(gòu)建了黑塞矩陣來生成興趣點(diǎn)。設(shè)在尺度σ處，圖像I(x,y)的Hessian矩陣定義為：

其中：Lxx(x,σ)是高斯函數(shù)二階偏導(dǎo)數(shù)?2g(σ)/?x2與圖像中的灰度值的二維卷積，Lxy(x,σ)、Lyy(x,σ)與Lnx(x,σ)的意義相似。

對上式的行列式進(jìn)行近似化簡可得：

Det=DxxDyy-0.9Dxy

其中Dxx、Dyy和Dxy分別是Lxx、Lyy和Lxy的近似值。由于SURF算法在極值點(diǎn)檢測時采用了盒式濾波器來代替高斯濾波器，而盒式濾波器代替高斯二階微分算子時，要加一定的權(quán)值，這里的0.9算是一個經(jīng)驗(yàn)值，在實(shí)際使用中，用常量0.9表示其相對權(quán)重系數(shù)不會對結(jié)果產(chǎn)生較大的影響。

ORB算法雖然具備快速性的優(yōu)點(diǎn)，但因?yàn)槠浔旧硎褂肍AST來檢測特征點(diǎn)，在具備快速性特點(diǎn)的同時也損失了尺度不變性，為了彌補(bǔ)這個缺點(diǎn)，在后續(xù)算法步驟中，算法引入了特征方向來彌補(bǔ)缺點(diǎn)，但算法本身的特征描述子依舊不具備尺度不變性。考慮到飛行器在飛行過程中，自身傳感器誤差引起的抖動或者根據(jù)需求需要在飛行高度上調(diào)整，目標(biāo)的大小就會發(fā)生變化，所以針對任務(wù)需要使得算法具備尺度不變性。為了解決這個問題，文中將SURF的尺度不變性特點(diǎn)和ORB算法快速性特點(diǎn)相結(jié)合，彌補(bǔ)了各自缺點(diǎn)。這樣既保留了ORB算法描述子運(yùn)算速度快的優(yōu)點(diǎn)，又解決了ORB算法不具備尺度不變性的缺點(diǎn)[5]。將此改進(jìn)的ORB算法定義為SURB算法。

表1 各類算法時間開銷

如表1所示，為了避免試驗(yàn)偶然性，采取多組試驗(yàn)驗(yàn)證當(dāng)特征點(diǎn)數(shù)目為500時，ORB時間開銷最短，SURB時間開銷與ORB相差約280 ms，可近似視為相等，而SURF的時間與其余兩種算法相差較大，有的甚至相差接近一個數(shù)量級。飛行器針對于飛行高度的變化，既要對尺度不變性有所要求，又對實(shí)時性要求較高，因此將改進(jìn)的ORB算法用于稀疏光流法的特征提取上，可以減小時間開銷，獲得較高的實(shí)時性。

2 飛行器速度估計(jì)

2.1 通過SURB改進(jìn)Lucas-Kanade光流提取

基于特征點(diǎn)的金字塔Lucas-Kanade算法(簡稱LK)是經(jīng)典的光流提取算法，該算法需要基于亮度恒定，幀與幀之間運(yùn)動范圍小和空間一致性的假設(shè)[6]。I(u,v,t)表示t時刻像素點(diǎn)處圖像的灰度值，基于上述假設(shè)可以得到像素點(diǎn)亮度的守恒方程：

LK算法的3個假設(shè)在實(shí)際中很難滿足，傳統(tǒng)的解決方法有：1)保證足夠大的圖像幀速率。2)引入圖像金字塔，通過不同尺度圖像的光流提取來保證連續(xù)性。特征點(diǎn)匹配過程中，由于噪聲等外界因素的影響，容易產(chǎn)生大量的誤匹配點(diǎn)，經(jīng)過漢明距離的匹配存在誤匹配點(diǎn)產(chǎn)生匹配誤差，影響匹配效果，PROSAC算法是通過多次迭代來達(dá)到去除誤匹配點(diǎn)的目的。

PROSAC算法與RANSAC算法略有不同，PROSAC算法和RANSAC算法都是一個多次迭代來達(dá)到去除誤匹配點(diǎn)目的的算法。但RANSAC是隨機(jī)抽樣的一個結(jié)果，也就是說RANSAC不能使最終的結(jié)果更準(zhǔn)確，而PROSAC算法是先對所有匹配點(diǎn)的結(jié)果進(jìn)行一個降序排序，不斷選取最佳結(jié)果進(jìn)行迭代，得到最優(yōu)解，提高了精度。

圖1 粗匹配

圖2 PROSAC算法匹配

經(jīng)過圖1粗匹配和PROSAC算法(圖2)雙重特征點(diǎn)匹配后，得到的匹配點(diǎn)精確度有了明顯提高，誤匹配點(diǎn)也明顯降低。

2.2 光流信息處理和慣導(dǎo)數(shù)據(jù)融合

攝像頭在采集圖像信息的過程中可能會受到干擾，進(jìn)而產(chǎn)生視頻不流暢或者圖像出現(xiàn)條紋的狀況，這將導(dǎo)致光流計(jì)算產(chǎn)生較大的誤差[7]。應(yīng)用卡爾曼濾波器，離散事件過程方程和測量方程表示如下：

Xk=AXk-1+ωk-1

Zk=HXk+?k

當(dāng)相機(jī)沒有水平移動而僅存在姿態(tài)變化時，依然會產(chǎn)生光流場，因此在實(shí)際由光流場解算速度時，考慮到姿態(tài)變化的影響。假設(shè)相機(jī)安裝在飛機(jī)的正下方重心處，像平面與機(jī)體x-y平面平行。當(dāng)機(jī)體發(fā)生如圖3變化時，將慣性測量單元獲得的姿態(tài)角數(shù)據(jù)與光流數(shù)據(jù)融合，可以消除光流旋轉(zhuǎn)分量的影響，進(jìn)一步提高對飛行器水平運(yùn)動估計(jì)的精度[8]。

圖3 飛行器姿態(tài)變化

根據(jù)針孔相機(jī)模型，知道攝像頭的圖像比例常數(shù)s、焦距f和飛行器的飛行高度h(這里近似飛行器的高度即為攝像頭光心距離地面的高度)，可以計(jì)算出飛行器實(shí)際水平位移：

式中：Δx和Δy分別是兩幀圖像之間x方向和y方向的飛機(jī)實(shí)際水平的位移增量；dxp,dyp是由上式計(jì)算出的光流水平分量。根據(jù)實(shí)際水平位移，矯正飛行器的姿態(tài)信息。

3 試驗(yàn)結(jié)果及分析

3.1 仿真試驗(yàn)平臺搭建

四旋翼的結(jié)構(gòu)簡圖如圖4所示。飛行時，1號、3號旋翼始終和2號、4號旋翼旋轉(zhuǎn)方向相反，旋轉(zhuǎn)方向如圖所示。4個旋翼由4個無刷電機(jī)控制，當(dāng)4個電機(jī)的轉(zhuǎn)速相同時，四旋翼將懸停或者做豎直方向的運(yùn)動，減小一組中的一個同時增大另一組中的一個電機(jī)轉(zhuǎn)速時，四旋翼將做俯仰或滾轉(zhuǎn)運(yùn)動。

圖4 四旋翼結(jié)構(gòu)簡圖

圖5 坐標(biāo)系

為了驗(yàn)證文中算法的合理性和優(yōu)化算法性能，搭建了基于Matlab的仿真試驗(yàn)平臺，利用Simulink建立模型。首先需要定義相關(guān)的坐標(biāo)系(見圖5)。O-xgygzg是地面慣性坐標(biāo)系，即相對于地面固定的坐標(biāo)系。o-xbybzb為機(jī)體坐標(biāo)系，其原點(diǎn)o固定于機(jī)體重心，x、y軸固定于機(jī)身支架。其中θ是xb與平面O-xgyg的夾角，代表俯仰角度；ψ為機(jī)體坐標(biāo)系xb軸在水平面上投影與地面坐標(biāo)系xg軸(在水平面上，指向目標(biāo)為正)之間的夾角，代表偏航角；φ為機(jī)體坐標(biāo)系zb軸與通過機(jī)體xb軸的鉛垂面間的夾角，代表滾轉(zhuǎn)角。不考慮兩個坐標(biāo)系之間的線運(yùn)動，即假設(shè)O與o相重合時，我們可以通過旋轉(zhuǎn)使得兩個坐標(biāo)系相互重合。所以，可以獲得兩個坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣：

其中Sθ形式代表sinθ，其它的符號以此類推。ψ、θ、φ分別表示偏航、俯仰、滾轉(zhuǎn)角度。在機(jī)體固連坐標(biāo)系下，設(shè)4個旋翼的升力為Fi(i=1,2,3,4)，則四旋翼受到的升力FB可表示為：

利用坐標(biāo)轉(zhuǎn)換矩陣R可得到地面慣性坐標(biāo)系下的升力FE為：

FE=[FX,FY,FZ]T=U1[SφSψ+CψSθCφ,SψSθCφ-CψSφ,CφCθ]T

忽略四旋翼所受的空氣阻力可得：

令I(lǐng)X、IY、IZ為四旋翼3個軸上的轉(zhuǎn)動慣量，U2、U2、U4為四旋翼的滾轉(zhuǎn)力矩、俯仰力矩和偏航力矩，假設(shè)四旋翼結(jié)構(gòu)完全對稱，忽略空氣阻力和陀螺效應(yīng)，四旋翼做小角度運(yùn)動有：

最后得到四旋翼的簡化數(shù)學(xué)模型為：

四旋翼的控制是通過調(diào)節(jié)4個旋翼的轉(zhuǎn)速實(shí)現(xiàn)的。通過四旋翼的簡化數(shù)學(xué)模型構(gòu)造了兩個控制器。一個是內(nèi)環(huán)姿態(tài)回路控制器，用于控制四旋翼的姿態(tài)；一個是外環(huán)位置回路控制器，用于控制四旋翼的位置。得到如圖6、圖7、圖8的仿真結(jié)果。

圖6 Z位置曲線

圖7 X變化范圍曲線

圖8 Y變化范圍曲線

通過觀察仿真試驗(yàn)輸出波形，Z位置的響應(yīng)曲線以及X變化范圍，Y變化范圍曲線，可以看出本次模擬結(jié)果最終達(dá)到了穩(wěn)定狀態(tài)，且滿足試驗(yàn)精度要求。

3.2 硬件試驗(yàn)平臺

文中搭建了圖9的四旋翼飛行器平臺，以自動的方式操作飛行器起飛，實(shí)時記錄數(shù)據(jù)，并將文中的算法用于飛行器控制中，驗(yàn)證飛行器的穩(wěn)定性。

圖9 試驗(yàn)平臺框圖

試驗(yàn)機(jī)載計(jì)算機(jī)訂閱了local/position主題來獲取飛行器當(dāng)前的位置信息和狀態(tài)信息，訂閱了mavros的setpoint/local_position主題來發(fā)送目標(biāo)位置，控制飛行器的起飛、懸停、降落[9]。期望高度為1 m，x、y的位置偏移量在0.3 m之內(nèi)。從飛行控制板上讀取的日志反映出本次試驗(yàn)的實(shí)際數(shù)據(jù)，飛行器高度、x方向偏移和y方向偏移的數(shù)據(jù)如圖10～圖12所示。

圖10 飛行器高度

圖11 飛行器x方向偏移

圖12 飛行器y方向偏移

圖11、圖12分別是飛行器在x、y軸隨著時間變化的曲線圖，可以看出飛行器在懸停過程中，偏差范圍基本滿足在-0.25～0.25之間，懸停效果較好，系統(tǒng)姿態(tài)控制效果較為明顯。且高度均保持在1 m左右，上下偏差范圍0.1 m左右，滿足本次試驗(yàn)的精度要求。從圖中可以看出，系統(tǒng)高度控制良好，左右平移精度也基本穩(wěn)定。

試驗(yàn)結(jié)果表明，四旋翼飛行器較為良好的實(shí)現(xiàn)了在室內(nèi)通過機(jī)載計(jì)算機(jī)來達(dá)到起飛懸停、降落的目的，搭建的平臺結(jié)構(gòu)較為合理，控制算法也達(dá)到了預(yù)期要求。

4 結(jié)論

文中設(shè)計(jì)搭建了一套適用于在室內(nèi)無GPS環(huán)境下的四旋翼飛行器自主起飛懸停的硬件平臺，本次平臺搭建采用了自行組裝完成的機(jī)體，選取了odroid XU4作為上層的開發(fā)板，滿足處理速度和精度的要求，并搭配了px4flow的智能光流傳感器達(dá)到懸停平移校正的目的。試驗(yàn)結(jié)果表明：在光線良好且具有一定紋理的地面情況下，光流傳感器可以計(jì)算出水平速度及位移量，并通過PID的反饋控制校正飛行器姿態(tài)，能夠有效克服在室內(nèi)無GPS環(huán)境下飛行器懸停飄移問題，基本滿足試驗(yàn)的精度要求。