INS/光流/磁强计组合导航在小型无人机中的应用

化雪荟,陈大力

(佛山职业技术学院电子信息系,广东 佛山 528000)

INS/光流/磁强计组合导航在小型无人机中的应用

化雪荟,陈大力*

(佛山职业技术学院电子信息系,广东 佛山 528000)

由于低成本的惯性导航系统存在较为严重的漂移,小型无人机在使用其导航时,往往会出现较大的误差。针对这一问题,提出了一种INS/光流/磁强计组合导航方案:基于扩展卡尔曼滤波,将INS与光流数据融合,估计无人机的速度和位置。当无人机静止或匀速运动时,将陀螺仪与加速计、磁强计的数据融合,估计无人机的姿态;当无人机加速或减速时,用陀螺仪估计无人机的姿态。利用无人机飞行数据对本文所提出的组合导航方案进行了验证,验证结果表明,该方案有效的减少导航时的速度、位置和姿态的误差,提高导航精度。

小型无人机,惯性导航,光流传感器,磁强计,扩展卡尔曼滤波

在过去的二十年间,得益于较高精度、高可靠性、低成本微型传感器的大量生产和普及,小型无人机取得了巨大发展并且也成为了研究的热点。目前已经在军事,生产和生活中取得了广泛的应用。作为无人机控制系统的核心模块,导航系统一直是无人机领域众多研究者所重点关注的问题。精确的飞行器导航信息是实现飞行器稳定控制的基础,而导航系统的功能就是为飞行器提供准确的飞行器状态信息。导航系统的传统解决方案包括:惯性导航、GPS导航、GPS/INS组合导航等。然而这些传统的导航方式,往往存在着如积分累积误差,信号失锁等问题。

近年来,国内外的一些研究人员受到昆虫利用视觉进行导航的启发,使用光流法进行小型无人机的导航[1]。国外方面,Nils Gageilk[2],Dominilk Honegger[3]等人利用光流传感器完成了无人机的定点悬停、自主驾驶以及速度估计,Mercado D A[4],Wang Jinling[5]等人进行了INS/GPS/Optical Flow的组合导航研究,可以对无人机的导航信息进行实时的估计。国内方面,吕强[6],张洪涛[7]等人也成功利用光流法进行了悬停实验,宋宇[8]利用两个光流传感器,完成了对室内小型无人机速度,位置和姿态信息的获取,但缺点是需要较强的光源,同时双光流传感器获取姿态信息时对无人机高度有一定要求。

在使用单个光流传感器进行导航时,原理上可以得到无人机的速度和位置信息,但是无法获取无人机的姿态信息。本文针对这一问题,在上述研究的基础上,提出了一种基于INS/光流/磁强计的组合导航方案。该方案利用扩展卡尔曼滤波器,将INS与光流传感器组合,对无人机的位置和速度进行估计;当无人机静止或匀速运动时,将陀螺仪与加速度计、磁强计组合,对无人机的姿态进行估计。对自主搭建的小型四旋翼无人机平台进行飞行数据采集,以验证本文所提出的组合导航方案。验证表明该方案有效的减少了惯性单元在导航时的时间累积误差,同时具有小型化、轻型化、低成本等优点。

1 速度、位置、姿态的估计原理

本节介绍通过光流传感器对无人机的速度和位置的估计方法以及加速度计结合磁强计对无人机姿态的估计方法。在进行原理介绍前,首先需要对坐标系进行定义。

1.1 光流传感器对速度、位置的估计

(1)光流的定义

光流的概念是由Gibson于1950年首先提出的,是指空间运动的物体在成像面上像素运动的瞬时速度,它表征了二维图像的灰度变化和场景中物体及其运动的关系,它根据像素灰度的时域变化和相关性确定各个像素点的运动速度,因此,可被观察者用来确定目标的运动情况[9]。

(2)光流的计算

