PaperlayerView exact revision on arXiv

Source-linked AI summary

Contact-Anchored Proprioceptive Odometry for Legged and Wheel-Legged Robots

Minxing Sun, Yao Mao

arXiv:2602.17393v3cs.ROeess.SP

TL;DR

CAPO is a purely proprioceptive, contact-anchored odometry framework for legged and wheel-legged robots using only IMU and motor measurements. Its contact records, optional velocity filtering, and geometric consistency improve drift handling, while heading errors remain sensitive to turning conditions.

  • Problem

    Double integration is extremely sensitive, and integrated yaw can degrade during prolonged standing.

  • Method

    CAPO uses contact anchoring for legged and wheel-legged robots, with wheel-contact propagation and an optional IKVel-CKF for smoother velocity estimates.

  • Results

    Contact anchoring with touchdown footfall records provides drift suppression, while geometric consistency arrests IMU yaw drift during prolonged standing; CAPO-CKE outperformed the SLAM baseline in low-texture settings.

  • Takeaways & Limitations

    The results support contact anchoring as a basis for proprioceptive odometry across legged and wheel-legged robots.

  • Takeaways & Limitations

    Heading errors are approximately 10° per closed turn for short-step turning and 30° per closed turn for long-step turning.

Abstract

from arXiv · show

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 estimate body pose and velocity, with a unified formulation applicable to quadruped and wheel-legged robots and extensible to other legged morphologies. The key idea is to treat each reliable contact as a kinematic anchor: joint-torque--based foot wrench estimation selects stance contacts, and the corresponding footfall records 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. For wheel-legged platforms, the recorded contact is further propagated by effective wheel rolling displacement with shank-motion compensation and a slope-aware rolling direction. To improve foot velocity observations under encoder quantization, we retain an inverse-kinematics cubature Kalman filter as an optional velocity-enhancement module that filters foot-end velocities from joint angles and velocities. The implementation further mitigates yaw drift through multi-contact geometric consistency, which is injected as a soft heading prior rather than as a hard reset of the attitude state. The method is evaluated on four quadruped platforms.

I. INTRODUCTION

The paper targets proprioceptive odometry that remains reliable without exteroceptive sensors, addressing IMU drift, imperfect contacts, position drift, and encoder-induced velocity spikes. CAPO unifies contact-anchored estimation across legged morphologies and adds corrections for elevation, velocity, wheel contact, and yaw drift.

  • IMU acceleration double integration is highly sensitive to bias, timing mismatch, and sampling noise, causing rapid drift.
  • Position drift can accumulate even when velocity estimation improves, while encoder quantization and differentiation produce spiky foot velocities.
  • CAPO uses only IMU and motor measurements, forming contact constraints from currently contacting feet or wheels across bipeds, quadrupeds, and wheel-legged robots.
  • Touchdown footfall records anchor stance constraints, while height clustering with time-decayed confidence limits long-horizon elevation drift.
  • An optional inverse-kinematics CKF smooths encoder-induced foot-velocity spikes, and multi-contact geometry reduces yaw drift through a soft heading prior.
  • The estimator is evaluated on four platforms using long closed-loop trajectories with horizontal and vertical motion, with implementation and representative Go2 EDU data released publicly.

II. CONTACT-ANCHORED PROPRIOCEPTIVE ODOMETRY

The contact-anchored estimator represents body pose and contact geometry in world and body frames, selects stance contacts from torque-derived foot forces, and forms kinematic observations from active end-effectors. The same chain-based formulation supports point feet and wheel contacts, while updates are suppressed when reliable contacts are unavailable.

  • A. Inputs, Frames, and Notation: The estimator uses trunk pose, joint angles, joint rates, estimated torques, and robot geometry to compute end-effector kinematics.
  • B. Stance Selection via Torque-Based Wrench Estimation: Forward kinematics provides end-effector position and joint-rate-induced velocity, while wrench estimation supplies a contact-normal force for stance gating.
  • B. Stance Selection via Torque-Based Wrench Estimation: End-effector forces are estimated from joint torques and the geometric Jacobian, and a leg is declared contacting when its vertical force component crosses a configurable threshold.
  • B. Stance Selection via Torque-Based Wrench Estimation: A touchdown event triggers footfall recording and height correction, providing the contact anchor used by subsequent stance updates.

