PaperlayerView exact revision on arXiv

Source-linked AI summary

FOCUS: Foot Observation Confidence for Robust Humanoid Proprioceptive Odometry

Kaixin Feng, Angsong Li, Shaopeng Zhang, Enyu Li, Peiwen Lin, Chuang Wang, You Li, Haiyu Lan

arXiv:2609.02222v1cs.RO

TL;DR

FOCUS addresses the limitation of binary contact gating, which cannot distinguish contact from reliable FK during dynamic locomotion. It learns continuous sensor-only per-foot FK reliability weights for blending FK with IMU velocity and adapting EKF covariance, achieving large ATE reductions across simulated and real walking and dynamic-motion evaluations.

  • Problem

    Binary contact gating treats an entire foot's FK as trusted or untrusted, although contact can coexist with toe scraping, partial support, or slip that makes FK unreliable.

  • Method

    FOCUS trains a causal Transformer from sensor-only proprioception and automatically generated simulation signals to predict continuous per-foot FK reliability weights for FK-IMU blending and EKF covariance modulation.

  • Results

    FOCUS reduces ATE by 83.7% in simulated walking, 70.8% across 19 real walking segments, and 42.7% across four real dynamic-motion routines.

  • Takeaways & Limitations

    FOCUS retains an explicit model-based FK-EKF estimator while learning continuous observation confidence instead of replacing odometry with direct displacement prediction.

Abstract

from arXiv · show

Foot forward kinematics (FK) is widely used to improve proprioceptive legged odometry by providing reliable velocity constraints during foot support. Existing contact-aided estimators generally rely on binary contact decisions to determine whether the FK measurements of an entire foot should be trusted. However, contact does not necessarily imply FK reliability. Dynamic locomotion often involves partial support, toe dragging, and foot slip, causing binary contact decisions to accumulate significant drift over long trajectories. To address this limitation, we propose FOCUS (Foot Observation Confidence from Unannotated Simulation), which predicts a continuous FK reliability weight for each foot instead of estimating binary foot contact. Rather than replacing the model-based estimator, the predicted reliability weights are used to blend FK velocity observations with IMU-propagated body velocity and to adapt the observation covariance of an extended Kalman filter (EKF), enabling smooth reliability-aware fusion without hard contact switching. The network is trained from automatically generated simulation signals using an FK-weighted velocity consistency loss with lightweight simulator-contact regularization, without manually annotated continuous FK-reliability labels. The deployed model relies only on IMU and joint kinematic measurements, making it suitable for hardware platforms with unreliable torque sensing. Experiments demonstrate that FOCUS reduces absolute trajectory error (ATE) by 83.7% on simulated walking episodes, preserves simulated dynamic-motion fidelity in motion scale and spectral energy, reduces ATE by 70.8% across 19 real walking segments, and reduces mean ATE by 42.7% across four real dynamic-motion routines.

I. INTRODUCTION

FOCUS addresses the mismatch between binary contact decisions and continuous FK reliability in dynamic humanoid locomotion. It learns sensor-only per-foot confidence weights and integrates them into FK-EKF odometry, with substantial ATE reductions across simulated and real evaluations.

  • Motivation: Binary contact gating can trust unreliable whole-foot FK during toe scraping, partial support, fast swings, or slip.A contacting foot may still violate the rigid, stationary-foot constraint required by FK.
  • Approach: FOCUS estimates continuous left/right FK reliability weights instead of making binary contact decisions.The approach preserves the model-based FK-EKF structure while representing intermediate confidence.
  • Approach: The lightweight causal Transformer uses short histories of IMU, joint position, and joint velocity measurements without torque input or manual continuous-reliability labels.Training uses automatically generated simulation signals, while deployment relies on sensor-only proprioception.
  • Results: 83.7% lower ATE is reported for simulated walking, 70.8% across 19 real walking segments, and 42.7% across four real dynamic-motion routines.These evaluations compare FOCUS with threshold-based and representative proprioceptive odometry baselines.

B. Learned Contact Estimation

