论文笔记（六十五）Schmidt-EKF-based Visual-Inertial Moving Object Tracking

文章概括

引用：

@inproceedings{eckenhoff2020schmidt,
  title={Schmidt-EKF-based visual-inertial moving object tracking},
  author={Eckenhoff, Kevin and Geneva, Patrick and Merrill, Nathaniel and Huang, Guoquan},
  booktitle={2020 IEEE International Conference on Robotics and Automation (ICRA)},
  pages={651--657},
  year={2020},
  organization={IEEE}
}

Eckenhoff, K., Geneva, P., Merrill, N. and Huang, G., 2020, May. Schmidt-EKF-based visual-inertial moving object tracking. In 2020 IEEE International Conference on Robotics and Automation (ICRA) (pp. 651-657). IEEE.

原文： https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9197352
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

摘要

在本文中，我们研究了紧耦合估计对视觉惯性定位和动态物体姿态跟踪性能的影响。特别地，我们展示了，当给定目标运动的“适当”模型时，联合估计系统优于其解耦的对手。然而，不一致的建模，例如选择不适当的目标传播噪声水平，实际上可能导致自我运动精度的下降。为了应对缺乏目标运动模型良好先验知识的现实场景，我们设计了一个基于Schmidt-Kalman滤波器（SKF）的新系统，在该系统中，目标测量值不会更新导航状态，但所有相关性仍会得到正确跟踪。这使得我们能够在保持目标误差一致性建模的同时，每当跟踪传感器接收到非目标数据（例如，指向静态3D环境特征的测量）时，更新目标估计。通过大量仿真，我们展示了该系统以及基于机器人中心的目标表示，即使在目标运动模型不一致的情况下，也能实现稳健的估计性能。最后，该系统在实际实验中得到了验证，并且显示出准确的定位和物体姿态跟踪性能。

I. 引言

传感器平台跟踪自身运动并感知其环境的能力是许多自主系统中的关键组成部分。由于相机和惯性测量单元（IMU）传感器的成本低廉和重量轻，这些传感器已成为许多应用（如无人自主车辆（UAV）和移动设备）的标准传感器配置[1]。因此，视觉惯性里程计（VIO）结合了视觉和惯性数据来估计平台的自我运动，近年来在研究中得到了广泛关注[2]–[6]。

在许多机器人应用中，用户不仅希望估计自己的运动，还希望跟踪外部物体（目标），这些物体的状态只能通过安装在跟踪机器人上的外感知传感器间接观察到。外部物体跟踪对于动态环境中的安全导航可能是必要的，例如在自动驾驶中[7]，或者甚至可能是传感器配置的总体目标，例如军事监视。因此，同时定位、建图和运动物体跟踪（SLAMMOT）问题既重要又具有挑战性[8]。然而，使用视觉惯性传感器估计外部物体的姿态和运动的问题较少受到关注，只有少数几个例外[9]、[10]。

在执行SLAMMOT时，至今仍然存在一个未解的问题，即是使用紧耦合方法还是松耦合方法[8]。在前者中，自我运动和物体跟踪被归纳为一个单一的联合估计问题[10]，而在后者中，这些过程是分开进行的，通常通过将目标跟踪条件化为定位模块的输出[11]。在本文中，基于我们之前在紧耦合视觉惯性定位和刚体目标跟踪方面的工作[10]，我们进一步研究了目标运动建模对整体估计性能的影响。特别地，我们展示了，虽然紧耦合系统能提高两个过程的准确性（即，当与目标共同估计时，定位性能比仅丢弃这些测量值时更好），但只有当目标运动噪声值被“适当地”选择时，这一优势才会显现。如果噪声值过于自信，这实际上会导致VIO性能的下降。这在自主车辆执行跟踪并依赖其VIO精度来维持操作时，尤其可能是灾难性的。

因此，我们提出利用Schmidt-Kalman滤波（SKF）[12]，这是对标准扩展卡尔曼滤波器（EKF）的扩展，并采用目标的局部（机器人中心）表示，特别是在对所选目标模型的信心较低时。这使得VIO估计结果可以证明与丢弃目标测量值时相同，然而目标和IMU之间的所有相关性仍然得到了正确建模。本文工作的贡献总结如下：

