慣性/光流/磁組合導(dǎo)航技術(shù)在四旋翼飛行器中的應(yīng)用*

楊天雨， 賈文峰， 賴際舟， 鄧一民

(南京航空航天大學(xué) 導(dǎo)航研究中心，江蘇 南京 210016)

?

應(yīng)用技術(shù)

慣性/光流/磁組合導(dǎo)航技術(shù)在四旋翼飛行器中的應(yīng)用*

楊天雨， 賈文峰， 賴際舟， 鄧一民

(南京航空航天大學(xué) 導(dǎo)航研究中心，江蘇 南京 210016)

摘要:微小型無(wú)人飛行器(MUAV)通常采用微慣性/GPS組合導(dǎo)航系統(tǒng)提供載體速度、位置、姿態(tài)等參數(shù)。然而GPS信號(hào)易受干擾，且面對(duì)特殊環(huán)境(如高樓林立的街道、隧道、峽谷等)易出現(xiàn)信號(hào)丟失情況，難以滿足飛行器的自主安全飛行要求。為解決該問(wèn)題，設(shè)計(jì)基于光流法輔助微慣性導(dǎo)航的無(wú)GPS自主導(dǎo)航方案，提出了一種基于無(wú)味卡爾曼濾波(UKF)的非線性融合導(dǎo)航方法，基于四旋翼飛行器的懸停和飛行實(shí)驗(yàn)進(jìn)行了驗(yàn)證。實(shí)驗(yàn)結(jié)果表明：該方案具有成本低、導(dǎo)航自主性強(qiáng)、精度高的優(yōu)點(diǎn)，具有一定的應(yīng)用參考價(jià)值。

關(guān)鍵詞：慣性/光流/磁組合； 多傳感器； 信息融合； 無(wú)味卡爾曼濾波

0引言

光流(optical-flow，OF)的概念最早來(lái)自于昆蟲(chóng)在運(yùn)動(dòng)過(guò)程中，四周環(huán)境的灰度模式在視網(wǎng)膜(即成像面)上形成一系列連續(xù)變化的圖像，產(chǎn)生了像素強(qiáng)度的時(shí)域變化[1]。光流傳感器根據(jù)光流成像原理制作，集圖像采集與圖像分析處理功能于一體，利用視覺(jué)傳感器獲取運(yùn)動(dòng)中的物體圖像[2]，并由內(nèi)嵌的光流算法得到載體的相應(yīng)導(dǎo)航參數(shù)。

借助于光流傳感器的光流法導(dǎo)航不需要提前預(yù)知周邊環(huán)境特征，具有實(shí)時(shí)性高、自主性強(qiáng)等特點(diǎn)[3,4]，克服了傳統(tǒng)微慣性導(dǎo)航系統(tǒng)對(duì)GPS的嚴(yán)重依賴。但由于其輸出的速度信息僅有二維，無(wú)法滿足六自由度導(dǎo)航參數(shù)需求。

本文在MEMS微慣性導(dǎo)航系統(tǒng)[5]基礎(chǔ)上，提出一種慣性/光流/磁傳感器數(shù)據(jù)融合方案。針對(duì)四旋翼飛行器的系統(tǒng)非線性強(qiáng)的特點(diǎn)，采用無(wú)跡卡爾曼濾波(unscented Kalman filtering,UKF)方法實(shí)時(shí)更新系統(tǒng)非線性狀態(tài)并量測(cè)，提高四旋翼飛行器在無(wú)GPS條件下的測(cè)速和定位能力[6]?；趯?shí)際四旋翼飛行器的實(shí)驗(yàn)表明，該融合方案能提供較為準(zhǔn)確的載體速度、位置信息，滿足四旋翼飛行器在無(wú)GPS或GPS緊急失效情況下所需的導(dǎo)航精度和可靠性要求。

1光流法測(cè)速原理與檢測(cè)算法

