一种支持传感器即插即用的模块化信息融合方法

杨斌，李恒，罗治斌

（1.中国电子科技集团公司第十研究所，四川 成都 610036；2.哈尔滨工程大学 智能科学与工程学院，黑龙江 哈尔滨 150001）

0 引言

随着信息化、智能化时代的到来，无人机（Unmanned Aerial Vehicle，UAV）和无人车（Unmanned Ground Vehicle，UGV）等逐渐进入大众视野。无人平台具有风险小、成本低、环境适应能力强等优点，在军事、民用方面得到了广泛的应用。在无人系统的工作过程中，感知、定位、决策、控制等四个方面相辅相成，共同决定无人系统的工作效能。其中，定位是无人系统精确、可靠地完成任务的必要前提。目前，导航定位技术种类繁多，每种类型都有优势和局限。由于无人系统的工作环境复杂多变，单一的导航传感器已无法满足无人系统对复杂环境下精确导航定位的需求，而基于多源信息融合的综合导航技术是满足这一需求的重要手段。随着无人平台装载的导航传感器种类不断增加，导航系统对环境的适应性越来越强，无人系统能够获取更精确的定位信息，但同时也增加了无人平台导航系统设计的复杂度。当前，综合导航系统设计属于“单点式”解决方案，各类导航方案独立设计，若接入系统传感器发生变化，需要对整个导航方案重新设计、验证，尚不能实现传感器“即插即用”的能力，从而增加了导航方案投入使用的时间。本文基于模块化信息融合架构的全源导航技术来解决这一问题。

1 研究现状

为满足精确获取导航定位信息的需求，导航系统经历了从单传感器导航到多传感器组合导航，再到全源导航系统的发展历程，通过先进的信息融合架构实现系统组件的即插即用、敏捷开发。针对全源导航中传感器的即插即用问题，部分研究者从信息融合架构的角度对多传感器信息融合方法进行了研究。其中，文献[3]为了实现全条件环境下的厘米级导航定位精度，分别对GPS、INS、伪卫星、超宽带、蓝牙等定位技术进行研究；同时为了保证各导航系统的自动切换能力，提出一种包含过程噪声管理器、状态管理器、量测管理器、预测、更新的即插即用滤波器框架，在理论上实现了不同情况下导航系统的切换。

Groves针对恶劣环境下对导航定位精度和可靠性的需求，提出一种用于多传感器导航定位系统的模块化解决方案，但并未说明各部分的详细实现，也未通过仿真验证其可行性。2020年，Kauffman等人为削弱导航系统对GPS的依赖性，提出了一种用于互补导航传感器的模块化传感器融合套件，并将其命名为Scorpion。该开放性框架分为三个部分：模块化导航滤波器、传感器组合策略、完好性算法。其中模块化滤波器被分为可插拔时间更新、量测更新两部分。最后，通过仿真实验验证了该框架的可插拔性。当前，国内尚无利用此架构进行导航系统设计的公开报道，且该论文在系统数学建模方面存在技术细节不完备的问题。

本文基于Scorpion架构，提出了可支持传感器即插即用的模块化信息融合架构具体设计方法，并通过组合导航仿真验证了系统组件“即插即用”的能力。

2 模块化信息融合架构

考虑到全源导航系统对传感器“即插即用”的要求，本文提出了一种由状态模块、量测模块、滤波器模块组成的支持传感器“即插即用”的模块化信息融合架构，以实现全源导航系统的增量式开发。设计过程中将系统状态模块进行解耦，每个状态模块的传播仅与状态模块自身相关，不依赖于其他的状态模块；每个量测模块和指定的状态模块相关联，所有状态模块通过一个状态向量合并成组合导航系统总的状态，量测模块根据量测更新的需要进行调用；滤波器作为一个单独的模块，并不指定具体的状态和量测，以实现其通用性，此外，滤波器模块具有状态、量测的添加和移除功能，可根据系统需要接入特定的状态和量测。

2.1 状态模块

状态模块用于表示某一特定的状态，它的传播不依赖于所给定状态模块以外的任何状态。状态模块的大小可以从单个状态（一个传感器的偏置）到多个状态（组合导航系统中的误差状态）。状态模块内部的具体信息流动过程如图1所示。

图1 状态模块内部的具体信息流动过程

2.2 量测模块

量测模块用于表示传感器量测和状态向量之间的关系，其主要由传感器量测值、量测函数、量测矩阵和量测噪声协方差矩阵构成。量测模块内部的具体信息流动过程如图2所示。

图2 量测模块内部的具体信息流动过程

2.3 滤波器模块

