arXiv:2510.10346cs.RO2025-10被引 2

提出高效稳定的视觉惯性定位系统,可在极短时间内完成初始化。

sqrtVINS: Robust and Ultrafast Square-Root Filter-based 3D Motion Tracking

  • 基于改进的平方根滤波器,保持数值稳定性并提升计算效率。
  • 在32位浮点下速度达当前最优方法两倍,100毫秒内成功初始化。
  • 适用于嵌入式设备,开源代码支持机器人与移动平台应用。

本文首次开源了基于平方根滤波器(SRF)的视觉惯性导航系统sqrtVINS,具备超高速、数值稳定和极端条件下动态初始化能力。尽管近年VINS进展显著,嵌入式系统因资源限制与有限精度仍面临数值不稳定性问题。平方根协方差滤波器可提供数值稳定性、高效内存使用和保证半正定性,但传统SRF在更新时破坏协方差矩阵的三角结构,导致效率下降。本文提出一种基于LLT分解的新型SRF更新方法,充分保留系统结构,显著提升效率。同时设计快速鲁棒的动态初始化方法:先恢复最小状态而无需三角化三维特征,再通过迭代SRF更新精炼完整状态,实现无缝运行。数值实验表明,该方法在32位单精度浮点下具有优异数值稳定性,性能优于当前最优方法一倍以上;初始化方法在移动工作站与Jetson Nano上均在100毫秒窗口内取得高成功率。sqrtVINS在多种场景下验证,展现出强效率、鲁棒性和可靠性。完整开源代码已发布,支持后续研究与应用。

原文摘要 · Abstract (English)

In this paper, we develop and open-source, for the first time, a square-root filter (SRF)-based visual-inertial navigation system (VINS), termed sqrtVINS, which is ultra-fast, numerically stable, and capable of dynamic initialization even under extreme conditions (i.e., extremely small time window). Despite recent advancements in VINS, resource constraints and numerical instability on embedded (robotic) systems with limited precision remain critical challenges. A square-root covariance-based filter offers a promising solution by providing numerical stability, efficient memory usage, and guaranteed positive semi-definiteness. However, canonical SRFs suffer from inefficiencies caused by disruptions in the triangular structure of the covariance matrix during updates. The proposed method significantly improves VINS efficiency with a novel Cholesky decomposition (LLT)-based SRF update, by fully exploiting the system structure to preserve the structure. Moreover, we design a fast, robust, dynamic initialization method, which first recovers the minimal states without triangulating 3D features and then efficiently performs iterative SRF update to refine the full states, enabling seamless VINS operation. The proposed LLT-based SRF is extensively verified through numerical studies, demonstrating superior numerical stability and achieving robust efficient performance on 32-bit single-precision floats, operating at twice the speed of state-of-the-art (SOTA) methods. Our initialization method, tested on both mobile workstations and Jetson Nano computers, achieving a high success rate of initialization even within a 100 ms window under minimal conditions. Finally, the proposed sqrtVINS is extensively validated across diverse scenarios, demonstrating strong efficiency, robustness, and reliability. The full open-source implementation is released to support future research and applications.

视觉惯性滤波器实时定位

Thank you to arXiv for use of its open access interoperability. PaperDance 不是 arXiv 官方产品;中文卡片由大模型生成,请以原文为准。