基于联邦滤波算法的组合导航信息融合方法设计

万晓帆，吕耀文，刘伟

（长春理工大学 光电工程学院，长春 130022）

随着导航技术的迅猛发展，多传感器信息融合技术逐渐成为了组合导航系统的关键技术［1-2］。目前应用最广泛、效率最高的融合方法是基于Kalman滤波技术的联邦滤波算法［3］。贾沛然等［4］通过跑车实验验证了联邦Kalman滤波技术应用于组合导航系统进行数据融合的可行性。解可新等［5］在GPS/SINS/OD组合导航系统中单一传感器信号缺失时，利用联邦卡尔曼滤波器增强了系统的容错性。雷克莱狄斯GV等［6］通过仿真实验验证了联邦卡尔曼滤波技术应用于导航数据融合中，导航精度仍有很大的提升空间。基于联邦Kalman滤波技术的数据融合方法包括各子滤波器估计相关时的融合方法和各子滤波器估计不相关时的融合方法，吴延昌等［7］采用方差上界方法将各子滤波器的最优估计值的相关状态转化成不相关状态，对于各不相关的子系统的估计值，传统的联邦滤波器将时间序列中的每一项数据做等权重处理，而组合导航系统实际运行过程中每个子滤波器对下一时刻的全局最优估计值影响并不相同［8］。

本文设计了基于Carlson联邦滤波器的混合联邦滤波器，主滤波器中通过对一个时间序列的每项数据进行加权处理，实现权重值的自适应更新，对全局最优估计值影响较大的子滤波器分配更大的权重值，进而提高了组合导航系统的导航精度、容错性等指标。

1 数据采集平台

数据采集平台以S5PV210为数据处理核心，S5PV210可以实现2000DMIPS的高性能运算能力，能够满足数据采集系统高效、实时的要求。设计中采用MPU6050传感器进行惯导数据（INS）采集；GPS数据采集以T326三星六频高精度定位定向板卡模块为核心，通过串行通信协议与S5PV210处理器进行数据传输；使用OV5640图像传感器进行图像采集，由于相机不同的拍摄角度会引起复杂的图像畸变，进而会增大数据处理的难度，所以本文相机采用水平拍摄，即相机和特征点在两个平行的平面内，利用Harris角点检测法对特征点进行提取，利用特征点周围的的灰度信息进行焦点匹配。数据采集平台如图1所示。

图1 组合导航系统数据采集平台

组合导航数据采集精度是实现高效数据融合的前提，惯导（INS）和GPS采用数字传感器模块，进而保证了数据采集精度能够满足系统要求。单目视觉传感器对速度的解算是整个数据采集平台数据输出精度的关键技术。单目视觉导航系统速度解算示意图如图2所示，导航参考系选择用当地东北天坐标系（地理坐标系），导航系用n表示，导航载体系用b表示，单目系用c表示，a是空间中特征点，At1和At2是空间中特征点a对应的在同一水平面内的两个像点。

图2 单目视觉导航系统示意图

像点随着载体的运动而变化，由单目系的像点坐标到导航系的转移方程如式（1）所示。

为提高单目速度解算的精度，简化数据计算量，单目运动平面为水平二维运动平面，当摄像机水平向前拍摄时，根据式（1）单目系的像点坐标到导航系的转移方程，导航系下单目解算的速度为：

2 混合联邦滤波器器模型

Carlson联邦滤波器由多个子滤波器和一个主滤波器构成，对数据进行并行、分级处理。本设计中混合联邦滤波器模型将INS作为公共参考系，分别与GPS和单目进行数据融合，主滤波器利用两个子滤波器的滤波结果对INS信息进行更新。组合导航滤波器结构图如图3所示。

图3 组合导航滤波器结构图

基于INS构建捷联惯导系统作为公共参考系，状态变量为：

式中，δLIEδλIN为纬度、经度误差；δvIE、δvIN为东向、北向速度误差；φIE、φIN、φIU为平台航向角、横滚角和俯仰角误差。∇E、∇N为东向加速度计零偏和北向加速度计零偏；εE、εN、εU为东向陀螺仪漂移，北向陀螺仪漂移和方位陀螺仪漂移。

2.1 INS/GPS组合导航系统

GPS状态变量为：

式中，δLG为GPS输出的纬度误差和经度误差；δvEG、δvGN为东向速度误差和北向速度误差；δψG为航向角误差，δθG为俯仰角误差，δγG为横滚角误差。

INS/GPS组合导航系统状态方程为：

