联邦卡尔曼滤波器在容错性组合导航系统中的应用_图文

维普资讯 http://www.cqvip.com

控 制 系 统 

联邦卡尔曼滤波器在容错性组合导航系统中的应用 
Appl a i     de at d  l a   i e  n T e antI e a e   i ton ofFe r e Ka m n F l ri   ol r  nt gr t d Navgat   s e   c t i i Sy t m on

(安 二 兵 程 ) 康 伟 王 宏 力  西 第 炮 工 学院李
L, n we W a g, n l i Ka g i n Ho gi  

摘 要 : 文 提 出 了基 于无 复 位 联 邦 卡 尔 曼 滤 波 的 信 - 合 算 法 。 立 了惯 性 / 星/ 文 组 合 导 航 系 统 的 误 差 模 型 . 进 行 了 本 E融 建 卫 天 井  

计 算 机 仿 真 。仿 真 结 果 表 明 。 算 法 具 有 较 强 的 容错 性 。 该   关键 词 : 邦 滤 波 ; 台 导 航 ; 错  联 组 容
中 图分 类 号 : TN9 4 1  文献 标 识 码 :  B

Ab t a t On te b s   f te N fd r td k ma   l r i fr t n f so   l oi m  a   e n p o o e   n   h   ro  d l o   e sr c :   h   a e o  h   R  e e ae   a l n f t   no ma i   u i n ag r h h s b e   rp s d a d t e e rr mo e  f t   i e o t h I / S C   ne r td n vg t n s se h s b e   s b ih d T e smu ain rs l   e n tae ta  h s a g rtm  a   o d p r  NS GP / NS i tg ae   a iai   y tm  a   e n e t l e . h   i l t   e ut d mo sr t h t t i l oi o a s o s     h h s g o   e-
fr n e o  a l — o ea c . oma c   f fut tlr n e 

Ke   o d :e e ae   l r i tg ae  a iain;a l- oe a c   yW r s fd r tdf t ;ne r td n vg t i e o f ut tlr n e

l 引 言 
惯 性 导航 (N . et l a i t nS s m) 二 十  I S I ra N v ai   yt 是 n i  g o e 世 纪 初 发展 起 来 的 , 它具 有 自主 导 航 能 力 , 需 要 任  不

2   S GP / N / S C S组 合 导 航 系 统 联 邦  滤 波 器 设 计 
21I SG SC S 合导航 系统 联邦 滤波器 结构  . N /P /N 组   联 邦 滤波器是 一种 两级 滤波 结果 , 图 1 如 所示 。  

何外界电磁信号就可给出载体的姿态 、 速度和位置信 
息 . 外 界干 扰强 , 初 始条 件 正确 的情 况下 , 性 导  抗 在 惯 航 系统 的短期 精度 高 。因此 惯性 导航 系统一般 作 为主  要 导航 设 备 。但 是 , 性 导航 系统存 在 导航误 差 随时  惯
间积 累 的问题 , 这是 惯导 系 统 的主要 缺 点 。全球 定 位  系统 ( P , l a P s inn yt 是 当前最 先 进 的  G S G o l oio igS s m) b  t e

导航定位系统 , 它的定位精度极高 。 也可给出位置信  号 和速度 信 号 。 但 GP S有 以下 缺点 :动态 环 境 中可  靠性差 : 自主性 ; 非 数据 输 出频 率 低等 。天 文导航 系统  (N , e sa N vgt nSs m) 利 用 天空 中 的恒  C S C l tl ai i  yt 是 e i  ao e
星作 为 导航 信息 源 , 提供 不 随时 间 漂移 的位 置 和航  能

图 1 N / P /N 组合导航 系统联邦滤波器 结构   ISG SC S

图中参 考 系统 的输 出一 方 面 直接 给 主 滤波 器 , 另 


