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

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

?

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

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

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

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

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

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

0引言

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

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

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

1光流法測速原理與檢測算法

1.1光流傳感器測速原理

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

圖1　光流場成像模型Fig 1　Imaging model of OF field

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

(1)

對上式進行微分

(2)

式中vflow為像素點的二維光流速度矢量，V表示空間中的物體實際運動速度。將上式詳細展開后可得

(3)

(4)

其中，T為P點的線運動速度，ω為P點的角運動速度。

1.2光流絕對偏差總和檢測算法

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

(5)

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

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

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

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

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

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

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

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

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

(6)

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

(7)

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

(8)

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

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

1)速度量測方程

對光流場速度模型表達式(3)、式(4)進行簡化并忽略高階項

(9)

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

H=pz.

(10)

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

(11)

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

(12)

2)高度量測方程

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

z2(t)=zsonar=pz.

(13)

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

采用加速度計重力矢量量測的方程為

(14)

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

(15)

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

(16)

3實驗數(shù)據(jù)驗證

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

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

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

3.1光流法輔助導(dǎo)航室外懸停實驗

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

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

圖5　四旋翼飛行器室外懸停實驗軌跡圖Fig 5　Track of quadrotor helicopter outdoor hoveringflight experiment

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

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

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

3.2光流法輔助導(dǎo)航室外航路點飛行實驗

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

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

圖7　四旋翼飛行器室外航路點飛行實驗軌跡圖Fig 7　Track of quadrotor helicopter outdoor waypointflight experiment

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

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

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

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

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

4結(jié)論

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

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

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

參考文獻:

[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)航方法及實現(xiàn)[J].南京航空航天大學(xué)學(xué)報,2013,45(5):575-582.

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

[7]鄧毅,林學(xué)訚.一種新的快速立體視覺導(dǎo)航算法[J].電子學(xué)報,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-)，女，江蘇南京人，碩士研究生，主要從事微小型無人機多傳感器信息融合導(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)

作者簡介:

中圖分類號：V 249

文獻標識碼：A

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

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