多传感器融合 | 多观测数据信息如何有效融合？

系统中存在多个传感器观测数据时该如何处理？

引言

最近根据先前的定位建图研究方向的思考，又尝试学习了一些“多传感器信息融合系统”和“多传感器信息故障检测和滤除”等方向的论文和原理知识。觉得再不花点时间总结以下的话，可能自己都会忘了。

本文的重点章节在于第五部分：Resilient Interactive Sensor-Independent-Update Fusion Navigation Method，我个人认为是一篇将系统框架及其数学原理说明地很清楚的论文，且实验部分也很容易引发读者思考，这里推荐感兴趣的话可以直接研读论文原文。前面四个章节的内容都是基于第五部分内容的前置背景和知识。

这里先感谢一下参考文献中各个研究学者们的工作贡献，感谢分享。

本文如有错误和表达不正确的地方，望大佬们批评指正，也期待和大家的交流。

1、背景概述

多传感器融合导航系统本质目的是为了在机器人的位姿估计时获得一个能正确表征机器人位姿的状态量及其置信度，结合来自不同传感器的互补或冗余信息可以提供比使用单一传感器更鲁棒的导航估计，在应用场景上则是为了解决针对退化场景给高精度鲁棒定位带来的挑战。

但是在导航定位的过程中融入其他的传感器数据，就可以完全解决上述的问题嘛？答案仍然是不确定的。传统的滤波或者优化系统是假设传感器的标准性能（故而在算法中将测量值的噪声建模为高斯白噪声），在具有挑战性的环境下，上述模型过于理想，不能反映传感器的实际状态（下文中会举例说明）。直白地阐明即是：当单个或者多个传感器失效或者传感器数据异常时，设计和考虑不完善的系统将仍会有导航定位精度丢失和系统失效的问题。常见的定位和SLAM系统都依赖IMU和轮速编码器提供的初始位姿输入，当在某些情况下这些传感器数据出现异常时（如IMU的静态数据漂移问题和轮式打滑问题等），尽管有外部传感器（如激光和视觉传感器）的观测信息进行移动机器人定位位姿的修正也无法彻底消除传感器失效和异常时带来的误差。在这里，我们可以考虑滤波和因子图优化的两种融合框架为例子：

“

1. 基于滤波框架，以INS为主要的机器人状态系统，GNSS和LiDAR的数据作为观测以修正INS系统的累积误差。
2. 基于因子图优化的紧耦合系统，系统中的因子有INS状态因子、LiDAR配准因子和GNSS位置因子，各因子之间联合优化以获取优化后的机器人状态量估计。

”

上述的两个多传感器融合系统都可以说是有 两个数据来作为“观测量” 用以修正误差和引入一个全局一致的约束信息，进一步地考虑两种不同场景下，上述两个融合系统的效果（两种）：

“

1. 在开阔的场景，一般来说GNSS数据基于RTK技术可以达到5cm内的位置精度，而LiDAR在空旷场景的配准有时候会有好几米的误差，在该场景下的多传感器融合系统在处理GNSS和LiDAR的观测数据时应该给GNSS数据更高的融合权重，以避免被LiDAR的错误配准信息“拉偏”；
2. 在室内结构化的场景，GNSS数据往往由于基站信号屏蔽和多路径效应等因素会有十几米的误差，而LiDAR在室内结构化的场景下可以稳定在厘米级别的精度。在该场景下的多传感器融合系统在处理GNSS和LiDAR的观测数据时应该与上述场景相反，给GNSS数据更低的融合权重，给LiDAR观测量更高权重，以避免被GNSS的错误位置信息“拉偏”；

”

有的人会说：“那我们可以针对不同的作业场景，在提前通过配置参数来控制测量数据的影响力。”该方案确实是一个捷径，但是 真正的鲁棒性和可靠性强调导航系统在不改变其初始稳定配置的情况下抗故障和噪声（resist faults and noises）的能力，这是挑战性环境下导航系统的关键性能指标 。针对上文中提出的问题和挑战，本文拟介绍当前多传感器融合系统工作考虑和建模出传感器失效时的情况和数学模型，并分析和讨论相应的解决策略以保证导航定位或者SLAM系统在挑战场景下的鲁棒运行。

2、基于卡尔曼滤波算法的多传感器融合