• 我们研究了在联合视觉惯性定位和目标跟踪滤波器中选择动态物体运动噪声的问题，并通过仿真展示了这些值的选择可以导致VIO精度的提高或降低。

• 我们提出了一种新的Schmidt-EKF公式，该公式不允许目标测量值更新导航状态，但仍然一致地跟踪所有相关性，并展示了该系统即使在模型选择不一致的情况下，也能实现稳健的估计精度。

• 我们提倡采用机器人中心的目标姿态表示，证明它能为EKF和SKF公式提供更好的性能。

• 所提系统在实际实验中得到了验证，并显示出准确的定位和动态物体姿态跟踪估计能力。

II. 相关工作

虽然SLAMMOT在其他研究中已有探讨[8]、[13]，但很少有工作利用视觉惯性传感器与目标的姿态表示相结合。特别是，许多目标跟踪系统将目标视为一个单一的3D点[14]。在这种情况下，仅能使用该代表性特征的测量值来更新目标，从而忽略了目标身体上通常存在的其他特征所提供的附加信息。点粒子模型的另一个限制是，它需要连续观察该单一的点才能进行更新。然而，在许多场景中，跟踪机器人可能从不同的视角观察目标，尽管目标仍在传感器视野内，但该特征可能会被遮挡。将目标视为姿态并与刚性连接的点云相结合，放松了这一假设，因为我们可以在任何特征被观察到时获得目标状态的信息。

进行完整目标姿态估计的工作通常采用更先进的传感器，如LIDAR[15]或RGB-D相机[16]，由于重量和成本的限制，这限制了其在轻量级微型飞行器（MAV）上的应用。其他基于目标姿态的估计方法需要额外的先验知识来处理估计问题的模糊性，例如在单目视觉中目标的尺度。例如，Li等人[11]使用了目标的尺寸先验，并将其条件化为自我运动估计的输出（即解耦方法），因此未考虑这些估计中的不确定性。

与我们工作最接近的研究是Qiu等人[9]提出的，该研究使用了一种稳健的单目视觉惯性批处理估计器[3]，该估计器首次用于跟踪传感器平台的运动。目标物体通过基于学习的物体识别系统YOLO[17]进行检测，然后使用纯视觉的结构光学问题估计目标的点云（至尺度）以及相机与目标之间的相对姿态，同时通过轨迹相关性估计度量尺度。尽管该系统展示了稳健的单目姿态跟踪性能，但由于缺乏明确估计目标运动参数，因此尚不清楚该方法是否能用于主动跟踪目的，因为未来的目标状态无法预测。与此不同，我们的系统将目标观测融合在紧耦合的概率公式中，并能够通过目标观测信息、假设的运动模型以及适当建模其不确定性来提高整体轨迹精度。由于我们还估计运动参数，我们能够预测目标在未来时间步的姿态，并提供该预测的相关不确定性，这在主动跟踪场景中可能是有用的[18]。

在我们之前的工作[10]中，基于轻量级多状态约束卡尔曼滤波器（MSCKF）框架[19]，我们提倡将跟踪器的视觉惯性导航与目标刚体姿态估计进行紧耦合，以提高两者的精度。我们在同一全球坐标系中表示跟踪机器人和目标，并分析了不同假设的目标运动模型的影响。与此相反，在本文中，我们提倡将目标姿态表示为跟踪机器人参考系中的“局部”表示。我们还研究了在某些场景下，原始紧耦合系统在假设不良的目标运动模型时，可能导致轨迹精度下降的情况。尽管以前的方法通过使用多个估计器，每个估计器使用不同的模型来处理模型不确定性，但这些方法会导致计算量的显著增加[20]。为了解决这个问题，我们提出了一种替代解决方案，利用Schmidt-Kalman滤波器[12]公式，通过一致估计目标，同时防止损坏跟踪机器人的VIO。

SKF公式仅更新某些变量的估计值，同时保持一组“干扰”参数的估计值不变，同时仍然一致地跟踪所有相关性。尽管这种方法带来了计算上的优势，正如在VIO社区中为降低复杂度而进行的研究[21]–[23]，但在这里我们利用SKF防止不一致的目标模型破坏跟踪机器人的轨迹估计。我们注意到，SKF曾在目标跟踪的背景下进行过研究，以解决这个问题或避免估计导航误差，但这些工作没有像本文这样考虑视觉惯性领域或物体姿态跟踪[24]–[26]。