在组合导航系统中，滤波器是融合传感器量测信息的重要工具。由于不同传感器的量测模型、噪声特性不同，其所适用的滤波器也不相同，但滤波器可大致分成时间更新、量测更新两部分，因此可将滤波器模块分为时间更新函数、量测更新函数以及操作状态、状态协方差的一些基本函数。在时间更新阶段，滤波器向状态模块请求状态模型、状态噪声协方差矩阵等相关量，以执行时间更新。在量测更新阶段，滤波器向量测模块请求量测模型、量测噪声协方差矩阵等相关量，以执行量测更新。

滤波器模块的具体信息流动过程如图3所示。

图3 滤波器模块的具体信息流动过程

3 组合导航系统及其误差模型

组合导航系统中所使用的误差模型对整个系统的性能有很大的影响。由于导航传感器的多样性，组合导航系统的误差模型被构建为一个模型复合体，其中包括INS、全球导航卫星系统（Global Navigation Satellite System，GNSS）、里 程 计（Odometer，ODO）、磁 力 计（Magnetometer，Mag）、视 觉 相 机（Vision Inertial Odometer，VIO）等。因为INS频率较高，能够提供连续的导航信息，所以将其误差模型作为系统模型的主要部分，其中INS的姿态、速度、位置误差模型构成了组合导航系统状态模型的核心。

3.1 状态误差模型

本文选取东北天（E⁃N⁃U）地理坐标系作为导航坐标系。系统状态方程可以表示为：

式中：()表示状态误差向量，由姿态失准角、速度误差、位置误差、陀螺仪常值偏置、加速度计常值零偏组成；()表示状态转移矩阵，其详细表示可见参考文献[6]；()表示输入过程噪声向量；()表示过程噪声驱动阵。

为了便于计算机处理，将连续状态空间模型离散化为如下形式：

式中：表示与状态向量同维度的单位矩阵；表示INS的采样时间；Φ ，Γ分别表示离散化后的状态转移矩阵和噪声驱动阵。

3.2 传感器量测模型

传感器量测模型可表达为：

式中：Ζ表示量测向量；H表示量测矩阵；v表示量测噪声向量。为了保证无人系统能够实时获取高精度的导航定位信息，本文利用INS作为基本导航系统，并将其与GNSS、ODO、Mag、VIO组合使用，从而抑制INS的发散，确保导航的连续性和可靠性。在使用分布式滤波架构的前提下，可构建4组量测方程。

1）INS与GNSS组合导航子系统，其量测向量包括位置误差和速度误差，它们分别是INS解算所得到的位置以及速度与GNSS量测所得到的位置和速度的差值。INS和GNSS组合导航系统的量测方程可表示为：

式中：表示INS和GNSS组合的量测矩阵；表示GNSS的量测噪声，一般建模为零均值的高斯白噪声。

2）INS和ODO组合导航子系统，里程计利用编码器提供运载器的位置增量或瞬时速度信息。本文以瞬时速度为里程计的原始输出数据，考虑到INS解算速度投影在地理坐标系下，INS和ODO组合导航系统的量测向量被选择为地理坐标系下的速度误差，其是由INS解算的速度和经过姿态变换的里程计速度作差得到的。考虑到里程计测量的是无人系统的前向速度，根据非完整运动约束，可以假定无人系统的另外两个方向上的速度为0。量测向量的具体表示形式如下所示：

式中：表示时刻INS解算的速度；表示时刻里程计的量测噪声。

3）INS和Mag组合导航子系统，INS解算的位置所对应的地磁总强度与磁力计量测到的地磁总强度的差值作为量测值，具体表示形式如下：

式中：Ζ表示时刻INS解算的位置所对应的地磁总强度；Ζ表示时刻磁力计测得的地磁总强度。由于地磁量测模型是一个强非线性模型，所以INS和Mag组合导航系统可表示为：

式中：(X)表示地磁量测模型所对应的量测非线性函数；表示时刻磁力计的量测噪声，一般被建模为零均值的高斯白噪声。

4）INS和视觉组合导航子系统，当环境中存在丰富的特征点时，视觉能提供精确的位置信息。故而将INS解算位置与VIO输出位置的差值作为组合导航系统的量测信息，可表示为：

式中：表示时刻INS解算得到的位置信息；表示时刻VIO系统输出的位置信息。INS和视觉组合系统的量测方程可表示为：

式中：表示时刻INS和视觉组合系统的量测矩阵；表示时刻视觉相机的量测噪声，一般假定为零均值的高斯白噪声。

4 联邦滤波算法

本文选择联邦滤波算法作为多源信息融合算法。联邦滤波算法在组合导航系统的多传感器信息融合中被广泛应用，其内部包含一个或多个子滤波器和一个主滤波器。下面将给出联邦滤波算法的具体实现。

为了能够清晰地表达联邦滤波器中子滤波器的信息处理过程，使用如下状态方程：

离散时间量测表达式为：

式中=1,2,,表示局部滤波器的序列号。

