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

化雪薈,陳大力

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

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

化雪薈,陳大力*

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

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

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

在過去的二十年間,得益于較高精度、高可靠性、低成本微型傳感器的大量生產(chǎn)和普及,小型無人機(jī)取得了巨大發(fā)展并且也成為了研究的熱點。目前已經(jīng)在軍事,生產(chǎn)和生活中取得了廣泛的應(yīng)用。作為無人機(jī)控制系統(tǒng)的核心模塊,導(dǎo)航系統(tǒng)一直是無人機(jī)領(lǐng)域眾多研究者所重點關(guān)注的問題。精確的飛行器導(dǎo)航信息是實現(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)航方式,往往存在著如積分累積誤差,信號失鎖等問題。

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

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

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

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

1.1 光流傳感器對速度、位置的估計

(1)光流的定義

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

(2)光流的計算

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

考慮到硬件平臺和計算的復(fù)雜程度,我們選擇了基于最小誤差絕對值和SAD(Sum of Absolute Differences)的塊匹配BMA(Block Matching Algorithms)算法,進(jìn)行光流的計算。該算法的大致原理和流程如圖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。基于最小絕對誤差和SAD匹配準(zhǔn)則的塊匹配算法就是在搜索區(qū)域里尋找滿足下式的Δi,Δj,從而獲得光流矢量V=r(Δi,Δj)T,V的單位為pixel/s,r為攝像頭的采集頻率,單位為frame/s,而U為絕對值誤差和的最小值,他們有如下的關(guān)系表達(dá)式:

(1)

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

(2)

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

(3)

圖1 塊匹配算法示意圖

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

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

(3)光流法的基本模型

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

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

圖2 針孔(Pin-hole)模型

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

(4)

(5)

(6)

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

Vc=-Tc-ω×Pc

(7)

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

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

(8)

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

(9)

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

(10)

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

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

(11)

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

(12)

(13)

vz=0

(14)

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

(15)

(16)

1.2 加速度計組合磁強計對姿態(tài)的估計

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

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

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

(17)

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

(18)

(19)

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

(2)利用磁強計獲取載體的航向角

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

(20)

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

(21)

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

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

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

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

具體的估計方法如圖3所示。

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

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

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

(22)

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

(23)

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

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

(24)

(25)

(26)

(27)

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

(28)

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

圖4 加速度計組合磁強計確定姿態(tài)

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

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

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

圖6 實驗平臺搭建

2.3 INS/光流/磁強計組合導(dǎo)航流程

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

圖5 INS/光流/磁強計組合導(dǎo)航流程

3 實驗結(jié)果與分析

3.1 實驗平臺簡介

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

3.2 實驗結(jié)果

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

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

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

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

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

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

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

4 結(jié)論

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

[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] 呂強,倪佩佩,王國勝,等. 基于光流傳感器的四旋翼飛行器懸停校正[J]. 裝甲兵工程學(xué)院學(xué)報,2014(3):68-72.

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

[8] 宋宇,翁新武,郭昕剛. 基于光流和慣性導(dǎo)航的小型無人機(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] 穆振興. 無人機(jī)姿態(tài)測量系統(tǒng)設(shè)計實現(xiàn)[D]. 哈爾濱:哈爾濱工業(yè)大學(xué),2009.

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

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

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

[16] 宋靜. 基于磁強計、加速度計和陀螺原理的姿態(tài)測試算法研究[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-),女,甘肅景泰人,碩士,高級實驗師,主要從事應(yīng)用電子設(shè)計、移動通信技術(shù)、傳感器應(yīng)用方向的研究;

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