定位定 向精度高 , 但易受气候条 件的限制 , 数据输 出 部 估 计 和 主 滤 波器 的估 计 值 一 起 进 行 融 合 以得 到全    频率 低 。 如 果将 I SG SC S导 航 系统进 行组 合便  局 最优 估计 x 。此外 , 图中还 可 以看 到 , 个滤波  N 、P 、N   从 每 可充分发挥各 自的优点 , 克服各 自的缺点 , 实现在高  阶段完成后 , 全局滤波器将全局结果 x 及  根据 “   信  动态 和 强 电子 干扰 环境 下 实 时高精 度 的导航 定 位 , 而  息守 恒 ”原则 向各 局 部 滤波 器 和 主滤波 器 进 行反 馈 。   且 可 以克 服美 国 GP S政 策 的限 制 , 航 空 、 天 导航  联邦卡尔曼滤波器 根据信息分配因子的不 同, 以构  在 航 可 和武器制导等方面应用 非常广泛 。如何将不同传感器  成不 同的结 构类 型 。 中以无 复位 ( R) 其 N 结构形 式 的容  被   的信 息进 行 融 合 是 组 合 导 航 系统 设 计 中关 键 技 术 之  错性 能为最 好 。 认 为是 容 错型滤 波结 构 。 22系统 状态 方程 和量测 方程 的建 立  . 卡 尔 曼滤 波是 目前应 用较 多 的信 息融 合算 法 , 而  当 IS为指北方位平 台式惯导 , N 而误差方程采用  联邦滤波器又以算法 简单和容错 性好而受到了普遍  东北天地理坐标系 . 且不考虑高度及垂向速度的变化  的重视 。   但 考虑 飞行 高度 和 地 球 为旋转 椭 球体 时 , 应 的 导航  相 参数误差共有七个变量 ,其中平 台姿态角误差三个 、   李康伟 : 士  硕 速 度误 差 两个 、 置误 差 两个 。 因为惯 性 测量 元 件 的  位 基 金 资 助 :第 二 炮 兵 预 研 项 目 编 号 不 公 开 


方面 它可 以输 给 各局部 滤波 器作 为量测 值 。各 局部  滤 波器 根 据状 态方 程 和量 测方 程 进行 滤 波 , 出建 立  输 向等辅助信息的导航 系统 。 它的隐蔽性好 、 自主性强 、 在 局 部量 测基 础 上 的估 计结 果 X,i =,, ,   i ( l m)这些 局  Pi 2



@ 用局  8 3 ̄ l— 邮订 2 6 /-9 躺 0 1  

维普资讯 http://www.cqvip.com

控 制 系 统 

中文核心期刊 < 微计算机信息  测控自动化)o6 ( 2o 年第2 卷第孓 期  2 1

误差是影响惯性 导航 系统导航精度 的主要误差源之  器噪声  0 0 一l       所 以在 应用 卡 尔曼 滤波 技术 估 计 系统 的误 差状 态  时, 应尽可能的将惯性元件的误差 ( 陀螺漂移 和加速  0 x  l 0 0 44       观测 矩 阵 H: )   (= f 0 1 0       度计 偏置) 估计 出来 , 故将惯性 元件误 差作 为状态变  量 的一 部分 。 量 方程 分 别 由 I SG S导航 子 系 统 和  测 N /P 1 O 0       ISC S 航子 系统 得到 。 N /N 导   2 . 3系统 的离散化及卡尔曼滤波方程 的建立  ( 系统 误差 状态 方程 为 : 1 )   将状 态方程和量测方程离散化后 . 得离散化 的系  统 方程 :   X, = O x, ) G OJ O    )   ) O +     w, ) () 1  






式中: 状态变 量  x, ) I , , , , , ,   , , , V r 0 =C         却 觑,     V ,    E 系统矩 阵 
。   。 衄 ,’   ’ _ 即 o皿   。   。   。   。 。 。 。  

