化雪薈,陳大力

(佛山職業(yè)技術(shù)學(xué)院電子信息系,廣東 佛山 528000)

INS/光流/磁強(qiáng)計(jì)組合導(dǎo)航在小型無(wú)人機(jī)中的應(yīng)用

化雪薈,陳大力*

(佛山職業(yè)技術(shù)學(xué)院電子信息系,廣東 佛山 528000)

由于低成本的慣性導(dǎo)航系統(tǒng)存在較為嚴(yán)重的漂移,小型無(wú)人機(jī)在使用其導(dǎo)航時(shí),往往會(huì)出現(xiàn)較大的誤差。針對(duì)這一問題,提出了一種INS/光流/磁強(qiáng)計(jì)組合導(dǎo)航方案:基于擴(kuò)展卡爾曼濾波,將INS與光流數(shù)據(jù)融合,估計(jì)無(wú)人機(jī)的速度和位置。當(dāng)無(wú)人機(jī)靜止或勻速運(yùn)動(dòng)時(shí),將陀螺儀與加速計(jì)、磁強(qiáng)計(jì)的數(shù)據(jù)融合,估計(jì)無(wú)人機(jī)的姿態(tài);當(dāng)無(wú)人機(jī)加速或減速時(shí),用陀螺儀估計(jì)無(wú)人機(jī)的姿態(tài)。利用無(wú)人機(jī)飛行數(shù)據(jù)對(duì)本文所提出的組合導(dǎo)航方案進(jìn)行了驗(yàn)證,驗(yàn)證結(jié)果表明,該方案有效的減少導(dǎo)航時(shí)的速度、位置和姿態(tài)的誤差,提高導(dǎo)航精度。

小型無(wú)人機(jī),慣性導(dǎo)航,光流傳感器,磁強(qiáng)計(jì),擴(kuò)展卡爾曼濾波

在過去的二十年間,得益于較高精度、高可靠性、低成本微型傳感器的大量生產(chǎn)和普及,小型無(wú)人機(jī)取得了巨大發(fā)展并且也成為了研究的熱點(diǎn)。目前已經(jīng)在軍事,生產(chǎn)和生活中取得了廣泛的應(yīng)用。作為無(wú)人機(jī)控制系統(tǒng)的核心模塊,導(dǎo)航系統(tǒng)一直是無(wú)人機(jī)領(lǐng)域眾多研究者所重點(diǎn)關(guān)注的問題。精確的飛行器導(dǎo)航信息是實(shí)現(xiàn)飛行器穩(wěn)定控制的基礎(chǔ),而導(dǎo)航系統(tǒng)的功能就是為飛行器提供準(zhǔn)確的飛行器狀態(tài)信息。導(dǎo)航系統(tǒng)的傳統(tǒng)解決方案包括:慣性導(dǎo)航、GPS導(dǎo)航、GPS/INS組合導(dǎo)航等。然而這些傳統(tǒng)的導(dǎo)航方式,往往存在著如積分累積誤差,信號(hào)失鎖等問題。

近年來(lái),國(guó)內(nèi)外的一些研究人員受到昆蟲利用視覺進(jìn)行導(dǎo)航的啟發(fā),使用光流法進(jìn)行小型無(wú)人機(jī)的導(dǎo)航[1]。國(guó)外方面,Nils Gageilk[2],Dominilk Honegger[3]等人利用光流傳感器完成了無(wú)人機(jī)的定點(diǎn)懸停、自主駕駛以及速度估計(jì),Mercado D A[4],Wang Jinling[5]等人進(jìn)行了INS/GPS/Optical Flow的組合導(dǎo)航研究,可以對(duì)無(wú)人機(jī)的導(dǎo)航信息進(jìn)行實(shí)時(shí)的估計(jì)。國(guó)內(nèi)方面,呂強(qiáng)[6],張洪濤[7]等人也成功利用光流法進(jìn)行了懸停實(shí)驗(yàn),宋宇[8]利用兩個(gè)光流傳感器,完成了對(duì)室內(nèi)小型無(wú)人機(jī)速度,位置和姿態(tài)信息的獲取,但缺點(diǎn)是需要較強(qiáng)的光源,同時(shí)雙光流傳感器獲取姿態(tài)信息時(shí)對(duì)無(wú)人機(jī)高度有一定要求。

