慣性/光流/磁組合導航技術在四旋翼飛行器中的應用*

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

(南京航空航天大學 導航研究中心，江蘇 南京 210016)

?

應用技術

慣性/光流/磁組合導航技術在四旋翼飛行器中的應用*

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

(南京航空航天大學 導航研究中心，江蘇 南京 210016)

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

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

0引言

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

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

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

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

1.1光流傳感器測速原理

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

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

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

(1)

對上式進行微分

(2)

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

(3)

(4)

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

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

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

(5)

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

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

2基于光流法輔助的非線性導航算法設計

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

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

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

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

2.2.1狀態方程的構建

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

(6)

地理系設為北東地坐標系，機體坐標系為前右下坐標系，建立系統的12維狀態變量

(7)

微小型四旋翼飛行器的系統狀態微分方程可表示為

(8)

2.2.2量測方程的構建

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

1)速度量測方程

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

(9)

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

H=pz.

(10)

導航系至機體系速度轉換矩陣為

(11)

則系統速度觀測方程可整理為

(12)

2)高度量測方程

根據四旋翼飛行器的平穩飛行假設，取超聲波傳感器的輸出作為本組合導航系統高度觀測量

z2(t)=zsonar=pz.

(13)

3)姿態量測方程

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

(14)

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

(15)

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

(16)

3實驗數據驗證

本文采用自主研發的微小型四旋翼飛行器作為實驗平臺(如圖4)，驗證慣性/光流/磁組合導航方案的實際性能[13]。與本實驗相關的機載各個傳感器性能如表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光流法輔助導航室外懸停實驗

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

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

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

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

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

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

3.2光流法輔助導航室外航路點飛行實驗

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

表2　懸停狀態下光流輔助導航方案與微慣性

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

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

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

對三組實驗中微慣性與光流輔助導航方法速度、位置誤差均值統計結果如表3。

表3　飛行狀態下光流輔助導航方案與微慣性導航方案的

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

4結論

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

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

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

參考文獻:

[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]劉建業,賈文峰,賴際舟,等.微小型四旋翼飛行器多信息非線性融合導航方法及實現[J].南京航空航天大學學報,2013,45(5):575-582.

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

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

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