图像处理的研究者为光流的计算提出了很多的方法,其中较为典型的有:Lucas-Kanade法,Horn-Schunck 法,图像内差值法,块匹配法,特征匹配法等。

考虑到硬件平台和计算的复杂程度,我们选择了基于最小误差绝对值和SAD(Sum of Absolute Differences)的块匹配BMA(Block Matching Algorithms)算法,进行光流的计算。该算法的大致原理和流程如图1所示[10]。

如图1所示,设xp(i,j)和xc(i+Δi,j+Δj)分别表示前一帧中选取的大小为n×n目标块的灰度值和当前帧搜索区域中待匹配目标块的灰度值,其中1≤i,j≤n;-d≤Δi,Δj≤d。基于最小绝对误差和SAD匹配准则的块匹配算法就是在搜索区域里寻找满足下式的Δi,Δj,从而获得光流矢量V=r(Δi,Δj)T,V的单位为pixel/s,r为摄像头的采集频率,单位为frame/s,而U为绝对值误差和的最小值,他们有如下的关系表达式:

(1)

U=min(Δi,Δj){SAD(Δi,Δj)}

(2)

V=r(Δi,Δj)T|U

(3)

图1 块匹配算法示意图

在初始状态时,在成像平面的原点处选取一个目标块,当无人机运动时,目标块会在后一帧图像中移动。在后一帧的搜索区域内(该搜索区域限定了两个方向的最大位移d),使当前帧的目标块和前一帧的目标块内像素灰度差的SAD最小,这样我们就可以得到目标块的光流矢量。

在实验中,采集垂直于摄像头方向的图像,并使用8位来的数据来表征每个像素的灰度。采集后的数据通过DMA存储在系统内存中。在整个采集过程中,选择8×8像素的数据块来作为块匹配的对象,搜索区域为±4个像素,所以对于每一帧图像,一共有64个像素点,同时有81个候选的向量方向。获得每一帧图像后,我们计算出各个候选向量的误差绝对值和,并选择其中的最小值作为光流失量。

(3)光流法的基本模型

光流法估计物体的运动模型本质都是将三维的运动投影到摄像头的二维像平面上。常见的光流法估计模型有两种:一种是针孔模型(Pin-hole Image Plane Approach),它来源于对脊椎动物视觉系统的仿生;另一种是球面模型(Spherical Imaging Surface Approach),它受到了昆虫复眼的启发[11]。在本文中,我们使用针孔模型,对无人机在地理坐标系下的运动进行估计。

针孔模型的示意图,如图2所示。

图2 针孔(Pin-hole)模型

设Pc=[Xc,Yc,Zc]T是在摄像头坐标系下的一个点,f代表了摄像头的焦距,于是这一点在摄像头成像平面下表示为p=[x,y,f]T,并有关系:

(4)

(5)

(6)

式中:因为从摄像头到成相平面的距离的始终为焦距,所以向量p=[x,y,f]T的第3项为常数并等于焦距。考虑地面上的任意一点P,在摄像头坐标系下P相对于无人机有如下的运动关系:

Vc=-Tc-ω×Pc

(7)

将式(7)在3个维度上展开,可以得到:

Vxc=-Txc-(ωyZc-ωzYc)

(8)

Vyc=-Tyc-(ωzXc-ωxZc)

(9)

Vzc=-Tzc-(ωxYc-ωyXc)

(10)

式中:ω=[ωx,ωy,ωz]T是无人机的角速度,Tc=[Txc,Tyc,Tzc]T是无人机在摄像头坐标系的平动速度.

对式(4)两边求导,可以得到Pc在摄像头坐标下的速度与p在成像平面下的速度的关系:

(11)

式中:v=[vx,vy,vz]T。将式(11)在3个维度上展开,可以得到:

(12)

(13)

vz=0

(14)

将式(8)～式(10)代入式(12)、式(13),可以得到:

(15)

(16)

1.2 加速度计组合磁强计对姿态的估计

