呂　濤，張長利，王樹文，王潤濤，張伶鳦，劉　超，欒吉玲，周雅楠

(東北農(nóng)業(yè)大學(xué) 電氣與信息學(xué)院，哈爾濱　150030)

?

多傳感器數(shù)據(jù)融合的無人機(jī)速率估算與定位

呂濤，張長利，王樹文，王潤濤，張伶鳦，劉超，欒吉玲，周雅楠

(東北農(nóng)業(yè)大學(xué) 電氣與信息學(xué)院，哈爾濱150030)

摘要：為了提高農(nóng)用無人機(jī)速率與位置的估算精度，同時(shí)降低無人機(jī)制作成本，提出了一種對于農(nóng)用無人機(jī)容易實(shí)現(xiàn)的傳感器數(shù)據(jù)融合算法，即通過使用離散型卡爾曼濾波，提高實(shí)驗(yàn)對象在三維空間中位置和速率變化的估算精度。由于傳統(tǒng)的慣性測量系統(tǒng)存在體積大、造價(jià)高等缺點(diǎn)，而廉價(jià)的慣性測量傳感器又存在較大的飄移，因此結(jié)合農(nóng)用無人機(jī)航拍的工作環(huán)境采用全球定位系統(tǒng)(GPS)提供位置的測量，由慣性導(dǎo)航系統(tǒng)(INS)給出加速度，并由光流傳感器提供速度的測量加以輔助。最終，通過實(shí)驗(yàn)驗(yàn)證了該算法的有效性。

關(guān)鍵詞：農(nóng)用無人機(jī)；數(shù)據(jù)融合；離散型卡爾曼濾波；光流

0引言

由于傳感器測量準(zhǔn)確性提高、體積縮小及成本降低，無人駕駛飛行器(無人機(jī))已成為勘探、監(jiān)測和救援等多方面的重要工具。無人機(jī)具有成本低、易操作、靈活性強(qiáng)等諸多優(yōu)點(diǎn)，在農(nóng)業(yè)生產(chǎn)中利用無人機(jī)檢測農(nóng)情，能更有效地得知農(nóng)作物的生長狀況、種植面積及各方面信息[1-2]。

對飛行中無人機(jī)姿態(tài)和速率的精確估算仍是一個(gè)巨大的挑戰(zhàn)。廉價(jià)的GPS傳感器可以提供相關(guān)信息，但由于其測量速率只有5Hz，即使在良好的環(huán)境中，其誤差范圍也會達(dá)到2m，并不適合實(shí)際應(yīng)用。此外，在環(huán)境較差的狀況下，GPS信號容易丟失；而光流法并不需要與外界進(jìn)行通訊，只需來自攝像頭采集的信息便能正常工作。因此，采用了光流法來代替GPS，通過使用視覺算法來估算對象的運(yùn)動速度。

關(guān)于無人機(jī)定位的數(shù)據(jù)融合研究中，大多數(shù)是對快速移動目標(biāo)所采集的數(shù)據(jù)進(jìn)行后期處理。Gross等人提出了對GPS、INS提供的數(shù)據(jù)，同時(shí)使用擴(kuò)展卡爾曼濾波(EKF)和無跡卡爾曼濾波(UKF)融合算法進(jìn)行后期處理，從而進(jìn)行姿態(tài)評估，最后通過實(shí)驗(yàn)驗(yàn)證[3]。同樣有人展示了使用卡爾曼濾波對GPS、INS數(shù)據(jù)進(jìn)行處理的一種姿態(tài)評估算法[4]。使用連續(xù)型擴(kuò)展卡爾曼濾波，繼承所有由衛(wèi)星傳輸?shù)目捎眯畔⒔oINS、GPS進(jìn)行數(shù)據(jù)融合，即使條件是在只有一個(gè)衛(wèi)星可以觀察的情況下仍可實(shí)行(傳統(tǒng)的離散型濾波器需要至少4個(gè)衛(wèi)星同時(shí)觀察)[5]。此外，介紹了一種基于EKF與協(xié)方差自適應(yīng)算法，使用GPS、INS、光流傳感器進(jìn)行測量[6]。這些工作的目的是將來自多個(gè)不同種類的傳感器的信息進(jìn)行數(shù)據(jù)融合，以得到精準(zhǔn)、可靠的位置和速率信息。