III. 视觉惯性估计

所提出的视觉惯性目标跟踪系统是标准MSCKF框架[19]的扩展。我们定义需要估计的IMU状态为：
$${\mathbf{x}}_I={\left[\begin{array}{lllll}{I}_G{\bar{q}}^{\top }& G_{\mathbf{p}}\top & G_{{\mathbf{V}}_I^{\top }}& {\mathbf{b}}_{\omega }^{\top }& {\mathbf{b}}_a^{\top }\end{array}\right]}^{\top }\text{\hspace{1em}(1)}$$

其中，$I_G\bar{q}$ 是JPL约定的四元数[27]，它参数化了旋转矩阵$I_{G}R$，该矩阵将向量从与重力对齐的世界坐标系旋转到本地IMU坐标系。${}^G{\mathbf{p}}_I$和${}^G{\mathbf{v}}_I$分别是IMU在全局坐标系中的位置和速度，${\mathbf{b}}_{\omega }$和${\mathbf{b}}_a$分别是陀螺仪和加速度计的偏置，它们会污染相应传感器的测量。向量误差状态$\tilde{x}$通过映射$\mathbf{x}=\hat{\mathbf{x}}⊞\tilde{\mathbf{x}}$定义，其中$\hat{\mathbf{x}}$是当前的最佳估计，$⊞$操作将流形估计值和修正向量映射到更新后的流形元素[28]。对于向量，操作是标准的加法，而对于四元数，我们使用左乘四元数误差[27]。

除了这个不断变化的惯性状态外，按照标准的MSCKF公式，我们还保持对过去$m$个成像时刻的IMU姿态的估计。我们将这一组克隆状态表示为：

$${\mathbf{x}}_{IC}={\left[{}_G^{I_{k-1}}{\bar{q}}^{\top }{}^G{\mathbf{p}}_{I_{k-1}}^{\top }{\cdots }_G^{I_{k-m}}{\bar{q}}^{\top }{}^G{\mathbf{p}}_{I_{k-m}}^{\top }\right]}^{\top }$$

该系统的目标是跟踪外部物体的姿态运动以及其本地点云中与物体刚性连接的特征，这些特征可以被传感器的相机看到并跟踪。尽管在此我们仅考虑本地速度模型，正如我们之前的工作[10]中所述，但实际上可以用多种不同的方式来建模目标的运动。在所选模型中，估计的目标状态表示为某参考坐标系{R}中的姿态和本地线性与角速度。

$${\mathbf{x}}_T={\left[{}_R^T{\bar{q}}^{\top }R{\mathbf{p}}_R^{\top }{}^T{\mathbf{v}}_T^{\top }{}^T{\omega }_T^{\top }\right]}^{\top }$$

请注意，角速度${}^T{\omega }_T$是目标的角速度，它与IMU平台的角速度不同。在本工作中，我们区分了两种目标姿态表示：全局表示和局部表示。在全局表示中，目标姿态以与IMU相同的全局坐标系（$R=G$）表示，而在局部表示中，目标姿态相对于IMU表示（$R=I$）。

最后，假设目标物体作为刚体运动，我们估计其由一组3D特征${}^T{\mathbf{p}}_{fi}$组成的本地点云，这些特征在目标坐标系中保持静态。为了更好地使用延迟初始化[29]将这些点加入到我们的状态中，我们还将保持一组目标姿态克隆：

请注意，局部公式中的目标克隆是相对于对应的IMU克隆表示的，而不是相对于不断变化的IMU状态。然后，单个目标特征的完整估计状态为：

$$\mathbf{x}={\left[\begin{array}{lllll}{\mathbf{x}}_I^{\top }& {\mathbf{x}}_T^{\top }& {\mathbf{x}}_{IC}^{\top }& {\mathbf{x}}_{TC}^{\top }& {}^T{\mathbf{p}}_f^{\top }\end{array}\right]}^{\top }\text{\hspace{1em}(2)}$$

A. 系统动态模型

所考虑的视觉惯性系统包含IMU（即传感器平台）和目标的动力学。惯性动力学是标准的，可以在[27]中找到。与传感器平台不同，我们无法获取任何关于目标的直接本体感知信息，只能通过其估计的运动参数进行预测。全局目标模型的动力学由以下公式给出：