C. Footfall Recording and Contact-Anchored Body Observations

During stance, stored world-frame footfall points act as stationary anchors for body position and velocity observations. The method corrects touchdown heights by associating them with persistent support-plane records, snapping matched heights and fading stale planes.

  • C. Footfall Recording and Contact-Anchored Body Observations: At touchdown, the estimator stores a world-frame contact point; during stance, it treats that contact as stationary and derives trunk position from body-frame kinematics.
  • C. Footfall Recording and Contact-Anchored Body Observations: The stance velocity observation is formed from the world-frame chain Jacobian, with wheel rolling velocity added along the estimated rolling direction.
  • C. Footfall Recording and Contact-Anchored Body Observations: For point feet, the contact is an instantaneous velocity anchor; IKVel-CKF can optionally replace or smooth the default Jacobian velocity term.
  • C. Footfall Recording and Contact-Anchored Body Observations: When no reliable contact exists, the contact-anchored update is suppressed or conservatively regularized, leaving proprioceptive prediction and inertial information.
  • D. Support-Plane Height Correction: Height correction addresses accumulated vertical drift by maintaining support-plane records with heights, confidence weights, and update times.
  • D. Support-Plane Height Correction: At touchdown, stale planes are pruned, heights are matched within tolerance, and matched records are snapped and refreshed or new records are created.

III. END-EFFECTOR RADIUS MODELING AND WHEEL CONTACT COMPENSATION

This section models end-effector contact geometry for rounded point feet and extends the formulation to wheel-legged platforms. The rounded-foot approximation has small practical bias, while wheel contact requires explicit rolling treatment.

  • A. Rolling-Contact Bias of Rounded Point Feet: Rounded point feet can roll during stance, so treating them as fixed rigid shank extensions introduces a systematic contact-anchor displacement bias.The bias arises from approximating physical hemispherical rolling with a fixed contact point.
  • B. Wheel-Contact Propagation for Wheel-Legged Platforms: Wheel-legged robots require contact propagation because their stance contact point translates through wheel rotation even under no-slip contact.The wheel-contact treatment is developed to address this behavior within the contact-anchored formulation.
  • A. Rolling-Contact Bias of Rounded Point Feet: The displacement bias scales linearly with foot radius R and grows with stance pitch excursion a2 − a1.The closed-form bias is Δx = R(cos a1 − cos a2 − (a2 − a1)) and Δz = R(−sin a1 + sin a2).
  • A. Rolling-Contact Bias of Rounded Point Feet: For R = 0.03 m and a2 ∈[60°, 120°], the practical error remains below ∼5 mm in Δz and ∼1 mm in Δx.This range covers the typical stance-to-swing transitions reported for the experiments.
  • A. Rolling-Contact Bias of Rounded Point Feet: The method deliberately uses the effective shank-extension approximation to preserve a unified, lightweight kinematic estimator across platforms.The approximation is reported to cause negligible practical accuracy degradation while maintaining computational simplicity.

B. Wheel-Contact Propagation for Wheel-Legged Platforms