1.1光流傳感器測(cè)速原理

相機(jī)與實(shí)際物體運(yùn)動(dòng)的三維幾何透視關(guān)系如圖1所示。以相機(jī)透鏡光學(xué)孔徑點(diǎn)為坐標(biāo)系中心原點(diǎn)C，按右手系原則建立三維空間坐標(biāo)系(X,Y,Z)，Z軸為光流軸心線。相機(jī)的成像面垂直于Z軸并以二者交點(diǎn)為原點(diǎn)，成像面至透鏡中心原點(diǎn)C的距離即為相機(jī)的焦距f。

圖1　光流場(chǎng)成像模型Fig 1　Imaging model of OF field

空間中的某點(diǎn)P(X,Y,Z)與成像面對(duì)應(yīng)像素點(diǎn)p(x,y,f)具有如下關(guān)系

(1)

對(duì)上式進(jìn)行微分

(2)

式中vflow為像素點(diǎn)的二維光流速度矢量，V表示空間中的物體實(shí)際運(yùn)動(dòng)速度。將上式詳細(xì)展開(kāi)后可得

(3)

(4)

其中，T為P點(diǎn)的線運(yùn)動(dòng)速度，ω為P點(diǎn)的角運(yùn)動(dòng)速度。

1.2光流絕對(duì)偏差總和檢測(cè)算法

為得到像素點(diǎn)光流速度，以基于匹配原則的絕對(duì)偏差總和(sum of absolute differences，SAD)算法對(duì)圖像進(jìn)行分析處理。SAD算法表達(dá)式如下[7]

(5)

假設(shè)fk(m,n)為第k幅圖像中(m,n)處的像素灰度值，fk+1(m+i,n+j)為k+1圖像中(m+i,n+j)處的像素灰度值，M×N為搜索區(qū)域范圍。當(dāng)判定兩塊像素灰度差異最小后，即可認(rèn)為t時(shí)刻(m,n)處的像素點(diǎn)在t+Dt時(shí)刻運(yùn)動(dòng)至(m+i,n+j)處，而(i,j)即為采樣周期Δt時(shí)間內(nèi)像素點(diǎn)的位移矢量，如圖2所示。

圖2　光流SAD搜索原理Fig 2　SAD-based OF searching principle

2基于光流法輔助的非線性導(dǎo)航算法設(shè)計(jì)

2.1微小型飛行器中的UKF融合方案設(shè)計(jì)

針對(duì)微小型四旋翼飛行器系統(tǒng)非線性強(qiáng)的特點(diǎn)[8～10]，本文采用UKF技術(shù)來(lái)建立四旋翼多傳感器非線性融合方案[11]，按直接卡爾曼濾波(DKF)原則設(shè)計(jì)組合導(dǎo)航系統(tǒng)非線性濾波器，在保證濾波器結(jié)構(gòu)簡(jiǎn)單的同時(shí)提高計(jì)算效率。該融合方案的系統(tǒng)結(jié)構(gòu)設(shè)計(jì)如圖3。

圖3　慣性/光流/磁組合導(dǎo)航方案Fig 3　Inertial/OF/magnetic integrated navigation scheme

2.2慣性/光流/磁傳感器多信息非線性融合算法

2.2.1狀態(tài)方程的構(gòu)建

低精度MEMS慣性器件的隨機(jī)噪聲表現(xiàn)為非平穩(wěn)噪聲[12]。為簡(jiǎn)化計(jì)算，取加表誤差為白噪聲，陀螺誤差為隨機(jī)游走和白噪聲,即有

(6)

地理系設(shè)為北東地坐標(biāo)系，機(jī)體坐標(biāo)系為前右下坐標(biāo)系，建立系統(tǒng)的12維狀態(tài)變量

(7)

微小型四旋翼飛行器的系統(tǒng)狀態(tài)微分方程可表示為

(8)

2.2.2量測(cè)方程的構(gòu)建

