PLV-IEKF：基于点、线，消失点一致性的视觉惯性里程计方案

文章：PLV-IEKF: Consistent Visual-Inertial Odometry using Points, Lines, and Vanishing Points

作者：Tong Hua, Tao Li , Liang Pang , Guoqing Liu, Wencheng Xuanyuan , Chang Shu and Ling Pei

编辑：点云PCL

摘要

本文提出了一种基于不变扩展卡尔曼滤波器（IEKF）的多特征人造环境下的视觉惯性里程计（VIO）方法。传统的基于扩展卡尔曼滤波器的VIO通常在特征点方法中自然发生的系统不一致性和角漂移方面存在问题。然而，在人为环境中，诸如线条和消失点等显著的结构规律为定位提供了有价值的线索。为了有效利用这些结构特征并保持系统一致性，作者设计了一种基于右不变滤波器的VIO方案，将点、线和消失点特征纳入考虑。有效证明了对于点特征的传统加性误差定义也能像不变误差定义一样保持系统的一致性，通过证明其在数学上等效于测量模型。对于线特征，同样得出了类似的结论。此外，在进行了基于不变滤波器的可观测性分析，证明了消失点测量能够自然地保持不可观测方向。我们进行了模拟和实际测试，验证了我们方法在姿态准确性和一致性方面的性能。实验结果证实了我们方法的竞争性能，突显了其在人造环境中提供准确且一致的姿态估计能力。

主要贡献

近年来，不变扩展卡尔曼滤波器（IEKF）已成功应用于机器人定位，特别是基于滤波的视觉惯性里程计（VIO），IEKF模型为估计的姿态和地标定义了一种替代的非线性误差，自动确保不可观测子空间的适当维度。然而，在MSCKF的情况下，地标未包含在状态中。这导致地标的两种不同误差定义：遵循IEKF的非线性误差定义和遵循传统MSCKF的加性误差定义。我们研究这两个定义之间的关系以及它们对滤波器一致性的影响是我们的动机。主要贡献总结如下：

• 提出了一种基于右不变滤波器的VIO，利用点、线和消失点，改善了姿态的一致性和准确性。
• 在滤波器设计中，证明了点特征的等效测量模型，证明即使在它们与状态向量解耦时，这些特征在传统误差表示中也不会改变滤波器的一致性。对于线特征也可以得出类似的结论。
• 进行了一项可观测性分析，以证明我们的方法自然地保持系统的不可观测子空间，并增强了线特征的可观测性。

内容概述

基于IEKF框架构建我们的系统，如图1所示

图1. PLV-IEKF系统概览

A. IMU运动学模型

这个连续运动学模型描述了惯性里程计单元（IMU）在运动中的变化。它包括旋转速度、全局速度和全局位置的更新，考虑了角速度、线性加速度以及陀螺仪和加速度计的偏差。通过高斯噪声引入不确定性。模型的线性化形式用于计算误差状态的传播，涉及连续状态转移矩阵和输入噪声雅可比矩阵。整体而言，这个模型描述了IMU的动态行为及其误差的演变过程。

B. 特征测量模型

该模型描述了在初始化点特征位置时，如何通过投影函数获得特征点的归一化坐标。通过引入误差向量，建立了线性化的测量模型，其中包括了雅可比矩阵和特征点的误差。特征点误差遵循不变误差定义，这通过方向误差和雅可比矩阵进行描述。一个命题揭示了不变形式和传统误差形式之间的等价关系。最后，等价的测量模型表明，无论采用不变误差还是加性误差，对于相同估计的地标，点的测量模型都具有相同的一致性属性，尽管加性误差定义更易于实现。

C. 线测量模型

该模型描述了直线特征在归一化图像平面上的投影误差。通过定义直线端点在图像中的距离与估计的2D直线之间的距离来构建误差项。线误差分为全局定义误差和局部定义误差，其等价性通过两者的误差形式得出。线性化测量模型考虑了全局定义误差，其中包括了雅可比矩阵和直线的误差。传统基于直线的VIO通常选择局部定义误差，而IEKF-based VIO可能选择全局定义误差。由于线特征未纳入状态变量，因此两种误差形式在算法实现中均可接受。

D. 消失点测量模型

在人造环境中，直线可分为具有消失点测量的结构线和非结构线。对于结构线，当其相应的消失点计算出时，可以通过计算消失点测量残差来获取消失点的观测。消失点残差由观测值与线方向向量的归一化表示之间的差异计算得出。消失点测量的雅可比矩阵包括了与线测量相似的项，使其成为线测量的补充，充分利用线的几何信息。整体而言，该模型描述了如何通过计算消失点测量残差来利用直线的消失点信息。

实验

为验证我们提出的方法的有效性，我们进行了模拟和实际测试。在实际测试中，我们将算法与一些最先进的VIO算法进行比较。我们的实验在一台配备Intel(R) Core(TM) i7-10710U CPU@1.10Ghz和16G RAM的笔记本电脑上进行。

A. 仿真

在仿真测试中，研究假设一辆车携带100Hz IMU和10Hz相机，沿着半径为6米的圆圈进行十次循环。沿着真实轨迹散布有200个点和140条线，这些点和线分别位于内外圆柱壁和方形墙上。总共有200个点和140条线散布在真实轨迹周围，如图3(a)所示。使用噪声生成传感器测量，并与几种VIO算法进行比较，包括标准点MSCKF、标准点IEKF、点线VP MSCKF和点线VP IEKF，在图3(b)和(c)中报告了姿态准确性指标均方根误差（RMSE）和一致性指标平均标准化估计误差平方（ANEES）。实验结果显示，PLV-IEKF算法在一致性和姿态估计方面均优于其他算法。

图3. 仿真轨迹和30次蒙特卡洛实验结果。(a): 真实轨迹（红色）、点地标（蓝色）和线地标（绿色）；(b): 平均NEES；(c): 平均RMSE。

B. 实际测试

在EuRoC数据集上进行的实际测试表明，我们提出的方法在与其他最先进的VIO算法的比较中表现出色。

图4. 在MH 03 medium序列中跟踪的点（绿色）、线（蓝色）和消失点（红色）

在MH 03 medium序列中的跟踪特征示例如图4所示。RMSE结果报告在表I中，轨迹示例如图5所示。很明显，在大多数情况下，使用线和消失点可以实现更好的姿态准确性，而PLV-IEKF由于更好的一致性表现比PLV-MSCKF更好。

图5. MH 03 medium序列上估计轨迹的X-Y图

实验结果还表明，PLV-IEKF在机器大厅序列中的表现优于PL-IEKF，这归因于环境中存在更多的结构特征。此外，PLV-IEKF在处理时间上表现出竞争性和更高的效率，相较于基于优化的方法。

总结

本文创新性地提出了一种适用于人工环境的不变视觉惯性定位（VIO）方法，充分利用了点、线和消失点特征。我们的框架中包含三种特征测量模型，并通过滤波器设计证明了点和线的两种等效测量模型。通过推导消失点观测矩阵，我们进一步证明了具有线特征的扩展卡尔曼滤波（EKF）模型可以确保理想的不可观测子空间。通过详尽的仿真测试和实际实验，我们验证了我们的算法在一致性和准确性方面相对于其他最先进算法的竞争性性能。未来，我们将致力于优化线和消失点的估计，以实现更为准确和高效的地图建模。