本小节将简单介绍和讨论两种情况（同步数据系统和异步数据系统）下的共四种滤波处理框架。为了避免歧义，先放上英文标题：KALMAN FILTER ALGORITHMS FOR A MULTI-SENSOR SYSTEM。常见的基于卡尔曼滤波的多传感器融合系统中只有一个一种观测量作为误差的修正，此处的多传感器系统特指存在多个观测量来源的传感器系统，比如，系统存在多个雷达观测量在同一时刻或者不同时刻到达或者系统存在多种不同的传感器观测量“雷达、相机、卫星等”在同一时刻或者不同时刻到达。如此一来，当多传感器系统的规模很大、亦或者说是相当数目的冗余观测数据到达系统时，滤波框架的计算效率就成了重中之重。

2.1、针对同步数据系统（For Synchronously Collected Data）

2.1.1. 并行滤波器框架（parallel filters config）：

将所有的观测量整合在一起构建出总观测量向量Z、对应的观测噪声R和对应的观测矩阵H。

系统框架如下图所示：

2.1.2. 串联滤波器框架（sequential filters config）：

串行的依次处理每一个观测量Z，通过每一个观测量对应的噪声R和观测矩阵H计算出每一次的卡尔曼增益K和状态量协方差，再利用每一次的卡尔曼增益更新状态量，系统中有多少个观测量以上流程就重复多少遍。

系统框架如下图所示：

2.1.3. 数据压缩滤波器方案（data compression filters）：

该方案是将所有相似的观测量信息先全部压缩得到一个伪观测量（pseudomeasurement），而在压缩之前会将所有观测量变换到统一坐标系下。

系统框架如下图所示：

2.2、针对异步数据系统(For Randomly Collected Data)

串联滤波器框架（sequential filters config）：其实这里也不是典型的串联滤波器框架，在异步的情况下系统的预测与更新就是基于当前时刻下可用的观测量信息了（有什么就用什么）。

结合小节的内容，我们可以看到上述的多传感器滤波框架虽然是有能力在融合系统中处理多种或者多个观测量数据，但是系统是不具备抗故障和强噪声的能力的，所以数据一般是需要在输入到滤波器框架前进行细致的数据预处理以滤除outliers。不论是并行框架还是串联框架，滤波器框架间是耦合在一起的，所以若是有垃圾观测量进入则融合系统有失稳的风险。除此之外，针对异步系统的融合计算量最大，同步系统中虽然并行框架的计算消耗会大于串联系统，但串联系统是基于一个关键的假设：多个多种观测量间是独立的，数据压缩即使需要有坐标转换的步骤但总体的效率是所有框架中最高的。

3、多传感器的信息融合准则和理论简述

本小节中提及的多传感器最优信息融合准则本质是基于线性最小方差意义下的协方差矩阵加权融合，其准则等价于正态分布假设下的最大似然融合准则。为了保证文章内容的可读性，这里仅作两个定理的介绍，用以协助理解第四和第五小节中的内容，定理的证明和应用不进行细节展开。

4、卡尔曼滤波器与IMM结合的融合系统

IMM(interactive multi-model)是一个非常经典和高效的多种动态模型估计器，针对数据融合，则IMM系统还需要结合卡尔曼滤波模型，以完成根据目标移动的动态特征并行运行针对多种状态转移的预测推算，然后基于不同观测量进行状态量更新，最后通过马尔可夫链中设置的先验状态转移模型和滤波器模型概率完成多个滤波器结果的融合。具体的IMM系统框架下应用卡尔曼滤波的模型如下图示：

更加具体的IMM内容可以参考论文：T. Kirubarajan and Y. Bar-Shalom, “Kalman filter versus IMM estimator: When do we need the latter?” IEEE Trans. Aerosp. Electron. Syst., vol. 39, no. 4, pp. 1452–1457, Oct. 2003.

或者参考知乎上一位大佬整理的IMM知识：怪兽吃苹果：卡尔曼滤波器应用——用于单目标追踪的IMM模型

5、弹性交互传感器独立更新方法

为了避免歧义，先放上英文标题：Resilient Interactive Sensor-Independent-Update Fusion Navigation Method（论文链接附上：），本小节参考的是东南大学的Meng Qian教授和香港理工大学Hsu Li-Ta发表的同名文章，该论文提出了一个“弹性交互的多传感器独立更新并融合”的导航方法，对处理多个测量数据的传统滤波器进行解耦，最终实现了对传感器故障数据和噪声的强抵抗能力，提升多传感器融合系统的鲁棒性和可依赖性。