針對農(nóng)用無人機(jī)低空、低速、在白天工作等特點(diǎn)，以GPS、INS、光流傳感器進(jìn)行測量[7]，以卡爾曼濾波進(jìn)行數(shù)據(jù)融合，完成對無人機(jī)位置和速率的準(zhǔn)確估算[8]。通過實(shí)驗(yàn)驗(yàn)證此法效果良好，為降低農(nóng)用無人機(jī)成本，提高其精確度提供了參考。

1實(shí)現(xiàn)方案

廉價(jià)GPS裝置測量誤差大，不足以滿足新興微小型無人機(jī)的要求。這些誤差來自于多方面的因素，如觀測衛(wèi)星的數(shù)量和觀測角度、GPS傳感器的質(zhì)量、數(shù)據(jù)傳輸中受到的信號干擾等。GPS提供的位置和速度測量的精度分別稱為accP與accV，其變化是動態(tài)的，根據(jù)實(shí)際測量情況得出。因此，為了提高精確度，加入光流傳感器和慣性傳感器來輔助GPS。光流是空間運(yùn)動物體在觀測成像平面上的像素運(yùn)動的“瞬時(shí)速度”。光流的研究是利用圖像序列中的像素強(qiáng)度數(shù)據(jù)的時(shí)域變化和相關(guān)性來確定各自像素位置的“運(yùn)動”。研究光流場的目的就是為了從圖片序列中近似得到不能直接得到的運(yùn)動場[9-11]。經(jīng)由光流傳感器的輔助測量后，再通過設(shè)計(jì)的卡爾曼濾波器進(jìn)行數(shù)據(jù)融合，最終對無人機(jī)速率和位置的估算提供準(zhǔn)確的數(shù)據(jù)。其流程如圖1所示。

圖1　無人機(jī)位置和速率估算流程

2離散卡爾曼濾波

卡爾曼濾波廣泛應(yīng)用于被白噪聲引起干擾的離散線性動力學(xué)系統(tǒng)的姿態(tài)評估中，被人們所熟知。

ξk=Aξk-1+Buk-1+ωk-1

(1)

Zk=Hξk+vk

(2)

其中，ξ為狀態(tài)向量；u為輸入向量；Z為測量向量；A、B和H分別代表狀態(tài)轉(zhuǎn)換、輸入和觀測矩陣；k為離散時(shí)間指數(shù)。過程噪聲和測量噪聲ω、v假定為符合正態(tài)分布的白噪聲，則

ω~N(0,Q)

(3)

v~N(0,R)

(4)

其中，Q和R分別是過程噪聲協(xié)方差和測量噪聲協(xié)方差。

(5)

(6)

由此可以確定卡爾曼增益矩陣Kk為

(7)

(8)

(9)

3GPS、INS、光流的數(shù)據(jù)融合

3.1　系統(tǒng)模型

系統(tǒng)所用到的狀態(tài)向量有

(10)

目標(biāo)飛行在三維空間中的位置坐標(biāo)為(x,y,z),速率為(Vx,Vy,Vz)。這些物理量都是參照在導(dǎo)航坐標(biāo)系下的。因此，系統(tǒng)的運(yùn)動方程可以寫成

(11)

(12)

(13)

Zk=ξk+Vk

(14)

其中，T為采樣周期。

3.2　光流計(jì)算

本研究中，將實(shí)驗(yàn)相機(jī)平臺安放在理想的三維空間場景中飛行[12]。相機(jī)的速度在慣性坐標(biāo)系中給出(Vx,Vy,Vz),像素點(diǎn)(xi,yi)的光流計(jì)算是由其平移和轉(zhuǎn)動的部分組成，方程為

(15)

轉(zhuǎn)動部分ROF由下式給出，則

(16)

平移部分為

(17)

