基于有向图切换的INS／GPS组合导航VSMM算法

李小将， 王志恒， 王建华， 张东来

（1.装备学院 航天装备系，北京101416； 2.装备学院 研究生管理大队，北京101416）

INS／GPS组合导航通常需要滤波器来实现数据融合并减小定位误差，交互式多模型（interacting multiple model，IMM）滤波算法是一种常用的INS／GPS组 合 导 航 滤 波 算 法［1－3］。采 用IMM算法进行组合导航状态估计时，如果模型集中的某个模型与系统实际模态匹配，则可以得到较好的滤波效果；如果实际模态不在模型集覆盖的范围内，IMM算法滤波精度降低甚至发散［4］。因此在组合导航系统中应用IMM算法时，通常需要一个较大的模型集，但是由于IMM算法使用多个并行的滤波器，实现对每一个模型进行计算，这会带来巨大的计算量，难以满足导航系统实时性要求。

IMM算法中，一方面要保证模型集有足够的模型来覆盖系统的实际模态；另一方面要求减小模型数量以保证计算速度，这是相互矛盾的。针对IMM算法的上述问题，LI X R在文献［5］中提出了变 结构多模型（variable structure multiple model，VSMM）算法的思想，并在文献［6］中给出了算法的具体实现。VSMM算法被广泛应用在机动目标跟踪领域［7－9］，但是VSMM算法在组合导航中的应用和研究的文献尚未见到，鉴于此，本文研究了应用VSMM算法对INS／GPS组合导航进行状态估计的问题，并进行了仿真计算。仿真结果表明，基于有向图切换的变结构多模型（DS－VSMM）算法的INS／GPS组合导航系统能够得到比使用IMM算法更高的导航精度，并且滤波计算量明显降低。

1 INS／GPS组合导航数学模型

以输出三维位置、速度和姿态信息的INS和输出三维位置和速度信息的GPS为组合对象进行研究。采用INS的误差方程作为系统的状态方程，导航坐标系选用东北天地理坐标系，通过对INS的性能及误差源的分析，可以获得滤波器的误差方程为

式中：系统的状态变量定义为15阶，即

式中：φe，φn，φu为数学平台误差角；Δve，Δvn，Δvu为速度误差；Δλ，ΔL，Δh为经度、纬度和高度误差；εe，εn，εu，e，n，u分别为陀螺和加速度计零偏；FI（t）为系统状态转移矩阵，GI（t）为系统噪声输入矩阵，WI（t）为零均值高斯白噪声，式（1）中各矩阵具体形式可见文献［10］。

组合导航系统的量测量为INS给出的位置、速度与GPS给出的位置、速度的差值，建立量测方程如下：

式中：量测矩阵H＝［03×6，I6×6，06×6］，V（t）为量测噪声，假设为零均值的高斯白噪声，其协方差R（t）通常随着载体的运动状态变化和外界扰动等因素的影响而发生变化［11－12］。

2 有向图切换变结构多模型算法

通常一个VSMM方法具有2层结构：高层和低层。高层负责模型集合自适应（model－set adeptation，MSA），低层负责在确定的模型集合内运用IMM算法进行计算。不同于IMM算法，所有时刻都使用同一个模型集合进行计算，而VSMM算法，每一时刻所使用的模型集合可能只是总体模型集合中的一部分，因此能够有效降低计算量。截至目前，根据模型集合自适应思想的不同，可以将VSMM算法分为有向图切换方法、激活有向图方法和自适应网格方法3类［13］，基于这些思想产生了：模型群切换（model group switching，MGS）算法、可能模型集（likely－model set，LMS）算法、期望模型扩张（expected－model augmentation，EMA）算法等具体的VSMM算法。VSMM算法的提出最初是为了解决IMM算法的模型数量与计算量的矛盾问题，它的成功主要依赖于高效的MSA方法。所谓MSA方法，就是确定一个可以在线决定哪些模型需要删除，哪些模型需要添加的规则。建立更加智能、高效的MSA方法是VSMM估计的理论研究的核心。有向图切换是一种比较简单通用的MSA方法，针对上述INS／GPS组合导航变噪声滤波问题，建立了DS－VSMM模型集有向图，并设计了有向图切换规则。

2.1 INS／GPS组合导航模型集合的建立

LI X R将图论的思想引入多模型估计问题中，并利用图论的相关理论，解决了模型集合自适应的问题。模型中的每一个模型可以看作图的一个顶点；模型之间的相互转化，可用一条带有权值的有向边表示，则VSMM算法所使用的模型集和模型转换法则一起，便构成了一个随机有向图［14］。

