Technical Summary: Contact-Anchored Proprioceptive Odometry (CAPO)

Problem Statement

Accurate state estimation (pose and velocity) for legged robots is critical for motion planning and stable control. While exteroceptive sensors (LiDAR, cameras) coupled with SLAM provide global feedback, they are often unreliable in challenging environments (e.g., poor lighting, airborne particulates, vegetation) and computationally expensive for lightweight platforms. Purely proprioceptive methods, which rely solely on IMU and motor measurements, face significant challenges:

1. IMU Drift: Double integration of acceleration leads to rapid position and velocity drift due to bias and noise.
2. Noisy Joint Sensing: Direct differentiation of encoder data introduces spikes and quantization noise, degrading velocity estimates.
3. Contact Ambiguity: Determining which legs are in reliable contact (stance) versus swinging is difficult, especially under imperfect terrain conditions.
4. Elevation and Yaw Drift: Long-term traversal often results in accumulated errors in height (elevation) and heading (yaw), particularly when IMU constraints are degraded.

Methodology

The paper proposes CAPO (Contact-Anchored Proprioceptive Odometry), a unified state estimator applicable to biped, quadruped, and wheel-legged robots. The core philosophy is to treat reliable ground contacts as kinematic anchors to intermittently correct the robot's state.

1. Unified Framework and Stance Selection

• Inputs: The system uses only IMU data (acceleration, angular velocity) and motor measurements (joint angles, velocities, and estimated torques).
• Stance Detection: A leg is considered in stance if the vertical component of the estimated foot wrench (derived from joint torques and the geometric Jacobian) falls below a configurable threshold. A soft confidence score is computed to handle transitions.
• Footfall Recording: Upon detecting a touchdown event, the world-frame position of the foot is recorded. During the stance phase, this point is treated as a stationary anchor in the world frame. The robot's body position is inferred by subtracting the current body-frame kinematics from this stored anchor.

2. Elevation Stability: Height Clustering

To prevent long-term vertical drift, the system employs a lightweight height clustering and time-decay correction:

• Touchdown heights are associated with a set of support-plane records.
• New footfall heights are "snapped" to previously observed support planes within a configurable resolution.
• Confidence weights for these planes decay over time, allowing the system to adapt to new terrain while suppressing noise-induced drift.

3. Wheel-Legged Propagation

For wheel-legged robots, the contact point is not stationary even without slip; it propagates via wheel rotation. The method introduces:

• Effective Rolling Angle: Compensates for apparent wheel rotation caused by shank pitching (leg motion) to isolate true ground rolling.
• Slope-Aware Propagation: Fits a local support plane from reliable contacts to determine the rolling direction, allowing the contact record to propagate along the terrain tangent rather than just the horizontal plane.
• Lateral Compensation: Accounts for shank roll-induced lateral displacement of the wheel contact.

4. Velocity Smoothing: IKVel-CKF

To mitigate impulsive spikes in foot velocity caused by encoder quantization and discrete differentiation, the paper introduces an optional Inverse-Kinematics Cubature Kalman Filter (IKVel-CKF):

• It estimates the hip-to-foot Cartesian velocity directly from joint angles and velocities using a nonlinear observation model.
• Unlike Extended Kalman Filters (EKF), CKF avoids Jacobian calculations and scaling parameters, offering numerical stability for inverse-kinematics measurements.
• This module (CAPO-CKE variant) provides smoother velocity feedback for the fusion update, reducing noise propagation into the state estimate.

5. Yaw Drift Suppression

The paper addresses yaw drift (which is weakly observable from IMU alone) using multi-contact geometric consistency:

• When multiple legs are in stance, the relative geometry between their world-frame footfall records is fixed.
• The system compares the world-frame relative vectors of contacts with the body-frame relative vectors (derived from joint kinematics).
• Any discrepancy reveals a yaw misalignment. This is applied as a soft heading prior to the attitude estimator rather than a hard reset, effectively arresting drift during prolonged standing and providing a fallback reference when IMU yaw constraints are degraded.

Key Contributions

1. Unified Morphology-Agnostic Odometry: A single framework handling bipeds, quadrupeds, and wheel-legged robots by dynamically forming constraints from the current set of contacting end-effectors.
2. Footfall Recording with Height Correction: A mechanism to record world-frame contact positions and correct elevation drift via support-plane clustering, ensuring long-term vertical stability.
3. Inverse-Kinematics Nonlinear Filtering: The use of IKVel-CKF to suppress encoder-induced velocity spikes, improving the robustness of velocity constraints without requiring external sensors.
4. Kinematics-Only Yaw Correction: A method to suppress long-horizon yaw drift by enforcing geometric consistency between body kinematics and world-frame contact records, acting as a soft prior.
5. Real-World Validation and Open Source: Evaluation on four distinct platforms (Unitree Go2 EDU and three Astrall robots) with public release of code and data for reproducibility.

Experimental Results

The method was evaluated on four platforms using closed-loop trajectories involving both horizontal and vertical motion:

• Simulation (Gazebo): CAPO and CAPO-CKE significantly outperformed a LiDAR-based SLAM baseline in low-texture environments, particularly in the vertical channel. CAPO-CKE demonstrated superior robustness against velocity outliers during stair climbing.
• Unitree Go2 EDU:
  • Closed a ~120 m horizontal loop with 2.21 m error.
  • Closed a ~8 m vertical loop with <0.1 m height error.
• Astrall Point-Foot Robot (A):
  • Closed a ~200 m horizontal loop with 0.16 m error.
  • Closed a ~15 m vertical loop with 0.22 m error.
• Astrall Wheel-Legged Robot (B):
  • Closed a ~200 m horizontal loop with 0.23 m error.
  • Closed a ~15 m vertical loop with 0.20 m error.
• Astrall Wheel-Legged Robot (C):
  • Closed a ~700 m horizontal loop with 7.68 m error (attributed to slip and contact violations over long ranges).
  • Closed a ~20 m vertical loop with 0.54 m error.

Significance and Claims

The paper claims that CAPO provides a robust, purely proprioceptive odometry solution that remains effective without vision or LiDAR. Its significance lies in:

• Drift Suppression: Successfully suppressing long-term drift in both position (via footfall anchoring) and elevation (via height clustering) using only onboard sensors.
• Versatility: Demonstrating that a unified kinematic formulation can handle diverse morphologies, including the complexities of wheel-legged locomotion.
• Reliability in Degraded Conditions: Providing a kinematics-derived heading reference that stabilizes the system when IMU yaw constraints are unreliable, a common failure mode in long-duration autonomous operation.
• Practicality: The authors note that while residual horizontal drift remains sensitive to heading errors and contact model violations (e.g., wheel slip), the system achieves practical accuracy for many applications, particularly in environments where exteroceptive sensors fail.

The authors acknowledge limitations, including the lack of explicit wheel slip detection and the reliance on fixed-force thresholds for stance detection, identifying these as targets for future work.