進(jìn)行光流計(jì)算時(shí)，特征點(diǎn)(xi,yi)的坐標(biāo)值都是已知的，并且其運(yùn)動狀態(tài)相同。通過使用所有的跟蹤特性，光流的平均值可表示為

(18)

(19)

(20)

(21)

(22)

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

為了驗(yàn)證提出的融合模式，開發(fā)了一套實(shí)驗(yàn)平臺，配備了GPS、INS和光流攝像頭，如圖2所示。所有算法都是由安裝在平臺上的便攜式計(jì)算機(jī)執(zhí)行。為了簡化實(shí)驗(yàn)，只在x、y平面上進(jìn)行狀態(tài)估算，因此z軸是一個(gè)恒定值。為了使估算擴(kuò)展到三維空間，使z軸可變，需要一個(gè)高度傳感器。OpenCV(開放源代碼計(jì)算機(jī)視覺類庫)用于從相機(jī)捕獲圖像，進(jìn)行光流和水平速度估計(jì)。同樣，卡爾曼濾波庫用來實(shí)現(xiàn)卡爾曼濾波。GPS提供的測量位置頻率為5Hz，其誤差在2m以內(nèi)。視覺算法的執(zhí)行頻率在40Hz，使用INS傳感器測量加速度。

圖2　實(shí)驗(yàn)平臺

測試在露天廣場進(jìn)行，只有自然光作為光源，結(jié)果如圖3~圖9所示。過程噪聲矩陣Q設(shè)置的對角值很小，即

(23)

協(xié)方差矩陣R的值很難精確測量到，因?yàn)镚PS并沒有提供其數(shù)據(jù)，此外光照條件的不同也是其中的原因，但這些參數(shù)可以用來得知濾波器測量的可靠性。當(dāng)涉及到對R矩陣的估算需要精確GPS參數(shù)(accP)時(shí)，濾波器就不在選用GPS的參數(shù)，直接選用與精確GPS(accP)成比例的值來代替，位置測量使用GPS，速度則使用光流來取代，即

(24)

INS測得的系統(tǒng)加速度如圖3所示。由于目標(biāo)沒有移動，它們非常接近于0。圖4和圖5為GPS傳感器測量的位置和速度信息。由于GPS受干擾性強(qiáng)，圖6中定位誤差在2m以內(nèi)。圖7為光流計(jì)算出的速度，經(jīng)由卡爾曼濾波融合后測量的位置和速度如圖8和圖9所示。

圖3　INS所測加速度

圖4　GPS所測位置

圖5　GPS所測速度

圖6　GPS測量精度

圖7　光流法所測速度

圖8　經(jīng)卡爾曼濾波所測位置

圖9　經(jīng)卡爾曼濾波所測速度

5結(jié)論

通過使用離散型卡爾曼濾波實(shí)現(xiàn)GPS、INS、光流的數(shù)據(jù)融合，從而對速度和位置進(jìn)行估算。實(shí)驗(yàn)結(jié)果顯示：通過光流代替低精度GPS測量得到的速度值更加準(zhǔn)確，位置和速度估算也得到了相應(yīng)的提高。

卡爾曼濾波是從傳感器測量數(shù)據(jù)與預(yù)期狀態(tài)的動態(tài)模型之間進(jìn)行權(quán)衡,這種權(quán)衡與過程噪聲和測量噪聲(測量噪聲大意味著測量數(shù)據(jù)不可靠)相關(guān)。這可以用來改善在復(fù)雜環(huán)境中數(shù)據(jù)測量的結(jié)果，如在噪聲很復(fù)雜的模型中[13]，通過改變動態(tài)的過程噪聲協(xié)方差矩陣來使測量數(shù)據(jù)變得可靠。當(dāng)然，前提是傳感器和測量過程是在理想條件下。

未來的工作包括實(shí)現(xiàn)和分析其他數(shù)據(jù)融合公式，對這3個(gè)傳感器(GPS、INS和光流)找出更佳的融合方案。

參考文獻(xiàn)：

[1]田振坤，傅鶯鶯，劉素紅，等.基于無人機(jī)低空遙感的農(nóng)作物快速分類方法[J].農(nóng)業(yè)工程學(xué)報(bào)，2013，29(7)：109-116.

