论文笔记（五）：Multimodal Sensor Fusion with Differentiable Filters

文章概括

作者：Michelle A. Lee，Brent Yi，Roberto Martín-Martín，Silvio Savarese，Jeannette Bohg
来源：arXiv:2010.13021v1 [cs.RO] 25 Oct 2020
原文：https://arxiv.org/pdf/2010.13021.pdf
代码、数据和视频：https://sites.google.com/view/multimodalfilter
系列文章目录：
上一篇：
论文笔记（四）：6-PACK: Category-level 6D Pose Tracker with Anchor-Based Keypoints
下一篇：
论文笔记（六）：A VR Teleoperation Suite with Manipulation Assist

1. 摘要

在本文中，作者研究了新的可分化滤波架构来融合异构传感器信息。作为案例研究，作者们评估了三个任务：两个在平面推动（模拟和真实）和一个在操纵一个运动学约束的门（模拟）。 在广泛的评估中，作者们发现利用跨模式传感器信息的可区分滤波器达到了与非结构化LSTM模型相当的准确度，同时呈现出可解释性的优势，这对于安全关键型系统来说可能很重要。
图1：
我们研究了模态的整合，用于可区分滤波器的递归状态估计；（上）来自力传感（局部信息）、联合编码器和视觉传感（全局信息）的信息被融合到不同的架构中，包括新的跨模态策略，利用一种模态的信息来评估另一种模态的不确定性；（下）通过在操纵过程中结合局部和全局信息，姿势的 可以正确估计一个物体（绿色图钉）的位置（红色覆盖）。

2. 介绍

为了融合具有不同特性的传感器信息，估计器需要了解如何平衡不同模式之间的置信度。我们提出了三种类型的可区分滤波器的融合机制。第一种是根据每个模态的不确定性估计，即单模态信息进行数据融合。第二种是将非模态特征连成一个多模态表征。而第三种则使用交叉模态信息对来自多个传感器的单模态估计进行加权。我们将所提出的融合机制与参数和非参数递归状态估计器相结合。我们将这些多模态可分化滤波器与非结构化的LSTM进行比较，并评估其在干净的模拟数据、有噪声的模拟数据和真实数据中的性能。

3. 相关工作

3.1 多模态融合

传统上，传感器融合是在递归滤波器内进行的，通过对每个观测值进行单独建模，然后将所得信息整合到一个共同的状态估计中。 这些方法支持这样的直觉，即来自不同模式的信息的最佳组合可以提高整体的估计精度。 这些方法的一般模式包括一个贝叶斯过滤器和一个多模态测量模型，该模型对每个模态量化了当前测量给定预测状态的可能性。其他方法则以交叉模式的方式使用测量，其中来自一个传感器模式的信息有助于解释另一个传感器模式。这些方法中的每一种都需要用户确定和定义分析性的正向和测量模型，而这些模型对于某些动态系统来说可能很难指定，或者难以在线计算。在本文中，我们通过用神经网络从注释数据中学习测量和前向模型，减少了多模态和跨模态融合对预定义模型的需求。

当使用来自多个传感器的观测值进行概率递归状态估计时，只有正确地描述了每个传感器的不确定性，才能对模态的信息进行优化整合。传统上，这些不确定度必须经过手工调整，使后置状态估计接近于地面真实。在本文中，我们学习了每个传感器模式的不确定性，从而减轻了手调。

Caron等人提出不在滤波器内递归融合传感器，而是对来自单模态和多模态卡尔曼滤波器的状态估计进行加权，并在单模态和多模态粒子滤波器的测量模型之间切换。然而，加权机制取决于用户为每个传感器定义和手工调整的阈值。在我们的工作中，我们提出并评估了融合模型，这些模型通过利用跨模态信息来学习合并来自单模态滤波器的状态估计。