联邦滤波的计算流程为：

1）子滤波器时间更新。时间更新是一个状态预测与误差协方差预测的过程，它可表示为：

式中Q表示输入过程噪声w的协方差矩阵。

2）子滤波器量测更新。量测更新是融合量测信息，修正状态估计值、状态协方差的一个过程。局部滤波器的最优卡尔曼增益可以表示为：

式中R表示时刻量测噪声v的协方差矩阵。基于此，后验状态估计值可以表示为：

后验状态估计的误差协方差矩阵可以表示为：

3）信息融合。信息融合的主要目的是通过融合所有子滤波器的状态信息得到系统的全局最优估计，具体融合算法如下所示：

4）信息共享。根据以下信息共享原则，将最优状态估计、状态误差协方差矩阵、过程噪声矩阵分配给各子滤波器：

式中β表示标量形式的信息共享因子。另外，根据信息分配原则，信息共享因子应该满足信息守恒定理：

5 仿真分析

本节以车载组合导航系统设计为例，通过仿真实验验证所设计的模块化信息融合方法。仿真中所使用的传感器包括惯性测量单元（Inertial Measurement Unit，IMU）、GNSS、ODO、Mag、视觉相机，其中IMU的陀螺偏置为1(°)h，加速度计零偏为500 μ；GNSS的位置量测噪声为5 m，速度量测噪声为0.1 m/s；ODO的速度量测噪声为0.5 m/s；Mag的量测噪声为1 nT。模拟的运动轨迹包含直线运动、转弯、加减速等。

5.1 状态模块可插拔性验证

以GNSS的杆臂估计为例，验证模块化信息融合方法中的状态模块的可插拔性。假设GNSS接收机的轴、轴、轴分别有5 m的杆臂误差，然后利用所设计的惯导误差状态模块、GNSS量测模块、卡尔曼滤波器模块设计组合导航系统，可得图4所示未估计杆臂时的位置误差结果。由图可知，在不建模杆臂效应的前提下，会引起三轴5 m左右的定位误差。

图4 未估计杆臂时的位置误差

利用模块化信息融合架构，在系统中添加所设计的GNSS杆臂状态模块，使其与惯导状态模块结合，共同构成系统的状态，可得图5、图6的估计结果。

图5 杆臂估计效果图

从图6可以看出，在系统中加入杆臂状态模块，可正确估计出GNSS的杆臂。通过对比图4和图6的位置误差图可知，杆臂的估计可以很好地消除位置估计偏差。仿真过程中，仅通过状态模块的添加或移除即可改变系统的状态，系统将自适应调整滤波计算相关矩阵，而不必因为系统状态模型的改变而重新设计整个系统，提升了系统设计的灵活性和工程开发效率。

图6 估计杆臂时的位置误差

5.2 传感器即插即用能力验证

为了验证传感器的即插即用能力，本文借助模块化信息融合架构，设计多传感器组合导航系统。在该仿真中，为了充分体现所设计模块化信息融合架构的灵活性，假设不同时段具有不同数量的传感器处于工作状态，具体设计如下：0~150 s为INS/GNSS/ODO/Mag/vision组合导航模式；150~300 s为INS/ODO组合导航模式；300~400 s为INS/ODO/Mag组合导航模式；400~500 s为INS/Mag组合导航模式；500~600 s为INS/ODO/Mag/vision组合导 航 模 式；600~900 s为INS/GNSS组 合 导航模式。在150 s，300 s，400 s，500 s，600 s时进行了传感器切换。

图7展示了组合导航系统的位置误差图。由图可知，系统开始时使用4个传感器能使得位置快速收敛，随着传感器切换依然保持运行稳定性，验证了模块化信息融合架构下传感器的即插即用能力。另外，由图7可知，组合切换系统过程中，位置估计具有很好的一致性。由此可知，模块化信息融合架构能根据传感器可用性，自适应调整系统模型和相关计算矩阵，减小传感器改变对系统运行的扰动，具有较好的环境适应性。

图7 组合导航系统位置误差

6 结 语

本文提出了一种支持传感器即插即用的模块化信息融合方法，通过将系统分为状态模块、量测模块、滤波器模块，实现综合导航系统各个组件的独立开发和高效复用，有效提高了开发效率和灵活性。基于该信息融合架构，设计基于INS/GNSS/ODO/Mag/vision传感器的组合导航系统，通过GNSS的杆臂估计验证了状态模块的可插拔性，通过车载组合导航系统工作模式无缝切换验证了传感器的即插即用能力。仿真结果表明，模块化信息融合架构在系统设计阶段具备使用灵活、开发效率较高的优势，在系统运行阶段可适应异类传感器接入和断开，具有较好的环境适应性。本文的研究工作对未来组合导航系统开发具有较强的指导意义。