本文根据INS／GPS导航系统的量测噪声方差矩阵的变化范围，建立一个包含5个模型的模型集合D，如图1所示。这里假设M1为R（k）变化的最小值；M5为R（k）变化的最大值；M3为R（k）变化的中间值，并且R（k）大部分时间在M3附近波动。将模型集合按照R（k）的变化规律划分为3个 子 模 型 集 合：D1、D2、D3。D1＝｛M2，M3，M4｝，D2＝｛M1，M2，M3｝，D3＝｛M3，M4，M5｝，3个子模型集合分别以M2、M3、M4为中心模型，3个子模型也可称作有向子图，中心模型的后验概率与有向图切换密切相关。

图1 INS／GPS组合导航模型集合

2.2 有向图切换规则的建立

根据图1的模型集合，本文根据中心模型的概率进行有向子图之间的切换。有向图切换规则具体为

式中：D（k）表示k时刻匹配的有向图；μM1、μM2、μM3分别为M1、M2、M33个模型k－1时刻的后验概率；μ为一个大于0.5的、根据经验设定的门限值。某个中心模型概率一旦大于门限值，即切换到以该模型为中心的有向子图进行IMM运算；若没有中心模型后验概率大于门限值，则不进行有向图切换。需要注意的是，由于模型切换的马尔可夫性，不允许从D2直接切换到D3，而必须通过D1作为过渡，反之亦然；每个模型也只允许在相邻的模型间切换，这些将在转移概率矩阵中体现。

2.3 DS－VSMM算法的具体步骤

DS－VSMM一个计算周期包括2个步骤：根据中心模型概率和有向图切换规则进行有向图切换；在匹配有向图内进行IMM滤波。DS－VSMM算法流程［15］如图2所示。

图2 DS－VSMM算法流程

2.3.1 有向图切换

根据上一周期的有向子图以及有向图切换规则选择当前周期与系统实际模态匹配的有向子图。

2.3.2 IMM滤波

以当前所位于的有向子图为IMM算法的模型集进行滤波估计。IMM算法主要包含4个步骤。

1）输入交互：新激活模型的初始概率由那些允许切换到这些新模型的上一时刻模型（状态转移概率矩阵中的对应值不为零）概率值得到，新激活模型的初始估计，也由那些允许切换到这些新模型的上一时刻模型的估计值确定。

2）模型滤波：选择适当的滤波器对当前有向图中的每个模型进行滤波计算，本文选择标准的卡尔曼滤波器。

3）模型概率更新：计算各模型的后验概率，用于输出融合和决定下一时刻的MSA。

4）输出融合：将不同模型的估计值，按照概率加权组合得到最终滤波估计。

IMM算法的计算过程可参见文献［16］。

3 仿真分析

为了检验算法的性能，将DS－VSMM算法应用到INS／GPS组合导航系统中。选取INS为主导航系统，以姿态、速度及位置误差为状态向量，建立滤波器状态方程和量测方程，如第1节中式（1）和式（3）所示，GPS作为辅助导航系统提供量测信息。

分别采用IMMKF，DS－VSMM滤波器对载体的姿态、速度及位置等误差信息进行估计，算法的性能好坏可通过INS／GPS组合导航系统的定位精度和滤波计算时间来衡量。假设载体初始位置为东经116°，北纬40°，高度50m，以20m／s的速度匀速向东运动，持续1 400s。INS的陀螺常值漂移为0.01°／h，随机漂移为0.001°／h；加速度计零偏为10－4g，随机漂移为10－5g；GPS水平位置误差为10m，高度误差为20m，水平速度误差为0.1m／s，垂直速度误差0.4m／s。地球半径r取6 378 137 m。INS采样周期为0.1s，GPS采样周期为1s。

量测噪声协方差矩阵的初始值为R0＝diag｛（10m）2；（10 m）2；（20 m）2；（0.1 m／s）2；（0.1m／s）2；（0.4m／s）2｝，在载体运动过程中，假设GPS受外界干扰使量测噪声R（k）出现如下变化：

DS－VSMM滤波器中模型集合如图1所示，M1～M5中量测噪声方差矩阵分别为0.25R0、0.5R0、R0、2R0、4R0，有向图划分与2.1节中D1、D2、D3相同，转移概率矩阵为

中心模型切换的门限值μ取为0.6。IMM滤波器模型集中同样采用上述5个模型，转移概率矩阵为