在慣性/光流/磁組合導(dǎo)航方案中，共設(shè)置以下三組觀測(cè)量：第一組為速度觀測(cè)量，建立光流速度與系統(tǒng)狀態(tài)的線性關(guān)系；第二組為超聲波傳感器高度觀測(cè)量；第三組為加速度計(jì)與磁強(qiáng)計(jì)觀測(cè)量，利用地理系的重力矢量和磁北矢量對(duì)載體的姿態(tài)矩陣進(jìn)行量測(cè)。

1)速度量測(cè)方程

對(duì)光流場(chǎng)速度模型表達(dá)式(3)、式(4)進(jìn)行簡(jiǎn)化并忽略高階項(xiàng)

(9)

假設(shè)微小型四旋翼飛行器處于平穩(wěn)飛行狀態(tài)，即橫滾角與俯仰角都較小,則光流傳感器至地面的高度H為

H=pz.

(10)

導(dǎo)航系至機(jī)體系速度轉(zhuǎn)換矩陣為

(11)

則系統(tǒng)速度觀測(cè)方程可整理為

(12)

2)高度量測(cè)方程

根據(jù)四旋翼飛行器的平穩(wěn)飛行假設(shè)，取超聲波傳感器的輸出作為本組合導(dǎo)航系統(tǒng)高度觀測(cè)量

z2(t)=zsonar=pz.

(13)

3)姿態(tài)量測(cè)方程

采用加速度計(jì)重力矢量量測(cè)的方程為

(14)

其中，gn=[00g]T為地理系下重力加速度矢量，vf為加速度計(jì)量測(cè)噪聲,采用磁傳感器的磁北矢量量測(cè)方程為

(15)

式中mn為地理系下的地磁場(chǎng)矢量，vm為磁強(qiáng)計(jì)量測(cè)噪聲,則系統(tǒng)姿態(tài)量測(cè)方程可整理為

(16)

3實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證

本文采用自主研發(fā)的微小型四旋翼飛行器作為實(shí)驗(yàn)平臺(tái)(如圖4)，驗(yàn)證慣性/光流/磁組合導(dǎo)航方案的實(shí)際性能[13]。與本實(shí)驗(yàn)相關(guān)的機(jī)載各個(gè)傳感器性能如表1。

圖4　四旋翼飛行器與地面站Fig 4　Quadrotor helicopter and ground station

傳感器類別型號(hào)性能慣性傳感器MPU—60000.05(°)/s0.05gn磁傳感器HMC5983航向精度1°超聲波傳感器HRLV—EZ40.01m光流傳感器PX4_Flow精度1cm/s,方差3σ=1cmGPS接收機(jī)LEA—6H2.5m(CEP)

3.1光流法輔助導(dǎo)航室外懸停實(shí)驗(yàn)

為對(duì)光流輔助導(dǎo)航方案做全面的測(cè)試，考慮四旋翼飛行器的懸停與航路點(diǎn)飛行兩種情況，以GPS位置作為航跡基準(zhǔn)。

在飛行器的定高定點(diǎn)懸停實(shí)驗(yàn)中，采集3組四旋翼飛行器于室外懸停狀態(tài)下的各傳感器數(shù)據(jù)。每組懸停實(shí)驗(yàn)時(shí)間約為1 min，懸停高度為2 m。其中，某組飛行實(shí)驗(yàn)軌跡如圖5所示。

圖5　四旋翼飛行器室外懸停實(shí)驗(yàn)軌跡圖Fig 5　Track of quadrotor helicopter outdoor hoveringflight experiment

將光流輔助導(dǎo)航結(jié)果與由IMU數(shù)據(jù)計(jì)算得到的純慣性導(dǎo)航結(jié)果分別與差分GPS基準(zhǔn)航跡作對(duì)比，得到飛行器測(cè)速誤差如圖6所示。