Wheel-legged stance contacts are propagated rather than treated as fixed anchors. The propagation compensates shank-induced encoder motion and lateral displacement, and follows a terrain-aware rolling direction.

  • Effective rolling angle: Wheel encoder motion must be corrected for apparent rotation induced by shank pitch before estimating true rolling during stance.The effective rolling increment subtracts the configured shank pitch contribution from the wrapped wheel encoder increment.
  • Heading direction on the ground plane: The rolling direction follows the horizontal body heading on flat terrain and can acquire a vertical component under slope-aware propagation.Reliable contacts fit a local support plane, and the direction is lifted to that plane’s tangent along the current heading.
  • Contact-point propagation: The world-frame contact record is propagated by wheel rolling displacement and includes a shank-roll-induced lateral correction.The lateral term is integrated perpendicular to the rolling direction only when the leg is deemed in contact.
  • Contact-point propagation: Shank-motion compensation and lateral correction reduce the systematic bias that results from treating a rolling wheel as a fixed stance anchor.The corresponding displacement is integrated into the footfall record along the terrain-aware rolling direction.
  • Degenerate case and point-foot platforms: When rw = 0, rolling propagation vanishes and the formulation reduces to the stationary point-foot contact-anchor case.Wheel-legged robots instead retain rw > 0 and apply explicit rolling propagation.

IV. INVERSE-KINEMATICS CUBATURE KALMAN FILTERING FOR FOOT-END VELOCITY

Encoder quantization and nonlinear kinematic projection create impulsive foot-velocity errors that can destabilize contact-anchored updates. IKVel-CKF filters these velocities before fusion, substantially reducing spikes at a computational cost.

  • Motivation: Encoder quantization and discrete-time differentiation produce noisy joint rates whose nonlinear kinematic projection creates impulsive hip-to-foot velocity spikes.These errors propagate into fused body velocity and degrade contact-anchored updates.
  • IKVel-CKF enhancement: IKVel-CKF estimates Cartesian hip-to-end-effector velocity from joint angles and velocities, replacing the raw Jacobian velocity when smoother feedback is needed.The filter remains purely proprioceptive and can be used as an optional enhancement module.
  • IKVel-CKF enhancement: The filter maintains a latent 6D Cartesian state and uses a cubature Kalman update for the nonlinear inverse-kinematics measurement model without Jacobians.The filtered Cartesian velocity is transformed to the world frame and contributes to stance velocity constraints.
  • Effect on body velocity estimation: IKVel-CKF suppresses impulsive excursions and high-frequency noise in representative leg velocity feedback, with modest smoothing-induced lag.Its main benefit is velocity smoothness and robustness; its effect on overall position accuracy is limited when footfall position feedback is reliable.
  • Computation–accuracy trade-off: Enabling IKVel-CKF increases per-cycle computation time by more than a factor of two in the MATLAB/MEX deployment.The authors therefore treat it as optional and recommend enabling it when required by downstream objectives.

V. KINEMATICS-BASED YAW ESTIMATION VIA MULTI-CONTACT GEOMETRIC CONSISTENCY

Because inertial sensing weakly observes yaw, the method estimates heading from geometric consistency among stationary multi-contacts. It injects this estimate as a soft prior that can arrest long-standing drift while retaining temporal filtering.

  • Motivation: Gravity does not constrain heading, so IMU-based yaw can drift during long-horizon operation, including prolonged stationary standing.The drift appears as slowly wandering integrated yaw while the robot should remain stationary.
  • Pairwise yaw from contact geometry: The yaw cue compares current body-frame end-effector geometry with world-frame footfall records from simultaneously stationary contacts.Fixed world-frame inter-foot geometry reveals yaw misalignment after tilt compensation.
  • Pairwise yaw from contact geometry: Pairwise yaw estimates are aggregated with a circular mean and weighted by the product of soft contact confidences.Multiple baselines improve robustness, while weak or uncertain contacts contribute less to the yaw cue.
  • Pairwise yaw from contact geometry: The estimator requires at least two simultaneous contacts; otherwise, no geometric yaw constraint is applied.The method forms pairwise relative vectors to remove dependence on global translation.
  • Drift suppression and kinematics-only fallback: During stable multi-contact standing, the correction continuously re-anchors heading to contact geometry and effectively arrests IMU yaw drift.The gain schedule ramps during prolonged full support and remains small during intermittent contact changes.
  • Drift suppression and kinematics-only fallback: The kinematics-derived heading is injected as a stabilizing prior rather than a hard attitude reset, preserving the estimator’s temporal filtering.In kinematics-only mode, residual drift can arise from unmodeled attitude coupling, contact compliance, and minor slip.

