距离-视觉-惯性里程计：无激励的尺度可观测性（ICRA2021）

Range-Visual-Inertial Odometry: Scale Observability Without Excitation

作者：Jeff Delaune,David S. Bayard,Roland Brockers

主要研究点在于，基于EKF的距离-视觉-惯性里程计框架下无激励的尺度可观察性

摘要：对于大多数机器人应用来说，以恒定速度行驶是最有效的轨迹。不幸的是，如果没有加速度计激励，单目视觉惯性里程计 (VIO) 无法观察尺度并遭受严重的误差漂移。这是在 NASA 的 Ingenuity Mars Helicopter 导航系统中加入一维激光测距仪的主要动机。但是，Ingenuity 的简化方法仅限于平坦地形。当前的文献介绍了一种基于使用方面约束的新型距离测量更新模型。由此产生的 range-VIO 方法不再局限于平面场景，而是扩展到通用机器人应用程序的任意结构。一个重要的理论结果表明，对于零加速度或恒加速度运动，尺度不再位于可观察性矩阵的右零空间中。实际上，这意味着在恒速运动下可以观察到尺度，从而可以在任意地形上进行简单而稳健的自主操作。由于测距仪体积小，Range-VIO 保留了 VIO 的最小尺寸、重量和功率属性，并具有相似的运行时间。这些好处是根据代表常见空中机器人场景的真实飞行数据进行评估的。使用indoor stress data和全状态真值证明了稳健性。我们将我们的软件框架 xVIO 作为开源发布。

1 引言

单目视觉惯性里程计 (VIO) 是机器人技术中的一种流行方法，用于在封闭场景或 GPS 拒止环境下获得准确的度量状态估计。事实上，就尺寸、重量和功率而言，相机和惯性测量单元 (IMU) 构成了一个最小的传感器套件，这在大多数机器人上都很容易使用。

但是，单目VIO只能在加速度不恒定的情况下观察运动尺度。这会导致在零或恒速轨迹下出现严重的误差漂移，这在机器人技术中很常见。对于必须依赖准确的 VIO 尺度估计进行控制的应用程序，此问题是关键问题。我们的工作受到Mars helicopters [1]、[2]的推动，但它一般适用于行星、军事和城市机器人；以及沿着笔直的走廊或隧道穿越室内或地下。

我们新颖的距离-视觉-惯性里程计算法甚至可以在零或恒定加速度轨迹下观察尺度。它使用一维激光测距仪 (LRF)，使传感器套件保持轻量化，同时有效地利用 VIO 稀疏结构估计。我们的主要贡献是：

一个距离测量模型，可防止 VIO 标度漂移并适应任何场景结构；线性化距离- VIO 可观测性分析，显示尺度在没有激励的情况下是可观测的；在真实数据集上进行户外演示；使用全状态真值进行室内案例分析；一个开源的 C++ 实现。

在 [1] 中，提出了一种 range-VIO 方法，该方法可以在相对平坦的地形上导航，同时支持演示 NASA 的 Ingenuity Mars Helicopter 所需的稳定静止悬停。当前的论文使用一种新方法扩展了这些 range-VIO 结果，该方法无需任何惯性激励即可观察 3D 地形的尺度。这种概括解决了机器人领域以及未来火星直升机的重要需求。当前论文是先前会议论文 [2] 的期刊扩展，该论文专门针对Mars helicopters应用。这包括在类似火星的地形上运行的候选航天硬件的实时演示。会议论文处理是非理论的，重点是获得概念验证的实证结果。当前的期刊论文推导出并分析了其理论可观察性属性。误差漂移减少是在城市航空机器人数据上评估的，该数据比火星环境更加复杂和 3D。通过全状态真值比较支持的室内测试证明了分面场景假设的稳健性。最后，我们公开源代码。

代码、论文地址：在公众号「3D视觉工坊」，后台回复「尺度可观测性」，即可直接下载。

2 相关文献