现在已经提出了一个多模态状态估计框架，用于平面推送(类似于本文中使用的三个案例研究中的两个)与因子图(iSAM)，然而他们使用了信标进行跟踪，而不是原始RGB数据。虽然作者表明iSAM提供了更准确和稳健的状态估计，但它仍然需要仔细规范特定模式的成本函数以及前向和测量模型，我们从数据中学习。

3.2 可微分滤波器

Differentiable滤波器提供了一种从数据中学习正向和测量模型的方法，同时保留了递归贝叶斯滤波器的算法结构。因此，对于动态和传感器观测难以分析建模的系统来说，可分化滤波器是有利的，同时可以保留状态表示和不确定性的可解释性，这对于安全关键型系统来说通常是至关重要的。在本文中，作者们研究了新的可区分滤波架构，以融合来自视觉、触觉和本体感知等异质传感器模式的信息。

4. 过滤器的背景

作者考虑从（多模态或单模态）观测值z和控制输入u的序列估计系统的状态x的问题。我们用一个对当前状态$x_t$的分布来表示我们对估计的知识和不确定性，这个分布的条件是所有之前的观测值$z_{1:t}$和控制输入$u_{1:t}$。作者将这个分布表示为$bel(x_t)=p(x_t|u_{1:t},z_{1:t})$。计算这个信念的一个解决方案是贝叶斯过滤器。

传统上，开发贝叶斯滤波器需要根据当前状态和控制输入，分析制定一个预测下一个状态的前向模型$p(x_t|x_{t-1},u_{t-1})$，一个测量模型$p(z_t|x_t)$，以计算给定状态下观察的似然，以及与预测和观察相关的噪声。制订这些模型和量化噪声往往需要对底层系统的特性做出强有力的假设。避免做出这些假设的方法是直接从观测和地面真实状态的标记数据中提取这些信息。这可以通过最近的可分化贝叶斯滤波器家族来实现。在这里，我们研究了融合来自多种模态的异质感觉数据的特殊情况下的可分化滤波器。在本节中，我们将首先对贝叶斯滤波器进行简要的总结，然后再为可分化滤波器提供一个统一的符号。

4.1 贝叶斯过滤器

Bayes滤波算法提供了一个遵循马尔科夫假设且观测值有条件独立的系统中状态估计的最优解。在这个过滤器中，信念分两步更新。在预测步骤中，运动模型$p(x_t|x_{t-1},u_{t-1})$用于预测当前状态$x_t$的信念$\overline{bel}(x_t)$，给定之前的状态$x_{t-1}$和控制输入$u_{t-1}$：
$$\overline{bel}(x_t)=\int p(x_t|x_{t-1},u_{t-1})bel(x_{t-1})d{x}_{t-1}$$
在更新步骤中，我们对这个初始预测进行修正，给定一个感官观测值$z_t$插入一个测量模型$p(z_t|x_t)$，该模型描述了这个观测值给定预测状态的可能性：
$$bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)$$
$\eta$是正常化因素。

卡尔曼滤波器是线性模型和高斯噪声系统的最优估计器。然而，在机器人技术中，大多数感兴趣的系统具有非线性模型，并且可能遵循更复杂的非高斯分布。对于这些情况，有一些非线性贝叶斯滤波器可以做出不同的近似来估计信念$bel(x_t)$。

其中，应用最广泛的有扩展卡尔曼滤波（Extended Kalman Filter，EKF）和粒子滤波。当涉及的系统模型不是线性的，而是可以通过泰勒展开进行线性化时，就会用到EKF。当系统的非线性不能线性化，或者底层状态分布不能假设为高斯分布时，例如分布有多个峰值时，研究者会采用粒子滤波器。在这项工作中，我们将研究如何使用可微分版本的EKF和粒子滤波器融合来自多个传感器模式的信息。

4.2 Differentiable 过滤器

对于复杂的物理系统，如何制定一个既准确又易于计算的动力学和观测模型往往是一个挑战。最近，有人提出了最流行的非线性滤波算法的可分化版本，从而可以学习动态和观测模型及其噪声参数。

4.2.1 扩展卡尔曼滤波器