X,k =矿I k— ) ( 1+rk一 t k一 ) () (, 1 i : X,k一 ) ( 1 ̄( 1  

z() -: =H-ix,^ + ( ) I i ( ) ()  ,: I : : I  i
Z:I) ( =H  I) ( )  :I) i : ( X, k + (   i : i :

(  4 )

。 。  
O  
.  

,  
咖 
q' z  
0  

奇  
旦- k  ̄ * 


。  
D  
n砷 , 
0  
。  

,  
o , 叫  
, ∞ 埘 —  十  
锄 啊  
。  

。 。…   ’
O  
-  'z f
嚆 ,—  

口 ,d 矗 +,  

,  
,  
? ,    

0 0    0    i 0  
口D口 1    0
口 0 口 0 1    
… … 。  

咖 ,  

垃 血 ,- r ●      
。   。  

根据以上离散系统方程 。 即可进行联邦卡尔曼滤  波 了 , 体 算法 如下 : 具   假设 v【( l ) W()  )= 。 和 ki 2 k满足 

。  

o  

o  

a  

。  

, ,    

o  

。  

f[ () 0 o () ( = (), EI k】 , y k    Qk   V =C     {  () 0 o () . = ()f E k】 , y k () Rk  =C  『 】  
I   C yw () ()=   o l k   】 0

(  5 )

系统 噪声 向量 , ) n   , , ,   ( =I f  ,   , l l  r  , 其方差 阵 为 Q = g PWg  W ,   嘲   , , , W 】 W

系 声 矩阵为Go= , 】 统噪 传播 ,)    r  
式 中 R ,N MR 分别是参考椭球子午 圈上各 点的曲 

式 中: ( 为系统噪声序列 的方差阵 , Qk ) 假设为非负  定 阵 , () Rk为量 测 噪声 序 列 的方差 阵 , 假设 为正定 阵 。   ( 局部滤波器算法 : 1 )   时 间更 新方程 为 
X ( k- )   , , 1   J- l D  J c I 1= ( 七一 ) ( Ik—   惫 X c )  

率半径和卯酉圈上各点的曲率半径. Q是地球 自传角 
速 度 , , 别 是地 理 纬度 和 经 度 , ‘ 入分 p   分别 是  东、 、 北 天方 向的 比力 。V , ,  别 为 东 、 、 向   V V 分 北 天 的加速度 计偏 置 , , , 分别 为 东 、 、 向的陀 螺      岛 北 天

( k一1=中,k k一1 ( kI ) (, ) k—l k—1 ( ,   l ) k k一1 ,、   ) ,  + lk一1 I一1 ( )f ,,   F( ) i ) k一1 =1 m) Q( :   ( 2

量 测更 新方 程 为 
X,kI ) ( k =X,kI ( k一1+K k) ) (   (  ) H Z 一   ) ,i k一 )   ( I 1  I : ) K,k)   ( k一1 ( = kI ) k) (   H ( ( k) 日, ( k一1 kI ) k)     H ( +R )

漂移, 均考虑为常值加白噪声。定义 
F  ^  ,

() 8 
f1 9 

RM + h 。  

( 系统 量测 方程 为 : 2 )   I SG S子系 统量测 方程 为 : N /P  

z O ; qx O + l) I) 日I) , ) vq 
式中: 观测 向量 zO =( 一% ) , 一 ) 。) 【 仍 足 (      

(  2 )

( I ) , K () ( ) kI 1 (=1 )1) k k =( -  kH, )     k  ( k- ) i , (0 2  

( 信息融合算法 : 2 )  
=  

R o ̄V       r Ⅳcs ,皿一 四,j Ⅳ一  


( X。   x   x    ~ +  +  )

(1 1)  

( 十 + )        一

(2  1)

4 阵 H。)   矩 O=

R  o  Ncs

O 俩 

( 主滤波器到子滤波器的信息反馈 : 3 )  


=  

’  

(3  11



 

V(为 G S接 收机 噪声  。) P t I SC S子 系统 量测方 程 为 : N /N  
z  ) 2 =日2 ) O + O   ( x, )  2 ) f

( =1 ,   j , m) 2

(4  1)

式中   , 是全局最终 的滤波值 ,   B是信息分配 
() 系数 , 3   在无复位结构中分别取为

