vslam论文2：FEJ-VIRO: A Consistent First-Estimate Jacobian Visual-Inertial-Ranging Odometry（ IROS-2022）

FEJ-VIRO:一种一致的第一估计雅可比视觉-惯性-测距里程计

一、摘要

       最近几年，VIO已经实现了很多显著的进步。然而，VIO方法在长期轨迹中会遭受定位飘移。在这篇文章中，我们提出FEJ-VIRO通过一致地将UWB测量值整合到VIO框架去减少VIO的定位飘移。考虑到UWB锚的原始位置通常无法获取，我们提出一种长短窗结构去初始化UWB锚的位置，和状态增广的协方差。初始化后，FEJ-VIRO同时估计UWB锚的位置和机器人的位置。我们进一步分析了视觉-惯性-测距估计器的可观测性，并证明在理想情况下有4个方向无法获得，并且由于虚假信息的获取，在现实情况下这其中一个方向会消失。基于这些分析，我们利用FEJ技术去增强不可观测的方向，因此减少了估计器的不一致性。最后，我们在仿真和现实实验中验证了我们的分析和评估所提出的FEJ-VIRO方法。

二、结论

       在这篇文章中，我们分析了视觉-惯性-测距估计器并提出一种一致滤波器整合来自相机、IMU和多UWB的测量。具体来说，我们证明了VIRO系统中存在4个无法观测的方向，这意味着我们不能通过融合UWB、IMU和相机的测量去设计出无飘移的里程计。我们进一步研究了估计系统的结构，得出当我们在每一个时间步中以最近状态估计评估雅可比时只有三个无法观测的方向，这导致了不一致性。基于这些分析，我们利用FEJ技术将UWB测量一致地融合到一种紧耦合的MSCKF框架中。最后，我们在仿真和现实的实验中都验证了我们的分析和所提出的系统。

       对于进一步的工作，我们会尝试其它方法去解决不一致问题，如不变滤波器或可观测约束技术去实现更高的准确率。

三、实验

       我们基于openvins3 实现我们所提出的一致视觉-惯性-测距滤波器，这是基于视觉-惯性估计器的最先进的滤波器。我们将UWB的测量整合进去并实现了前面描述的UWB初始化。也通过ceres-solver解决了非线性优化问题。

A.仿真实验

 图1：在三个不同仿真数据集上测试的 OpenVINS、VIRO、FEJ-VIRO-S 和 FEJ-VIRO 的归一化估计误差平方 (NEES) 结果。其中 FEJ-VIRO-S 是没有长窗口的 FEJ-VIRO。我们为 OpenVINS 启用了 FEJ，对于 VIRO 的视觉-惯性部分，因此 VIRO 的不一致性仅来自于与 UWB 范围测量的状态更新的结果。

图2：在三个不同的仿真数据集上测试VIRO和FEJ-VIRO的方向误差和3-σ边界。 

B.现实实验

表 I：VIRAL 数据集中六次运行的平均 ATE (m)

 四、引言

       状态估计是自主机器人系统在一些场景中能够运行的基本能力，如自动驾驶、无人空中汽车等。在现实世界的应用中，鲁棒和可靠的状态估计是必要的，因为状态估计中的大误差将导致机器人破坏。在最近几年，VIO由于他的轻量、准确和可靠性越来越受到关注，其已经成功地应用于很多实时机器人系统。

       在VIO系统的车载自定位模块中，长期存在着显著的估计飘移。因此，环路闭合和BA技术被需要以矫正飘移，但通常伴随着对机器人引入不稳定估计中的剧烈变化。所以在机器人系统的实时操作中，这些技术往往不能确保稳定的运行。为了实现准确的车载自定位，一种可替代的解决方式是融合来自GPS运动捕捉器的额外定位信息，或者人工的视觉标记（如AprilTags）.然而，GPS只能在开阔的空间中工作，在一个固定的室内环境中mocap需要昂贵而复杂的设置，且两者都不能滚出密集的城市区域。人工视觉标记的使用不灵活且难以将此标记覆盖到一个大的区域。因此，整合UWB的测量是一种由前景的解决方案，这在室内室外环境中都是低成本。在这篇文章中，我们关注未知UWB锚位置的情况并要求在运行期间实时估计。

       对于UWB锚的融合，这有2种线性方法。第一种是基于优化的方法，通过建立成本函数融合UWB进行优化。然而，这种方法计算量很大。由于机器人系统中车载计算资源的限制，需要轻量化的融合方法。另一种轻量级线是基于滤波的方法，此方法使用EKF滤波进行状态估计。基于滤波方法的关键问题是UWB测量的位置模块，包括状态容器和它的雅可比矩阵估计的设计。在进行使用现存的基于滤波融合方法的实验后，我们发现不合理地 处理不确定性，精度下降。因此，在基于滤波方法中融合UWB测量要求一个明确的理论去指导其状态模块。

       在这篇文章中，我们遵循基于滤波方法的轻量级线，第一次提出一种多状态约束的卡尔曼滤波器（MSCKF）基于VIRO的融合解决方案，这比基于EKF方法的计算量小，且满足机器人系统的车载计算要求。实际上，我们提出的两种技术在理论上去解决UWB测量的状态建模。一种是UWB的稳定初始化，另一种是一致地将UWB测量融合进VIRO。对于UWB的初始化问题，长短滑动窗口策略被提出解决了初始化的问题，该初始化允许用足够的测量去初始化UWB锚的位置，同时评估状态增广的协方差。对于UWB测量的一致融合，我们首先分析了带有未知UWB锚的位置的VIRO系统的可观测性，证明了尽管测距传感器的融合，这仍有4个无法观测的方向，对应于关于重力的全局平移和全局旋转。虽然VIRO系统不能实现无飘移的估计，我们也可以通过融合UWB的测量改善估计的准确度。然而，我们证明通过直接EKF更新操作融合UWB测量导致了不一致性。因此，我们提出FEJ-VIRO框架，这在状态的第一估计中线性化测距模块，也就是我们都知道的FEJ技术，解决了不一致问题。实验表明合理的初始化和一致的滤波设计明显改善了FEJ-VIRO的估计准确度。总的来说，我们这篇文章的贡献如下：

（1）提出了一个基于MSCKF的轻量化视觉-惯性-测距里程计（VIRO）方法，和初始化UWB锚和状态增强的长短滑动窗口策略。

（2）通过分析视觉-惯性-测距估计器的可观测性指出理想情况下四个无法观测的方向。

（3）提出一种FEJ-VIRO框架扩展FEJ技术去维护一致性，获得了估计准确度中可以看见的改善。

（4）在多个仿真和现实数据集的实验中验证了提出的FEJ-VIRO方法的可观测性分析和有效性。