基于光流传感器的位置信息融合方法

卢艳军，张 前，张晓东，刘 冬

(1.沈阳航空航天大学自动化学院，辽宁 沈阳 110136；2.辽宁通用航空研究院，辽宁 沈阳 110136)

0 引言

飞行器位置信息的获取需要相关的传感器测量和相关信息计算，光流传感器和加速度计可以测量飞机的飞行速度，通过积分计算速度信息，得到飞机的位置信息[1]。超声波传感器垂直安装在机身下方，可以直接测量到一个飞机高度，全球定位系统(GPS)是通过太空中的卫星系统，直接给出飞行器的地理位置信息[2]。

目前，无人机主要是利用GPS传感器获取位置信息，但无人机在近地面空间飞行时，受建筑物、树木对GPS信号的遮挡，且GPS传感器本身误差较大，无法获得准确的定位信息[3]。国内外研究人员将四旋翼飞行器的GPS传感器提供的数据与惯性测量单元(IMU)输出的数据进行融合，得到位置信息[4]，此位置信息相较于单一的GPS传感器较精准；但在GPS信号变弱甚至丢失时，且在飞行过程中IMU会产生积分累积误差，因此融合后的位置信息有局部震荡，造成位置信息的不可靠，影响飞行器飞行效果[5]。文献[6]抛弃GPS传感器使用光流传感器辅助惯性单元测得飞行器的速度，实现了飞行器定点悬停。文献[7]通过光流传感器采集数据设计了基于扩展Kalman滤波(EKF)的导航算法，实现了飞行器在无GPS环境下的自主导航，但由于缺少GPS，无人机在高空机动飞行的情况下，相关传感器性能下降，导致飞行控制的精度降低[8]。

目前，基于GPS/IMU的位置信息融合方法存在积分累积误差，且由外界因素导致飞行器上的GPS传感器的信号变弱甚至丢失时，产生位置信息误差过大。本文针对此问题，提出基于光流传感器的位置信息融合方法。

1 IMU与光流传感器分析

1.1 IMU中加速度计的数据输出模型

本文基于自主研发的IMU进行相关研究。IMU由MPU6050(加速度计、陀螺仪)、HMC5883L(磁力计)组成[9]。主要使用加速度计测得的速度信息。

加速度计数据输出：

(1)

(2)

1.2 光流传感器工作原理

采用的光流传感器为PX4LOW型号，利用SAD块匹配算法，其原理是以一定速率连续采集物体表面图像，由于相邻的两幅图像总会存在相同的特征，通过对比这些特征点的位置变化信息，便可以判断出物体表面特征的平均运动，最终得到无人机相对于地面的水平速度(Vfx,Vfy)。

1) 采用SAD块匹配算法[11]，并选择连续2帧图像中的最佳匹配块

(3)

假设fk(m,n)为第k幅图像中(m，n)处的像素灰度值，fk+1(m+i,n+j)为k+1图像中(m+1，n+1)处的像素值，m×n为搜索范围。当寻找到两块像素灰度差异最小后，即可认为t时刻(m，n)处像素点在t+Δt时刻运动(m+i，n+j)处，而(i，j) 即为Δt时间像素点的偏移值。

2) 旋转补偿

飞行器位置固定不变，飞行器做横滚或俯仰运动时，绕机体坐标系x轴或y轴的任何旋转，导致姿态有小范围的抖动，光流检测到数据的变化[12]。而飞行器绕z轴旋转时，由于光流围绕着图像中心旋转将被平均，导致光流为零，所以绕z轴旋转不用进行补偿。

利用陀螺仪测得的角速度ωx，ωy，ωz和摄像头的焦距f对光流传感器进行补偿，分别得到图像水平方向的光流分量ux,uy。

(4)

式(4)中，T=[Tx,Ty,Tz]为坐标系变换矩阵。x，y分别为x和y方向的像素偏移量，z为高度。

3) 尺度放缩

根据投影公式计算得到飞行器的水平移动速度(Vfx,Vfy)：

(5)

式(5)中，z为高度。

2 基于光流传感器的位置信息融合方法

2.1 扩展Kalman滤波器

扩展Kalman滤波采用高斯系统的最优无偏估计，该算法得到结果的方差是最小的，相较于梯度下降算法，互补滤波算法效果更佳[13]。扩展Kalman滤波器是利用当下时刻的几个相关数据估计下一时刻系统的状态。在本文中，需要信息融合的数据有光流传感器的数据、惯性器件的输出数据、GPS的数据和超声波传感器的数据。

扩展Kalman滤波器系统模型：

(6)

上述表达式中，Xk与Xk-1表示k，k-1时的状态向量，Φk,k-1是状态Xk-1从k-1到k的转换矩阵，Zk是观测矩阵，观测量为惯性单元的各传感器输出量，如GPS和光流传感器和超声波提供的输出数据，式(6)中，Hk表示为量测转移矩阵，测量噪声矩阵Vk，系统状态噪声Wk-1，由各个参量组成的Kalman滤波器基本方程组[14]，如式(7)所示：

(7)

2.2 传统GPS/IMU算法

建立GPS/IMU组合模型的状态方程[15]，首先需要知道估计的IMU导参数系统的状态方程：

Xt=[δVxδVyδVzφzφyφzδLδλδhεxbεyoεzbVa]，

(8)

式(8)中，δVx，δVy，δVz表示的是在三个不同方向上的速度误差；φz，φy，φz是姿态角误差；εxb，εyo，εzb是陀螺仪误差；IMU解算的速度信息为Va=(Vx,Vy,Vz)。

GPS的实时带有误差观测值：

(9)