加速度计通过敏感重力场,在载体不存在自身加速度的情况下,可以确定无人机的横滚角和俯仰角;磁强计敏感地磁场,借助于加速度计得到的载体姿态信息,可以解算得到无人机航向角。两者相组合,就可以得到不随时间积累误差的全姿态信息[12]。

(1)利用加速度计获取载体的横滚角和俯仰角

重力矢量在地理坐标系下的分量为[0 0 -g]T,当载体处于静止态(相对导航坐标系无加速度)时,安装在载体坐标系下的加速度计测量值为ab=[axbaybazb]T,由于重力加速度垂直于水平面,故载体的航向角不影响x向和y向加速度计的输出。于是有关系:

(17)

联立可以解得俯仰角和横滚角为:

(18)

(19)

此方法利用地球重力加速度在机体坐标系三轴上的投影信息反映载体的姿态信息,故只有在载体无加速运动的情况下上述算式才成立。而在实际系统中,载体并不是始终保持静止或匀速运动的,一旦载体有了加速运动,加速度计的测量值就不再与重力加速度在载体系三轴上的分量相等。所以该方案只能用于静态姿态测量,动态时的姿态测量还要另寻方案[13]。

(2)利用磁强计获取载体的航向角

设地磁强度在地理坐标系三轴上的分量为Hn=[HxnHynHzn]T,磁强计沿体坐标系Fb三轴方向安装,地磁强度在载体坐标系三轴上的分量为Hb=[HxbHybHzb]T,地磁强度在地理坐标系和载体坐标系各轴上的投影可由两坐标系之间的转换矩阵来表示:

(20)

式中:[HxnHynHzn]T的值通过查表给出[14],在太原地区(北纬37.8度,东经112.5度),其值如式(21)所示,而[HxbHybHzb]T的数值由机载的磁强计给出。

(21)

假定无人机在飞行过程中地磁场保持恒定,联立式(20)、式(21),再结合由加速度计确定的俯仰角和横滚角,就可以解算出无人机在地理坐标系下的航向角。

2 利用扩展卡尔曼滤波器(EKF)进行数据融合

2.1 基于扩展卡尔曼滤波的加速度计/光流传感器组合模型

本文采用扩展卡尔曼滤波将加速度计数据和光流传感器的数据进行融合。选取无人机在导航坐标系下的速度和位置信息作为状态量,光流传感器和超声波传感器的输出作为观测量,联合估计无人机的位置和速度信息。

具体的估计方法如图3所示。

图3 IMU组合光流传感器估计速度和位置

考虑非线性系统的状态方程和观测方程:

Xk+1=f[Xk,k]+BkUk+ΓkWk

(22)

Zk+1=h[Xk+1,k+1]+Vk+1

(23)

式中:Xk=[XnYnZnVxnVynVzn]T是状态向量,它包括无人机的速度信息和位置信息;Zk=[xyZc]T是观测向量,它包括光流传感器输出的x方向光流和y方向光流和超声波传感器的读数Zc。Uk=[0 0 0axnaynazn]T是系统的控制向量,由加速度计的数值经过坐标系矩阵变换得出;Bk是控制分配矩阵;Γk是噪声分配矩阵;Wk是过程噪声矩阵;Vk是测量噪声;f代表系统状态函数;h代表观测函数。

将状态方程和观测方程代入扩展卡尔曼滤波器,可以得到:

(24)

(25)

(26)

(27)

Pk+1= (I-Kk+1Hk+1)Pk+1/k(I-Kk+1Hk+1)-1+

(28)

通过上述扩展卡尔曼滤波过程,可以得到在地理坐标系下对无人机的速度和位置信息。

图4 加速度计组合磁强计确定姿态

2.2 基于扩展卡尔曼滤波的陀螺/加速度计/磁强计组合模型

