基于卡尔曼滤波的数据融合算法与应用研究

徐恩松，陆文华，刘云飞，李宝磊，李 洋

(上海工程技术大学，上海 201620)

1 概 述

1.1 数据融合的概念

多传感器数据融合主要是针对一个系统使用多种传感器的问题而研究出来的一种信息处理方法。国外研究成果表明，信息融合的概念为：利用计算机技术对按时序获得的若干传感器的观测信息在一定准则下加以自动分析、综合以完成所需的决策和估计任务而进行的信息处理过程[1]。

信息融合处理过程的硬件基础是传感器系统，处理对象是多源信息，融合的核心是协调优化和综合处理。融合后，融合的信息效果远好于单个传感器的信息。

信息融合的功能特点有:改进多源信息的可信度和目标的可探测性，降低推算模糊程度，提升探测性能，增强系统的自适应性容错调整能力，减少了信息获取的时间，从而提高整个系统的信息处理性能。

1.2 信息融合理论算法

信息融合中要用到许多数学工具和方法，主要有概率论、模糊理论、小波法、神经网络等方法。信息融合作为一种智能化超高的数据综合处理技术，集合并应用了许多学科和新技术。数据融合广义上的范围包括检测技术、信号处理与通信技术、模式识别、决策论、估计理论、最优化理论、计算机科学、人工智能和神经网络等诸多学科[2-4]。近几年信息融合技术的发展很快，很多应用研究领域的人员已经根据各自的具体应用背景，提出了许多比较成熟的融合算法。在自主避障系统中的信息融合方法有以下几种：

Blackman S S等提出了加权平均信息融合方法[5]；Luo R C等研究了贝叶斯估计信息融合方法[6]；Kamberova G提出统计决策理论信息融合方法[7]；Xu L等人采用了DempsterShafer证据理论[8]；MOORE J B等研究了卡尔曼滤波信息融合方法[9]；SASIADEK J Z等人提出了神经网络—模糊信息融合方法[10]。

文中主要研究基于卡尔曼滤波的信息融合算法，针对传统集中卡尔滤波方式存在的问题，着重研究改进的Sage-Husa滤波算法，该算法可增加滤波的精度和稳定性能。并提出了基于改进的Sage-Husa滤波算法的联邦卡尔曼滤波器的设计，最后通过对特定的自主避障系统进行了多传感器信息的数据融合动态仿真，并给出了整个滤波融合的仿真结果。

2 卡尔曼滤波基本原理

设动态系统的方程和观测方程分别为：

xk=Ak-1,kxk-1+Bk-1,kuk-1+wk-1

(1)

zk=Hkxk+vk

(2)

卡尔曼滤波算法的具体流程为：

预估计：

(3)

计算预估计协方差矩阵：

(4)

计算卡尔曼增益矩阵：

(5)

更新估计：

计算更新后估计协方差矩阵：

(6)

递归循环计算：

(7)

卡尔曼滤波的实质是通过测量值重构系统的状态向量。它以“预测—实测—修正”的逻辑顺序递推[12]，得到系统的测量值以此来消除随机干扰，再重构出系统的状态。KF计算分为时间更新过程和测量更新两个过程，如下所示：

(1)时间更新过程(预测)。

(a)向前推算状态变量：

(b)向前推算误差协方差：

(2)测量更新过程(修正)。

(a)计算卡尔曼增益：

(b)由观测变量更新Zk估计：

(c)更新误差协方差：

3 自主避障系统数据融合解决方案

对于自主避障中的组合导航系统的数据融合，通常采用的方法是分散卡尔曼滤波和集中化卡尔曼滤波。组合导航系统中所用到的参数多，采取单独一个滤波器进行全局数据集中处理不能保证滤波的实时性，如果组合系统中的惯导系统或GPS系统出现故障，整个系统的输出都会受到影响，可看出集中滤波的容错性能差，不利于系统的故障诊断。相对来说，分散化滤波可以提高系统的容错性能、减少计算量。它通常采用由若干个局部滤波器和一个主滤波器组成的两级滤波器结构，各子状态估计的相关性利用信息原则来消除，其结构设计灵活，各子系统的数据用局部滤波器处理，然后整个系统全局数据的融合处理通过主滤波器进行。本节通过利用Calson提出的联邦滤波器[13](federated filter)，对自主避障系统进行分散化滤波数据融合进行研究。