VI. EXPERIMENTAL EVALUATION

The estimator is evaluated in simulation and on four quadruped platforms using proprioceptive sensing, with comparisons against SLAM and an optional velocity-filtering variant. Results show stable elevation and reduced drift, while IKVel-CKF improves robustness under impacts and vibration.

  • Simulation: Simulation compares Lidar-based SLAM, CAPO, and CAPO-CKE on identical flat-ground and stair-climbing trajectories using Gazebo ground truth.CAPO-CKE denotes CAPO with the IKVel-CKF module enabled.
  • Simulation: CAPO and CAPO-CKE maintain stable elevation and substantially reduce drift relative to SLAM during flat-ground walking.The two proprioceptive methods achieve comparable terminal accuracy on this trajectory.
  • Simulation: Under sustained stance constraints, contact-anchored position updates dominate long-horizon drift suppression, while velocity denoising mainly improves robustness under stronger excitation.This explains the comparable flat-ground terminal accuracy of CAPO and CAPO-CKE.
  • Simulation: Encoder-derived joint rates produce jitter and impulsive outliers during stair climbing, and a spike near t ≈115.5 s causes CAPO to make a false contact update.The resulting support-height bias persistently degrades the z estimate.
  • Simulation: IKVel-CKF attenuates velocity outliers, preventing the false contact event and preserving long-horizon height tracking, whereas SLAM continues drifting in the low-texture environment.The comparison is reported for stair-climbing height and velocity estimates.
  • Platform evaluation: Real-world evaluation covers a Unitree Go2 EDU, one point-foot Astrall platform, and two wheel-legged Astrall platforms using IMU and motor sensing only.The Go2 planar-loop trial additionally uses mapping-only slam_toolbox with scan matching disabled, while remaining trials report real-time estimated traces.

1) Unitree Go2 EDU:

Go2 and Astrall closed-loop trials assess horizontal and vertical consistency across planar, step, mixed 3D, and long-range trajectories. The estimator maintains vertical consistency, while wheel slip and intermittent contact constrain long-range horizontal performance.

  • Unitree Go2 EDU: The Go2 planar closed loop returns with terminal errors Δx = 1.61 m and Δy = 1.52 m, corresponding to a planar distance error of 2.2138 m.The mapping-only visualization shows a wall-orientation inconsistency consistent with residual heading drift when scan matching is disabled.
  • Unitree Go2 EDU: Across five repeated step-up and step-down cycles, vertical closure remains |∆z| < 0.1 m on return to the initial region.The result indicates stable elevation consistency under repetitive elevation changes.
  • Astrall Platforms (A, B, C): Point-foot stance detection and contact anchoring are generally more reliable than on wheel-legged platforms, which face slip and intermittent-contact challenges.The current implementation does not explicitly detect or compensate for slip, and step-down ballistic phases can violate stationary-contact assumptions.
  • Public artifacts: The authors release the complete implementation and Unitree Go2 EDU test data, including ROS bags, CSV files, and videos.These artifacts are intended to support reproducible evaluation.

C. Discussion and Limitations

