惯性/光流/磁组合导航技术在四旋翼飞行器中的应用*

杨天雨， 贾文峰， 赖际舟， 邓一民

(南京航空航天大学 导航研究中心，江苏 南京 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