基于多模型联合滤波的MIMU/GPS组合导航方法

沈 强，刘洁瑜，李 灿，崔亚龙，李新三

（1. 火箭军工程大学 导弹工程学院，西安 710025；2. 中国人民解放军96851 部队，盘锦 124214）

惯性导航系统具有隐蔽性、自主性、宽频性以及信息全面等优势，在军用和民用领域均得到了广泛应用。其缺点为导航误差随时间累积，特别是对于MIMU 这种精度相对较低的惯性系统而言，有必要通过组合导航的方式对导航误差进行校正[1]。

组合导航需要高性能导航滤波技术的支撑。目前，导航滤波通常采用概率化滤波方法来解决[2-4]。这类方法解决了很多应用中的问题，但是，这些概率化方法对噪声的分布都有严格的要求，并要求其统计特性已知，导致这类方法存在一定的缺陷。为解决该问题，可采用自适应的卡尔曼滤波算法，将系统的动态变化看作随机干扰归到模型和噪声中去，对模型参数和噪声统计特性进行估计和修正使之适应系统动态变化[5-7]，组合导航中使用的自适应滤波的方法有多种，包括模糊自适应滤波、衰减记忆滤波、Sage-Husa自适应滤波等，研究表明，在组合导航中自适应滤波可以修正一定的噪声统计特性。近年来，另一类滤波方法，集员估计方法[8-10]正逐渐受到重视。该类方法只要求噪声未知但有界（Unknown But Bounded, UBB），由于在实际系统中噪声的边界信息较之概率化假设来说更容易获取，可克服概率化滤波方法在应用中存在的局限性。

但是，对于MIMU/GPS 组合导航系统，两类噪声是同时存在的，GPS 接收机跟踪环路中的热噪声是典型的高斯白噪声，而动态应力和振动引起的时钟相位抖动则属于非高斯噪声[11]，MEMS 陀螺的随机漂移是白噪声与多种非高斯噪声的叠加。对于这些包含了未知不确定干扰的非高斯噪声，获取边界往往比获取其随机统计特性更加可行。在这种情况下，采用随机噪声的滤波方法会导致估计结果过于乐观甚至失去收敛性，而采用集员估计方法又会造成边界过于保守，所以，对于含有两种噪声的系统，人为地忽略其中一种不确定性，单独使用随机噪声类方法或集员估计方法对估计结果都是不利的。为克服单一方法的局限，充分利用两种方法的优势，保证可靠的估计结果，有学者提出了结合两种噪声模型的联合滤波（Combined Filter）方法。文献[12]用概率密度函数集合取代单一的概率密度函数来描述噪声造成的不确定性，提出了集合概率的概念。Liu[13]等针对纯方位机动目标的跟踪问题，在椭球集员滤波的更新过程中考虑了随机噪声，仿真结果显示其跟踪性能优于EKF。文献[14]则是采用可加性随机噪声和有界噪声的设定，并假设系统中的两种噪声独立存在，随机噪声采用卡尔曼滤波处理，有界噪声则采用椭球定界方法处理，然后结合在一起构成联合滤波算法。仿真结果显示，这类算法可以避免非高斯噪声导致的卡尔曼滤波性能下降，同时得到鲁棒性较强的边界保证估计。但是，对于导航类应用，当运动状态多变时，单模型方法会因为无法跟踪载体的机动状态而导致精度损失，而交互多模型（Interacting Multiple Model, IMM）方法因其优异的性能得到了广泛的应用[15,16]。

为此，本文将高斯噪声融入集员框架下，构造双重不确定模型集，同时推导了多个椭球集的加权Minkowski 和的运算过程并给出了参数优化方法，从而实现集合运算与IMM 交互过程相结合，将其中的UBB 噪声应用集员估计的思想进行处理，提出了考虑两种不确定干扰的交互多模型联合滤波（IMM-based Combined filter, IMMCF ）方法，并将用于MIMU/GPS 组合导航中。

1 双重不确定性组合导航系统模型

在松组合[1]模式下，GPS 和MIMU 各自独自工作，然后通过适当的滤波算法对两种系统的数据进行融合，得到最终的导航结果，系统状态方程中的状态向量共15 维，包括纬度、经度、高度误差 δL、δ λ 、δ h ，东向、北向、天向速度误差 δ VE、δVN、 δVU，数学平台姿态角误差δφE、δφN、 δφU以及x、y、 z 轴上的陀螺漂移误差 εbx、εby、εbz和加速度计漂移误差Bbx、Bby、Bbz。

状态方程为:

式中，系统噪声为w=[ wgx,wgy,wgz,wax,way,waz]T，其中的元素通常假设为x、 y 、 z 轴上的陀螺白噪声和加速度计白噪声，为了提高导航的精度和鲁棒性，本文采用高斯/有界双重噪声来描述其误差特性。系统噪声分配阵为

量测方程为：

式中，L、λ 、h 、 VE、VN、 VU分别为系统纬度、经度、高度和东向、北向、天向速度，而下标I 代表MIMU 解算值，G 代表GPS 测量值。 RM、 RN分别为载体位置对应的子午圈和卯酉圈的曲率半径。量测阵H 为6×15 维矩阵，其非零元素为H1,1=RM，H2,2=RN， H3,3= 1， H4,4= 1， H5,5= 1， H6,6= 1。观测噪声为 v=其中的元素为GPS 沿地理坐标系各轴向的距离、速度测量误差，本文采用高斯/有界双重噪声来描述其误差特性。

离散化后的组合导航系统方程如下所示：

过程噪声和量测噪声分别描述为：

其中， Dk和 Ek为已知的正定矩阵。由此可以得到过程噪声和量测噪声的分布分别为

初始状态 x0符合如下分布：

2 交互多模型联合滤波算法

2.1 多椭球加权Minkowski 和运算

在标准IMM 算法中，模型间的交互是以加权求和的方式实现的。当系统中高斯噪声和有界噪声均存在时，模型的交互涉及到集合运算，最终会表现为多个椭球的加权Minkowski 和，并利用椭球近似外包实现状态的重初始化和更新。为求解多椭球加权Minkowski 和的外定界椭球，首先给出如下引理[17]：

给定椭球集：

则对于

∀ α ∈Rr满足αk＞ 0且包含其中：

对于椭球集 E（ak,Mk），给定 μk∈R，容易得到结合引理1，可以得到如下结论：给定 μk∈R 和椭球集 E（ak,Mk）则对于∀ α ∈Rr满足αk＞ 0且椭球 E（a ,M）包含其中：

依照上述结论得到一个与参数α 有关的椭球簇，该椭球簇包含多个椭球的加权Minkowski 和，需要依据一定准则寻求其中的最优解。考虑到导航的实时性要求，本文采用最小迹准则。待优化的目标函数可以写为

利用拉格朗日乘子法，可以得到最小迹准则下的最优参数取值为

2.2 算法推导

IMMCF 与标准IMM 算法的主要区别在于加入了集合的运算，也就是其估计结果包括协方差阵 Ck和椭球集该集合本质上是状态估计均值的集合。当考虑UBB 噪声时，各滤波器的重初始化过程和总体估计计算过程实际上是多个椭球集的加权Minkowski 和，需要根据 2.1 节内容进行加权Minkowski 和外包椭球的求解和参数的优化，而各子滤波器的更新则需要同时进行椭球集和协方差的更新。

IMMCF 算法的结构如图1 所示：

图1 IMMCF 算法结构 (三个模型)Fig.1 Structure of IMMCF algorithm (with three models)

在第1 节的双重不确定性组合导航系统模型基础上，本文对于组合导航算法模型集的构造思路与文献[16]相似，同样是采用不同大小的量测噪声来构造多个模型，通过模型的交互使噪声大小可以覆盖一定的范围，最终实现高精度的导航信息估计。同时将量测噪声和过程噪声作为双重不确定性噪声进行处理。各模型状态空间方程与式(5)(6)一致，只是量测噪声不同，不同模型量测噪声信息描述如下：

其中，上标（i）表示模型m（i），R为噪声基准，di、ei为模型系数。模型构造后，即可按照如下步骤进行导航信息的更新：

Step 2：模型条件重初始化。根据标准IMM 算法过程计算混合概率协方差阵根据2.1 节的结论计算重初始化的混合估计椭球

Step 3：模型条件滤波。计算各匹配模型滤波器的估计椭球和协方差阵，单模型具体更新过程参考文献[14]；

Step 4：模型概率更新。根据标准IMM 算法过程计算模型概率；

Step 5：估计融合。根据2.1 节的结论计算总体估计椭球以及协方差阵则为当前时刻的导航误差信息估计；

Step 6：令k←k+ 1并返回Step 2，直到程序终止。

3 试验验证

系统采用的惯导为HX-IMU-M02 惯性组件，由MIMU 和外部采集板两部分构成。MIMU 集成了3 轴MEMS 陀螺、加速度计、磁强计，试验中应用MEMS陀螺和加速度计进行惯性导航解算。GPS接收机为北斗星通的C200 系列接收机，搭配高精度板卡BDM682。试验设备主要技术指标如表1 所示。