在局部表示中，目标的演化与IMU的演化是耦合的：

在这两种表示中，我们将局部线性速度和角速度建模为随机游走：

$${}^T\underset{T}{\overset{.}{\mathbf{v}}}={\mathbf{n}}_{vt}{,}^T\underset{T}{\overset{.}{\omega }}={\mathbf{n}}_{\omega t}\text{\hspace{1em}(6)}$$

因此，目标与假定运动模型的符合程度通过控制其传播噪声的值来表示（6）。

协方差传播：基于上述跟踪机器人和目标的运动模型，我们可以定义堆叠系统误差状态的传播。令$x_S$表示零动态静态变量的集合（其真实值随时间不变，例如静态环境或目标特征，以及可能的固定校准参数）。整体线性化误差状态的演化由下式给出：

$$\left[\begin{array}{c}{\stackrel{.}{\tilde{\text{x}}}}_I\\ {\stackrel{.}{\tilde{\text{x}}}}_T\\ {\stackrel{.}{\tilde{\text{x}}}}_S\end{array}\right]\approx \left[\begin{array}{ccc}{\mathbf{F}}_x& 0& 0\\ {\mathbf{A}}_x& {\mathbf{A}}_T& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\tilde{\text{x}}}_I\\ {\tilde{\text{x}}}_T\\ {\tilde{\text{x}}}_S\end{array}\right]+\left[\begin{array}{cc}{\mathbf{G}}_x& 0\\ {\mathbf{\Gamma }}_x& {\mathbf{\Gamma }}_T\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_I\\ {\mathbf{n}}_T\end{array}\right]$$

其中${\mathbf{F}}_x$是IMU误差状态演化关于IMU误差的雅可比矩阵，${\mathbf{A}}_x$和${\mathbf{A}}_T$分别是目标误差状态演化关于IMU误差和目标误差的雅可比矩阵。最后${\mathbf{G}}_x$、${\Gamma }_x$和${\Gamma }_T$是分别关于IMU和目标传播噪声(${\mathbf{n}}_I$和${\mathbf{n}}_T$)的雅可比矩阵。在全局目标姿态表示的情况下，IMU和目标的演化将解耦，因此${\mathbf{A}}_x$和${\Gamma }_x$将是零矩阵。根据这些连续的误差状态动力学，可以执行标准的EKF传播[27]。

B. 一般目标更新

在跟踪传感器平台和目标的运动过程中，收集到指向目标特征的相机方位测量值。测量函数由下式给出：

$${\mathbf{z}}_{if}=\mathbf{\Pi }\left({}^{C_i}{\mathbf{p}}_f\right)+{\mathbf{n}}_{if}\text{\hspace{1em}(7)}$$

这里${}^{Ci}{\mathbf{p}}_f$表示3D特征在测量相机坐标系中的位置，$\Pi$是投影函数$\Pi ([xyz{]}^{\top })=[x/zy/z{]}^{\top }$，将表示在相机坐标系中的特征位置映射到相应的标准化像素坐标测量值，${\mathbf{n}}_{if}\sim N(0,{\mathbf{R}}_{if})$是污染测量的高斯白噪声。特征在相机坐标系中的位置，使用全局和局部表示分别给出：

$$\begin{array}{rl}& {}^{C_i}{\mathbf{p}}_f{=}_I^C{\mathbf{R}}_G^I\mathbf{R}\left({}_T^G{\mathbf{R}}^T{\mathbf{p}}_f{+}^G{\mathbf{p}}_T{-}^G{\mathbf{p}}_I\right){+}^C{\mathbf{p}}_I\\ & {=}_I^C\mathbf{R}\left({}_T^I{\mathbf{R}}^T{\mathbf{p}}_f{+}^I{\mathbf{p}}_T\right){+}^C{\mathbf{p}}_I\end{array}\text{\hspace{1em}(8)(9)}$$

其中 { I C R , C p I } \{^C_I{\mathbf{R}}, ^C{\mathbf{p}}_I\} {IC​R,CpI​}表示IMU与相机之间的相对姿态。通过对当前状态估计进行线性化，我们对目标特征测量(7)进行EKF更新，更新状态估计$\hat{\mathbf{x}}$和协方差$\mathbf{P}$：