3.1 联邦滤波器的基本原理

联邦滤波器是一种由多个子滤波器和一个主滤波器组成的数据融合技术。通常把惯导系统作为组合导航系统中公共参考的传感器。一方面惯导系统的输出直接输入给主滤波器，另一方面各子滤波器用它作为量测值，其他导航系统对应各子滤波器的输出结果也进入主滤波器进行数据融合，达到整个惯导系统参数的最优滤波化。

3.2 组合系统联邦滤波器设计

联邦滤波器有四种实现模式，依据信息分配策略的不同，四种实现模式为：无反馈模式、融合-反馈模式、变比例模式、零复位模式。通过分析考虑组合系统的容错性能、定位精度、滤波融合算法的难易程度，文中设计的联邦滤波器为：βm=0、系统无重置，系统主滤波器状态估计无信息分配不进行滤波运算，只负责完成组合系统信息融合[15]。最终设计了公共参考系统为惯导系统，子系统为GPS位置子系统和速度子系统的联邦滤波器结构，如图1所示。

图1 SINS/GPS联邦滤波器结构

4 联邦滤波器的数据融合算法研究

在第二节提出的联邦卡尔曼滤波器结构中，各子滤波器没有考虑反馈作用和重置结构进行独立滤波。这样能够减少主滤波器到子滤波器的反馈信息和数据计算量。但是如果没有反馈信息的作用，会降低滤波器的估计精度和滤波的稳定性。综合对以上因素的分析，本节在标准联邦滤波器的基础上应用优化算法，以提高系统容错性能和滤波的实时性。

在实际的导航系统中，并不能准确知道系统的噪声方差阵Q和观测噪声方差阵R。从而在对不准确的系统模型进行滤波时，会出现滤波发散，导航系统在工作过程中Q、R、Φ、H参数容易发生变化。为了防止滤波发散，需要在滤波前对系统参数的变化进行有效的估计，在滤波过程中及时调整滤波增益阵Kk，这便是自适应滤波的基本思想。自适应滤波算法有贝叶斯法、相关法、协方差匹配法等。文献[16]介绍了基于Sage-Husa自适应滤波算法和强跟踪滤波算法改进的Sage-Husa滤波算法。本小节采取该滤波算法设计出了基于改进的Sage-Husa滤波算法的联邦滤波算法。

4.1 Sage-Husa自适应滤波算法

Sage-Husa自适应滤波算法可以对观测数据进行递推滤波，同时通过时变噪声统计估计器实时估计和修正系统噪声和观测噪声的统计特性，以此抑制滤波发散、提高滤波精度。其算法可描述为：

(8)

(9)

其中，b定义为遗忘因子，且0

4.2 强跟踪卡尔曼滤波

上一节中在Sage-Husa自适应滤波算法中增加了对系统噪声统计特性的计算，计算量大幅增加后会降低滤波的实时性。组合导航系统中状态变量维数高，Sage-Husa算法在对高阶系统滤波的过程中，容易出现Q和R失去正定性滤波发散现象。针对这些问题，为了保证滤波的稳定性，可以通过牺牲一定的精度换取滤波稳定性，这就是强跟踪卡尔曼滤波算法[17]将状态估计误差验前协方差阵乘以加权系数1，加强突变状态跟踪能力，在滤波器达到稳态时，保持这种能力，该滤波算法对初值和噪声统计特性的敏感也比较低。其算法的具体实现为：

(10)

其中，αi由先验知识确定。

4.3 改进的Sage-Husa联邦滤波器的设计