表1 主要技术指标Tab.1 Main technical specifications

为检验算法性能，利用上述设备进行了推车试验，试验中将MIMU 和GPS 接收机一同放置在试验平台上，GPS 天线放置在推车上部，试验装置如图2所示。

图2 导组合航试验装置Fig.2 Test device of integrated navigation

试验进行两次，第1 次试验推车沿矩形轨迹行进，时间为1100 s，第2 次试验推车沿8 字轨迹行进，时间为1500 s，两次试验中推车的运动轨迹如图3 所示。

图3 试验装置运动轨迹Fig.3 Trajectory of test device

试验中系统的初始位置、速度误差分别设置为2 m、0.1 m/s，GPS 数据更新率为1 Hz，惯导系统数据更新率为200 Hz。该系统可以使用信号接收器接收到的多GNSS 系统对载体位置和速度进行解算，所以试验中将多系统联合解算的结果作为试验参考值。上位机通过USB 接口接收GPS 和MIMU 数据并利用IMMCF 算法进行处理，采用三模型结构，噪声基准： R=diag （0.5m20.5m20.5m23(m/s)23(m/s)25(m/s)2） 模型系数如表2 所示。

表2 不同模型的噪声系数值Tab.2 Noise coefficient of different models

通过计算，得到组合导航的位置、速度、姿态信息，如图4-9 所示。

图4 试验1 姿态角Fig.4 Attitude angle in test 1

作为对比，同时也使用标准IMM 滤波器以及使用单个模型（模型2）的联合滤波方法进行了组合导航解算。另外，表3 和表4 给出了使用不同算法得到的导航误差的统计特性。

表3 试验1 导航误差统计特性Tab.3 Statistical characteristics of navigation error in Test 1

表4 试验2 导航误差统计特性Tab.4 Statistical characteristics of navigation error in Test 2

图5 试验1 位置信息Fig.5 Location in test 1

图6 试验1 速度信息Fig.6 Velocity in test 1

图7 试验2 姿态角Fig.7 Attitude angle in test 2

图8 试验2 位置信息Fig.8 Location in test 2

图9 试验2 速度信息Fig.9 Velocity in Test 2

由于推车试验只是在小范围的地面上运动，所以结果中忽略了天向速度和高度。同时，为方便比较，表中位置误差的统计结果用单位米（m）描述。首先，无论是从图中的导航位置速度信息还是表中的误差统计特性来看，本文提出的方法在MIMU/GPS 组合导航系统中能正确导航解算，且两次试验中多模型方法和单模型方法相比都具有明显优势。如试验1 中采用单模型方法导航的最大位置误差达到了10 m，且有逐渐增大的趋势，利用多模型方法则将试验1 的位置误差限制在了1.5 m 以内，这充分体现了多模型的优势。同是多模型方法，IMMCF 算法与标准IMM 算法相比具有一定的优势，特别是在速度估计方面精度提高明显。

另外，根据IMMCF 估计结果中的边界椭球，得到了各导航信息的估计边界，如图10-13 所示。可以看出，多系统输出结果包含在IMMCF 算法估计得到的边界之内，这对于提高导航系统的鲁棒性和载体后续的制导、控制和路径规划都具有重要的意义，而这种边界估计是标准IMM 算法无法实现的。

图10 试验1 位置估计边界Fig.10 Location boundary in Test 1

图11 试验1 速度估计边界Fig.11 Velocity boundary in Test 1

图12 试验2 位置估计边界Fig.12 Location boundary in Test 2

图13 试验2 速度估计边界Fig.13 Velocity boundary in Test 2

3 结 论

本文根据组合导航系统中有界噪声和随机噪声两种不确定性噪声共存的问题，提出了交互多模型联合滤波算法。首先，将随机噪声融入集员框架中，用分布集合的概念来描述双重不确定模型，并介绍了以此为基础的双重不确定系统联合滤波方法；而后，推导了多椭球加权Minkowski 和的外定界椭球的运算过程，并提出参数优化方法，将该过程融入IMM 交互过程中，给出了交互多模型联合滤波算法的具体迭代过程；最后，将提出的方法用于MIMU/GPS 组合导航，试验结果表明，IMMCF 算法估计精度优于标准IMM算法，同时也优于单模型滤波方法。由于充分利用了两种不同性质的噪声，该方法既可以避免单一噪声假设导致的导航性能下降，又可以得到保证边界估计，具有较强的鲁棒性。