$$\begin{array}{rl}& \mathbf{K}={\mathbf{P}}^{\ominus }{\mathbf{H}}^{\top }{\mathbf{S}}^{-1}:=\mathbf{L}{\mathbf{S}}^{-1}\\ & {\hat{\mathbf{x}}}^{\oplus }={\hat{\mathbf{x}}}^{\ominus }⊞\mathbf{K}\left({\mathbf{z}}_{if}-\mathbf{\Pi }\left({}^{C_i}{\hat{\mathbf{p}}}_f\right)\right)\\ & {\mathbf{P}}^{\oplus }={\mathbf{P}}^{\ominus }-\mathbf{K}{\mathbf{H}}^{\top }{\mathbf{P}}^{\ominus }\end{array}\text{\hspace{1em}(10)(11)(12)}$$

其中$\mathbf{H}$是测量的雅可比矩阵，$\mathbf{S}=\mathbf{HP}\ominus {\mathbf{H}}^{\top }+{\mathbf{R}}_{if}$，${}^C{\hat{\mathbf{p}}}_f$是使用当前最佳状态估计计算(8)或(9)的结果。此外，我们使用$\ominus$和$\oplus$来区分更新前后的状态/协方差。除了这些描述的目标测量外，系统还使用指向静态环境特征的方位测量值，以标准MSCKF方式[19]执行更新。

IV. 建模误差的数值分析

尽管已有研究表明，在紧耦合滤波器中共同估计目标跟踪和定位可以提高性能[30]，但通过仿真我们发现这个结论更为复杂。所提框架的根本假设是，观测到的目标将遵循我们选择的运动模型，以便预测目标的未来轨迹。在目标模型不准确的情况下，例如在(6)中使用了过小的噪声，系统将引入不一致性，因为我们对目标的演化过于自信，最坏情况下，这会负面影响跟踪机器人轨迹的准确性。相反，如果我们选择过大的噪声值，则运动参数估计变得不稳定，并且我们无法获得足够的信息来改进跟踪机器人估计。

为了证明这一点，创建了一个数值仿真场景，其中一架配备IMU和立体相机的无人机跟踪一个沿半圆形轨迹运动的平面目标机器人。通过对跟踪机器人轨迹进行B样条拟合，计算了真实的角速度和线性加速度。基于这些数据，模拟了噪声IMU测量值（以及相应的随机游走偏差），采样率为200 Hz。仿真了一个沿工作空间地面和边界的3D点地图。通过将地图点投影到真实相机姿态中，并加入1像素噪声，在10 Hz的频率下生成了合成的立体图像。目标的点云通过在每个时间步将一组位于1米立方体表面上的点刚性地附加到真实目标的姿态上进行仿真。为了模拟现实中前端面临的问题，模拟了由于目标遮挡和随机丢失给定特征的情况。由于目标物体在平面上操作，我们仅在局部x和y方向上将线性速度建模为随机游走，而角速度则沿局部z轴建模为随机游走。我们注意到，这种类型的目标在现实场景中非常常见，例如在跟踪地面车辆时。

为了评估不同噪声参数对估计的影响，我们选择了单一噪声标准差${\sigma }_t={\sigma }_{wz}={\sigma }_{vx}={\sigma }_{vy}$来驱动假定的随机游走，见公式(6)，并对一系列值进行了参数扫掠。请注意，在所有实验中，我们使用了相同的真实IMU和目标轨迹，仅在目标传播过程中变化了滤波器使用的噪声水平。虽然可以进行更复杂的扫掠，变动所有方向上的噪声，但我们发现使用单一参数已经足够演示所讨论的问题。对于每个假定的噪声强度，使用相同的真实轨迹进行了30次蒙特卡罗仿真，每次仿真具有不同的IMU和像素测量噪声实现。表I中显示的平均均方根误差（RMSE）值清楚地说明了假定不正确的模型噪声时引起的不一致性问题。标准VIO在不估计外部目标的情况下能够实现0.231米和1.397度的平均RMSE，对于一个165米的轨迹。当使用“适当”的噪声值时，定位性能有所改善，但选择过小的目标传播噪声所引入的过度自信破坏了整个系统。此外，我们发现噪声过大也可能损害VIO的准确性。这很可能是因为在这些噪声水平下，运动参数估计变得非常不稳定，导致目标预测不准确，从而可能引发较大的线性化误差。

