Multi-Sensor Fusion for Quadruped Robot State Estimation using Invariant Filtering and Smoothing

📄 arXiv: 2504.20615v1 📥 PDF

作者: Ylenia Nisticò, Hajun Kim, João Carlos Virgolino Soares, Geoff Fink, Hae-Won Park, Claudio Semini

分类: cs.RO

发布日期: 2025-04-29

备注: Accepted for publication in IEEE Robotics and Automation Letters

DOI: 10.1109/LRA.2025.3564711

💡 一句话要点

提出基于不变滤波与平滑的多传感器融合框架，提升四足机器人状态估计精度。

🎯 匹配领域: 支柱一：机器人控制 (Robot Control) 支柱三：空间感知与语义 (Perception & Semantics)

关键词: 四足机器人 状态估计 多传感器融合 不变滤波 不变平滑 激光雷达里程计 群仿射性质

📋 核心要点

  1. 基于本体感受的四足机器人状态估计易受z轴漂移影响，限制了其在复杂环境中的应用。
  2. 提出E-InEKF和E-IS框架，利用不变滤波与平滑理论，融合多种传感器数据，抑制漂移。
  3. 实验表明，该方法在室内外均优于LIO-SAM和FAST-LIO2等激光雷达里程计方法，显著降低了轨迹误差。

📝 摘要（中文）

本文提出两种用于四足机器人状态估计的多传感器融合框架，分别基于不变扩展卡尔曼滤波(InEKF)和不变平滑(IS)，命名为E-InEKF和E-IS。这些方法融合了运动学、IMU、激光雷达和GPS数据，以减轻位置漂移，特别是沿z轴的漂移，这是基于本体感受方法的常见问题。我们推导了满足群仿射性质的观测模型，以将激光雷达里程计和GPS集成到InEKF和IS中。激光雷达里程计通过在并行线程上使用迭代最近点(ICP)配准来整合，从而保持了基于本体感受的状态估计的计算效率。我们使用KAIST HOUND2机器人在室内和室外实验中评估了E-InEKF和E-IS，无论是否使用外部传感器，并将其与基于激光雷达的里程计方法进行基准测试。我们的方法实现了更低的相对位置误差(RPE)，并显著降低了绝对轨迹误差(ATE)，与LIO-SAM和FAST-LIO2相比，室内提升高达28%，室外提升高达40%。此外，我们还比较了E-InEKF和E-IS在计算效率和精度方面的性能。

🔬 方法详解

问题定义：四足机器人状态估计是其自主导航和控制的关键。然而，仅依赖本体感受传感器（如IMU和关节编码器）的方法容易累积误差，尤其是在垂直方向（z轴）上，导致长时间运行后的位置漂移。现有的视觉或激光雷达里程计方法虽然可以提供更准确的定位，但计算成本较高，且可能在特征不明显的区域失效。

核心思路：本文的核心思路是利用不变滤波和平滑理论，设计一种多传感器融合框架，该框架能够有效地融合本体感受传感器（IMU、运动学）、激光雷达和GPS数据，从而在保证计算效率的同时，提高状态估计的精度和鲁棒性。不变滤波能够保证估计器的一致性，降低估计误差。

技术框架：整体框架包含两个主要变体：E-InEKF（基于不变扩展卡尔曼滤波）和E-IS（基于不变平滑）。两个框架都包含以下模块：1) 本体感受传感器数据预处理；2) 激光雷达里程计（使用ICP在并行线程中计算）；3) GPS数据处理；4) 不变滤波或平滑模块，用于融合所有传感器数据并估计机器人状态。E-InEKF进行在线状态估计，而E-IS则进行离线状态估计，可以提供更精确的结果。

关键创新：该方法最重要的技术创新点在于将激光雷达里程计和GPS数据以满足群仿射性质的方式集成到不变滤波和平滑框架中。通过推导合适的观测模型，可以保证估计器的一致性，并有效地利用外部传感器的信息来抑制位置漂移。此外，将ICP配准放在并行线程中运行，保证了整体计算效率，使其适用于实时应用。

关键设计：激光雷达里程计通过ICP算法获得，并将其转化为满足群仿射性质的观测模型。GPS数据也以类似的方式进行处理。不变滤波和不变平滑的具体实现细节，包括状态向量的定义、过程噪声和观测噪声的建模，以及滤波和平滑算法的迭代更新步骤，都根据具体应用场景进行了优化。具体参数设置未在摘要中详细说明，属于未知信息。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，E-InEKF和E-IS在室内和室外环境中均优于LIO-SAM和FAST-LIO2等先进的激光雷达里程计方法。在室内实验中，该方法将绝对轨迹误差(ATE)降低了高达28%，在室外实验中降低了高达40%。此外，该方法还实现了更低的相对位置误差(RPE)，表明其具有更好的长期一致性。

🎯 应用场景

该研究成果可应用于各种需要高精度状态估计的四足机器人应用场景，例如搜索救援、地形勘测、物流运输和农业巡检。通过提高定位精度和鲁棒性，可以使四足机器人更好地适应复杂和动态的环境，从而扩展其应用范围和提升其工作效率。

📄 摘要（原文）

This letter introduces two multi-sensor state estimation frameworks for quadruped robots, built on the Invariant Extended Kalman Filter (InEKF) and Invariant Smoother (IS). The proposed methods, named E-InEKF and E-IS, fuse kinematics, IMU, LiDAR, and GPS data to mitigate position drift, particularly along the z-axis, a common issue in proprioceptive-based approaches. We derived observation models that satisfy group-affine properties to integrate LiDAR odometry and GPS into InEKF and IS. LiDAR odometry is incorporated using Iterative Closest Point (ICP) registration on a parallel thread, preserving the computational efficiency of proprioceptive-based state estimation. We evaluate E-InEKF and E-IS with and without exteroceptive sensors, benchmarking them against LiDAR-based odometry methods in indoor and outdoor experiments using the KAIST HOUND2 robot. Our methods achieve lower Relative Position Errors (RPE) and significantly reduce Absolute Trajectory Error (ATE), with improvements of up to 28% indoors and 40% outdoors compared to LIO-SAM and FAST-LIO2. Additionally, we compare E-InEKF and E-IS in terms of computational efficiency and accuracy.