CAPO extends contact-anchored proprioceptive odometry across legged morphologies, with wheel-contact propagation, velocity smoothing, and yaw-drift correction. Experiments show strong low-texture and long-horizon performance, while slip, contact classification, heading errors, and computational cost remain boundaries.

  • Limitations: The current wheel-legged pipeline does not explicitly detect or compensate wheel slip, which can violate rolling assumptions and contribute to long-horizon drift.Future work proposes slip-aware gating or weighting using kinematic–inertial inconsistency and wrench cues.
  • Limitations: Fixed vertical-force contact thresholds are robust for typical point-foot gaits but can be suboptimal for prolonged light wheel contact or stair climbing.An adaptive or probabilistic contact model is proposed as a future improvement.
  • Morphology scope: CAPO extends to bipeds through a time-varying set of contacting end-effectors, with demonstrations spanning changing quadruped contact morphologies.The supplementary demonstrations include two-foot and four-foot configurations during a closed loop.
  • Core framework: CAPO uses touchdown footfalls as intermittent world-frame anchors and enforces contact-anchored kinematic consistency during stance.A support-plane height correction clusters touchdown heights with time-decayed confidence to stabilize elevation.
  • Wheel-legged extension: Wheel-legged contact propagation compensates shank motion and lifts rolling direction onto an estimated support-plane tangent.This improves anchoring on moderate ramps and stairs, but depends on sufficient contacts, stable timing, and limited slip.
  • Evaluation: Real-robot trials on four quadruped platforms used closed-loop trajectories with horizontal and vertical motion, supporting practical long-horizon proprioceptive odometry.Residual horizontal drift remained most sensitive to heading errors and contact-model violations such as slip.
  • Evaluation: In simulation, CAPO and CAPO-CKE significantly outperformed the SLAM baseline in low-texture settings, especially in the vertical channel.CAPO-CKE also improved robustness against occasional encoder-driven velocity outliers during stair climbing.

APPENDIX A

Appendix A specifies configurable transform-chain kinematics and a closed-form 3-DoF specialization for end-effector position, velocity, and contact-force computation. The resulting Jacobian supports both velocity evaluation and torque-based stance gating.

  • Kinematic models: The released implementation uses a configurable transform chain, while the closed-form 3-DoF model provides an analytically transparent specialization.The transform chain recursively propagates world-frame node transforms from the body pose and joint measurements.
  • Kinematic models: The closed-form leg model represents quadruped point-foot and wheel-legged geometries using joint angles, link lengths, side signs, and wheel radius.For wheel-legged robots, Lcalf reaches the wheel axis and rw is positive; for point feet, the end-foot radius is absorbed into the effective shank length.
  • Kinematic models: End-effector position is obtained from the hip offset and link geometry, then differentiated to produce relative linear velocity.The formulation includes wheel radius through the effective shank length for wheel-legged platforms.
  • Velocity and wrench computation: The Jacobian maps joint velocities to end-effector velocity and supports the same virtual-work relation used for torque-based wrench estimation.Measured joint torques yield body-frame end-effector force through f_B = J^-T τ when J is invertible.
  • Velocity and wrench computation: The vertical component of the estimated end-effector force is used to gate stance selection.The transform-chain implementation can construct the Jacobian directly in the world frame without a separate body-frame rotation.

APPENDIX B

Appendix B defines an inverse-kinematics measurement model and an optional CKF recursion for estimating end-effector states from joint measurements. The filter combines a constant-velocity prior, side-consistent kinematics, analytic inverse kinematics, and cubature-based prediction and update.

  • State and measurement model: IKVel-CKF, identified as Estimator1003, filters each end-effector’s Cartesian position and velocity state.The state contains x, y, z and vx, vy, vz for an end-effector.
  • State and measurement model: The process model uses a constant-velocity prior with timestamp-difference truncation for robustness to anomalous timing.When the time difference exceeds a small bound, the implementation sets it to zero and adds process noise.
  • State and measurement model: A side constraint maintains a consistent left/right-leg kinematic branch using the leg side sign.The measurement consists of joint configuration and joint velocity, with link lengths and modeled end-effector radius defining the geometry.
  • Inverse kinematics: Analytic inverse kinematics first solves ab/adduction, removes the hip offset, and then determines the remaining planar angles.The hip-offset removal is expressed as z̄ = z − siLhip sin θ1 and ȳ = y − siLhip cos θ1.
  • Inverse kinematics: Differential kinematics maps Cartesian end-effector velocities to predicted joint velocities through the Jacobian relationship.The formulation defines θ and v explicitly and uses trigonometric terms for the second and third joints.
  • CKF recursion: CKF constructs 2n equal-weight cubature points, propagates them through the process and observation models, and applies the gain-based posterior update.Cholesky factorization is used for numerical stability in the implemented recursion.
Loading 2602.17393v3…