如图1所示，查看典型的单次运行及其误差边界，目标跟踪的紧耦合相较于独立VIO减少了不确定性，同时在“良好”噪声参数的情况下保持一致性（即误差保持在协方差提供的边界内）。当选择的噪声过小时， resulting 轨迹误差是不一致的，从而显示出由目标运动建模不正确引起的破坏。因此，紧耦合估计的有效性在很大程度上取决于运动噪声参数的选择。这对现实世界中紧耦合目标估计算法的应用具有深远的影响，因为正确的噪声值选择是困难的，并且可能使系统容易受到不准确性的影响，最坏情况下甚至可能导致滤波器发散。这促使了所提出的Schmidt-EKF公式，它可以在不让目标估计的不一致性影响跟踪机器人轨迹的情况下，建模跟踪机器人状态与目标之间的不确定性。

图1：使用单独的VIO、紧耦合跟踪（"良好"目标${\sigma }_t=0.1$）以及过于自信的${\sigma }_t=0.005$时IMU全局姿态的估计误差。在紧耦合系统中，使用“适当”噪声时，误差边界减小，估计保持一致，显示目标信息的融合改善了定位性能。对于过于自信的噪声，估计器变得不一致。

V. Schmidt EKF更新

在我们希望防止跟踪机器人估计值被破坏的情况下，我们利用SKF[12]来更新目标状态；也就是说，在处理目标的测量时，我们对惯性状态（及其对应的克隆）进行“Schmidt”处理，从而只更新目标估计值及其与跟踪机器人状态的相关性。更明确地，我们将状态分为两部分，跟踪机器人的状态$x_R$和目标参数$x_T$，使得$\mathbf{x}=[{\mathbf{x}}_R^{\top }{\mathbf{x}}_T^{\top }{]}^{\top }$，并将协方差分解为：$\mathbf{P}=\left[\begin{array}{cc}{\mathbf{P}}_{RR}& {\mathbf{P}}_{RT}\\ {\mathbf{P}}_{TR}& {\mathbf{P}}_{TT}\end{array}\right]$。现在，让我们考虑一个用于更新状态的测量残差$r$。SKF通过将跟踪机器人的卡尔曼增益设为零来不更新跟踪机器人的状态（即$\mathbf{K}=[{\mathbf{K}}_R^{\top }{\mathbf{K}}_T^{\top }{]}^{\top }\to [0^{\top }{\mathbf{K}}_T^{\top }{]}^{\top }$）。更新方程变为以下形式：

$$\begin{array}{rl}& {\hat{\mathbf{x}}}^{\oplus }={\hat{\mathbf{x}}}^{\ominus }-\left[\begin{array}{c}0\\ {\mathbf{K}}_T\end{array}\right]\mathbf{r}={\hat{\mathbf{x}}}^{\ominus }⊞\left[\begin{array}{c}0\\ {\mathbf{L}}_T\end{array}\right]{\mathbf{S}}^{-1}\mathbf{r}\\ & {\mathbf{P}}^{\oplus }={\mathbf{P}}^{\ominus }-\left[\begin{array}{cc}0& {\mathbf{L}}_R{\mathbf{S}}^{-1}{\mathbf{L}}_T^{\top }\\ {\mathbf{L}}_T{\mathbf{S}}^{-1}{\mathbf{L}}_R^{\top }& {\mathbf{L}}_T{\mathbf{S}}^{-1}{\mathbf{L}}_T^{\top }\end{array}\right]\end{array}\text{\hspace{1em}(13)(14)}$$

其中$[{\mathbf{L}}_R^{\top }{\mathbf{L}}_T^{\top }{]}^{\top }=\mathbf{L}$来自(10)。显然，在此更新中，跟踪机器人的状态${\mathbf{x}}_R$和边际协方差${\mathbf{P}}_{RR}$保持不变，从而防止在目标估计不一致的情况下这些估计被破坏。因此，结果VIO估计等同于如果目标测量值被丢弃的情况。需要注意的是，SKF仍然在协方差中跟踪相关性，可以证明它是EKF更新的保守近似[21]。我们还强调，SKF仅在使用目标测量值进行更新时使用。在处理与静态场景对应的视觉数据时，完整的状态会正常更新。这允许IMU的修正来修正目标估计，这只有在一致跟踪的相关性存在的情况下才有可能。

