第 3期 2006年 6月 湖南人文科技学院学报 Journal of Hunan Institute of Humanities, Science and Technology No. 3 Jun. , 2006 GPS/ SINS组合导航系统的多传感器最优融合算法 (1. 湖南人文科技学院数学系 ,湖南 　娄底 　417000; 2. 湖南娄底广播电视大学 ,湖南 　娄底 　417000) 宋迎春 1 , 刘希贤 2 , 李琼 2 摘 　要 :对 GPS/ SINS组合导航的工作原理和模型建立进行详细的分析 ,设置了两个传感器 。提出一种基于信息融 合的导航参数最优估计滤波方法 。通过计算机仿真说明该方法可提高导航系统的计算精度和速度 ,有较好的容错性和 环境适应性 ,具有实际使用价值 。 关键词 :组合导航 ;卡尔曼滤波 ;信息融合 ;多传感器 中图分类号 : O242. 1　　文献标识码 : A　　文章编号 : 1673 - 0712 (2006) 03 - 0001 - 04 Optimal Information Fusion Algorithm Ba sed on M ulti Sensor in GPS /SINS Integrated Navigation System SON G Ying chun1 , L IU X i x ian2 , L I Q iong2 (1. Math　Department, Hunnan Institute of Humanities, Science and Technology, Loudi, 417000, China; 2. Loudi B roadcast Television College, Loudi, 417000, China) Abstract: The p rincip les and mathematics model of GPS/ SINS integrated navigation system was analyzed in detail, two sen sors were set up. An op timal information fusion algorithm based on multi - sensor was given. A t last, this paper gives the comput er simulation. The results showed that this method was p ractical for imp roving accuracy and calculation speed of navigation sys tem. Key words: integrated navigation; Kalman filtering; information fusion; multi sensor 在现代导航系统中 ,对导航信息量的要求越来越多 , 对导航精度的要求越来越高 。要使系统性能得到提高 ,靠 提高单一导航系统 (如 INS)的精度 ,不仅在技术上难度很 大 ,而且在实际中效果也不十分明显 。组合导航技术的出 现 ,使得这一问题有了完美的解决方案 。在实际应用中 , 将几种导航系统安装在同一载体上 ,构成多传感器组合导 航系统 [ 1 ] 。利用卡尔曼滤波器进行导航参数和系统状态 估计 [ 2 ] ,可以综合各种系统的优点 ,达到取长补短 、提高性 能的目的 ,更好地满足载体对导航系统的要求 。具有高精 度 、实时 、连续导航的全球定位系统 GPS自诞生之日起就 备受人们关注 。然而它是非自主式导航系统 ,在动态环境 或受遮挡时易出现“丢星 ”现象而导致定位中断 。而惯性 导航系统 INS的最大优点是在不依赖任何外界信息的情 况下 ,可完全自主的导航 ,但其定位误差随时间不断积累 。 可以看到 ,二者具有极强的互补性和非相似性 ,将他们组 合起来 ,相互取长补短 ,就能充分利用各子系统的信息 ,提 高导航精度和可靠性 ,扩大使用范围 。过去 ,组合导航系 统通常采用集中化卡尔曼滤波器 [ 3 - 5 ] ,这种滤波器的主要 缺点是滤波器的阶次太高 ,计算量太大 ,而且任何一个导 航系统出现故障 ,都直接影响组合系统的性能 ,甚至使组 合导航失败 ,即系统没有容错能力 。最近十余年里 ,分散 卡尔曼滤波方法 [ 6 - 8 ]引起了人们的广泛注意 ,这种方法把 来自每种导航传感器的观测信息各用一个子滤波器处理 , 然后再用一个主滤波器把各子滤波器的输出信息进行融 合 。它的基本思想是先分散处理 、再全局融合 。在联合滤 波中 ,标准的卡尔曼滤波器分成对应于不同传感器的多个 局部滤波器 ,每个局部滤波器并行工作 ,并通过全局滤波 器进行信息分析与综合 。产生最优滤波结果 。概括地说 , 多传感器信息融合技术就是指通过一定的算法“合并 ”来 自多个信息源的信息 ,以产生比单个传感器所得到数据更 可靠 、更准确的信息 ,并根据这些信息作出最可靠的决策 。 信息融合技术的采用能够扩展整个系统的时间 、空间覆盖 　收稿日期 : 2005 - 12 - 29. 　基金项目 :国家自然科学基金资助项目 (40574003) ;教育部博士点基金资助项目 (20050533057). 　作者简介 :宋迎春 (1965 - 　) ,男 ,副教授 ,博士研究生 ,从事测量、导航数据处理与研究。 