在使用單個(gè)光流傳感器進(jìn)行導(dǎo)航時(shí),原理上可以得到無(wú)人機(jī)的速度和位置信息,但是無(wú)法獲取無(wú)人機(jī)的姿態(tài)信息。本文針對(duì)這一問題,在上述研究的基礎(chǔ)上,提出了一種基于INS/光流/磁強(qiáng)計(jì)的組合導(dǎo)航方案。該方案利用擴(kuò)展卡爾曼濾波器,將INS與光流傳感器組合,對(duì)無(wú)人機(jī)的位置和速度進(jìn)行估計(jì);當(dāng)無(wú)人機(jī)靜止或勻速運(yùn)動(dòng)時(shí),將陀螺儀與加速度計(jì)、磁強(qiáng)計(jì)組合,對(duì)無(wú)人機(jī)的姿態(tài)進(jìn)行估計(jì)。對(duì)自主搭建的小型四旋翼無(wú)人機(jī)平臺(tái)進(jìn)行飛行數(shù)據(jù)采集,以驗(yàn)證本文所提出的組合導(dǎo)航方案。驗(yàn)證表明該方案有效的減少了慣性單元在導(dǎo)航時(shí)的時(shí)間累積誤差,同時(shí)具有小型化、輕型化、低成本等優(yōu)點(diǎn)。

1 速度、位置、姿態(tài)的估計(jì)原理

本節(jié)介紹通過光流傳感器對(duì)無(wú)人機(jī)的速度和位置的估計(jì)方法以及加速度計(jì)結(jié)合磁強(qiáng)計(jì)對(duì)無(wú)人機(jī)姿態(tài)的估計(jì)方法。在進(jìn)行原理介紹前,首先需要對(duì)坐標(biāo)系進(jìn)行定義。

1.1 光流傳感器對(duì)速度、位置的估計(jì)

(1)光流的定義

光流的概念是由Gibson于1950年首先提出的,是指空間運(yùn)動(dòng)的物體在成像面上像素運(yùn)動(dòng)的瞬時(shí)速度,它表征了二維圖像的灰度變化和場(chǎng)景中物體及其運(yùn)動(dòng)的關(guān)系,它根據(jù)像素灰度的時(shí)域變化和相關(guān)性確定各個(gè)像素點(diǎn)的運(yùn)動(dòng)速度,因此,可被觀察者用來(lái)確定目標(biāo)的運(yùn)動(dòng)情況[9]。

(2)光流的計(jì)算

圖像處理的研究者為光流的計(jì)算提出了很多的方法,其中較為典型的有:Lucas-Kanade法,Horn-Schunck 法,圖像內(nèi)差值法,塊匹配法,特征匹配法等。

考慮到硬件平臺(tái)和計(jì)算的復(fù)雜程度,我們選擇了基于最小誤差絕對(duì)值和SAD(Sum of Absolute Differences)的塊匹配BMA(Block Matching Algorithms)算法,進(jìn)行光流的計(jì)算。該算法的大致原理和流程如圖1所示[10]。

如圖1所示,設(shè)xp(i,j)和xc(i+Δi,j+Δj)分別表示前一幀中選取的大小為n×n目標(biāo)塊的灰度值和當(dāng)前幀搜索區(qū)域中待匹配目標(biāo)塊的灰度值,其中1≤i,j≤n;-d≤Δi,Δj≤d。基于最小絕對(duì)誤差和SAD匹配準(zhǔn)則的塊匹配算法就是在搜索區(qū)域里尋找滿足下式的Δi,Δj,從而獲得光流矢量V=r(Δi,Δj)T,V的單位為pixel/s,r為攝像頭的采集頻率,單位為frame/s,而U為絕對(duì)值誤差和的最小值,他們有如下的關(guān)系表達(dá)式:

(1)

U=min(Δi,Δj){SAD(Δi,Δj)}

(2)

V=r(Δi,Δj)T|U

(3)