式中，W为均值为零、方差为Q的白噪声，FINS为12×12矩阵［9］，FGPS为7×7矩阵，

其中相关时间τLG,τλG,τνEG,τνNG,τψG,τθG,τγG,在100～200s［10］之间选取。以上假设GPS的位置误差、速度误差、姿态误差均为一阶马尔科夫过程。

传统的INS/GPS组合导航系统采用位置、速度组合方式，随着GPS技术的发展，GPS模块可以实时输出高精度的姿态数据，在本设计中将姿态也作为系统观测量。INS/GPS组合导航系统量测方程为：

量测噪声V是零均值，方差为R的白噪声，且V、W互不相关。

2.2 INS/单目组合导航系统

INS/单目视觉组合导航采用速度组合模式，INS和单目视觉解算出的载体速度之差作为观测

式中δLk2,δλk2为载体纬度误差、经度误差，δvNk2,δvEk2为载体北向和东向速度，δφNk2,δφEk2和δφEk2分别为载体北东和天向的姿态误差角度，δk为摄像头标度因数误差。δk包括常值误差和随机误差，本系统中用随机常数代替常值误差，而随机误差用作一阶马尔科夫过程表示，反时间相关常数设为1τc。系统状态方程为：量。选取状态量为：

式中FSINS为7×7的矩阵［9］。取INS系统解算的载体速度和单目解算的载体速度的差值为量测向量，系统的量测方程为：

2.3 全局滤波最优估计

采用最小二乘迭代加权算法对两个子滤波器的最优估计值进行融合。一般最小二乘法将时间序列中的各项数据的重要性同等看待，而事实上时间序列各项数据对未来的影响作用应是不同的，因此比较合理的方法就是使用加权的方法，对相对重要的数据赋以较大的权数。两个子滤波器更新的局部最优估计值分别为，两个子滤波器的权重分配系数关系如式（10）所示。

式中，a和b为需要确定的权值。取k个时刻的测量数据进行最小二乘计算，公式如（11）所示。

求出K个时刻内的参数a和b，进而根据式（10），将两个局部最优估计值Xk1(t)和Xk2(t)发送到主滤波器得到最优估计X(t)。

3 Matlab仿真结果与分析

导航系统数据采集平台数据更新周期为0.1s，数据更新次数N=500，数据采集平台静止GPS初始北纬L0=43°50′12.97″，东经λ0=125°18′17.02″，地球的自转角速度ωie=7.27×10-5rad/s，INS中陀螺的一阶马尔科夫过程的白噪声和相关时间分别为ωr=0.1、Tr=0.1s。INS中加速度计的一阶马尔科夫过程白噪声和相关时间分别为ωa=0.01、Ta=0.01s。OV5640镜头分辨率2592×1944，视场角54.1H°×41.9V°。在PC端Matlab环境下，对数据采集平台采集到数据进行仿真分析，仿真分析流程图4所示。

图4 仿真分析流程图

姿态误差对比曲线如图5所示，图中实线表示应用采用等权处理的Carlson联邦滤波器得到的误差曲线，虚线表示采用基于最小二乘加权处理的混合联邦滤波器的误差曲线。俯仰角、翻滚角和航向角误差曲线均收敛于0值。

图5 Carlson联邦滤波器与混合联邦滤波器姿态误差对比曲线

位置误差对比曲线如图6所示，图中实线表示采用等权处理的误差曲线，虚线表示采用基于最小二乘加权处理的误差曲线。等权处理的Carlson联邦滤波算法位置误差呈现发散状态，而采用混合联邦滤波器位置误差稳定在±2m范围内。

图6 Carlson联邦滤波器与混合联邦滤波器位置误差对比曲线

速度误差对比曲线如图7所示，图中实线表示采用等权处理的误差曲线，虚线表示采用基于最小二乘加权处理的误差曲线。采用混合联邦滤波器速度误差曲线虽然波动范围较大，但是相比于等权处理的Carlson联邦滤波器得到的速度误差曲线并未出现明显的发散现象，速度稳定在±0.5m/s范围内。

图7 Carlson联邦滤波器与混合联邦滤波器速度误差对比曲线

4 结论

多传感器信息融合方法是提高组合导航系统导航精度的关键技术，本文基于Carlson联邦滤波器模型设计了混合联邦滤波器，主滤波器中采用最小二乘加权方法给每个子滤波器分配动态、自适应系统的权重值。Matlab仿真结果表明，相比于等权重值Carlson联邦滤波器，混合联邦滤波器导航精度更高。为组合导航数融合方法的研究提供了参考。