表一：跟踪机器人和目标的平均RMSE（以米/度为单位）在不同的${\sigma }_t$值下，分别为全局（G）和局部（L）紧耦合估计器。我们评估了跟踪机器人IMU坐标系中的目标姿态${}_I^T\mathbf{T}$，全局坐标系中的目标姿态${}_G^T\mathbf{T}$，以及全局坐标系中的IMU姿态${}_G^I\mathbf{T}$。

表 II：所提SKF公式的跟踪机器人和目标的平均RMSE。对于非常过度自信的噪声值，全球表示会导致目标估计发散。

由于SKF不更新状态的某一部分，估计变量的表示对性能变得至关重要[24]。在全局模型中，滤波器可以完全更新目标的全局姿态，而在局部模型中，它完全更新IMU与目标之间的相对姿态。直观地说，由于目标测量是相对于传感平台的，因此完全更新两者之间的相对关系更为合理。为了研究这种以机器人为中心的表示的影响，我们使用局部公式重新进行了上一节的噪声扫掠，如表I所示。这些结果表明，对于紧耦合系统，以机器人为中心的滤波器在目标和VIO精度方面都优于其全局版本。这很可能是由于在局部坐标系中线性化误差通常较小。然而，当使用不良的运动模型时，该系统仍然表现出相同的问题。

我们使用全局和局部表示测试了所提的SKF公式（表II）。如预期的那样，对于“适当”噪声值，系统无法从紧耦合估计中受益，而即使目标模型不一致，VIO也不会受到破坏。此外，我们实验发现，在较低噪声水平下（通常在进行大幅目标更新后），全局目标姿态滤波器更容易出现目标估计发散，而以机器人为中心的方法即使在使用不一致的运动模型时，始终能够提供准确的跟踪性能。这促使我们在EKF和SKF公式中选择局部表示，并表明我们的局部SKF滤波器对于不良目标模型具有鲁棒性。

VI. 扩展到现实世界跟踪

接下来，我们在一个现实世界的场景中评估了基于局部SKF的定位和物体跟踪，使用了立体视觉惯性设备。在接下来的部分，我们首先讨论了所需的视觉特征前端，既要对目标进行分割，又要在其轨迹上跟踪该目标上的特征。从高层次来看，我们使用了一种UNet变体[31]对输入图像进行分割，为每张图像生成目标的掩码。利用这个掩码，我们随后对环境特征和目标特征分别进行了视觉跟踪，然后可以将这些信息融合到所提的估计器中。

A. 基于深度学习的目标检测

由于缺乏带有机器人和目标真实轨迹的标注视觉惯性目标跟踪数据集，我们决定收集并标注自己的数据集，跟踪一辆大型遥控汽车。我们从左侧相机以1Hz的频率收集了训练、验证和测试数据集，共收集了279张用于训练和验证的图像（其中10％随机选择用于验证），以及85张用于测试。这些图像通过手工标注生成了真实标签掩码。用于收集图像的相机使用了鱼眼镜头，但掩码是在不去畸变图像的情况下生成的，以避免以后需要重新畸变。然而，由于这个原因，我们只使用了随机的左右翻转来增强数据，而没有使用旋转和随机裁剪。

我们使用了流行的UNet架构的一个变体来进行目标分割。在我们的测试中，我们使用了子像素卷积[32]来替代原始的上采样层，以提高效率。此外，我们用ELU激活函数[33]替换了ReLU激活函数，以避免使用批量归一化——同样是为了提高效率。我们还使用了零填充，以避免裁剪跳跃连接中使用的特征。该网络在使用GTX 1080Ti显卡并采用320 × 240分辨率时，平均可以在不到20毫秒内进行预测。为了测试网络，我们使用了平均交并比（mIOU）和逐像素准确度指标。在我们的例子中，逐像素准确度定义为$\frac{TP+TN}{TP+TN+FP+FN}$，其中$TP$、$TN$、$FP$和$FN$分别是真阳性、真阴性、假阳性和假阴性预测。该网络在测试数据集上达到了0.628的mIOU和99.1%的逐像素准确度（见图2）。