圖1 塊匹配算法示意圖

在初始狀態(tài)時(shí),在成像平面的原點(diǎn)處選取一個(gè)目標(biāo)塊,當(dāng)無(wú)人機(jī)運(yùn)動(dòng)時(shí),目標(biāo)塊會(huì)在后一幀圖像中移動(dòng)。在后一幀的搜索區(qū)域內(nèi)(該搜索區(qū)域限定了兩個(gè)方向的最大位移d),使當(dāng)前幀的目標(biāo)塊和前一幀的目標(biāo)塊內(nèi)像素灰度差的SAD最小,這樣我們就可以得到目標(biāo)塊的光流矢量。

在實(shí)驗(yàn)中,采集垂直于攝像頭方向的圖像,并使用8位來(lái)的數(shù)據(jù)來(lái)表征每個(gè)像素的灰度。采集后的數(shù)據(jù)通過DMA存儲(chǔ)在系統(tǒng)內(nèi)存中。在整個(gè)采集過程中,選擇8×8像素的數(shù)據(jù)塊來(lái)作為塊匹配的對(duì)象,搜索區(qū)域?yàn)椤?個(gè)像素,所以對(duì)于每一幀圖像,一共有64個(gè)像素點(diǎn),同時(shí)有81個(gè)候選的向量方向。獲得每一幀圖像后,我們計(jì)算出各個(gè)候選向量的誤差絕對(duì)值和,并選擇其中的最小值作為光流失量。

(3)光流法的基本模型

光流法估計(jì)物體的運(yùn)動(dòng)模型本質(zhì)都是將三維的運(yùn)動(dòng)投影到攝像頭的二維像平面上。常見的光流法估計(jì)模型有兩種:一種是針孔模型(Pin-hole Image Plane Approach),它來(lái)源于對(duì)脊椎動(dòng)物視覺系統(tǒng)的仿生;另一種是球面模型(Spherical Imaging Surface Approach),它受到了昆蟲復(fù)眼的啟發(fā)[11]。在本文中,我們使用針孔模型,對(duì)無(wú)人機(jī)在地理坐標(biāo)系下的運(yùn)動(dòng)進(jìn)行估計(jì)。

針孔模型的示意圖,如圖2所示。

圖2 針孔(Pin-hole)模型

設(shè)Pc=[Xc,Yc,Zc]T是在攝像頭坐標(biāo)系下的一個(gè)點(diǎn),f代表了攝像頭的焦距,于是這一點(diǎn)在攝像頭成像平面下表示為p=[x,y,f]T,并有關(guān)系:

(4)

(5)

(6)

式中:因?yàn)閺臄z像頭到成相平面的距離的始終為焦距,所以向量p=[x,y,f]T的第3項(xiàng)為常數(shù)并等于焦距?？紤]地面上的任意一點(diǎn)P,在攝像頭坐標(biāo)系下P相對(duì)于無(wú)人機(jī)有如下的運(yùn)動(dòng)關(guān)系:

Vc=-Tc-ω×Pc

(7)

將式(7)在3個(gè)維度上展開,可以得到:

Vxc=-Txc-(ωyZc-ωzYc)

(8)

Vyc=-Tyc-(ωzXc-ωxZc)

(9)

Vzc=-Tzc-(ωxYc-ωyXc)

(10)

式中:ω=[ωx,ωy,ωz]T是無(wú)人機(jī)的角速度,Tc=[Txc,Tyc,Tzc]T是無(wú)人機(jī)在攝像頭坐標(biāo)系的平動(dòng)速度.

對(duì)式(4)兩邊求導(dǎo),可以得到Pc在攝像頭坐標(biāo)下的速度與p在成像平面下的速度的關(guān)系:

(11)

式中:v=[vx,vy,vz]T。將式(11)在3個(gè)維度上展開,可以得到:

(12)

(13)

vz=0

(14)

將式(8)～式(10)代入式(12)、式(13),可以得到:

(15)

(16)

1.2 加速度計(jì)組合磁強(qiáng)計(jì)對(duì)姿態(tài)的估計(jì)

