CoCo-InEKF: State Estimation with Learned Contact Covariances in Dynamic, Contact-Rich Scenarios

📄 arXiv: 2605.15122v1 📥 PDF

作者: Michael Baumgartner, David Müller, Agon Serifi, Ruben Grandia, Espen Knoop, Markus Gross, Moritz Bächer

分类: cs.RO, cs.LG, eess.SY

发布日期: 2026-05-14

备注: RSS 2026

💡 一句话要点

CoCo-InEKF：利用学习的接触协方差实现动态、富接触场景下的状态估计

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 状态估计 扩展卡尔曼滤波器 腿式机器人 接触建模 机器学习 机器人控制 动态系统

📋 核心要点

  1. 腿式机器人在动态、富接触场景下的鲁棒状态估计面临挑战，传统方法依赖二元接触状态，无法捕捉部分接触或定向滑移的细微差别。
  2. CoCo-InEKF利用连续接触速度协方差代替二元接触状态，通过学习的协方差动态调整接触置信度，从而应对更复杂接触情况。
  3. 实验表明，CoCo-InEKF在双足机器人上实现了更优的线性速度估计精度和滤波器一致性，支持更复杂的运动控制。

📝 摘要（中文）

本文提出了一种名为CoCo-InEKF的可微不变扩展卡尔曼滤波器，用于解决腿式机器人在动态、富接触场景中进行鲁棒状态估计的难题。该方法使用连续接触速度协方差代替二元接触状态，从而动态地调整接触置信度，能够捕捉从牢固接触到定向滑移或无接触等更细微的状态。为了预测一组预定义接触候选点的协方差，我们采用了一个轻量级神经网络，并使用状态误差损失进行端到端训练，无需启发式真值接触标签。此外，我们提出了一种自动接触候选点选择程序，并证明该方法对它们的精确位置不敏感。在双足机器人上的实验表明，该方法在线性速度估计方面具有卓越的精度-效率权衡，并且与基线方法相比，滤波器一致性得到了改善。这使得机器人能够稳健地执行具有挑战性的运动，包括跳舞和复杂的地面交互——无论是在仿真环境还是在现实世界中。

🔬 方法详解

问题定义：现有腿式机器人的状态估计方法，尤其是在动态、富接触场景下，通常依赖于二元接触状态（接触/非接触）。这种方法无法准确描述部分接触、定向滑移等复杂情况，导致状态估计精度下降，进而影响运动控制的鲁棒性。因此，需要一种能够更精细地建模接触状态的方法。

核心思路：CoCo-InEKF的核心思路是使用连续的接触速度协方差来代替二元接触状态。接触速度协方差能够反映接触的置信度以及滑移方向等信息，从而更准确地描述接触状态。通过学习这些协方差，可以动态地调整卡尔曼滤波器的更新过程，提高状态估计的精度和鲁棒性。这样设计的目的是为了能够更好地处理实际机器人运动中遇到的各种复杂接触情况。

技术框架：CoCo-InEKF的整体框架是一个不变扩展卡尔曼滤波器（InEKF），其核心在于接触协方差的预测。首先，预定义一组接触候选点。然后，使用一个轻量级神经网络预测这些候选点的接触速度协方差。这些协方差被用于InEKF的更新步骤中，从而得到更准确的状态估计。整个过程是可微的，可以进行端到端训练。

关键创新：CoCo-InEKF的关键创新在于使用学习的连续接触速度协方差来代替传统的二元接触状态。与现有方法相比，CoCo-InEKF能够更精细地建模接触状态，从而提高状态估计的精度和鲁棒性。此外，该方法还提出了一种自动接触候选点选择程序，并证明其对候选点的精确位置不敏感，降低了人工设计的难度。

关键设计：神经网络的结构是一个轻量级的多层感知机（MLP），输入是机器人的状态和环境信息，输出是每个接触候选点的接触速度协方差矩阵。损失函数采用状态误差损失，即通过最小化估计状态与真实状态之间的误差来训练神经网络。此外，还设计了一种自动接触候选点选择程序，该程序基于机器人几何形状和运动学信息，自动选择合适的接触候选点。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，CoCo-InEKF在双足机器人上实现了优于基线方法的线性速度估计精度和滤波器一致性。具体来说，在模拟和真实世界的实验中，CoCo-InEKF能够更准确地估计机器人的速度，并减少状态估计的偏差。这使得机器人能够更稳健地执行包括跳舞和复杂地面交互在内的具有挑战性的运动。与依赖二元接触状态的方法相比，CoCo-InEKF在精度和效率之间取得了更好的平衡。

🎯 应用场景

CoCo-InEKF在腿式机器人领域具有广泛的应用前景，可用于提高双足或四足机器人在复杂环境中的运动能力和自主性。例如，可以应用于服务机器人、搜救机器人、以及工业机器人等，使其能够在非结构化环境中更稳定、更可靠地执行任务。此外，该方法还可以推广到其他具有接触交互的机器人系统，例如机械臂操作等。

📄 摘要（原文）

Robust state estimation for highly dynamic motion of legged robots remains challenging, especially in dynamic, contact-rich scenarios. Traditional approaches often rely on binary contact states that fail to capture the nuances of partial contact or directional slippage. This paper presents CoCo-InEKF, a differentiable invariant extended Kalman filter that utilizes continuous contact velocity covariances instead of binary contact states. These learned covariances allow the method to dynamically modulate contact confidence, accounting for more nuanced conditions ranging from firm contact to directional slippage or no contact. To predict these covariances for a set of predefined contact candidate points, we employ a lightweight neural network trained end-to-end using a state-error loss. This approach eliminates the need for heuristic ground-truth contact labels. In addition, we propose an automated contact candidate selection procedure and demonstrate that our method is insensitive to their exact placement. Experiments on a bipedal robot demonstrate a superior accuracy-efficiency tradeoff for linear velocity estimation, as well as improved filter consistency compared to baseline methods. This enables the robust execution of challenging motions, including dancing and complex ground interactions -- both in simulation and in the real world.