EKF允许非线性前向模型和观察模型，但仍然假设状态遵循高斯分布，即$bel(x_t)\sim N({\mu }_t,{\Sigma }_t)$。具体来说，我们假设系统动力学和测量遵循以下非线性函数：
$$x_t=f(x_{t-1},u_{t-1},q_t)$$
$$z_t=h(x_t,r_t)$$
其中随机变量$q$和$r$为过程和观测噪声。在EKF中，预测的步骤如下：
$${\hat{\mu }}_t=f({\mu }_{t-1},u_{t-1},0)$$
$${\hat{\Sigma }}_t=A_{t-1}{\Sigma }_{t-1}{A}_{t-1}^T+Q_{t-1}$$
这里$A_t$是雅可比行列式$\frac{\partial f({\mu }_t,u_t,0)}{\partial {\mu }_t}$、$Q$是过程噪声$q$的协方差$q\sim N(0,Q)$，我们假设它是零均值的高斯型。因此，$\overline{bel}(x_t)\sim N({\hat{\mu }}_t，{\hat{\Sigma }}_t)$。

更新步骤如下：
$$K_t={\hat{\Sigma }}_t(H_t^T{H}_t{\hat{\Sigma }}_t{H}_t^T+R_t)$$
$${\mu }_t={\hat{\mu }}_t+K_t(z_t-H_t{\hat{\mu }}_t)$$
$${\Sigma }_t=(I_n-K_t{H}_t){\hat{\Sigma }}_t$$
这里$H_t$是雅可比行列式$\frac{\partial h({\mu }_t,0)}{\partial {\mu }_t}$、R为测量噪声的协方差$r～N(0,R)$，我们假设它是高斯的零均值。

对于EKF的可分化版本，我们用多层感知器(MLP)实现$(5)$中的动态模型，其可训练权重为$\theta ：f_{\theta }({\mu }_{t-1},u_{t-1},0)$。学习一个真实的测量模型，将状态映射到我们原始传感器空间中的预期观测值，是一个欠确定的问题，并且容易过度拟合。我们学习一种有辨别性的虚拟传感器。我们的虚拟传感器$g_{\theta }(D)$将原始感知数据$D$作为输入，并输出一个包含完整状态$x$或部分状态的观测值的向量$z_t$。因此，测量模型及其$Jacobian$ $H$是单位矩阵或选择矩阵。与动力学模型类似，我们用一个可训练权重$\theta$的MLP来实现虚拟传感器$g_{\theta }(D)$。

4.2.2 粒子过滤器

EKF假设系统状态的底层分布是高斯分布，而粒子过滤器可以用一组粒子来模拟任意分布。具体来说，我们将以神经网络的形式学习一个测量模型，输出每个粒子预测的观测值的对数似然。 具体来说，我们用一组粒子$X_t=x_t^{[1]},x_t^{[2]},\cdot \cdot \cdot ,x_t^{[n]}$的权重$w_t^{[1]},w_t^{[2]},\cdot \cdot \cdot ,w_t^{[n]}$来逼近信念。像任何贝叶斯滤波器一样，这个非参数滤波器有一个预测和更新步骤。$(1)$中的预测步骤是通过从生成运动模型中对每个粒子进行随机扰动采样来实现的：
$${\forall }_i:x_t^{[i]}\sim p(x_t|u_{t-1},x_{t-1}^{[i]})(10)$$
$(2)$中的更新步骤是通过设置每个粒子的权重$w_t^{[i]}$来实现的，权重$w_t^{[i]}$等于当前测量值$z_t$由该粒子的预测状态$x_t^{[i]}$产生的可能性：
$${\forall }_i:w_t^{[i]}\sim p(z_t|x_t^{[i]})(11)$$
然后，粒子集按比例对每个粒子的重量进行重新采样。需要注意的是，只有EKF需要一个虚拟传感器，而不是粒子过滤器。对于粒子过滤器，$z_t$等于未经任何处理的原始感官数据D。