加速度計(jì)通過敏感重力場(chǎng),在載體不存在自身加速度的情況下,可以確定無(wú)人機(jī)的橫滾角和俯仰角;磁強(qiáng)計(jì)敏感地磁場(chǎng),借助于加速度計(jì)得到的載體姿態(tài)信息,可以解算得到無(wú)人機(jī)航向角。兩者相組合,就可以得到不隨時(shí)間積累誤差的全姿態(tài)信息[12]。

(1)利用加速度計(jì)獲取載體的橫滾角和俯仰角

重力矢量在地理坐標(biāo)系下的分量為[0 0 -g]T,當(dāng)載體處于靜止態(tài)(相對(duì)導(dǎo)航坐標(biāo)系無(wú)加速度)時(shí),安裝在載體坐標(biāo)系下的加速度計(jì)測(cè)量值為ab=[axbaybazb]T,由于重力加速度垂直于水平面,故載體的航向角不影響x向和y向加速度計(jì)的輸出。于是有關(guān)系:

(17)

聯(lián)立可以解得俯仰角和橫滾角為:

(18)

(19)

此方法利用地球重力加速度在機(jī)體坐標(biāo)系三軸上的投影信息反映載體的姿態(tài)信息,故只有在載體無(wú)加速運(yùn)動(dòng)的情況下上述算式才成立。而在實(shí)際系統(tǒng)中,載體并不是始終保持靜止或勻速運(yùn)動(dòng)的,一旦載體有了加速運(yùn)動(dòng),加速度計(jì)的測(cè)量值就不再與重力加速度在載體系三軸上的分量相等。所以該方案只能用于靜態(tài)姿態(tài)測(cè)量,動(dòng)態(tài)時(shí)的姿態(tài)測(cè)量還要另尋方案[13]。

(2)利用磁強(qiáng)計(jì)獲取載體的航向角

設(shè)地磁強(qiáng)度在地理坐標(biāo)系三軸上的分量為Hn=[HxnHynHzn]T,磁強(qiáng)計(jì)沿體坐標(biāo)系Fb三軸方向安裝,地磁強(qiáng)度在載體坐標(biāo)系三軸上的分量為Hb=[HxbHybHzb]T,地磁強(qiáng)度在地理坐標(biāo)系和載體坐標(biāo)系各軸上的投影可由兩坐標(biāo)系之間的轉(zhuǎn)換矩陣來(lái)表示:

(20)

式中:[HxnHynHzn]T的值通過查表給出[14],在太原地區(qū)(北緯37.8度,東經(jīng)112.5度),其值如式(21)所示,而[HxbHybHzb]T的數(shù)值由機(jī)載的磁強(qiáng)計(jì)給出。

(21)

假定無(wú)人機(jī)在飛行過程中地磁場(chǎng)保持恒定,聯(lián)立式(20)、式(21),再結(jié)合由加速度計(jì)確定的俯仰角和橫滾角,就可以解算出無(wú)人機(jī)在地理坐標(biāo)系下的航向角。

2 利用擴(kuò)展卡爾曼濾波器(EKF)進(jìn)行數(shù)據(jù)融合

2.1 基于擴(kuò)展卡爾曼濾波的加速度計(jì)/光流傳感器組合模型

本文采用擴(kuò)展卡爾曼濾波將加速度計(jì)數(shù)據(jù)和光流傳感器的數(shù)據(jù)進(jìn)行融合。選取無(wú)人機(jī)在導(dǎo)航坐標(biāo)系下的速度和位置信息作為狀態(tài)量,光流傳感器和超聲波傳感器的輸出作為觀測(cè)量,聯(lián)合估計(jì)無(wú)人機(jī)的位置和速度信息。

具體的估計(jì)方法如圖3所示。

圖3 IMU組合光流傳感器估計(jì)速度和位置

考慮非線性系統(tǒng)的狀態(tài)方程和觀測(cè)方程:

Xk+1=f[Xk,k]+BkUk+ΓkWk

(22)

Zk+1=h[Xk+1,k+1]+Vk+1

(23)