[2]周志艷，臧英，羅錫文，等. 中國農(nóng)業(yè)航空植保產(chǎn)業(yè)技術(shù)創(chuàng)新發(fā)展戰(zhàn)略[J].農(nóng)業(yè)工程報(bào)，2013，292(4)：1-10.

[3]Gross J，Gu Y，Rhudy M，et al.Flight Test evaluation of sensor fusion algorithms for attitude estimation[J]. IEEE Transactions on Aerospace and Electronic Systems，2012，48(3)：2128-2139.

[4]Yoo C，Ahn I. Low cost GPS/INS sensor fusion system for UAV navigation[J].Digital Avionics Systems Conference，2003，27(10)：1-9.

[5]George M，Sukkarieh S. Tightly coupled INS/GPS with bias estimation for UAV applications[J].Proceedings of the 2005 Australian Conferences on Robotics and Automation，2005，13(4)：315-326.

[6]Ding W，Wang J，Almagbile A. Adaptive filter design for UAV navigation with GPS/INS/optical flow integration[J].International Conference on Electrical and Control Engineering，2010，73(1)：361-372.

[7]王先敏，曾慶化，熊智,等.視覺導(dǎo)航技術(shù)的發(fā)展及其研究分析[J].信息與控制，2010，39(5)：607-613.

[8]秦永元，張洪鉞，汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998：175-188.

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

[10]馬頌德，張正友. 計(jì)算機(jī)視覺—計(jì)算理論與算法基礎(chǔ)[M].北京：科學(xué)出版社，1997：52-59.

[11]于合龍，劉浩洋，蘇恒強(qiáng). 基于光流追蹤技術(shù)的變形位移測量方法[J].吉林大學(xué)報(bào)：理學(xué)版，2014，52(2)：331-335.

[12]Zing Simon，Scaramuzza Davide，Weiss Stephan，et al. MAV navigation through indoor corridors using optical flow[C]//IEEE International Conference on Robotics and Automation.Anchorage:IEEE,2010：3361-3368.

[13]Zhang Chao, Wang Daobo, Farooq M. Real-Time Tracking for Fast Moving Object on Complex Background[J].Transactions of Nanjing University of Aeronautics & Astronautics，2010，27(4)：321-325.

Data Fusion for Velocity and Position Estimation of UAV

Lv Tao，Zhang Changli，Wang Shuwen，Wang Runtao，Zhang Lingyi， Liu Chao， Luan Jiling，Zhou Yanan

(College of Electricity and Information, Northeast Agricultural University, Harbin 150030, China)

Abstract：In order to improve the estimation accuracy of UAV speed and position，and reduce the production cost of UAV，put fusion algorithm，by using the discrete Kalman filter，improve the accuracy of estimating changes of the position of the object in three-dimensional space and speed. Due to the disadvantages of large volume，high cost of the traditional inertial measurement system，and low-cost inertial measurement sensors have bigger drift，this combination of UAV aerial work environment using the global positioning system (GPS) provides the position measurement，by inertial navigation system (INS) is given by the acceleration，and optical flow sensor speed the measurement of auxiliary. The results show that the proposed algorithm.

Key words：agricatural unmanned aerial vehicle; data fusion; discrete Kalman filter； optical flow

中圖分類號：S251

文獻(xiàn)標(biāo)識碼：A

文章編號：1003-188X(2016)10-0001-05

作者簡介：呂濤(1988-),男，黑龍江佳木斯人，碩士研究生，(E-mail)348867635@qq.com。通訊作者：張長利(1957-)，男，哈爾濱人，教授，碩士生導(dǎo)師，(E-mail)zhangcl@neau.edu.cn。

基金項(xiàng)目：國家“863計(jì)劃”項(xiàng)目(AA2013102303)；黑龍江省博士后科研啟動基金項(xiàng)目(LBH-Q13022)；東北農(nóng)業(yè)大學(xué)研究生科研創(chuàng)新基金項(xiàng)目(yjscx14003)；黑龍江省自然科學(xué)基金面上項(xiàng)目(C2015006)

收稿日期：2015-09-21