A. Visual-Inertial Odometry

VIO 的一个分支基于松耦合的视觉惯性传感。在这些方法中，在与 IMU [4] 融合之前，仅视觉算法可以根据尺度估计位置和速度，根据重力估计方向。视觉里程计模块可以在计算机视觉开发的任何现代算法之间交换，例如 PTAM [5]、SVO [6]、ORB-SLAM [7] 或 DSO [8]。

最准确和稳健的 VIO 方法来自紧耦合的方法，其中由特征轨迹或图像块强度组成的视觉测量直接限制在一个单一估计器中的惯性状态集成。这些方法需要更大的状态向量，这会导致更高的计算成本。但是它们通过惯性和视觉状态 [9] 之间的互相关提高了准确性，并且即使在没有或很少跟踪图像基元时也能传播状态的鲁棒性。最近的方法包括基于滤波器的 [10]、[11] 和基于非线性优化的方法 [12]-[13][14]。一些解决方案使用图像特征坐标进行测量 [10]、[12]、[13]，而其他解决方案使用图像强度值 [11]、[14]。在良好的激励下，典型的位置误差可以低于前进距离的 1% [15]。

B. VIO Observability Analysis

文献中已经详细研究了具有未知 IMU 偏差的 VIO 可观察性。在通用激励下，除了全局位置和围绕重力矢量的旋转外，VIO 状态被发现是可观察的。[16]-[17][18] 为非线性系统证明了这一点；而[19]，[20]证明了线性化系统并提高了其一致性。这些不可观察的量意味着 VIO 位置和航向估计在任何有噪声的条件下都会发生漂移。在实践中，这种漂移在许多小规模运行的机器人场景中是可以接受的。

[21]进一步分析了具有未知偏差的线性化系统在两种特定运动下不可观察的方向。首先，他们表明，如果系统没有自己的旋转运动，那么所有三个全局旋转都将变得不可观察。其次，他们表明，在恒定加速度下，运动的规模是不可观察的。[22] 得出的结果与这些特定的悬停情况一致。在大多数实际应用中完全没有旋转是不太可能的，即使发生了，相机相对于场景结构的相对方向仍然保留。然而，沿着直线运动可能会出现恒定或零加速度，并且在位置和速度漂移方面的尺度误差对机器人轨迹的规划和控制可能是灾难性的。

C. VIO Scale Drift Mitigation

距离传感器及其等效物可用于补充或替代单目相机，以消除 VIO 的尺度可观察性问题。大多数方法利用激光雷达或雷达扫描 [23]、RGBD 相机 [24] 或双目视觉测量 [25]。与 VIO 不同，这些选项受到范围限制或集成成本的影响，从而限制了它们在机器人应用中的使用。

一维激光测距仪 (LRF) 是 SLAM 文献中代表性不足的传感选项。现代单位可以以厘米的分辨率感知数十米。它们适合小型、轻便且节能的封装，即使在资源受限的机器人上也能容纳。在我们之前为 NASA 的 Ingenuity Mars Helicopter [1] 所做的工作中，我们实施了一种距离-视觉-惯性里程计算法，该算法集成了 LRF 测量值，使尺度可观。该结果估计器是低维度，只有 21 个状态，代价是假设场景是平坦和水平的，这与大多数机器人场景不兼容。[26] 通过使用超声波距离测量初始化一些 VIO 特征的深度来解决 3D 场景中的类似问题。这将场景假设从全局平坦放宽到局部平坦，但它也假设局部地形坡度垂直于测距区域内的距离传感器轴。鉴于超声波传感器的大波束宽度，这在 3D 场景中存在问题。

在本文中，我们使用新颖的 LRF 测量模型消除了任何场景结构上的 VIO 尺度漂移。LRF 的精度和窄波束宽度对 VIO 在扩展卡尔曼滤波器 (EKF) 中估计的视觉特征的深度产生了很强的范围约束。这个约束假设场景可以被划分为三角形面，以视觉图像特征为顶点。