在对Sage-Husa自适应滤波和强跟踪卡尔曼滤波的特点进行分析综合后，考虑到滤波的精度和稳定性，决定在收敛的条件下滤波时，采用Sage-Husa滤波算法；在滤波存在发散时，采用强跟踪卡尔曼滤波。因此在滤波前需要用滤波的收敛性判断滤波是否发散。在算法中可以利用新序列：

(11)

的性质来构造滤波器的收敛性判据。在式(9)中已经包含了新序列的平方和，因此利用下面的新序列方差阵描述误差大小的信息。

(12)

因而，可用：

(13)

作为收敛判据。其中γ≥1为可调系数，当上式成立时，用Sage-Husa自适应滤波，若不成立则说明滤波误差超出预测值的γ倍，此时为滤波发散状态，改用强跟踪卡尔曼滤波。在第二节设计的标准联邦卡尔曼滤波器结构的基础上应用改进的Sage-Husa算法，得到的联邦滤波器结构如图2所示。

5 仿真实验与结果分析

在实际的惯性导航系统和GPS系统中，两者拥有不同的采样率，各自的采样率周期不成整数倍，需要考虑到系统数据同步的问题。在本仿真实验中假设惯导系统数据更新采样周期为0.01 s，GPS测量输出周期为1 s，假设GPS在采样周期内数据保持不变，程序设计中不考虑数据同步问题。整个滤波仿真参数如下：

地球自转角速度：7.292 115 8E-5 rad/s；

陀螺仪测量均方差：σg=0.001；

图2 Sage-Husa联邦滤波结构

加速度测量均方差：σa=0.001；

GPS位置均方差：σp=25；

GPS速度均方差：σv=0.15；

惯导初始位置误差：δφ=30''，δλ=30''，δh=15 m；

惯导初始速度误差：δvx=0.1 m/s，δvy=0.1 m/s，δvz=0.1 m/s；

惯导初始姿态误差：δψ=300''，δθ=300''，δγ=300''；

GPS初始位置误差：δφ=1'' ，δλ=1''，δh=25 m；

GPS初始速度误差：δvx=0.15 m/s，δvy=0.15 m/s，δvz= 0.15 m/s；

滤波初值：

X0= [30″,30″,15,0.1,0.1,0.1,300″,300″,300″, 0.001° ,0.001° ,0.001° ,0.001,0.001,0.001]；

P0=diag([30″,30″,15,0.1,0.1,0.1,300″,300″,300,0.001, 0.001,0.001,0.005,0.005,0.005]2)；

Q=diag([0.0012;0.0012;0.0012;0.0012;0.0012;0.0012])；

RV=diag([0.152;0.152;0.152])；

RP=diag([1″, 1″, 25]2)。

在上述滤波仿真程序中，及时对惯导系统的位置参数和速度参数进行了修正。在滤波开始初期，利用估计的误差状态对惯导系统进行反馈校正和输出校正，当滤波稳定后继续进行输出校正，同时实施周期性的反馈校正，以便达到较好的滤波精度和稳定性。仿真结果见图3～图5。

图3 联邦滤波与标准滤波纬度误差

图4 联邦滤波与标准滤波经度误差

图5 联邦滤波与标准滤波高度误差

通过纬度、经度、高度三个方向的仿真结果可看出，采用改进的联邦卡尔曼滤波数据融合算法比标准卡尔曼滤波算法的导航系统的定位误差更小，组合导航系统采用GPS测量值可以及时修正惯导系统的测量误差，可以避免纯惯导系统的发散问题，提高了单一导航系统的定位精度。

6 结束语

重点研究了无人机自主避障系统中多传感器信息融合时采用的卡尔曼滤波算法问题。在分析了传统卡尔曼滤波在数据融合存在的问题以后，提出了分散化联邦卡尔曼滤波的方式，采用改进Sage-Husa联邦滤波方法对组合导航进行数据信息融合以此提高滤波精度和滤波稳定性。最后进行了数据融合MATLAB仿真实验，实验结果表明比传统滤波效果明显提高。