B 2 , 1- , l = 32   2 =

    式 中 ::) 星 体跟 踪器 输 出值 ,:) 星体 跟 踪  B 0 。 Z(为 t V(为 t


1 0—3 0L/ 邮 局 订 阅号 :2 9 6 2 6 ̄ 年 8 —4  

维普资讯 http://www.cqvip.com

控 制 系 统 
果 表 明 : )当各 子滤 波 器均 工 作 正常 时 , ( 1 主滤 波器 能  够 较好 的估计 出陀 螺和加 速 度计 的 漂移 。 利用 估 计 出  设 初 始地 理 位 置 为经 度 10, 2o纬度 3o飞行 器 以  的结果 对 惯导 系统 进 行校 正 , 够提 高 组合 系 统 的定  0, 能 lO / Oms的速 度匀 速 向东运 动 。 行 总时 间 为 60, S 位精 度 。 ) ISG S子 系统或 I SC S子系统 发生  飞 0sN   I ( 当 N /P 2 N /N 的数据 输 出率 为 01,N .s S的 数据 输 出率 为 05 , P  故障( C . GS s 软件故障或硬件故障) , 时 由于采用无复位( R  N) 的数据输出率为 1. s主滤波器的融合周期为 2。 s   联邦卡尔曼滤波结构 , 主滤波器能够检测出故障子系  

3 仿 真 研 究 

状 态矢 量 初始 值 取 为 X ( =00 ,.8 . ,, , 统 并将 其 隔离 ,进 而 将工 作 良好 的子 系统 重新 组 合 。 0 【. 00 , 1 11 ) 8 06     10 1 0 0101 01 00 0 ,. 0 ]P 和 Q取 中等  对 惯导 系统进 行校 正 。 0 ,0 ,.,.,., . 1 0 0 1 ,0 0 0   精度惯性测量组件 的对应值 ,即陀螺 的常值偏移 为  参考文献:   1 】 李兴国 , 国伟 , 娄 李跃华 2 世纪初先进组合 中制导  1 00% ,随机 偏移 为 00  ,加 速 度计 的常值 偏 置 为  【 赵 占祥 , . 2 .1 G SI S关键技术概述与展望 飞航导弹 20 . P/ N 05   2 10 g 随机偏 置为 5 1 。星体 跟踪器 观测误 差 为 3, 0 ̄ 。 0L - g     G S定 位误 差为 1m, 速误差 为 08 /。 P 8 测 .ms  与实现【微 计算机信息, 0 ,212 14 J J 2 51 : — 4   0 4 在 以上条件 的基 础 上 ,采 用 N R联邦卡 尔曼 滤波 
【 2 】李小勇, 张卫. 基于 M L P S的 I P组播在 Lnx环境 下的设计  iu

算法 , 利用 M T A A L B语言进行计算 机仿真 , 仿真结果  如图 2 图 3 、 所示 , 其中图 2 为各子系统均正常工作时  的误 差 估 计 曲线 . 3为 在 10 至 30 之 间 G S无  图 0s 0s P 信 号输 出时 的误差估 计 曲线 。  

f】 张崇猛 等 信 息融合 理论及其 在 ISG SD p lr 3 N /P /o pe 组合导航  系统中的应用 中国惯性技术学报 19 . 9 99   【 刘瑞 华 , 建业 联邦滤波信 息分配新方法 中国惯 性技术  4 】 刘 学报 2 0 . 01   9

作者简介 : 李康伟(92 )男 , 18 一 , 汉族, 第二炮兵工程学  院在读研究生, 研究方 向: 导弹精确制导与突防技术,  
E-ma l lk ng i 20 0 1 3 c n . i:i a we 0 @ 6 .o  
_

(1 0 5陕西 西安 第 二炮 兵工 程 学 院) 李康 伟 王 宏  70 2
力 

( e S c n   tl r   n i e rn   l g , h n   Th   e o d Ari e y E gn e ig Col e S a n l e

x ,i a   1 0 5 L , n we W a gIo #i ix n 7 0 2 ) i   Ka g i   n, n   - I 通 讯 地址 :7 0 2 (1 0 5西 安 第二 炮 兵 工 程学 院研 究生 管 
理 大 队 二 队 ) 康 伟  李
( 日 : 0 .1) ( 日 : 0.2) 投稿 期 2 5 . 0 8 3 修稿 期 2 5 .   0 93

( 1 1 它使 得使 所 有方 程偏 差 的平 方 和达 到最 小  接 0 页)
图 2各子系统 均正常T作时陀螺误差估计 曲线 


这实 际上兼 顾 了所有 方程 的近 似程 度 , 整体 误差 达  使 到最小 

6 结 论 
通 过对 动力 调 谐 陀螺 漂移 测试 的研究 , 析误 差  分 参数 , 能够找到导致陀螺漂移的主要 因素. 用最小二  乘估计 各 误差 参数 虽 然精 度 不是 很高 , 算法 简单 适  但 用 , 以满 足一般 工程上 的要 求 。 可  
参考文献:   【 林 伟, 1 】 薛峰, 张晔晖 . 声光可 调谐滤光器 的原理 与应用[ 微计  J 】

图3IS P 系统出现故障时陀螺误差估计曲线    / S N G

算机信 息 2 0 , :2 — 2  051 17 18 0 【 郭秀 中 , 波 , 2 】 于 陈云相. 陀螺仪理论及应用 【 】北京 : M . 北京 

从图中可以看 出, 采用联邦卡尔满滤波器对 IS  N、 G SC S的输出信息进行融合 , P 、N 可以提高导航系统的  精 度 ,克 服惯 性 器 件 的误 差 随 时 间 积 累 的缺 陷 。 当  I SG S子 系统在 l O<< O s N/P O s t3 O 发生 硬件故 障 时 , 主滤  波器可 以将其隔离 , 仅利用 ISC S N /N 子系统的信息对  惯 导进 行校正 , 能保证 较高 的导航 精度 。 仍  

航空工业出版社 , 8. 17 9   [】 3杨惠连 , 张涛. 差理论及数据处理[ . 误 M】天津 : 天津大学出版 
社 , 9. 1 2 9  

『 邹达 明。 4 1 谢李李. 关于动 力调谐陀螺 8位置法漂 移测试 的研  究. 应用科技第 3 卷第 8期  1

作 者 简介 : 月慧 (9 6 ) , 士 , 邵 17 一 女 硕 主要从 事 导航 、 制 

导 与控制 方面 的研究 .m i2 12O 2O @btd . . E alO OO 3O7 ieuc   : . n
(0 0 1北京理 工大 学 自动 控制 系) 月 慧 10 8 邵 崔燕 

4 结 论 
组合导航系统 中的信息融合算法进行 了研究 , 仿真结 

( ej g Is tt  fT c n lg  Auo t   nr l B i n  n t ue o  eh oo y i i tma cCo to  i
f 投稿 日 20 .i) 倦 稿 日 20. . ) 期: 5 . 0 9s 期: 51 I   0 O8

p rme t10 8 C ia h oY e u C i n Y 本 文就联邦 卡尔曼 滤波器应 用于 IS P /N   De a t n 0 0 1, hn )S a , u h i u, a   N/ S S G C

@圄弼局阅: 46 , 1  自 邮订号26o 年 2 控 8   元 -1 -3 9 —


相关文档

联邦卡尔曼滤波在水下航行器组合导航系统中的应用
联邦卡尔曼滤波技术在多卫星组合导航系统中的应用
自适应联邦滤波器及其在组合导航系统中的应用
容错联邦强跟踪卡尔曼滤波算法在组合导航中的应用研究
联邦卡尔曼滤波器在无人机导航系统中的应用
卡尔曼滤波器在船用惯性导航系统中的应用
卡尔曼_粒子滤波器在船用组合导航中的应用
混合粒子联邦滤波在多信息组合导航系统中的应用
自适应卡尔曼滤波方法及其在容错组合导航系统的系统级故障检测中的应用
自适应卡尔曼滤波在GPS_DR组合导航中的应用
电脑版