3 距离-视觉-惯性里程计

图 1 中我们框架的架构基于扩展卡尔曼滤波器 (EKF)。它将视觉和距离更新与惯性状态传播紧耦合。我们在技术报告 [3] 中提供了完整的推导细节。

图1 距离-视觉-惯性里程计架构。距离和视觉测量z~、雅可比矩阵 J 和协方差矩阵 R 用于校正 EKF 中的惯性导航误差。轨迹管理器将匹配的图像特征分类到轨迹中，而状态管理器则动态地添加和删除或视觉状态。

这使包含在状态向量 [31] 中的特征的 SLAM 更新成为可能。未包含在状态向量中的特征使用 MSCKF [10] 进行处理。与 SLAM 的三次成本相反，MSCKF 更新具有每个特征的线性成本。然而，MSCKF 需要平移运动，因为必须对特征进行三角测量，这在实践中并不总是令人满意。因此，我们总是执行 SLAM 更新，并且仅在平移运动允许时才使用 MSCKF。这种混合方法也是计算效率最高的 [29]。SLAM 特征要么使用半无限深度不确定性进行初始化 [30]，要么在可能的情况下使用 MSCKF 先验 [29]。

使用 FAST 算法 [32] 检测图像中的视觉角点特征，并使用 Kanade-Lucas-Tomasi 算法 [33]、[34] 的金字塔实现进行跟踪。异常特征在两个级别检测：首先在图像级别使用 RANSAC [35]，然后在过滤器级别使用 Mahalanobis 距离测试。图 1 中的轨迹管理器模块根据轨迹长度、检测分数和图像坐标将每个特征分配给 SLAM 或 MSCKF 范例。我们使用图像块来确保 SLAM 特征分布在整个视野中，并确保强位姿约束。

C. Ranged Facet Update

我们的主要贡献是一种新颖的距离测量模型，用于约束 VIO 标度漂移。与 VIO 一样，它旨在处理任意未知的 2D 或 3D 场景。

1) 测量模型

距离测量取决于距离传感器的姿态和场景的结构。贝叶斯估计器中的相关测量模型应考虑两者的不确定性。由于结构不确定性包含在 SLAM 特征状态中，我们利用这些状态来构建新的范围更新模型。

我们的关键假设是，在 LRF 光束与场景的交点周围的三个 SLAM 特征之间，结构是局部平坦的。这一假设源于视觉特征通常位于深度不连续处，并且特征之间的场景结构通常是平滑的。结果部分讨论了这一假设对现实世界序列的影响。在本文中，为了简化起见，我们还假设相机的光学中心和 LRF 的原点之间的平移为零。图 2 说明了场景的几何形状。uri 是在时间 i 沿 LRF 光轴方向的单位向量。Ii 是该轴与地形的交点。F1、F2 和 F3 是 SLAM 特征，在图像空间中围绕 Ii 形成一个三角形。n 是包含 F1、F2、F3 和 Ii 的平面的法向量。

图 2 在时间 i 的距离测量 ⁱz_r 的几何形状。假设场景在由视觉特征 F1、F2 和 F3 形成的小平面内局部平坦以构建范围约束。

如果点积 u_ri⋅n≠0，我们可以将时间 i 的距离测量表示为

2) Delaunay 三角剖分

为了在实践中构建范围更新，我们在图像空间中对 SLAM 特征执行 Delaunay 三角剖分，并选择 LRF 光束与场景相交所在的三角形。我们选择了 Delaunay 三角剖分，因为它最大化了所有可能的三角剖分中的最小角度 [36]。此属性避免了不提供强局部平面约束的“又长又瘦”的三角形。