根据以上条件应用IMM算法和DS－VSMM算法对量测数据进行滤波计算，有量测信息时进行滤波，无量测信息时只根据各个模型进行预测，得到INS／GPS组合导航系统的东向位置误差、东向速度误差、航向角误差分别如图3～图5所示。

图3 2种滤波东向位置误差对比

图4 2种滤波东向速度估计误差对比

图5 2种滤波航向角误差对比

从图3～图5可以看出，在采用相同模型集的情况下，分别采用IMMKF和DS－VSMM滤波时，INS／GPS组合导航系统的速度误差、航向角误差大小基本一致，但使用DS－VSMM滤波时，位置误差要小于IMMKF滤波。表1给出了使用2种算法的计算时间。

表1 IMMKF和DS－VSMM滤波计算时间

IMMKF算法每一时刻都使用5个模型计算；DS－VSMM算法每一时刻使用3个模型计算，理论上，单从每个模型计算量来看，DS－VSMM滤波器计算量应该是IMM滤波器的60%。但是由于有向图切换的计算增加了部分计算时间。因此，DS－VSMM算法滤波的计算时间是IMMEKF算法滤波的63.62%，这是可以接受的。可以看出，DS－VSMM滤波器相 对于IMMKF滤波器明显降低了计算量。

4 结 论

本文针对INS／GPS组合导航的滤波过程中量测噪声变化的问题，研究了将变结构多模型算法应用到INS／GPS组合导航中的基本流程。根据有向图切换的思想，建立了INS／GPS组合导航模型集合有向图，并设计了一种模型集合自适应规则，对应用DS－VSMM算法的INS／GPS组合导航进行了仿真分析，仿真结果表明，DSVSMM滤波算法定位精度略优于IMMKF算法，同时有效降低了滤波器的计算量。

（

）

［1］曹梦龙，崔平远.多模型高精度组合导航算法研究［J］.系统工程与电子技术，2008，30（7）：1304－1307.

［2］王晓旭，赵琳.自适应融合滤波算法及其在INS／GPS组合导航中的应用［J］.宇航学报，2010，31（11）：2503－2511.

［3］QIAN Huaming，AN Di，XIA Quanxi.IMM－UKF based land－vehicle navigation with low－cost GPS／INS［C］／／IEEE.Proceedings of the 2010IEEE International Conference on Information and Automation.Harbin：IEEE，2010：2031－2035.

［4］徐田来，崔平远，崔祜涛.组合导航自适应多模型算法研究［J］.系统工程与电子技术，2008，30（11）：2071－2074.

［5］LI X R.Multiple－model estimation with variable structure：some theoretical considerations［C］／／IEEE.33rdIEEE Conf.Decision and Control.Lake Buena Vista：IEEE，1994：1199－1204.

［6］LI X R，SHALOM Y B.Multiple－model estimation with variable structure［J］.IEEE Transactions on Automatic Control，1996，41（4）：478－493.

［7］陈晓峰，嵇成新，陈阳.一种新的机动目标跟踪中的变结构多模型算法［J］.火控雷达技术，2007，36：31－35.

［8］雷世文，吴慈伶，孙伟.一种基于VSMM的自适应高机动目标跟踪方法［J］.现代雷达，2010，32（6）：54－58.

［9］郭云飞，张幸，林岳松.基于DS－VSMM的声网络低空机动目标跟踪［J］.光电工程，2011，38（8）：1－6.

［10］卞鸿巍，李安，覃方君，等.现代信息融合技术在组合导航中的应用［M］.北京：国防工业出版社，2010：100－106.

［11］肖乾.伪距GPS／INS组合导航系统的多模型自适应卡尔曼滤波器设计［J］.船舶工程，2005，27（6）：59－62.

［12］刘睿，王常虹，李葆华.自适应交互多模型滤波在INS／CNS组合导航中的应用［J］.红外与激光工程，2010，39（5）：843－847.

［13］刘扬，刘火平，吴勤章.多模型估计的新分支：变结构多模型方法［J］.现代防御技术，2011，39（3）：163－169.

［14］许江湖，嵇成新，张永胜，等.基于当前统计模型的有向图切换IMM算法［J］.火力与指挥控制，2003，28（2）：52－56.

［15］黄翔宇，彭冬亮.基于无味有向图切换的机动目标跟踪VSMM算法［J］.光电工程，2010，37（12）：30－34.

［16］林雪原.基于交互式多模型的多传感器组合导航系统［J］.兵工自动化，2011，30（6）：27－30.