FOCUS differs from learned contact, covariance, and end-to-end odometry methods by learning reliability for an existing FK velocity observation. It retains explicit FK and EKF computation while using confidence to blend measurements and modulate noise.

  • Comparison with learned contact estimation: Learned contact methods estimate contact-related quantities, whereas FOCUS specifically assesses the reliability of an existing FK-derived body-velocity observation.The distinction concerns both the learning target and how the result enters the filter.
  • Learning target and supervision: FOCUS trains per-foot FK-velocity weights with FK-weighted velocity consistency and lightweight binary-contact regularization, without continuous reliability or covariance labels.The primary signal is simulated velocity consistency rather than manually annotated continuous reliability.
  • Filter integration: At deployment, each weight changes confidence in FK, blends FK velocity with IMU-propagated velocity, and modulates EKF observation noise.This differs from methods that insert replacement measurements or only adapt covariance.
  • Positioning: FOCUS occupies a middle ground between threshold gating, learned contact estimation, and pure end-to-end odometry by retaining explicit metric FK and EKF updates.The learned module estimates how much each foot should be trusted rather than directly predicting odometry or displacement.

A. System Overview

The system combines a model-based EKF with a causal Transformer that predicts two per-foot reliability weights from proprioceptive history. These weights blend FK-derived and IMU-propagated velocity observations while inflating noise for unreliable feet.

  • Estimator structure: The EKF maintains body position, body velocity, and foot positions using IMU propagation and FK-derived foot observations.The learned module outputs reliability weights rather than odometry states.
  • Reliability estimation: A causal Transformer processes proprioceptive history and predicts left and right weights w_L, w_R ∈ [0, 1].The inputs include joint positions, joint velocities, and IMU measurements.
  • Weight use: The predicted weights blend FK velocity observations with IMU-propagated body velocity and inflate FK observation noise in the EKF.This provides smooth confidence modulation rather than hard observation switching.
  • EKF state: The filter uses a 12-dimensional translational state and treats normalized pelvis-IMU orientation and gyroscope angular velocity as measured inputs.It does not estimate attitude or IMU biases.
  • Observations: The 14-dimensional observation contains body-relative FK positions, FK-derived body velocities for both feet, and two foot-height constraints.The FK velocity observation depends on a valid non-slipping ground constraint whose reliability is estimated continuously.

C. Per-Foot FK Reliability Modulation

FOCUS replaces binary contact gating with continuous per-foot modulation of FK observation trust. The predicted weight separately controls velocity blending and covariance scaling, with lower reliability favoring IMU propagation.

  • FOCUS replaces binary contact gating with continuous modulation for each foot’s FK velocity observation covariance.The covariance uses nominal FK velocity noise and a maximum amplification factor.
  • The same reliability modulation applies to foot-position covariance, foot-height covariance, and foot-state process noise.Reported amplification factors are S_vel = 100, S_p = 1000, and S_h = S_q = 100.
  • The reliability weight also modulates velocity trust through a saturated ramp that produces a coefficient τ_i in [0, 1].The ramp is equivalent to τ_i = min(1, 2.5w_i) because w_i lies in [0, 1].
  • Low reliability favors IMU-propagated velocity, whereas high reliability relies on FK-derived velocity.The raw weight controls covariance scaling, while the ramped coefficient controls velocity trust.

D. Causal Transformer Reliability Estimator

The reliability estimator receives a normalized, sensor-only proprioceptive sequence consisting of IMU and lower-limb joint measurements. It uses a one-second temporal window at the training frequency and excludes torque input.

  • Each frame provides a 30-dimensional input comprising IMU acceleration, IMU angular velocity, lower-limb joint positions, and joint velocities.The components have dimensions 3, 3, 12, and 12 respectively.
  • The model processes a temporal window of T = 50 frames, corresponding to one second at 50 Hz.This window captures short-history proprioceptive information for causal estimation.
  • The final deployed input excludes joint torque because current-to-torque calibration and hardware bias make estimated torque sensitive.The estimator therefore relies on IMU and joint kinematic measurements.
  • All channels are normalized with training-set means and standard deviations before entering the reliability network.No additional finite-difference augmentation is applied.

2) Layer Structure:

FOCUS learns reliability from simulated FK–body-velocity consistency, using contact supervision only as an auxiliary regularizer. Its training data are generated through replayed Isaac Lab motion-tracking episodes and post-processed FK signals.

  • 2) Layer Structure:: A causal Transformer projects each frame to a 128-dimensional token and processes the sequence with four causal encoder layers using four-head self-attention.A two-layer reliability head maps the final token to two sigmoid weights, [w_L, w_R].
  • 2) Layer Structure:: The main training principle is that reliable FK-derived body velocity should agree with ground-truth body velocity.The network weights left- and right-foot velocity estimates using automatically generated simulation signals.
  • 2) Layer Structure:: A speed-dependent factor emphasizes high-speed motions, where incorrect FK reliability produces larger odometry drift.
  • 2) Layer Structure:: Binary cross-entropy on simulator contact states acts only as lightweight regularization against degenerate weights.Neither the FK-weighted velocity loss nor the auxiliary contact term requires manually annotated continuous reliability labels.
  • 2) Layer Structure:: The Isaac Lab collection pipeline freezes a PPO motion-tracking policy for replay recording, then uses a URDF model to produce per-foot FK velocity supervision.Dataset splitting occurs by motion identity after post-processing.

F. Simulation Data Collection

Simulation data are collected by replaying a domain-randomized humanoid motion-tracking policy over diverse motion clips, preserving execution imperfections for learning. FK velocity supervision is generated offline from recorded joint states using the robot’s URDF and Pinocchio.

  • The motion library combines open-source motion clips with real motion-capture clips tracked by a PPO controller in Isaac Lab.The policy is frozen and replayed over the library to record executed states rather than reference motions.
  • Replay preserves tracking errors, imperfect contacts, actuator effects, and randomized dynamics in the training data.These effects arise from recording executed robot motions under domain randomization.
  • The dataset covers walking, jumping, standing, running, and martial-arts motions across four domain-randomized repeats.The splits contain 5420 training episodes and 1151 validation episodes collected at 50 Hz.
  • Offline FK supervision is computed by processing smoothed joint states with the robot URDF model and Pinocchio.The two foot Jacobians provide body-velocity estimates used only as training signals.
  • Hardware evaluation uses A3 Ultra humanoids with proprioceptive IMU and lower-limb encoder measurements, while exteroceptive sensors are not used by FOCUS.The five evaluated units share the same kinematic model and sensor interface.

IV. EXPERIMENTS

FOCUS is evaluated across simulated and real humanoid-robot trajectories using common proprioceptive-estimation settings and reference trajectories. The experiments report trajectory errors and compare FOCUS with threshold-based and representative proprioceptive odometry methods.

  • Evaluation data: The evaluation covers simulated walking and dynamic motions, 19 real walking segments, and real dynamic-motion sequences across four routines.Real-robot experiments use A3 Ultra platforms and offline LiDAR or motion-capture references, which are not supplied to estimators.
  • Metrics: ATE is reported as root-mean-square horizontal trajectory error, while Drift normalizes ATE by ground-truth path length.Results spanning multiple sequences are arithmetic means of per-sequence metrics.
  • Baselines: Comparisons include torque-threshold whole-foot FK gating, Pronto, CoCo-InEKF, and Legolas under matched robot models, FK implementation, synchronization, and proprioceptive inputs.Learning-based baselines were retrained or adapted only on the Isaac Lab training split when necessary.

B. Simulation Evaluation