式中:Xk=[XnYnZnVxnVynVzn]T是狀態(tài)向量,它包括無(wú)人機(jī)的速度信息和位置信息;Zk=[xyZc]T是觀測(cè)向量,它包括光流傳感器輸出的x方向光流和y方向光流和超聲波傳感器的讀數(shù)Zc。Uk=[0 0 0axnaynazn]T是系統(tǒng)的控制向量,由加速度計(jì)的數(shù)值經(jīng)過坐標(biāo)系矩陣變換得出;Bk是控制分配矩陣;Γk是噪聲分配矩陣;Wk是過程噪聲矩陣;Vk是測(cè)量噪聲;f代表系統(tǒng)狀態(tài)函數(shù);h代表觀測(cè)函數(shù)。

將狀態(tài)方程和觀測(cè)方程代入擴(kuò)展卡爾曼濾波器,可以得到:

(24)

(25)

(26)

(27)

Pk+1= (I-Kk+1Hk+1)Pk+1/k(I-Kk+1Hk+1)-1+

(28)

通過上述擴(kuò)展卡爾曼濾波過程,可以得到在地理坐標(biāo)系下對(duì)無(wú)人機(jī)的速度和位置信息。

圖4 加速度計(jì)組合磁強(qiáng)計(jì)確定姿態(tài)

2.2 基于擴(kuò)展卡爾曼濾波的陀螺/加速度計(jì)/磁強(qiáng)計(jì)組合模型

通過陀螺輸出角速率信號(hào)積分可以獲取無(wú)人機(jī)的姿態(tài),但是MEMS陀螺儀存在較為嚴(yán)重的漂移問題,而加速度計(jì)/磁力計(jì)組合能提供不同噪聲、無(wú)漂移的姿態(tài),所以利用擴(kuò)展卡爾曼濾波器去融合各個(gè)傳感器中的數(shù)據(jù)。濾波過程如圖4所示,系統(tǒng)狀態(tài)向量可表示為Xk=[q0q1q2q3ωxωyωz]T,其中[q0q1q2q3]T為系統(tǒng)的狀態(tài)四元數(shù),可以通過四階龍格庫(kù)塔法[15]求出,[ωxωyωz]T為陀螺儀的輸出值。

系統(tǒng)的觀測(cè)向量可以表示為Zk=[q0q1q2q3]T,其中的4個(gè)四元數(shù)可以利用加速度計(jì)和磁強(qiáng)計(jì)的讀數(shù)結(jié)合通過高斯牛頓法[16]求出。

圖6 實(shí)驗(yàn)平臺(tái)搭建

2.3 INS/光流/磁強(qiáng)計(jì)組合導(dǎo)航流程

利用擴(kuò)展卡爾曼濾波對(duì)加速度計(jì)和光流的組合,無(wú)論是在靜止?fàn)顟B(tài)還是運(yùn)動(dòng)狀態(tài)下,都可以對(duì)無(wú)人機(jī)的運(yùn)動(dòng)速度和位置進(jìn)行有效的估計(jì)。根據(jù)陀螺儀和加速度計(jì)/磁強(qiáng)計(jì)測(cè)量姿態(tài)的特點(diǎn),當(dāng)載體靜止或勻速運(yùn)動(dòng)的時(shí)候,利用擴(kuò)展卡爾曼濾波定時(shí)對(duì)陀螺儀積分得到的姿態(tài)進(jìn)行修正;而當(dāng)檢測(cè)到無(wú)人機(jī)在進(jìn)行加速、減速或高速轉(zhuǎn)動(dòng)時(shí),就不再進(jìn)行姿態(tài)修正,而是在前一步更新后的姿態(tài)基礎(chǔ)上用陀螺儀數(shù)據(jù)進(jìn)行捷聯(lián)解算,直到載體恢復(fù)靜止或勻速狀態(tài)。整個(gè)導(dǎo)航的流程如圖5所示。

圖5 INS/光流/磁強(qiáng)計(jì)組合導(dǎo)航流程

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

3.1 實(shí)驗(yàn)平臺(tái)簡(jiǎn)介

