Contact-Anchored Proprioceptive Odometry for Quadruped Robots
作者: Minxing Sun, Yao Mao
分类: cs.RO, eess.SP
发布日期: 2026-02-19
备注: 28 pages, 30 figures
💡 一句话要点
提出一种基于接触锚定的四足机器人纯本体感觉里程计，解决无视觉或激光雷达下的里程计漂移问题。
🎯 匹配领域: 支柱一：机器人控制 (Robot Control) 支柱七：动作重定向 (Motion Retargeting)
关键词: 四足机器人 本体感觉里程计 状态估计 接触锚定 运动学约束
📋 核心要点
- 在无视觉或激光雷达的情况下，腿式机器人可靠的里程计仍然具有挑战性，主要受限于IMU漂移和噪声大的关节速度传感。
- 该方法将每个接触腿视为运动学锚点，通过足端力估计选择可靠接触点，并利用落脚点位置提供世界坐标系约束，抑制长期漂移。
- 实验在多种四足机器人平台上验证了该方法的有效性，在水平和垂直环路中均取得了较低的误差。
📝 摘要（中文）
本文提出了一种纯本体感觉的状态估计器，仅使用IMU和电机测量来联合估计机器人本体的姿态和速度。该方法采用统一的公式，适用于双足、四足和轮腿机器人。核心思想是将每个接触的腿视为一个运动学锚点：基于关节力矩的足端力估计选择可靠的接触点，相应的落脚点位置提供间歇性的世界坐标系约束，从而抑制长期漂移。为了防止长时间运动中的高度漂移，引入了一种轻量级的高度聚类和时间衰减校正，将新记录的落脚点高度对齐到先前观察到的支撑平面。为了改善编码器量化下的足端速度观测，应用逆运动学容积卡尔曼滤波器，直接从关节角度和速度中滤波足端速度。该实现进一步通过多接触几何一致性来减轻偏航漂移，并在IMU偏航约束不可用或不可靠时优雅地退化为运动学导出的航向参考。在四个四足机器人平台上进行了评估，验证了该方法的有效性。
🔬 方法详解
问题定义：腿式机器人，尤其是在没有视觉或激光雷达等外部传感器的情况下，准确估计自身位姿是一个长期存在的难题。现有的基于IMU和电机编码器的本体感觉里程计方法容易受到IMU漂移和关节速度测量噪声的影响，导致长期运行中出现显著的位姿估计误差。
核心思路：本文的核心思路是将机器人的腿部与地面的接触点视为“运动学锚点”。通过估计这些锚点的位置，并将其与机器人自身的运动学模型相结合，可以有效地约束机器人的位姿，从而减少漂移。这种方法的关键在于选择可靠的接触点，并利用这些接触点提供的信息来校正IMU的漂移。
技术框架：该方法主要包含以下几个模块：1) 基于关节力矩的足端力估计，用于选择可靠的接触点；2) 逆运动学容积卡尔曼滤波器，用于从关节角度和速度中估计足端速度；3) 基于高度聚类和时间衰减校正的高度漂移抑制方法；4) 多接触几何一致性方法，用于减轻偏航漂移；5) 状态估计器，用于融合来自IMU、电机编码器和接触点的信息，估计机器人本体的姿态和速度。
关键创新：该方法最重要的创新点在于将腿部与地面的接触视为运动学锚点，并利用这些锚点的信息来约束机器人的位姿。与传统的里程计方法相比，该方法不需要外部传感器，并且能够有效地抑制长期漂移。此外，该方法还提出了一种轻量级的高度聚类和时间衰减校正方法，用于防止长时间运动中的高度漂移。
关键设计：在足端力估计中，使用关节力矩信息来判断接触的可靠性。在逆运动学容积卡尔曼滤波器中，直接滤波足端速度，而不是先估计关节速度再进行运动学计算，从而减少了误差累积。高度聚类和时间衰减校正方法使用简单的聚类算法将新记录的落脚点高度对齐到先前观察到的支撑平面，并使用时间衰减因子来降低旧的支撑平面对当前估计的影响。
🖼️ 关键图片
📊 实验亮点
该方法在四种不同的四足机器人平台上进行了验证，包括三种Astrall机器人和一个Unitree Go2 EDU。在Astrall point-foot robot A上，水平环路（约200米）和垂直环路（约15米）的误差分别为0.1638米和0.219米。在Unitree Go2 EDU上，水平环路（约120米）的误差为2.2138米，垂直环路（约8米）的误差小于0.1米。实验结果表明，该方法能够有效地抑制长期漂移，并在多种机器人平台上具有良好的泛化能力。
🎯 应用场景
该研究成果可应用于各种腿式机器人，尤其是在缺乏视觉或激光雷达等外部传感器的环境中，例如搜救、巡检、物流等。该方法能够提高腿式机器人在复杂地形下的自主导航能力，降低对外部基础设施的依赖，具有重要的实际应用价值和潜在的商业前景。
📄 摘要（原文）
Reliable odometry for legged robots without cameras or LiDAR remains challenging due to IMU drift and noisy joint velocity sensing. This paper presents a purely proprioceptive state estimator that uses only IMU and motor measurements to jointly estimate body pose and velocity, with a unified formulation applicable to biped, quadruped, and wheel-legged robots. The key idea is to treat each contacting leg as a kinematic anchor: joint-torque--based foot wrench estimation selects reliable contacts, and the corresponding footfall positions provide intermittent world-frame constraints that suppress long-term drift. To prevent elevation drift during extended traversal, we introduce a lightweight height clustering and time-decay correction that snaps newly recorded footfall heights to previously observed support planes. To improve foot velocity observations under encoder quantization, we apply an inverse-kinematics cubature Kalman filter that directly filters foot-end velocities from joint angles and velocities. The implementation further mitigates yaw drift through multi-contact geometric consistency and degrades gracefully to a kinematics-derived heading reference when IMU yaw constraints are unavailable or unreliable. We evaluate the method on four quadruped platforms (three Astrall robots and a Unitree Go2 EDU) using closed-loop trajectories. On Astrall point-foot robot~A, a $\sim$200\,m horizontal loop and a $\sim$15\,m vertical loop return with 0.1638\,m and 0.219\,m error, respectively; on wheel-legged robot~B, the corresponding errors are 0.2264\,m and 0.199\,m. On wheel-legged robot~C, a $\sim$700\,m horizontal loop yields 7.68\,m error and a $\sim$20\,m vertical loop yields 0.540\,m error. Unitree Go2 EDU closes a $\sim$120\,m horizontal loop with 2.2138\,m error and a $\sim$8\,m vertical loop with less than 0.1\,m vertical error. github.com/ShineMinxing/Ros2Go2Estimator.git