图 3 显示了 Delaunay 三角剖分，以及选择作为范围方面的三角形，在我们室外测试序列的样本图像上。它还说明了将场景划分为三角形面，在它们的角落处具有 SLAM 特征。请注意，如果状态估计器以轻量级方式仅使用 3 个 SLAM 特征，则这等效于全局平坦世界假设。相反，如果 SLAM 特征的密度增加，小平面的面积趋于零，小平面场景假设几乎消失。

图 3 在室外飞行数据集中跟踪的 SLAM 图像特征之间的 Delaunay 三角剖分。红点代表 LRF 光束与表面的交点。周围的红色三角形是远程刻面。

3）距离异常值拒绝

在用于过滤器之前，距离测量值通过马氏距离测试来检测异常值。该门控将距离测量与根据小平面中三个视觉特征的坐标构建的先验进行比较。它拒绝违反从误差协方差矩阵导出的先验不确定性模型无法解释的方面假设。

4 可观测性分析

我们对线性化 range-VIO 系统进行可观察性分析，因为它基于 EKF。尽管非线性系统的可观测性是完整性所必需的，但这不在本文的范围内。

A. Observability Matrix

B. Unobservable Directions

1) 通用运动

可以验证跨越全局位置的向量或围绕重力向量旋转的向量仍然属于 Mk 的右零空间。因此，在通用运动[19]下，the ranged facet update不会改善 VIO 的可观察性，这是直观的。同样，在没有旋转的情况下，仍然无法观察到全局方向 [21]。

2) 恒定加速

在本小节中，我们证明在恒定加速度的情况下，与 VIO [21] 不同，跨越尺度维度的向量

与 VIO 不同，range-VIO 因此即使在没有加速度激励的情况下也能实现尺度收敛。

3) 零速度

它对应于未包含在 facet 中的 SLAM 特征的深度。这个结果意味着在没有平移运动的情况下，当特征深度彼此不相关时，范围小平面对小平面外的特征没有约束。一旦平台开始移动，视觉测量就开始关联所有特征深度，并且所有特征的深度都可以从单个距离的小平面观察到。

5 实验结果

本节讨论我们的 range-VIO 算法在上一节中介绍的序列上的性能。视觉状态设置为在滑动窗口中容纳 M=4 个位姿，以及 N=27 个 SLAM 特征。捕获异常值的马氏距离测试设置为 2σ，σ 是估计的范围标准偏差。在我们所有的比较测试中，VIO 的运行设置与 range-VIO 完全相同。唯一的区别是使用 range-VIO 中的 ranged facet 模型对距离测量进行了额外处理。

A. Outdoor Flight Tests

图 4 比较了 range-VIO 和 VIO 在室外穿越过程中的位置误差。每个轴上的 Range-VIO 最大误差保持在 1 m 以下，低于行进距离的 0.6%。这种性能类似于最先进的激励下 VIO [15]。相反，VIO 误差从启动时开始沿横移方向（X 轴）上升，与 range-VIO 相比，值最高可达 9 倍。

图 4室外数据集上 range-VIO（顶部）和 VIO（底部）的位置误差。X 和 Y 轴是水平的，Z 是向上的。X 与导线方向对齐。

我们注意到，VIO 误差与尺度误差一致，在恒定加速度下，VIO 无法观察到这种误差。这清楚地说明了 Range-VIO 在机器人常用轨迹上的可观察性优势。我们还注意到，图 4(a) 中的距离 VIO 误差不会受到平坦地形和 3D 结构之间的过渡的影响，这种过渡发生在 t=425 s。这是一个很好的迹象，表明用真实世界的视觉特征构建的面有效地捕捉了场景的结构。此外，当 LRF 碰到路灯时，在 t=410 s 时发生了 7 米的远程刻面异常值。这可以在我们的视频材料中显示的范围剖面中观察到。然而，它不影响图 4(a) 中的 range-VIO 估计，显示了我们的范围异常值拒绝方案的效率。

B. Indoor Stress Tests