如圖6所示,在實(shí)驗(yàn)驗(yàn)證過程中使用了自主搭建的四旋翼飛行器平臺(tái)(圖6(a))。飛行控制處理器采用了意法半導(dǎo)體的STM32F103Z(圖6(b)),光流傳感器選擇了3DRobotics公司生產(chǎn)的PX4FLOW(圖6(c)),IMU選擇了MPU6050(圖6(d)),三軸磁強(qiáng)計(jì)選擇了霍尼韋爾公司的HMC3883L(圖6(d)),飛行器的電機(jī)選擇了大疆公司生產(chǎn)的DJ1 2312(圖6(e))。此外還使用了HC-12無(wú)線串口模塊(圖6(f)),可以將機(jī)載的傳感器數(shù)據(jù)實(shí)時(shí)傳回到PC端。

3.2 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)過程中,無(wú)人機(jī)沿正北方向飛行,在X方向基本保持恒定,Y方向前進(jìn)了大約50 m,飛行高度控制在1 m。無(wú)人機(jī)的3個(gè)姿態(tài)角均使用PID算法調(diào)節(jié)在10°之內(nèi)。用MATLAB將采集到的數(shù)據(jù)進(jìn)行作圖,并對(duì)比了僅使用慣導(dǎo)單元時(shí)的情況和使用EKF組合算法時(shí)情況,可得如圖7～圖9所示結(jié)果。

通過實(shí)驗(yàn)數(shù)據(jù)可以得出:

(1)在速度估計(jì)和位置估計(jì)部分,光流與加速度計(jì)組合擴(kuò)展卡爾曼濾波器可以有效的降低加速度計(jì)的隨機(jī)漂移誤差以及由于時(shí)間累積而產(chǎn)生的積分誤差。例如圖8所示,因?yàn)楸疚氖褂玫氖堑统杀镜腗EMS加速度計(jì),如果加速度值不經(jīng)過濾波和補(bǔ)償,在大約50 m的行駛路程中,就出現(xiàn)了近8 m的時(shí)間累積誤差,而使用組合算法后,所產(chǎn)生的誤差不超過1 m;

(2)在姿態(tài)估計(jì)部分,在靜態(tài)和勻速運(yùn)動(dòng)階段濾波器的輸出可以很好的跟蹤3個(gè)姿態(tài)角,很大程度的減少了由于陀螺常值漂移和隨機(jī)漂移產(chǎn)生的誤差。并且可以發(fā)現(xiàn)在加速運(yùn)動(dòng)結(jié)束后,本文提出的組合方法可以及時(shí)的修正航向。

圖8 位置估計(jì)結(jié)果

圖7 速度估計(jì)結(jié)果

圖9 姿態(tài)估計(jì)結(jié)果

4 結(jié)論

針對(duì)小型無(wú)人機(jī)在飛行過程的導(dǎo)航問題,本文提出了一種INS/光流/磁強(qiáng)計(jì)組合導(dǎo)航的方案。該組合導(dǎo)航方案基于擴(kuò)展卡爾曼濾波的多傳感器信息融合,可以對(duì)小型無(wú)人機(jī)進(jìn)行位置,速度和姿態(tài)的估計(jì)。實(shí)驗(yàn)表明:該方案可以有效提高小型無(wú)人機(jī)的導(dǎo)航性能,相比于單純的捷聯(lián)慣性導(dǎo)航可以顯著減少導(dǎo)航時(shí)的位置,速度和姿態(tài)誤差,具有工程應(yīng)用的價(jià)值。

[1] Chao Haiyang,Cao Yongcan,Chen Y. Autopilots for Small Unmanned Aerial Vehicles:A Survey[J]. International Journal of Control,Automation and Systems,2010,8(1):36-44.

[2] Gageik N,Strohmeier M,Montenegro S. An Autonomous UAV with an Optical Flow Sensor for Positioning and Navigation[J]. International Journal of Advanced Robotic Systems,2013,10(4):301-319.

[3] Honegger D,Meier L,Tanskanen P,et al. An Open Source and Open Hardware Embedded Metric Optical Flow CMOS Camera for Indoor and Outdoor Applications[C]//IEEE International Conference on Robotics and Automation. IEEE,2013:1736-1741.