废话不多说，直接上论文中的系统框图：

通过上面的系统框图，我们可以将整个系统划分成六大核心板块：

“

state propagation；
model（elemental） filter processing；
mode posterior prob update；
model（elemental） filter estimation fusion；
reliability check；
model reinitialization。

”

5.1、State Propagation & Model（elemental） Filter Processing

第一个和第二部分的操作流程和传统多传感器滤波相关的操作基本一致，基于上一个时刻的“后验”状态量和协方差矩阵，先通过控制量信息（系统状态量转移矩阵）和系统噪声量做推算获得当前时刻的预测状态量和预测协方差，再根据当前时刻下的观测量z、观测矩阵h和测量噪声矩阵r计算出每一个独立滤波器（model filter）下的“后验”状态量及其协方差矩阵。

5.2、Mode Posterior Prob Update：

该部分的流程是通过每个独立滤波器的输出量和协方差计算该独立滤波器的似然概率，结合每个滤波器的先验置信息以计算得出模型的后验置信概率，该后验置信概率即作为每个模型更新的状态量在后续融合阶段的权重。本质上，上述的流程就是为了处理平行计算框架下的各个滤波器结果，在状态空间下利用滤波模型间的交互信息完成融合，以得到一个完整的最优的状态估计：

其中上式每项中的期望值E即为每一个滤波器模型得出的最小均方差估计：

，

上式中的上标i表示第i个滤波器模型，M表示滤波器模型的数量。目前期望值E已经完成了定义，后验模型概率该如何计算呢？

我们接着从上面Algorithm1中的定义中得出几个关键的表征量：

观测量与预测量之间的残差（measurement innovatio/residual）及其协方差：

然后是由上述残差定义的似然函数：该函数表征的是得出这些预测量、观测量等的滤波器的模型似然函数L（model likelihood），该模型似然函数L再结合对应滤波器的预测先验概率u_k/k-1可以得到对应的后验模型概率：

由此关键的滤波器模型的后验概率更新就完成了。

上面的残差及其协方差除了可以用来计算滤波器模型的似然函数外，还可以用来做故障检测和滤除（fault detection and exclusion），具体的就不再本文中展开了。

5.3、Model（elemental） Filter Estimation Fusion：

基于前三个模块的结果，该部分就是对系统中所有滤波模型更新所得结果（状态量及其协方差矩阵）进行加权平均，每个结果的权重系数即为每个滤波器的mode posterior probabilitty。

5.4、Reliability Check：

该部分用作对整个多传感器融合方案的完整性监控，但考虑到对完整状态量的的完整性监控会有极大的出错风险和计算复杂难度，应用层面上可简化操作而只考虑postion方向上的系统完整性，由此可以达到一个实时性的要求。

该可靠性检测本质上是计算求出EPU(Estimate Positioning Uncertainty)，先从Estimation Fusion后状态协方差矩阵中的位置协方差分离出来，再将其对应转换到ENU坐标系下，最后在无故障的条件下，可以轻易地分离出位置协方差里对应北方和东方的位姿标准差：

随后求解特征值和推算出对应的概率密度函数：

最后可得EPU：

5.5、Model Reinitialization：

在完成estimation fusion后，系统状态量和协方差矩阵并不能直接输入给到下一个时刻的推算和更新过程，因为该系统是一个复合的多传感器融合系统，并不是一个滤波器串行所有观测测量量。所以针对该类模型，需要根据系统的特性定义针对模型置信转移的马尔可夫链和模型置信度状态转移矩阵。基于状态转移矩阵对mode posterior prob预测出下一个时刻的prior mode prob，并再基于此prior prob重新调整状态量及其协方差的权重系数，得出输入到下一个时刻的融合状态量及其协方差矩阵。

上式中Πji表示基于马尔可夫链的从状态节点j到状态节点i的模型置信度状态转移矩阵。

综上，其实可以发现本小节提出的isiu系统框架是与imm结合滤波后的系统有类似的地方，但两个系统间主要的差异在于两点：1. isiu中系统状态量的传播过程是一体的，待状态量传播预测完成后由各个观测量进行独立的更新，各滤波器完成更新后再根据外部交互信息完成状态融合，2. 在使用交互信息进行融合时，系统中的马尔可夫链给isiu系统提供的是传感器的先验置信度转移矩阵，而不是imm的滤波模型置信度转移矩阵。