为了进一步评估平面模型的稳健性，将 V-C 小节中讨论的室内序列用作应力情况。图 5 比较了距离 VIO 和 VIO 在位置、速度和方向上的误差，因为在室内所有这些状态都可以使用地面实况。在图 5(a) 和 5(d) 中，沿着行进方向可以清楚地看到刻度漂移的减少。Range-VIO 的最大位置误差为 30 cm，即 2.5%，而 VIO 误差在这些具有挑战性的视觉条件和无激励下增长到 2 m，即 17%。

图 5 室内数据集上 range-VIO（顶部）和 VIO（底部）的位置（左）、速度（中心）和姿态（右）误差。X 和 Y 轴是水平的，Z 是向上的。X 与导线方向对齐。

速度和方向图很好地说明了小平面假设如何在具有挑战性的环境中工作。图 5(b) 和 5(e) 显示速度误差受益于 range-VIO 的尺度可观测性，因为它们比 VIO 低两倍。同样，图 5（c）中的距离-VIO 方向误差与图 5（f）中的 VIO 略有不同，尤其是在 Z（偏航）轴上。我们将这些差异解释为由于范围方面假设违反太小而无法被 Mahalanobis 范围异常值拒绝捕获而导致的错误累积。这仅发生在全局偏航轴周围，这两种方法都无法观察到。然而，即使在这种极端压力情况下，偏航误差在 range-VIO 和 VIO 之间也具有相同的数量级，而 range-VIO 在位置和速度漂移减少方面明显优于 VIO。

最后，我们建议读者参考我们的视频材料，以获得具有大激励和良好视觉纹理的序列中的其他比较结果。在这些最佳条件下，VIO 的性能与 range-VIO 相当，最大位置误差为 40 cm。这证实了 VIO 之前没有失谐，而只是缺乏激发。它还证实，在良好的激发和视觉条件下，range-VIO 不会降低 VIO 性能。

6 结论

基于 VIO 的机器人应用受到无法在没有激励的情况下观察尺度的限制。在空中机器人技术中，即使是最基本的悬停和直线轨迹，无激励的尺度可观测性也是至关重要的。我们的主要兴趣是对遥远世界的空中探索，比如火星 [1]。常见的地面应用包括 GPS 不可用、退化（高楼、峡谷）或不够准确（室内）的情况。

使用简单的一维激光测距仪，我们的 range-VIO 方法在没有激发的情况下消除了尺度漂移，同时保持了 VIO 的最小尺寸、重量和功率要求。理论分析证明了在这种条件下规模的可观察性。恒速真实飞行数据的结果表明，与 VIO 相比，误差减少了 9 倍。

新颖的距离更新基于小平面场景假设，该假设有效地利用 VIO 特征深度估计来处理未知结构。基于视觉特征密度，facets 可以从一个平坦的世界假设扩展到几乎没有结构假设。这篇论文和补充报告 [3] 提供了 range-VIO 模型的完整推导。Range-VIO 不需要相对于 VIO 的额外状态，并且不会增加显着的计算成本。我们在压力情况下证明了我们的方面假设的稳健性。

未来的扩展包括增加场景上 LRF 撞击点周围的视觉特征密度，以进一步提高准确性。我们还研究了使用磁力计和太阳传感器来解决下一个主要的不可观察方向：关于重力矢量的方向。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区

原创征稿

初衷
3D视觉工坊是基于优质原创文章的自媒体平台，创始人和合伙人致力于发布3D视觉领域最干货的文章，然而少数人的力量毕竟有限，知识盲区和领域漏洞依然存在。为了能够更好地展示领域知识，现向全体粉丝以及阅读者征稿，如果您的文章是3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、硬件选型、求职分享等方向，欢迎砸稿过来~文章内容可以为paper reading、资源总结、项目实战总结等形式，公众号将会对每一个投稿者提供相应的稿费，我们支持知识有价！

投稿方式

邮箱：vision3d@yeah.net 或者加下方的小助理微信，另请注明原创投稿。

▲长按加微信联系

▲长按关注公众号