4.2.3 LSTM

循环滤波器类似于长短期存储器的结构：内部状态与最新输入信号循环合并，产生更新的输出。因此，我们认为LSTM架构是可区分滤波器的基线，我们称之为LSTM基线。与可分化滤波器相比，LSTM中预测和更新步骤之间没有明确的分离。也没有明确的测量和远期模型。 一个LSTM也不构成马尔可夫假设。之前的研究表明，与通用LSTM架构相比，利用贝叶斯过滤器的算法结构的可区分架构可以带来更快的学习和更好的泛化。

5. 多模态融合架构

我们研究了不同的策略，为了将来自多个传感器模式的信息整合成一个连贯的状态估计。我们现在描述这些策略，如图2所示。
图2：

（多模态结构：a)特征的融合：对模态进行处理，并将其合并为多模态特征，进行状态估计。b) 单模态加权融合：根据两个单模态递归滤波器的不确定性对其估计进行合并。c)和d)跨模态加权融合：两个模态都用来学习系数，以融合单模态估计；系数用于c)状态的加权平均或d)加权测量模型。）

5.1 特征融合

在可分化滤波器中融合多种模态信息的一种策略是，首先分别从每个模态中提取特征，然后利用融合后的单模态特征递归估计状态（图2，a）。实现这一目标的方法是：每一种模式都有独立的编码器网络，而完全连接的网络则将单模式的特征整合成多模式的特征。然后，多模态特征被用作递归滤波器架构中的观察。我们称我们的过滤器为Feature Fusion，EKF和Feature Fusion，PF。

5.2 单模态加权融合

这种架构(图2，b)是针对每个正态分布的多个状态估计的融合程序。在这种情况下，我们假设有两个单模 EKF 滤波器提供独立的状态估计$bel(x_t^{M_1})\sim N({\mu }_t^{M_1},{\Sigma }_t^{M_1})$和 $bel(x_t^{M_2})\sim N({\mu }_t^{M_2},{\Sigma }_t^{M_2})$。我们将两个非模态信念进行融合，将两个分布相乘，产生一个正态分布的多模态信念$bel(x_t^{MM})$：
$$bel(x_t^{MM})=N({\mu }_t^{MM},{\Sigma }_t^{MM})(1)$$
$${\mu }_t^{MM}=\frac{({\Sigma }_t^{M_1}{)}^{-1}{\mu }_t^{M_1}+({\Sigma }_t^{M_2}{)}^{-1}{\mu }_t^{M_2}}{({\Sigma }_t^{M_1}{)}^{-1}+({\Sigma }_t^{M_2}{)}^{-1}}(2)$$
$${\Sigma }_t^{MM}=(({\Sigma }_t^{M_1}{)}^{-1}+({\Sigma }_t^{M_2}{)}^{-1}{)}^{-1}(3)$$

这两个高斯的乘积相当于Product of Experts。在这个架构中，不存在跨模式的信息流，因为假设各模式相互独立：来自一个模式的信息无助于估计或评估另一个模式的不确定性。我们将所得模型称为Unimodal Fusion EKF。

5.3 跨模态加权融合

在这个架构中，我们还假设有单模滤波器提供单个状态估计。与Unimodal Fusion EKF类似，我们假设估计值是正态分布且独立的。它们的估计值的整合由系数${\beta }_t^{M_1}$和${\beta }_t^{M_2}$控制，这两个系数将每个单模值（从传感器$M1$或$M2$的观测值中估计出来的）的贡献加权到一个融合的估计值中。每个系数都是根据多模态信号中包含的信息推断出来的。这样，来自一种模式的信息被用来评估另一种模式的不确定性，反之亦然，形成了一种跨模式的信息流。这个架构假设最后和每个单模态的信念都可以用高斯来忠实地表示。我们将这种融合架构应用于整合几个可分化的扩展卡尔曼滤波器的信息，我们将所得模型称为Crossmodal Fusion EKF。