通过陀螺输出角速率信号积分可以获取无人机的姿态,但是MEMS陀螺仪存在较为严重的漂移问题,而加速度计/磁力计组合能提供不同噪声、无漂移的姿态,所以利用扩展卡尔曼滤波器去融合各个传感器中的数据。滤波过程如图4所示,系统状态向量可表示为Xk=[q0q1q2q3ωxωyωz]T,其中[q0q1q2q3]T为系统的状态四元数,可以通过四阶龙格库塔法[15]求出,[ωxωyωz]T为陀螺仪的输出值。

系统的观测向量可以表示为Zk=[q0q1q2q3]T,其中的4个四元数可以利用加速度计和磁强计的读数结合通过高斯牛顿法[16]求出。

图6 实验平台搭建

2.3 INS/光流/磁强计组合导航流程

利用扩展卡尔曼滤波对加速度计和光流的组合,无论是在静止状态还是运动状态下,都可以对无人机的运动速度和位置进行有效的估计。根据陀螺仪和加速度计/磁强计测量姿态的特点,当载体静止或匀速运动的时候,利用扩展卡尔曼滤波定时对陀螺仪积分得到的姿态进行修正;而当检测到无人机在进行加速、减速或高速转动时,就不再进行姿态修正,而是在前一步更新后的姿态基础上用陀螺仪数据进行捷联解算,直到载体恢复静止或匀速状态。整个导航的流程如图5所示。

图5 INS/光流/磁强计组合导航流程

3 实验结果与分析

3.1 实验平台简介

如图6所示,在实验验证过程中使用了自主搭建的四旋翼飞行器平台(图6(a))。飞行控制处理器采用了意法半导体的STM32F103Z(图6(b)),光流传感器选择了3DRobotics公司生产的PX4FLOW(图6(c)),IMU选择了MPU6050(图6(d)),三轴磁强计选择了霍尼韦尔公司的HMC3883L(图6(d)),飞行器的电机选择了大疆公司生产的DJ1 2312(图6(e))。此外还使用了HC-12无线串口模块(图6(f)),可以将机载的传感器数据实时传回到PC端。

3.2 实验结果

实验过程中,无人机沿正北方向飞行,在X方向基本保持恒定,Y方向前进了大约50 m,飞行高度控制在1 m。无人机的3个姿态角均使用PID算法调节在10°之内。用MATLAB将采集到的数据进行作图,并对比了仅使用惯导单元时的情况和使用EKF组合算法时情况,可得如图7～图9所示结果。

通过实验数据可以得出:

(1)在速度估计和位置估计部分,光流与加速度计组合扩展卡尔曼滤波器可以有效的降低加速度计的随机漂移误差以及由于时间累积而产生的积分误差。例如图8所示,因为本文使用的是低成本的MEMS加速度计,如果加速度值不经过滤波和补偿,在大约50 m的行驶路程中,就出现了近8 m的时间累积误差,而使用组合算法后,所产生的误差不超过1 m;

(2)在姿态估计部分,在静态和匀速运动阶段滤波器的输出可以很好的跟踪3个姿态角,很大程度的减少了由于陀螺常值漂移和随机漂移产生的误差。并且可以发现在加速运动结束后,本文提出的组合方法可以及时的修正航向。

图8 位置估计结果

图7 速度估计结果

图9 姿态估计结果

4 结论

针对小型无人机在飞行过程的导航问题,本文提出了一种INS/光流/磁强计组合导航的方案。该组合导航方案基于扩展卡尔曼滤波的多传感器信息融合,可以对小型无人机进行位置,速度和姿态的估计。实验表明:该方案可以有效提高小型无人机的导航性能,相比于单纯的捷联惯性导航可以显著减少导航时的位置,速度和姿态误差,具有工程应用的价值。

[1] Chao Haiyang,Cao Yongcan,Chen Y. Autopilots for Small Unmanned Aerial Vehicles:A Survey[J]. International Journal of Control,Automation and Systems,2010,8(1):36-44.

