基于MIMU/GPS/EC的联邦卡尔曼滤波器设计❋

张 樨,李 杰,蒋 窍,刘晓玲

(1.中北大学信息与通信工程学院,山西太原 030051;2.中北大学电子测试技术国防重点实验室,山西太原 030051;3.电子科技集团第2研究所,山西太原 030024）

0 引 言

随着组合导航水平的不断提高,组合导航理论与组合导航技术也同步发展,其中最常用的组合导航算法即为卡尔曼最优估计理论.作为组合导航系统的核心算法,卡尔曼滤波技术将各个导航系统的信息最优地进行组合,估计出系统的各种误差状态,再用误差状态去校正系统,从而构成具有更高精度、更高容错性能、更高可靠性的组合导航系统[1,2].利用卡尔曼滤波技术进行最优组合,通常有两种途径:集中式卡尔曼滤波和分散化卡尔曼滤波.集中式卡尔曼滤波是利用一个卡尔曼滤波器来集中地处理所有的导航信息,存在状态维数高,计算量大、不利于故障诊断、容错性差等不足,不利于滤波器的实时运行和控制[3].

而在众多的分散化卡尔曼滤波方法中,Carlson提出的联邦滤波器(Federated Filter)因为设计灵活、算法简单、计算量小、容错性高、便于实时执行等突出优点,致力于解决如何在保证系统容错性能的同时,提高滤波器的精度,特别适用于组合导航系统的信息融合处理[4-5].本文针对MIMU/GPS/EC组合导航系统的特点,提出并设计了位置、速度、姿态分别组合的无重置式联邦卡尔曼滤波器,旨在进一步增强系统的容错性能、提高导航定位精度.

1 MIMU/GPS/电子罗盘组合导航系统信息融合方法

在分析MIMU、GPS、电子罗盘工作原理的基础上,利用MIMU自主性强、可靠性高、输出信息全面等诸多优点,选择将其作为公共参考系统,GPS为观测子系统1,利用MI MU/GPS组合构成位置子滤波器和速度子滤波器;电子罗盘选取为观测子系统2,利用MIMU/电子罗盘构成姿态子滤波器.子滤波器之间相互独立,无反馈重置带来的相互影响,为系统保证了最高的容错性能.具体信息融合方案如图1所示.

从图1可以得出,MIMU/GPS组合采取位置、速度组合模式,将MIMU和GPS输出的位置和速度的差值分别作为量测值,最优地估计出MIMU的误差,然后对MIMU进行校正.对于电子罗盘,它在导航过程中只能提供三维姿态角,故MIMU和电子罗盘选择姿态组合模式.将电子罗盘提供的三维姿态角与MIMU解算输出姿态角取差值作为姿态角信息的量测值,通过滤波算法估计出系统姿态角误差,然后对MIMU进行反馈校正,进一步提高姿态解算精度.

图1 组合导航系统最优估计融合方案Fig.1 Optimal estimation fusion program for the designed integrated navigation system

2 组合系统数学模型建立

2.1 MIMU公共参考系统状态方程

首先,建立MIMU公共参考系统状态方程.考虑到MIMU的误差主要来自传感器自身的常值漂移和随机误差,以及标度因数、温度特性等,为减小计算量,通常将模型简化,重点考虑其主要误差项及误差特点[6].以MIMU作为公共参考系统,取状态变量为

式中:δφe,δφn,δφu分别为平台东向、北向和天向的失准角;δVe,δVn,δVu分别为系统东向、北向和天向的速度误差;δL,δλ,δh分别为纬度、经度和高度的误差;εbx,εby,εbz为陀螺的常值漂移;εrx,εry,εrz为陀螺误差模型的一阶马尔科夫过程;▽x,▽y,▽z为加速度计的常值漂移.

MIMU的状态方程为:

式中:FI,GI,WI分别为状态转移矩阵、噪声驱动矩阵和系统噪声.

2.2 MIMU/GPS位置子滤波器

GPS系统的定位误差主要包括GPS卫星有关的误差、与GPS信号传播有关的误差、与观测和接收机有关的误差等.为方便实际应用,通常将各种误差都投影到距离上进行分析.虽然各种误差源具有不同的统计特性,但如果观测时间足够长,所有的误差都可以看作是零均值的随机过程,因此对于GPS误差也可当成零均值随机过程来处理.

2.3 MIMU/GPS速度子滤波器

选取公共状态为速度子滤波器的状态向量,即X2=XI.那么,速度子滤波器的状态方程即为

式中:F2,G2,W 2分别等同于F I,G I,W I.

取MIMU输出的速度信息与GPS接收机输出的速度信息之差值作为速度子滤波器的量测值.建立速度子滤波器的量测方程如下

2.4 MIMU/电子罗盘姿态子滤波器

选取公共状态为状态向量,即 X3=XI.则姿态子滤波器的状态方程

式中:F3,G3,W 3分别等同于F I,G I,W I.

选MIMU输出的姿态信息与电子罗盘的姿态信息的差值为该子滤波器的量测值,并建立姿态子滤波器的量测方程如下

式中 :H3=[03×3I3×303×4];V3(t)=[PePnPu]T为电子罗盘的量测白噪声.

2.5 主滤波器融合算法

3 试验验证

为了考核本文所设计的联邦滤波算法的有效性、准确性,设计了Matlab理论仿真试验.仿真过程中,假设车辆作匀速直线运动,时速80 km/h,初始航向角为 45°,初始姿态角误差 0.05°,0.05°,0.1°,初始速度误差 0.1m/s,轨迹起点位置设为东经 112°、北纬 38°、高度840m,初始位置误差 1m,具体初始条件如表1.

试验中,设置三个子滤波器为各自的采样频率,主滤波器的滤波频率为 0.1 Hz,滤波总时间为500 s.其中,MIMU、电子罗盘、GPS的采样频率分别为1 000 Hz、10Hz、10 Hz,且保持采样同步.同时,为使仿真条件与实际情况尽量一致,在添加系统噪声时,首先对实际组合系统中MIMU的输出进行性能测试分析,得到该种 MIMU中陀螺和加速度计的噪声特性,再将相同量级的噪声加入仿真试验中;同样地,根据实际所测得GPS接收机的噪声特性设定仿真试验中接收机输出噪声的大小,根据测得的电子罗盘噪声特性设定仿真试验中电子罗盘输出的噪声大小.

表1 仿真模型误差源参数Tab.1 Error source param eters of the simulation m odel

图2 纯惯导解算与组合导航系统解算误差对比图Fig.2 Error comparison char t of inertial navigation and integrated navigation system

从图2中可以看出,纯惯导的解算误差随着时间的增长而不断累积,输出数据不断偏移真实值,出现了严重的误差发散现象.而组合导航系统通过联邦卡尔曼滤波算法对MIMU解算输出进行实时修正,使组合系统的滤波精度得到了明显改善,较好地抑制了误差发散问题.滤波后系统位置误差控制在1.8 m以内,速度误差在0.9 m/s以内,滤波后航向角误差控制在0.5°之内,有效克服了MIMU/GPS组合在加速度较小时航向误差观测性差的问题.结果表明,所设计的位置/速度/姿态组合的联邦卡尔曼滤波算法对于提高系统精度、可靠性是十分有效的.此外,考虑到实际选用的是低成本MIMU,在仿真时为了与实际情况保持一致,所设置的MIMU精度也相对较低,若采用高精度传感器集成 MIMU,则组合系统的精度还可以进一步提高.

4 结 论

本文针对MIMU/GPS/电子罗盘组合导航系统进行研究,为提高MIMU误差的修正精度,引入GPS、电子罗盘辅助导航,构建位置、速度、姿态分别组合的无重置式联邦卡尔曼滤波器,将MIMU、GPS、电子罗盘进行最优组合、形成优势互补,为系统提供最高的容错性能.所设计的滤波算法充分利用各种导航传感器信息,有效提高了导航系统的综合性能,具有设计简单、可操作性强、精度高的优点.与纯惯导系统相比,该系统具有更好的可靠性、容错性和实时性,能够输出连续高精度的位置、速度和姿态信息,为进一步开展组合导航系统的研究试验奠定了良好的基础.

[1] 刘俊,石云波,李杰.微惯性技术[M].北京:电子工业出版社,2005:186-190.

[2] 秦永元,张洪钺,汪叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社,2010:238-286.

[3] 张海龙,郝静如,李启光.联邦卡尔曼滤波在捷联惯导/全球定位/里程仪组合系统中的优化设计[J].探测与控制学报,2009,31(1):66-68.

Zhang Hailong,Hao Jingru,LiQiguang.Theop tim ization design of federal kaiman filter on integrated navigation system of SINS/GPS/odometer[J].Journalof Detection&Control,2009,31(1):66-68.(in Chinese)

[4] Gao Shesheng,Zhong Yongm in,Lia Wei.Robustadap tive filteringmethod for SINS/SAR integrated navigation system[J].Aerospace Science and Technology,2011,15:425-430.

[5] 李鹏程.组合导航及其滤波算法研究[D].西安:西安电子科技大学,2010.

[6] 黄丽.卡尔曼滤波在车载组合系统中的应用[D].上海:上海交通大学,2009.

[7] Tariq S Abuhashim,Mamoun F Abdel-Hafez,Mohammad Ameen A I-Jarrah.Building a robust integrity monitoring algorithm for a low cost GPS-aided-INS system[J].International Journalof Control Automation and Systems,2010,8(5):1108-1122.

[8] 徐田来.车载组合导航信息融合算法研究与系统实现[D].哈尔滨:哈尔滨工业大学,2007.

[9] 陈海明.容错多传感器组合导航系统算法研究及仿真实现[D].南京:南京航空航天大学,2011.

[10] 刘明雍,周志远,赵涛.联邦滤波器的SINS/GNS/DVS水下组合导航[J].火力与指挥控制,2009,34(12):41-44.

Liu Mingyong,Zhou Zhiyuan,Zhao Tao.Research on SINS/DNS/DVS underwater integrated navigation based on federal filter[J].Fire Control&Command Control,2009,34(12):41-44.(in Chinese)