式(9)中，LG来自GPS的接收到的纬度数据，λG是经度信息，hG接收机得到实时高度数据，NG，NY，Nh是噪声数据。其中的观测值是利用GPS所测量的位置值，因此位置观测方程为：

(10)

因此，系统测量位置方程为：

Zp(t)=Hp(t)X(t)+Vv(t)。

(11)

通过上述表达式可以推算出测量噪声矩阵Vv(t)以及测量矩阵Hp(t)为：

(12)

将式(12)代入式(11)，即可得出飞行器飞行过程中位置信息Xt。

2.3 基于光流传感器的位置信息融合改进方法

2.3.1光流传感器与加速度计融合

IMU中的加速度计测出的速度含有积分累计误差，且加速度计动态特性较差，在高频段易受到干扰，产生高频噪声，光流传感器的高频段动态响应特性好，即短时间内的数据稳定可靠，利用光流测得的速度与IMU测得的速度进行互补滤波融合，能有效地解决积分累计误差的问题，融合前后效果如图1和图2所示。

图1 加速度计测得的速度波形图Fig.1 Velocity waveform measured by accelerometer

图2 融合后的速度波形图Fig.2 Velocity waveform after fusion

2.3.2基于光流传感器的位置信息融合方法

本文把光流传感器测得的速度信息和加速度计测得的速度信息先进行融合得到一个融合速度，再将融合后的速度与超声波传感器所测得的高度信息作为扩展Kalman滤波器中的预测量，然后选取GPS所提供的水平位置信息以及高度信息作为观测量，最后通过扩展Kalman滤波器融合得到水平位置信息和高度信息，从而估计无人机的位置信息。改进方法原理如图3 所示。

图3 基于光流传感器的位置信息融合改进方法框图Fig.3 Block diagram of improved method of position information fusion based on optical flow sensor

此系统的状态方程和观测方程可列为：

(13)

式(13)中，Bk为控制分配矩阵，Γk为噪声分配矩阵，Wk为过程噪声矩阵，Vk为测量噪声，F为系统状态函数，h为观测函数。

此方法的状态量设为：

(14)

式(14)中，Vfx和Vfy表示无人机的光流传感器在x方向和在y方向上测得的速度信息；Zc为超声波传感器的读数；加速度计速度信息Va=(Vx,Vy,Vz)。

由此观测向量可设为：

(15)

观测向量中的各个元素分别表示的是GPS所提供位置信息Xn,Yn,Zn。

将状态方程和观测方程代入扩展的Kalman滤波器中可得：

(16)

式(16)中，

采用上述EKF法，可以求出X(t)=(Xt,Yt,Zt)，即可得无人机的位置信息。

3 实验仿真分析

3.1 实验平台简介

基于自主研发的四旋翼飞行器平台进行实验验证，如图4所示。飞控处理器选用Pixhawk乐迪版，光流传感器选开源的，型号为PX4FLOW，IMU型号选的是MPU6050 ，GPS选用M8N，超声波传感器型号选的是US-100。通过型号为HC-12的无线串口模块，将飞行器的各项数据实时传回到电脑终端。

图4 四旋翼飞行器平台Fig.4 Quadrotor aircraft platform

3.2 实验结果及分析

在实验中，光流传感器通过处理图像信息获得飞行器的速度信息，GPS可以直接提供飞行器的位置信息，IMU可以获取飞行器在飞行过程中的速度信息和姿态信息，超声波垂直地面安装在飞行器上可获得飞行高度的测量值，做匀速螺旋上升运动，轨迹如图5所示。本文基于自研的实验平台做了位置信息融合算法实验，并对比了传统GPS/IMU算法和光流/超声波/IMU/GPS组合模型改进算法，最终结果如图6、图7所示。

图5 轨迹图Fig.5 Trajectories

图6 x，y，z 方向的位置变化对比图Fig.6 Comparison chart of position change in x，y，z directions

图6为飞行器在两种算法下x，y，z三个方向的位置变化对比图，可以很明显地看出，通过传统GPS/IMU组合模型算法的扩展Kalman滤波器得到的位置信息与实际位置偏差幅度很大，局部存在着震荡情况，通过光流/超声波/IMU/GPS组合模型改进算法的扩展Kalman滤波器得到的位置信息与实际位置几乎重合，位置偏差幅度很小。图7为飞行器在两种算法下x，y，z三个方向的误差变化对比图，可以明显地看出，通过传统GPS/IMU组合模型算法的扩展Kalman滤波器得到的位置信息误差比较大，通过光流/超声波/IMU/GPS组合模型改进算法的扩展Kalman滤波器得到的位置信息误差很小。两种算法的具体的对比信息如表1所示。

图7 x，y，z方向的误差变化对比图Fig.7 Comparison chart of position error in x，y，z directions

表1 算法对比Tab.1 Algorithm comparison

从表1可知，光流/超声波/IMU/GPS组合模型改进算法的实验结果在x，y，z三个方向的误差和位置偏差幅度均优于传统GPS/IMU组合模型算法，所以表明基于扩展Kalman滤波的光流/超声波/IMU/GPS组合模型改进算法明显误差更小，精度更高。

4 结论

本文提出基于光流传感器的位置信息融合方法。该方法引入了光流传感器，将其与IMU融合后的速度信息和超声波传感器的高度信息作为扩展Kalman滤波中的预测量，观测量为GPS提供的位置信息和高度信息，最后融合得到精准的位置信息。仿真实验结果表明，该算法能更精准地获取四轴飞行器的位置信息，有效地解决了位置信息误差大的问题，帮助飞行器更好地完成飞行任务，具有极高的工程应用价值。