圖6　光流輔助導(dǎo)航與微慣性導(dǎo)航的速度誤差結(jié)果對(duì)比Fig 6　Velocity error result comparison between OF-aidednavigation and MEMS-inertial navigation

三組懸停實(shí)驗(yàn)中四旋翼飛行器的速度、位置誤差均值統(tǒng)計(jì)結(jié)果見(jiàn)表2。

3.2光流法輔助導(dǎo)航室外航路點(diǎn)飛行實(shí)驗(yàn)

在航路點(diǎn)飛行模式下，設(shè)定四旋翼飛行器按照矩形軌跡飛行，并同樣采集三組傳感器數(shù)據(jù)。每組實(shí)驗(yàn)的時(shí)長(zhǎng)約為60 s，飛行速度約為1.5 m/s，懸停高度為2 m。其中，某組飛行實(shí)驗(yàn)軌跡如圖7所示。

表2　懸停狀態(tài)下光流輔助導(dǎo)航方案與微慣性

圖7　四旋翼飛行器室外航路點(diǎn)飛行實(shí)驗(yàn)軌跡圖Fig 7　Track of quadrotor helicopter outdoor waypointflight experiment

由于四旋翼飛行狀態(tài)下微慣性傳感器誤差發(fā)散速度比懸停狀態(tài)下更快，因此,省略對(duì)微慣性導(dǎo)航方案的誤差統(tǒng)計(jì)，僅列出光流輔助導(dǎo)航方法的載體速度誤差,如圖8。

圖8　光流輔助導(dǎo)航方案速度誤差Fig 8　Velocity error of OF-aided navigation scheme

對(duì)三組實(shí)驗(yàn)中微慣性與光流輔助導(dǎo)航方法速度、位置誤差均值統(tǒng)計(jì)結(jié)果如表3。

表3　飛行狀態(tài)下光流輔助導(dǎo)航方案與微慣性導(dǎo)航方案的

由上述實(shí)驗(yàn)結(jié)果可知,懸停狀態(tài)下，光流輔助導(dǎo)航方案的定位誤差在0.5 m以內(nèi)，而速度誤差在0.15 m/s內(nèi)。在實(shí)驗(yàn)的60 s時(shí)間內(nèi)，比微慣性導(dǎo)航方案誤差精度提高1個(gè)數(shù)量級(jí)以上；在航跡飛行狀態(tài)下，光流輔助導(dǎo)航方案的定位誤差在2.5 m以內(nèi)，測(cè)速誤差在0.2 m/s內(nèi)。在實(shí)驗(yàn)的60 s時(shí)間內(nèi)，比微慣性導(dǎo)航方案誤差精度提高2個(gè)數(shù)量級(jí)以上。

4結(jié)論

1)采用光流輔助導(dǎo)航方案的四旋翼飛行器測(cè)速、定位精度相對(duì)于微慣性導(dǎo)航方案有很大提高。

2)懸停狀態(tài)下的光流輔助導(dǎo)航方案測(cè)速、定位精度高于航跡點(diǎn)飛行狀態(tài)。造成該現(xiàn)象的原因在于動(dòng)態(tài)情況下光流圖像噪聲增大，降低了光流法導(dǎo)航精度。

3)在室內(nèi)無(wú)GPS情況下可采用光流輔助導(dǎo)航方案提供的載體速度、位置等導(dǎo)航信息。而在室外實(shí)際飛行時(shí)，光流輔助導(dǎo)航方案同樣可以滿足四旋翼飛行器的導(dǎo)航精度需求，保證四旋翼飛行器GPS信號(hào)中斷情況下一定時(shí)間內(nèi)正常工作。

參考文獻(xiàn):

[1]Fumiya Iida.Biologically inspired visual odometer for navigation of a flying robot[J].Robotics and Autonomous Systems,2003,44(3):201-208.