在我们的实施中，${\vec{\beta }}_t^{M_i}$由元素${\beta }_{t,1}^{M_i},{\beta }_{t,2}^{M_i},\cdot \cdot \cdot ,{\beta }_{t,n}^{M_i}$组成，其中$n$为状态空间的维度。协方差矩阵的加权通过正半定矩阵$B_t^{M_i}\in {\mathbb{R}}^{n\times n}$完成，定义为：
$$B_t^{M_i}={\left[\begin{array}{ccc}{\beta }_{t,1}^{M_i}& \cdots & {\beta }_{t,n}^{M_i}\\ ⋮& \ddots & ⋮\\ {\beta }_{t,1}^{M_i}& \cdots & {\beta }_{t,n}^{M_i}\end{array}\right]}^T\left[\begin{array}{ccc}{\beta }_{t,1}^{M_i}& \cdots & {\beta }_{t,n}^{M_i}\\ ⋮& \ddots & ⋮\\ {\beta }_{t,1}^{M_i}& \cdots & {\beta }_{t,n}^{M_i}\end{array}\right]$$
然后再进行加权平均的整合，如下所示：
$${\mu }_t^{MM}=\frac{{\vec{\beta }}_t^{M_1}⨀{\mu }_t^{M_1}+{\vec{\beta }}_t^{M_2}⨀{\mu }_t^{M_2}}{{\vec{\beta }}^{M_1}+{\vec{\beta }}_t^{M_2}}(4)$$
$${\Sigma }_t^{MM}=\frac{B_t^{M_1}⨀{\Sigma }_t^{M_1}+B_t^{M_2}⨀{\Sigma }_t^{M_2}}{B_t^{M_1}+B_t^{M_2}}(5)$$

请注意，在这种情况下，上标$M_1$和$M_2$并不表示起源模态，而是表示它所应用的估计状态。

5.4 单峰加权测量模型

在这个架构中，我们假设单模态测量模型$p(z_t|x_t{)}^{M_1}$和$p(z_t|x_t{)}^{M_2}$，为每个粒子提供单模态权重$w_t^{M_1,[i]}$和$w_t^{M_2,[i]}$。与第 5.2 不同的是，5.2中结合了每个滤波器的估计状态，现在结合了每个测量模型输出的似然。一种方法是使用混合模型，我们将粒子权重计算为单模权重之和：
$$w_t^{[i]}=w_t^{M_1,[i]}+w_t^{M_2,[i]}(6)$$

使用标准的粒子过滤器动态更新和重采样程序，我们可以应用这个架构来整合来自单一动态模型和多个单模测量模型的粒子过滤器的估计。我们称由此产生的模型为Unimodal Fusion PF。

5.5 带学习交叉模式权重的测量模型

最后，我们还可以在5.4中假设的单模态测量模型的融合中加入跨模态信息流。我们通过学习一个混合加权模型，从完整的多模态观测输入中生成非负标量系数${\beta }_t^{M_1}$和${\beta }_t^{M_2}$。这是对以前的工作所采用的离散测量模型切换方法在陆地车辆定位问题中融合全局和局部传感器信息的一种可区分的概括。因此，每个粒子的权重设置为：
$$w_t^{[i]}={\beta }_t^{M_1}\ast w_t^{M_1,[i]}+{\beta }_t^{M_2}\ast w_t^{M_2,[i]}(7)$$
我们将所得模型称为Crossmodal Fusion PF。

6. 实现

6.1 神经网络结构

对于可分化的EKF和可分化的PF估计器，我们使用一个共享动力学模型，该模型的参数化项为可训练权重$\theta$：
$$x_t=x_{t-1}+f_{1,\theta }(x_{t-1},u_{t-1})\cdot \sigma (f_{2,\theta }(x_{t-1},u_{t-1}))(1)$$

