sqrtVINS: Robust and Ultrafast Square-Root Filter-based 3D Motion Tracking
作者: Yuxiang Peng, Chuchu Chen, Kejian Wu, Guoquan Huang
分类: cs.RO
发布日期: 2025-10-11
💡 一句话要点
提出基于平方根滤波的sqrtVINS，实现快速、鲁棒的三维运动跟踪，适用于资源受限的机器人系统。
🎯 匹配领域: 支柱八：物理动画 (Physics-based Animation)
关键词: 视觉惯性导航 平方根滤波 Cholesky分解 动态初始化 机器人 SLAM 数值稳定性
📋 核心要点
- 现有VINS方法在资源受限的嵌入式系统上存在数值不稳定和计算效率低下的问题，限制了其应用。
- 提出基于Cholesky分解的平方根滤波更新方法，保持协方差矩阵的三角结构，提高计算效率和数值稳定性。
- 实验表明，sqrtVINS在32位单精度浮点数上运行速度是SOTA方法的两倍，且具有很高的初始化成功率。
📝 摘要（中文）
本文首次开发并开源了一种基于平方根滤波(SRF)的视觉惯性导航系统(VINS)，命名为sqrtVINS。该系统具有超快的速度、数值稳定性，并且即使在极端条件下（即极小的时间窗口内）也能进行动态初始化。尽管VINS最近取得了进展，但在资源受限且精度有限的嵌入式（机器人）系统上，资源约束和数值不稳定性仍然是关键挑战。基于平方根协方差的滤波器通过提供数值稳定性、高效的内存使用和保证正半定性，提供了一个有希望的解决方案。然而，典型的SRF由于更新期间协方差矩阵的三角结构中断而效率低下。本文提出的方法通过一种新颖的基于Cholesky分解(LLT)的SRF更新，充分利用系统结构来保持结构，从而显著提高了VINS的效率。此外，我们设计了一种快速、鲁棒的动态初始化方法，该方法首先恢复最小状态而不三角化3D特征，然后有效地执行迭代SRF更新以细化完整状态，从而实现无缝的VINS操作。所提出的基于LLT的SRF通过数值研究得到了广泛验证，证明了其卓越的数值稳定性，并在32位单精度浮点数上实现了鲁棒高效的性能，运行速度是现有最先进方法(SOTA)的两倍。我们的初始化方法在移动工作站和Jetson Nano计算机上进行了测试，即使在最小条件下100毫秒窗口内也实现了很高的初始化成功率。最后，所提出的sqrtVINS在各种场景中得到了广泛验证，证明了其强大的效率、鲁棒性和可靠性。完整的开源实现已发布，以支持未来的研究和应用。
🔬 方法详解
问题定义：现有VINS方法在嵌入式系统上，尤其是在资源受限和低精度计算环境下，面临着数值不稳定性和计算效率的挑战。传统的卡尔曼滤波及其变种在更新过程中容易引入数值误差，导致协方差矩阵失去正定性，进而影响系统的稳定性和精度。此外，计算复杂度也限制了其在实时性要求高的机器人应用中的部署。
核心思路：本文的核心思路是利用平方根滤波（SRF）的数值稳定性优势，并结合Cholesky分解来优化SRF的更新过程。通过保持协方差矩阵的平方根形式，可以避免直接计算协方差矩阵，从而减少数值误差的累积。同时，利用Cholesky分解的特性，可以高效地更新平方根形式的协方差矩阵，提高计算效率。
技术框架：sqrtVINS的整体框架包括视觉和惯性数据的预处理、状态估计和地图构建三个主要阶段。首先，对视觉图像和IMU数据进行预处理，提取特征点并进行运动畸变校正。然后，利用基于Cholesky分解的SRF进行状态估计，包括位姿、速度、IMU偏差等。最后，根据状态估计的结果，构建三维地图。动态初始化模块用于在系统启动时快速恢复初始状态。
关键创新：本文最重要的技术创新点在于提出了基于Cholesky分解（LLT）的平方根滤波更新方法。传统的SRF更新过程会破坏协方差矩阵的三角结构，导致计算效率下降。而本文提出的方法通过充分利用系统结构，保持了协方差矩阵的三角结构，从而显著提高了计算效率。此外，快速动态初始化方法避免了三角化3D特征，加速了初始化过程。
关键设计：在SRF更新过程中，采用了Cholesky分解来保持协方差矩阵的平方根形式。具体而言，利用LLT分解将协方差矩阵分解为下三角矩阵L和其转置LT的乘积。在更新过程中，只更新L矩阵，从而避免了直接计算协方差矩阵，减少了数值误差。此外，设计了一种迭代SRF更新方法，以细化初始状态估计。
🖼️ 关键图片
📊 实验亮点
实验结果表明，sqrtVINS在数值稳定性和计算效率方面优于现有方法。在32位单精度浮点数上，sqrtVINS的运行速度是SOTA方法的两倍。此外，该系统在Jetson Nano等嵌入式平台上实现了很高的初始化成功率，即使在100毫秒的时间窗口内也能成功初始化。在各种场景下的验证表明，sqrtVINS具有很强的鲁棒性和可靠性。
🎯 应用场景
sqrtVINS具有广泛的应用前景，尤其是在资源受限的机器人平台上，如无人机、移动机器人和增强现实设备。其高效率和鲁棒性使其能够应用于需要实时性和高精度的场景，例如自主导航、SLAM和三维重建。该系统还可用于开发更可靠和高效的视觉惯性导航解决方案。
📄 摘要（原文）
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.