[2]Chao Haiyang,Gu Yu,Gross Jason,et al.A comparative study of optical flow and traditional sensors in UAV navigation[C]∥American Control Conference(ACC),Washington DC,2013:3858-3863.

[3]Yang Dongxiao,Li Jie,Xiong Kun,et al.A real-time optical flow computation method used in vision-based navigation control[J].Energy Procedia,2011,13:3396-3402.

[4]Franck Ruffier,Nicolas Franceschini.Optic flow regulation:The key to aircraft automatic guidance[J].Robotics and Autonomous Systems,2004,50(4):177-194.

[5]劉建業(yè),賈文峰,賴際舟,等.微小型四旋翼飛行器多信息非線性融合導(dǎo)航方法及實(shí)現(xiàn)[J].南京航空航天大學(xué)學(xué)報(bào),2013,45(5):575-582.

[6]劉小明,陳萬(wàn)春.光流/慣導(dǎo)多傳感器信息融合方法[J].北京航空航天大學(xué)學(xué)報(bào),2012,38(5):620-624.

[7]鄧毅,林學(xué)訚.一種新的快速立體視覺(jué)導(dǎo)航算法[J].電子學(xué)報(bào),2006(11):2090-2093.

[8]Bouabdallah S.Design and control of quadrotors with application to autonomous flying[D].Swiss:Federal Institute of Technology in Lausanne,2007.

[9]Nemra A,Aouf N.Robust INS/GPS sensor fusion for UAV localization using SDRE nonlinear filtering[J].IEEE Sensors Journal,2010,10(4):789-798.

[10] Vlatko Becˇanovic＇，Takayuki Matsuo,Alan A Stocker.An embedded vision system based on an analog VLSI optical flow vision sen-sor[J].International Congress Series,2006,1291:249-252.

[11] Lai Jizhou,Lü Pin,Liu Jianye,et al.Noncommutativity error analysis of strapdown inertial navigation system under the vibration in UAVs[J].International Journal of Advanced Robotic System,2012,9:136-144.

[12] Unhelkar V V,Perez J,Boerkoel J C,et al.Towards control and sensing for an autonomous mobile robotic assistant navigating assembly lines[C]∥2014 IEEE International Conference on Robotics and Automation(ICRA),IEEE,2014:4161-4167.

[13] 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]∥Proc of IEEE Int’l Conf on Robotics and Automation(ICRA),2013:1736-1741.

楊天雨(1990-)，女，江蘇南京人，碩士研究生，主要從事微小型無(wú)人機(jī)多傳感器信息融合導(dǎo)航研究、兵器工程技術(shù)。

Application of inertial/optical-flow/magnetic integrated navigation technique in quadrotor helicopter*

YANG Tian-yu， JIA Wen-feng， LAI Ji-zhou， DENG Yi-min

(Navigation Research Center,Nanjing University of Aeronautics and Astronautics, Nanjing 210016,China)

Abstract:MEMS-inertial/GPS integrated navigation system is usually used in MUAV to provide parameters of its velocity,position,attitude.However,GPS is susceptible to interference in complex environment,such as high-rise street,tunnels,canyon,etc,which will not satisfy the safety needs of autonomous flight.To solve this problem, a MEMS-inertial/optical-flow autonomous navigation scheme without GPS and analyzed based on a non-linear fused navigation method based on unscented Kalman filtering(UKF)fusion technology is proposed,hovering and waypoint flight experiments of quadrotor helicopter are conducted to verify the system performance.Experimental results show that the inertial/optical-flow/magnetometer integrated system has advantages such as low-cost,strong autonomy of navigation and high precision,which implies practical application value of MUAV.

Key words:inertial/optical-flow/magnetic integrated; multi-sensors; information fusion; unscented Kalman filtering(UKF)

作者簡(jiǎn)介:

中圖分類號(hào)：V 249

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

文章編號(hào)：1000—9787(2016)01—0156—05

DOI:10.13873/J.1000—9787(2016)01—0156—05