图2：网络在测试数据集上的视觉结果。真阳性像素标记为绿色，假阳性像素标记为蓝色，假阴性像素标记为粉色。

由于目标通常只占整个图像的一小部分，我们选择从分割掩码的边界框中提取目标特征，而不是从整个图像中提取，同时环境特征则从完整图像中提取。为此，我们首先通过一系列膨胀和腐蚀操作去除掩码中大部分的假阳性。然后，边界框被定义为包围面积最大的斑点的矩形。为了平滑噪声掩码，使用了一个单独的卡尔曼滤波器来跟踪这个边界框。

B. 视觉特征跟踪

我们在图像的多个网格上使用FAST检测[34]，以确保特征分布均匀。只有落在掩码内的特征才被标记为目标特征。我们使用OpenCV[36]中的实现对环境特征进行了KLT跟踪[35]，并对目标特征进行了ORB描述符跟踪[37]。这种跟踪既包括在相同时间戳下从左到右的图像，也包括从前一帧图像进行的左到右和右到右的跟踪。使用8点RANSAC对离群值进行拒绝，这对于静态特征和目标特征分别独立执行。

通过这种方式跟踪了若干帧的目标特征被初始化为永久物体特征。此外，我们还进行了ORB描述符匹配，匹配到当前估计的目标点云，以确定新的跟踪是否对应于之前见过的特征。我们发现，这些目标回环事件对于估计器的性能至关重要，因为它们大大限制了目标相对于IMU的漂移（因此也限制了全局漂移）。由于在实际实验中常常会出现不匹配的特征，我们使用了马哈拉诺比斯距离检验来拒绝不一致的测量值。

C. 实验结果

在这次实验中，一台手持视觉惯性设备跟踪了一辆遥控汽车。网络通过先前收集的数据集以之前描述的方式训练来检测该车辆。需要注意的是，与现有技术[9]不同，我们不需要高分辨率的相机（虽然高分辨率相机肯定能提高精度）。我们允许最多跟踪150个来自点云的特征。为了限制视觉惯性漂移，我们添加了最多10个SLAM特征，这些特征来自于达到窗口大小（本次测试中为10张图像）的视觉轨迹[29]，当它们丢失时会被边缘化。我们还进行了IMU-相机外参的在线估计，以处理可能的较差先验校准。由于这是一个（主要）平面场景，我们选择将驱动局部z轴线性速度和俯仰/滚转角速度的噪声设为零，正如在仿真示例中所做的那样。在此实验中，图像采集频率为10 Hz，整个系统在一台商业笔记本上实时运行。

图3：所提SKF系统的IMU和目标全局姿态的真实轨迹和估计误差。IMU和目标的总轨迹分别为62米和52米。

为了评估精度，跟踪场景在一个大型Vicon房间内进行，从而提供了传感器平台和目标的高精度姿态估计。选用了一个故意过于自信的目标运动模型来说明所提方法的鲁棒性。在超过30次蒙特卡罗运行中，基于SKF的目标跟踪的VIO RMSE为0.153米/1.091度，而全局目标误差为0.183米/3.443度。一个示例运行的误差随时间变化的图如图3所示。相比之下，紧耦合系统在IMU上的误差为0.409米/1.640度，目标上的误差为0.492米/3.037度，显示了由于不良运动模型导致的VIO性能下降。这些结果表明，即使使用过于自信的目标模型，所提系统仍能准确估计目标和导航状态。此外，网络提供的掩码在整个测试过程中多次在连续帧中失败，但由于目标运动参数的估计，我们的系统能够处理这一问题。

VII. 结论

在本工作中，我们解决了紧耦合视觉惯性定位和3D刚体目标跟踪框架中不一致的目标运动建模问题。我们展示了当模型准确描述目标运动时，紧耦合估计能提高定位和跟踪任务的准确性。我们还阐明了过度自信的模型选择所固有的风险，并展示了使用SKF方法可以防止不一致的运动噪声破坏VIO任务，同时仍能正确跟踪目标与导航状态之间的所有相关性。此外，我们提倡在EKF和SKF公式中使用目标的局部表示，以提高估计性能。所提方法在现实世界的跟踪场景中得到了验证，并证明即使不使用高分辨率相机或持续成功的视觉跟踪，也能准确估计自我和目标的运动。