Simulation and real-walking evaluations test FOCUS across varied motion patterns and long trajectories. FOCUS achieves the strongest reported simulated-walking and real-walking ATE results, while dynamic-motion metrics reveal complementary fidelity trade-offs.

  • Simulation Evaluation: 20 Isaac Lab evaluation episodes each for walking and dynamic motions expose accumulated contact errors across varied motion patterns.The simulation evaluation is designed to test performance beyond a single locomotion condition.
  • Simulation Evaluation: 0.166 m mean ATE is achieved by FOCUS on simulated walking, versus 1.016 m for torque-threshold gating.The proposed weighting also gives the lowest simulated-walking drift.
  • Simulation Evaluation: Dynamic-motion evaluation reports amplitude fidelity, discrete Fréchet distance, and spectral ratio because small net displacement can make ATE insufficient.Amplitude and spectral ratios equal 1 under perfect agreement; lower Fréchet distance is better.
  • Simulation Evaluation: FOCUS best preserves simulated dynamic-motion extent and spectral energy, while Threshold attains the lowest ATE and Fréchet distance.The result separates displacement-error performance from motion-fidelity preservation.
  • Real Walking Evaluation: 70.8% mean ATE reduction is obtained across 19 real walking segments, from 2.634 m to 0.768 m, with FOCUS outperforming threshold gating on every segment.A paired two-sided Wilcoxon signed-rank test reports p < 10^-5.
  • Real Walking Evaluation: Domain-randomized simulation and continuous weights support transfer by allowing the EKF to partially trust FK under ambiguous support.The design reduces sensitivity to threshold mismatch, actuator compliance, encoder noise, and torque-estimation error.

D. Real Dance Evaluation

FOCUS is evaluated on four real dynamic-motion routines containing partial support, toe contacts, fast swings, and lateral motion. It achieves the lowest aggregate ATE while also preserving motion-scale and spectral fidelity.

  • D. Real Dance Evaluation: The benchmark covers Charleston, Chaosha dance, Zero-Frame Start, and Cyberwalk using motion-capture references.The routines include nonperiodic kicks, fast swings, partial support, toe contacts, and lateral motion, with little net displacement.
  • D. Real Dance Evaluation: 0.542 m mean ATE is achieved by FOCUS across the four routines, versus 0.947 m for Threshold and 0.605 m for Pronto.FOCUS has the lowest aggregate ATE among compared methods.
  • D. Real Dance Evaluation: FOCUS remains closest to perfect agreement in amplitude and spectral ratio and obtains the lowest Fréchet distance.These metrics complement ATE for motions with little net displacement.
  • E. FK Reliability Versus Contact: Across 139,013 Charleston frames, reliability correlates negatively with FK foot height and six-joint speed magnitude at −0.608 and −0.663, respectively.The correlations are consistent with lower FK reliability during foot lift and rapid motion.
  • E. FK Reliability Versus Contact: Fig. 6 compares per-foot reliability with torque-based contact, relative foot height, and leg-joint speed during a representative 4-s interval.Torque-based contact is presented as a comparison cue rather than a reliability label.

F. Ablation Study

The ablation compares deployment choices for learned and thresholded foot-reliability handling, showing that full continuous FOCUS is strongest while preserving real-time feasibility. Qualitative evidence also distinguishes continuous reliability from binary contact during dynamic locomotion.

  • Qualitative analysis: Continuous reliability differs from binary contact in a representative 4-s Charleston window, with per-foot weights compared against torque-based contact and kinematic signals.The figure shows reliability weights, relative FK foot heights, and leg-joint speeds for the left and right feet.
  • Ablation results: Full FOCUS achieves the strongest ablation performance among the compared deployment choices on the same 19 real walking segments.The comparison includes hand thresholding, learned binary contact, thresholded continuous reliability, covariance-only modulation, and full FOCUS.
  • Ablation results: Hard-thresholding the learned weights and removing the velocity blend both degrade performance, supporting continuous modulation and velocity blending.Covariance-only modulation is the closest ablation to learned contact-covariance filtering.
  • Statistical analysis: p < 10^-5 for both Threshold versus FOCUS and covariance-only versus full FOCUS in paired two-sided Wilcoxon tests.These tests were conducted on the paired 19-segment real-walking set.
  • Runtime: 1.62 ms CPU time per inference enables the 693,768-parameter ONNX model to support real-time deployment at 50 Hz on one Intel i7-13700K thread.The model uses 8.1% of one CPU core at 50 Hz.
  • Overall performance: 83.7% simulated-walking ATE reduction, 70.8% reduction across 19 real walking segments, and 42.7% reduction across four real dynamic-motion routines are reported for FOCUS.The real-walking mean ATE changes from 2.634 m to 0.768 m, while dynamic-motion mean ATE changes from 0.947 m to 0.542 m.
Loading 2609.02222v1…