[2] Gageik N,Strohmeier M,Montenegro S. An Autonomous UAV with an Optical Flow Sensor for Positioning and Navigation[J]. International Journal of Advanced Robotic Systems,2013,10(4):301-319.

[3] 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]//IEEE International Conference on Robotics and Automation. IEEE,2013:1736-1741.

[4] Mercado D A,Flores G,Castillo P,et al. GPS/INS/Optic Flow Data Fusion for Position and Velocity Estimation[C]//International Conference on Unmanned Aircraft Systems. IEEE,2013:486-491.

[5] Ding W,Wang J,Almagbile A. Adaptive Filter Design for UAV Navigation with GPS/INS/Optic Flow Integration[J]. International Conference on Electrical and Control Engineering,2010:4623-4626.

[6] 吕强,倪佩佩,王国胜,等. 基于光流传感器的四旋翼飞行器悬停校正[J]. 装甲兵工程学院学报,2014(3):68-72.

[7] 张洪涛,张广玉,李隆球,等. 微型二维光流传感器设计[J]. 哈尔滨工程大学学报,2014,(5):619-623.

[8] 宋宇,翁新武,郭昕刚. 基于光流和惯性导航的小型无人机定位方法[J]. 传感器与微系统,2015,34(1)13-16.

[9] KOENDERINKJ. Optic Flow[J]. Vision Research,1986,26(1):161-180.

[10] Lan H,Mei S J,Hua C P,et al. Visual Navigation for UAV Using Optical Flow Estimation[C]//Control Conference. IEEE,2014:816-821.

[11] Chao H,Gu Y,Napolitano M. A Survey of Optical Flow Techniques for UAV Navigation Applications[C]//International Conference on Unmanned Aircraft Systems. IEEE,2013:710-716.

[12] 穆振兴. 无人机姿态测量系统设计实现[D]. 哈尔滨:哈尔滨工业大学,2009.

[13] 黄旭,王常虹,伊国兴,等. 利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J]. 中国惯性技术学报,2005,13(2):27-30.

[14] 赵捍东,曹红松,朱基智,等. 基于磁强计和陀螺的姿态测量方法[J]. 中北大学学报(自然科学版),2010,31(6):631-635.

[15] 张春慧,吴简彤,何昆鹏,等. 四阶龙格-库塔法在捷联惯导系统姿态解算中的应用[J]. 声学与电子工程,2005,32(1):37-38.

[16] 宋静. 基于磁强计、加速度计和陀螺原理的姿态测试算法研究[D]. 太原:中北大学,2011.

ApplicationofINS/OpticalFlow/MagnetometerIntegratedNavigationSysteminSmallUAV

HUAXuehui,CHENDali*

(Department of Electronic Information,Foshan Polytechnic Institute,Foshan Guangdong 528000,China)

The drift of inertial navigation system(INS)will lead to large navigation error when a low-cost INS is used in small UAV. To overcome the above problem,an INS/Optical Flow/Magnetometer integrated Navigation scheme is proposed. The scheme,which is based on the extended Kalman filter,combines INS and Optical Flow information to estimate the velocity and position of UAV,and combines gyro,accelerator and magnetometer information to estimate the UAV attitude when the UAV is at rest or in uniform motion,and uses gyro to estimate the UAV attitude when the UAV is accelerating or decelerating. The UAV flight data is used to verify the proposed integrated navigation scheme,and the verification results show that the proposed scheme can effectively reduce the errors of navigation parameters and improve navigation precision.

UAV;INS;optical flow;magnetometer;EKF

10.3969/j.issn.1005-9490.2017.06.017

2016-09-01修改日期2017-02-24

V279

A

1005-9490(2017)06-1414-07

化雪荟(1976-),女,甘肃景泰人,硕士,高级实验师,主要从事应用电子设计、移动通信技术、传感器应用方向的研究;

陈大力(1975-),男,吉林四平人,副教授,主要从事工业控制、电子设计方面研究。