[4] Mercado D A,Flores G,Castillo P,et al. GPS/INS/Optic Flow Data Fusion for Position and Velocity Estimation[C]//International Conference on Unmanned Aircraft Systems. IEEE,2013:486-491.

[5] Ding W,Wang J,Almagbile A. Adaptive Filter Design for UAV Navigation with GPS/INS/Optic Flow Integration[J]. International Conference on Electrical and Control Engineering,2010:4623-4626.

[6] 呂強(qiáng),倪佩佩,王國(guó)勝,等. 基于光流傳感器的四旋翼飛行器懸停校正[J]. 裝甲兵工程學(xué)院學(xué)報(bào),2014(3):68-72.

[7] 張洪濤,張廣玉,李隆球,等. 微型二維光流傳感器設(shè)計(jì)[J]. 哈爾濱工程大學(xué)學(xué)報(bào),2014,(5):619-623.

[8] 宋宇,翁新武,郭昕剛. 基于光流和慣性導(dǎo)航的小型無(wú)人機(jī)定位方法[J]. 傳感器與微系統(tǒng),2015,34(1)13-16.

[9] KOENDERINKJ. Optic Flow[J]. Vision Research,1986,26(1):161-180.

[10] Lan H,Mei S J,Hua C P,et al. Visual Navigation for UAV Using Optical Flow Estimation[C]//Control Conference. IEEE,2014:816-821.

[11] Chao H,Gu Y,Napolitano M. A Survey of Optical Flow Techniques for UAV Navigation Applications[C]//International Conference on Unmanned Aircraft Systems. IEEE,2013:710-716.

[12] 穆振興. 無(wú)人機(jī)姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)實(shí)現(xiàn)[D]. 哈爾濱:哈爾濱工業(yè)大學(xué),2009.

[13] 黃旭,王常虹,伊國(guó)興,等. 利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器[J]. 中國(guó)慣性技術(shù)學(xué)報(bào),2005,13(2):27-30.

[14] 趙捍東,曹紅松,朱基智,等. 基于磁強(qiáng)計(jì)和陀螺的姿態(tài)測(cè)量方法[J]. 中北大學(xué)學(xué)報(bào)(自然科學(xué)版),2010,31(6):631-635.

[15] 張春慧,吳簡(jiǎn)彤,何昆鵬,等. 四階龍格-庫(kù)塔法在捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算中的應(yīng)用[J]. 聲學(xué)與電子工程,2005,32(1):37-38.

[16] 宋靜. 基于磁強(qiáng)計(jì)、加速度計(jì)和陀螺原理的姿態(tài)測(cè)試算法研究[D]. 太原:中北大學(xué),2011.

ApplicationofINS/OpticalFlow/MagnetometerIntegratedNavigationSysteminSmallUAV

HUAXuehui,CHENDali*

(Department of Electronic Information,Foshan Polytechnic Institute,Foshan Guangdong 528000,China)

The drift of inertial navigation system(INS)will lead to large navigation error when a low-cost INS is used in small UAV. To overcome the above problem,an INS/Optical Flow/Magnetometer integrated Navigation scheme is proposed. The scheme,which is based on the extended Kalman filter,combines INS and Optical Flow information to estimate the velocity and position of UAV,and combines gyro,accelerator and magnetometer information to estimate the UAV attitude when the UAV is at rest or in uniform motion,and uses gyro to estimate the UAV attitude when the UAV is accelerating or decelerating. The UAV flight data is used to verify the proposed integrated navigation scheme,and the verification results show that the proposed scheme can effectively reduce the errors of navigation parameters and improve navigation precision.

UAV;INS;optical flow;magnetometer;EKF

10.3969/j.issn.1005-9490.2017.06.017

2016-09-01修改日期2017-02-24

V279

A

1005-9490(2017)06-1414-07

化雪薈(1976-),女,甘肅景泰人,碩士,高級(jí)實(shí)驗(yàn)師,主要從事應(yīng)用電子設(shè)計(jì)、移動(dòng)通信技術(shù)、傳感器應(yīng)用方向的研究;

陳大力(1975-),男,吉林四平人,副教授,主要從事工業(yè)控制、電子設(shè)計(jì)方面研究。