其中$f_{1,\theta }$为与$x_t$相同维度的状态更新向量，$f_{2,\theta }\in \mathbb{R}$为标量门控/缩放项，$\sigma$为$\sigma (z)=(1+e^{-z}{)}^{-1}$的$sigmoid$函数。将相对状态更新分离成这两个项，使网络可以独立学习状态更新的方向和幅度。对于粒子过滤器，我们通过在我们的状态空间中注入加法高斯噪声，在每次动态更新之后，保持粒子的多样性。输出$f_{1,\theta }$和$f_{2,\theta }$的网络通过三层编码器送入"喂养"输入$x_{t-1}$和$u_{t-1}$。输出的内容被连缀起来，并传递给七层共享层集。

可分化PF的测量模型使用作为输入的观测值（图像、F/T信号和/或末端执行器位置）和状态（物体在桌面上的位置）进行训练，如存储在每个粒子中。可微分PF的测量模型是利用每个粒子中存储的观测值（图像、F/T信号和/或末端执行器位置）和状态（物体在桌面上的位置）作为输入进行训练的。测量模型的输出是给定每个粒子的状态，这些观测值的对数似然。图像输入用一组二维卷积编码，而本体感和触觉则用标准的全连接层处理。编码后的特征被串联起来，然后送入一系列共享的输出层。

虚拟传感器模型架构与我们的粒子过滤器测量模型几乎相同，但网络不是输出一个对数似然，而是输出我们EKF的估计状态和状态协方差。

在跨模态权重模型中，输入也是观测值(图像、F/T信号和/或末端执行器位置)，输出是学习的跨模态融合系数，用于平衡来自每个模态的估计。每个模态都通过一个与我们的测量/虚拟传感器模型具有相同架构的编码器运行，并在被送入输出层之前进行连接，从而产生我们的最终权重。

LSTM基线模型的设计与我们其他估计器的架构和参数数相似，每个模态和控制输入的编码器架构相同。来自每个模态和动作输入的编码特征被连接起来，并通过一系列共享层；与我们的跨模态权重模型相反，这些层以两个LSTM层终止。最后LSTM层的输出再通过一个额外的全连接层进行映射，然后再应用非线性映射来产生状态估计。

我们在所有网络中使用ReLU激活和ResNet式跳转连接。

为了保证比较的公平性，我们对每个模型进行了架构搜索。对于图像编码器，我们尝试了三种架构的变化，将二维卷积的输出映射到我们的全连接层：(a)简单的扁平化操作，(b)空间软最大值，和©平均池化。我们发现，简单的架构（a）对于输出标量权重和对数似然的模型表现良好，而选项（c）对于直接回归XY坐标的网络通用性最好。此外，我们还探索了不同的网络宽度和LSTM隐藏状态维度；对所有全连接层使用64个单元，对每个LSTM层使用512个隐藏状态大小，报告了最终结果。

6.2 训练流程

我们首先对动力学和测量模型进行预训练，然后再以端到端的方式对其进行微调。动态模型（4.2）的预训练是为了使单步预测误差最小化，然后是4步、8步，最后是16步预测误差。

粒子过滤器的测量模型经过预训练，可以预测以粒子过滤器的真实状态为中心的多变量高斯的观测条件对数概率密度函数。同样，EKF的虚拟传感器模型也是经过预训练来生成观测条件下的预测状态。测量不确定度是完全由端到端学习的，而不是预先训练的。

在预训练之后，我们通过时间的反向传播来训练我们的每一个状态估计模型，直到收敛，在越来越长的子序列上。30个粒子被用于训练粒子过滤器。

7. 结论

我们介绍了在状态估计的可微分递归滤波器领域内的融合架构研究。在我们的实验中，我们使用平面推送和开门的案例研究来整合视觉、触觉和本体感觉数据。我们表明，将这些感官输入融合在可区分的过滤器中，利用跨模式信息，可以在不牺牲性能的情况下为可解释性提供宝贵的机会。这对于我们提出的跨模态融合架构尤其如此，它允许对每个模态如何贡献于我们的最终状态估计进行细粒度分析。