湖南人文科技学院学报 总第 90期 率 ,增加系统的信息利用率并提高整个系统的可靠性和精 度 。 1　系统状状态方程 1. 1　捷联惯性导航系统 ( SINS)的误差模型 捷联惯性导航的平台系统取为当地坐标系统 , 其 xyz 分别指向东 ,北 ,天方向 。捷联惯性导航系统 ( S IN S ) 的状 态变量为 : X I ( t) = (φe ,φn ,φu ,δV e ,δV n ,δV u ,δL,δλ,δh,εre ,εrn , e , u ) T n , εru , 其中 姿态误差为 (φe ,φn ,φu ) , 速度误差为 (δV e ,δV n , δV u ) ,位置误差为 (δL,δλ,δh) , 陀螺漂移 (εrn ,εre ,εru ) , 加 速度计误差为 ( u ) 。 n , e , ( 1) ( t) = A I ( t) X I ( t) + GI ( t)W I ( t) 捷联惯性导航系统 ( S IN S ) 的误差模型 [ 1 ] 为 : · X I A I ( t) ∈ R15 ×15是惯导系统误差传导矩阵 , GI ( t) ∈ R15 ×6 是系数矩阵 , W I ( t) 是白噪声 , W I ( t) = (ωre ,ωrn ,ωru ,ωae ,ωan ,ωau ) T 具有统计性质 E{W I } = 0, E{W I ( t) W T 其中 δ(τ) 是 D irac delta函数 , Q 动态噪声的协方差 I ( t +τ) } = Qδ(τ) 矩阵. 1. 2　GPS误差模型 GPS误差模型可以简单地表述为 · D 其中 (D tur , Etur ) 时钟偏差和时钟漂移 , β相关时间常 · tur = Etur + wdu , E tur = - βEtur + weu 数 ; (wdu , weu ) 是白噪声 。 设 XG ( t) = [D tur , Etur ] T , 。那么 · XG ( t) = AG ( t) XG ( t) + W G ( t) 其中 AG = 0　1 0　 - β ,W G ( t) = wdu , weu T GPS/ IN S组合导航的动态模型为 : · I ( t) X X · G ( t) = A I　0 0　AG X I ( t) XG ( t) + GI ( t) 　0 0　 I W I ( t) W G ( t) y, z) 可以通过下式求得 : [ RN (1 - f) + h ] sinL x = (RN + h) cosL cosλ, y = (RN + h) cosL sinλ, z = (3) 设 IN S系统输入的飞行器地理位置是经度 λI、纬度 L I 和高度 hI,则对应于惯导给出的飞行器在地球坐标系中的 真实位置 ( xI, yI, zI) 与经度 λI、纬度 L I和高度 hI 也有 (3) 式的关系 。设第 i颗卫星在地球坐标系中的位置是 ( xsi , ysi , zsi ) ,则对应于惯导给出的飞行器 ( xI , yI, zI) 到第 i颗卫星 的伪距为 : ρli = ( xI - xsi ) 2 + ( yI - ysi ) 2 + ( zI - zsi ) 2 ( i = 1, 2, 3, 4) (4) 设 (δx,δy,δz) 指惯导给出的飞行器在地球坐标系中 的位置误差 ,则 xI = x +δx, yI = y +δy, zI = z +δz。把 (4) 在 ( x - xsi , y - ysi , z - zsi ) 展开 ,并忽略掉二阶以上的的高 次项 ,可得 : ρli = ri + eixδx + eiyδy + eizδz　 其中 ri = ( x - xsi ) 2 + ( y - ysi ) 2 + ( z - zsi ) 2 为飞 (5) 行器真实位置至第 i颗卫星的距离 ; eix = ( x - xsi ) / ri , eiy = ( y - ysi ) / ri , eiz = ( z - zsi ) / ri 分别为相应的的方向余弦 。 GPS接收机测得的到第 i颗卫星的伪距可表示为 ρGi = ri - δtu - vρi 伪距的测量值可以表示为 : δρli = ρli - ρGi = eixδx + eiyδy + eizδz +δtu + vρi 取 ( i = 1, 2, 3, 4) ,可得 : δρ = e[δx,δy,δz ] T + D tuδtu + V1 (8) 其中 e = [ eij ]4 ×3 , ( i = 1, 2, 3, 4 j = x, y, z) ; D tu = [ 1, (6) (7) 1, 1, 1 ] T , V1 = [ vρ1 , vρ2 , vρ3 , vρ4 ] T。 由于 (δx,δy,δz) 指惯导给出的飞行器在地球坐标系 中的位置误差 ,而惯导误差方程中给出的位置误差是 (δL, δλ,δh) ,所以由 (3) 式可得 : [δx,δy,δz ] T = C ( t) ·[δL,δλ,δh ] T 其中 C ( t) = (9) - (Rn + h) sinL cosλ - (Rn + h) sinL sinλ [ Rne (1 - e2 ) + h ] cosL - (Rn + h) cosLsinλ cosLcosλ (Rn + h) cosLcosλ cosLsinλ 0 sinL ( t) = A ( t) X ( t) + G ( t)W ( t) · 即 X 其中 X ( t) = X I ( t) , XG ( t) T ,W ( t) = W I ( t) ,W G ( t) T。 2　系统观测方程 (2) 将上式代入 (8) 得 : δρ = e·D ( t) ·[δL,δλ,δh ] T + D tuδtu + V1 据此可得到伪距观测方程如下 : Z1 ( t) = H1 ( t) X ( t) + V1 ( t) 其中 H1 ( t) = [ 04 ×6 , e·D ( t) , 04 ×6 , D tu , 04 ×1 ] 是观测矩 (10) 阵 。 2. 1　伪距观测方程 [ 9 ] 如果飞行器真实地理位置以经度λ、纬度 L和高度 h表 示 ,则与此对应的飞行器在地球坐标系中的真实位置 ( x, 为 : 2. 2　伪距率观测模型 [ 9 ] 飞行器真实位置 ( x, y, z) 至第 i颗卫星的伪距变化率 2 

¾ ¾ xsi ) + eix ( y - ysi ) + eix ( z - zsi ) x - ri = eix ( 由 (9) 两边求导有 : ρli = ri + eixδ x + eiyδ y + eizδ z + Eixδx + Eiyδy + Eizδz (11) ri eiy ) / ri , ysi - y - 其中 Eix = ( ri eix ) / ri , Eiy = ( Eiz = ( z - zsi - xsi - x - ri eiz ) / ri。 y,δ 上式中的 (δx,δy,δz) 表示惯导给出的飞行器在地球 坐标系中的位置误差 ,可采用 (9) 换成 (δL,δλ,δh) ; (δ x, δ z) 表示惯导给出的飞行器在地球坐标系中的速度误 差 ,其中 x, zI ] T = n (L I,λI) ·[ vxI, vy I, vzI ] T , Ce n (L,λ) 为导航坐标系到地球 Ce 坐标系的转换矩阵 ,下标 I表示由惯导给出的值 ,无下标者 表示真实值 。由于 : n (L,λ) · [ vx , vy , vz ] T , [ z] T = Ce yI, xI, y, [ n (L,λ) = Ce - sinλ - sinL cosλ cosL cosλ cosλ - sinL sinλ cosL sinλ 0 cosL sinL n (L I,λI ) 在 L,λ附近 按泰勒公式展开 ,并忽略掉二次及二次以上的高次项 ,得 : 又 L I = L +δL,λI =λ +δλ,将 Ce (12) 总第 90期 宋迎春 ,刘希贤 , 李琼 : GPS / S IN S组合导航系统的多传感器最优融合算法 n (L I,λI ) = Ce Ce 其中 n (L,λ) + DLδL + Dλδλ 0 0 - cosL cosλ - sinL cosλ - cosL sinλ - sinL sinλ , DL = (13 ) cosL - sinL 0 - cosλ sinL sinλ - cosL sinλ - sinλ - sinL cosλ cosL sinλ 0 0 0 Dλ = 于是 ≈ Ce n (L I,λI) xI yI zI vx +δxx vy +δxy vz +δxz + DL vx vy vz δL + Dλ δλ vx vy vz z ] T x,δ y,δ 据上述各式可推得 : [δ n (L,λ) ·[δvx ,δvy ,δvz ] T + DE ·[δL,δλ,δh ] T 式中 = Ce (14) (15) DE = - vy cosL cosλ - vz sinL cosλ - vz cosλ + vy sinL sinλ - vz cosL sinλ 0 - vy cosL sinλ - vz sinL sinλ - vz sinλ - vy sinL cosλ + vz cosL cosλ 0 0 - vy csinL + vz cosL 0 ri - δtru - v ρi GPS接收机测得的飞行器到第 i颗卫星的伪距率为 : ρGi = 　 (16) 由 (9) 式及 (16) 可得伪距率观测方程 : δ ρi = eixδ z + Eixδx + x + eiyδ y + eizδ Eizδy + Eizδz +δtru + V ρi (17) 把 (7) , (15) 代入上式 ,并取 i = 1, 2, 3, 4,可得如下的 伪距率观测 δρ = Dv ·[δvx ,δvy ,δvz ] T + DP ·[δL,δλ,δh ] T + D truδtru + V2 上式中 Dv = eCe n , DP = eDE + ED ( t) , D tru = [ 1, 1, 1, 1 ] T ,其中 E = [ Eij ]4 ×3 , ( i = 1, 2, 3, 4 j = x, y, z　) 。最后可 以得到伪距率观测方程如下 : (18) Z2 ( t) = H2 ( t) X ( t) + V2 ( t) H2 ( t) = [Dv , 04 ×3 , Dρ, 04 ×7 , D tru ] (19) (20) 3　多传感器的最优融合估计 把 (2) 离散化 ,得到离散化的动态动态方程为 : X ( k + 1) = Φ ( k + 1, k) X ( k) +Γ( k + 1, k)W ( k) (21) 两个传感器的观测方程为 : Zi ( t) = H i ( t) X ( t) + V i ( t) i = 1, 2 (22) 设 V i ( k) 是 m维观测高斯白噪声 ,即 V i ( k) ～ N [ 0, R i ( k) ] ,状态向量的初始值为 X0 的随机向量 , 并且有 : E{X (0) } = X0 , E{ (X (0) - X0 ) (X (0) - X0 ) T } = P0 ,这 里 V1 ( k) , V2 ( k) ,W ( k) 及初始状态 X (0) 是相互独立的 。 总的观测方程为 : HT Z( k) = H ( k) X ( k) + V ( k) , 　V ( k) ～ N [ 0, R ( k) ] (23) 1 ( k) , 1 ( k) , ZT 1 ( k) , VT 其中 : Z( k) = [ ZT 2 ( k) ] T , V ( k) = [VT 2 ( k) ] T , H ( k) = [ HT 2 ( k) ] T , R ( k) = diag[ R1 ( k) , R2 ( k) ]。 对于传感器 i的 Kalm an滤波为 : ^Xi ( k + 1 | k + 1) = ^Xi ( k + 1 | k) + Ki ( k + 1) [ Zi ( k + 1) - H i ( K + 1) ^Xi ( k + 1 | k) ] ^Xi ( k + 1 | k) = Φ ( k + 1, k) ^Xi ( k | k) 滤波增益为 : Ki ( k + 1) = Pi ( k + 1 | k) HT i ( k + 1) [ H i ( k + 1) (24) (25) P ( k + 1 | k) HT i ( k + 1) + R i ( k + 1) ] - 1 (26) 方差为 : Pi ( k + 1 | k) = Φ ( k + 1, k) Pi ( k | k)ΦT ( k + 1, k) + (27) Γ( k) Q ( k)ΓT ( k) Pi ( k + 1 | k + 1) = Pi ( k + 1 | k) - Ki ( k + 1) H i ( k + 1) Pi ( k + 1 | k) 基于全局信息的最优融合估计值 (28) 3 

2 2 湖南人文科技学院学报 总第 90期 组合导航系统具有高精度 、高容错性及高可靠性 。当融合 周期以各子滤波器运行周期的倍数增加时 ,精度损失并不 严重 ,大大地增强了系统的数据处理能力. 对于实时工作 滤波系统具有很大的实用意义 。 3 ^X ( k + 1 | k + 1) = P ( k + 1 | k + 1) [ ∑ - P- 1 ( k + 1 | k) ^X ( k + 1 | k) ] P- 1 i 2 i = 1 ( k + 1 | k + 1) ^Xi ( k + 1 | k + 1) P ( k + 1 | k + 1) = P- 1 ( k + 1 | k) + ∑[ P- 1 i ( k + 1 | k + 1) - P- 1 ( k + 1 | k) ] (29) (30) 其中 ^X ( k + 1 | k) 为一步预测估计值 ^X ( k + 1 | k) = Φ ( k + 1, k) ^X ( k | k) P ( k + 1 | k) 为一步误差协方差阵 P ( k + 1 | k) = Φ ( k + 1, k) P ( k | k)ΦT ( k + 1, k) + (32) (31) Q ( k) 4　数字仿真 飞行轨迹按照实际复杂的高动态飞行而设计 。飞机 的初始位置为东经 1320,北纬 280,高度 1000米 。从 0秒 开始 ,飞机以初始速度 50米 /秒 ,匀速平直向前飞行 10秒 , 加速向前飞行 50秒 ,匀速平直前飞行 10秒 ,拉起 2秒 ,爬 行 50秒 ,改平 2秒 ,匀速平直前飞行 10秒 ,飞机进入右拐 弯 1秒 ,右拐弯 50秒 ,改平 1秒 ,匀速平直前飞行 10秒 ,进 入俯冲 2秒 ,持续俯冲 2秒 。整个飞行过程 200秒 ,滤波周 期设置为 1秒 。滤波器的初始状态方差设置如表 1所示 , 系统噪声和观测噪声如表 2所示 。 表 1　滤波器的初始状态值设置 初始状态 位置误差 速度误差 姿态误差 陀螺零漂 加速度计零偏 接收机时钟偏差 接收机时钟漂移 设定值 1000m 2m / s 100’ 10 / h 10 - 4g 1000m 3m / s 表 2　系统及观测噪声方差 噪声方差 设定值 位置噪声方差 东 ,北向 1000m2, 天 2000 m2 速度噪声方差 东 ,北向 1 (m / s) 2, 天 1. 44 (m / s) 2 姿态噪声方差 100 ( secm in) 2 陀螺随机漂移 10 / h 加速度计零偏 4 伪距噪声方差 400 m2 伪距率噪声方差 0. 01 (m / s) 2 10 - 4g 我们利用 Matlab对集中式滤波和多传感器最优融合 算法对东向位置误差的滤波结果进行了模拟 ,见下图 。 从模拟的结果看 ,由于集中式卡尔曼滤波器应用到组 合导航系统 ,具有以下问题 : 1)计算量大 ; 2)容错性差 ; 3) 无法处理串联滤波的解 ,使得集中式卡尔曼滤波的滤波精 度比多传感器最优融合算法的精度要小得多 。多传感器 4 图 1　东向位置误差 ,多传感器和集中式滤波的比较 5　结论 可靠性 、高精度 、连续性以及实时性都是检验导航系 统性能的重要指标 ,在一些特殊情形环境下 ,可靠性尤其 重要 。本文对 GPS/ INS组合的导航系统进行了论述和分 析 ,建立了一个多传感器的滤波系统 ,并通过一个仿真实 例说明了多传感器组合导航系统具有 ( 1)高精度 、高容错 性及高可靠性 ,它克服了 GPS非自主性的缺点 。 ( 2)采用 信息融合算法能够减少计算量 ,提高效率 。综上所述 ,利 用联合卡尔曼滤波器对多传感器组合系统进行信息融合 处理 ,是求解导航参数最佳估计的有效途径 。 参考文献 : [ 1 ] S. A. B roatch, A. J. Henley. An Integrated Navigation system Man ger U sing Federated Kalman Filtering[M ]. [ 2 ] Farhan A. Faruqi , Kenneth J. Turner. Extended Kalman Filter synthesis for integrated global positioning/ inertial navigation sys tem s[ J ]. App lied Mathematics and Computation 115 (2000) 213 - 227. [ 3 ]Curtis D. Evans. The Design and Analysis of Integrated Navigation System s U se Real INS and GPS Data[M ]. [ 4 ]何秀风 ,陈永奇. GPS/ INS组合导航系统抗差滤波器设计 [ J ]. 测绘学报 , 1998, 27 (2) , 177 - 184. [ 5 ]X. F. He, Y. Q. Chen, B. V ik. Design of Robust Filtering for An Integrated GPS/ INS System [ J ]. Journal of Geodesy, 73 ( 1999 ) : 407 - 411. [ 6 ]刘瑞华 ,刘建业 ,孙永荣. 多传感器组合导航系统研究 [ J ]. 航 天控制 , 2001, (2) : 67 - 72. [ 7 ] Shu - li Sun. Multi - sensor op timal information fusion Kalman fil ters with app lications [ J ]. Aerospace Science and Technology. 8 (2004) 57 - 62. [ 8 ]Hong Zhuan Q iu, Hong Yue Zhang, Hong J in. Fusion algorithm of correlated local estimates[ J ]. Aerospace Science and Technology 8 (2004) 619 - 626. [ 9 ]杨静 ,张洪钺 ,朱士青. 基于伪距、伪距率的 GPS/ SINS容错组 合导航系统 [ J ]. 航天控制 , 2003, (4).