PaperlayerView exact revision on arXiv

Source-linked AI summary

VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots

David Wisth, Marco Camurri, Maurice Fallon

arXiv:2107.07243v3cs.ROcs.CV

TL;DR

Legged-robot odometry must handle slippage, terrain deformation, and situations where individual sensors fail. VILENS uses a factor graph that tightly fuses inertial, legged, lidar, and visual sensing while estimating a leg-odometry velocity bias online. Across its dataset, the complete system achieved 0.96 % translation and 0.0956 deg /m rotation mean RPE, and supported onboard mapping, control, and planning.

  • Problem

    Legged odometry must remain reliable when contact deformation, slippage, or degraded lidar and camera sensing make individual estimation modalities unreliable.

  • Method

    VILENS tightly integrates inertial, kinematic, lidar, and visual measurements in one factor graph, using a dedicated preintegrated kinematic velocity factor with an online velocity bias.

  • Results

    0.96 % translation and 0.0956 deg /m rotation mean RPE were obtained by the complete VILENS system across the entire dataset.

  • Takeaways & Limitations

    Tight multimodal fusion lets VILENS continue estimating with IMU and leg odometry when lidar and camera sensors are deprived, without hard switching.

Abstract

from arXiv · show

We present visual inertial lidar legged navigation system (VILENS), an odometry system for legged robots based on factor graphs. The key novelty is the tight fusion of four different sensor modalities to achieve reliable operation when the individual sensors would otherwise produce degenerate estimation. To minimize leg odometry drift, we extend the robot's state with a linear velocity bias term, which is estimated online. This bias is observable because of the tight fusion of this preintegrated velocity factor with vision, lidar, and inertial measurement unit (IMU) factors. Extensive experimental validation on different ANYmal quadruped robots is presented, for a total duration of 2 h and 1.8 km traveled. The experiments involved dynamic locomotion over loose rocks, slopes, and mud, which caused challenges such as slippage and terrain deformation. Perceptual challenges included dark and dusty underground caverns, and open and feature-deprived areas. We show an average improvement of 62% translational and 51% rotational errors compared to a state-of-the-art loosely coupled approach. To demonstrate its robustness, VILENS was also integrated with a perceptive controller and a local path planner.

I. INTRODUCTION

Legged-robot state estimation must remain reliable despite slippage, terrain deformation, sensor failure, and demanding environments. VILENS addresses these challenges by tightly fusing inertial, kinematic, lidar, and visual sensing, while estimating leg-odometry bias online.

  • Accurate state estimation is essential for environmental representation, trajectory planning, and execution during field deployment.
  • Deformable terrain, leg flexibility, and foot slippage can make local terrain reconstruction unusable and prevent multistep trajectories, especially during dynamic motion.
  • Existing tightly or loosely coupled multisensor approaches commonly assume fixed contact locations affected only by Gaussian noise, assumptions violated by terrain deformation, leg flexibility, and repeated slippage.
  • VILENS tightly fuses IMU, kinematics, lidar, and camera measurements, integrating leg kinematics directly as a factor and estimating a velocity bias to compensate for drift.The system targets operation when individual modalities fail or become degenerate, including underground and open environments.
  • A gravel foot-contact sequence shows terrain and foot deformation plus changing contact points, producing systematic modeling error and accumulated biased drift.
  • The paper evaluates VILENS across multiple ANYmal platforms and experiments involving challenging terrains and environments, supporting onboard use for mapping and navigation.The broader validation includes dynamic locomotion and deployment-oriented sensing configurations.

B. Modeling Contact Deformation and Slippage

Prior legged-robot methods commonly treat stance contacts as static, but deformation, flexibility, and slippage violate that assumption. VILENS instead computes kinematic velocities internally and incorporates them into its multisensor factor-graph formulation.

  • Contact Modeling: Static-contact assumptions can fail under slippage, deformation, and model uncertainty, causing persistent leg-odometry drift.Incorrect contact detection on soft ground is identified as a key contributor to drift.
  • Prior Approaches: Earlier methods used filters or probabilistic models to detect unreliable contacts, often prioritizing contact timing for control rather than estimation accuracy.Approaches included Mahalanobis-distance rejection, HMMs, Kalman filters, and gait-specific impact or force thresholds.
  • Prior Approaches: The prior factor-graph approach fused leg contributions into one biased velocity measurement but depended on an external filter and omitted lidar estimation.These limitations motivated internal kinematic velocity computation and broader sensor fusion.
  • VILENS Formulation: VILENS computes base velocities from joint states internally, avoiding double IMU usage and enabling more accurate kinematic covariance modeling.The method also propagates errors from joints to feet within the factor graph.
  • VILENS Formulation: The system estimates robot poses, velocities, and visual or lidar landmarks using cameras, IMUs, lidars, and joint sensors.Visual landmarks are 3D points, while lidar landmarks are planes and lines.

D. Measurements Definition

VILENS synchronizes heterogeneous camera and lidar measurements before incorporating them with IMU and kinematic data into a factor graph. Its optimization minimizes covariance-weighted residuals from multiple sensor-specific factors.

  • Measurement Streams: Camera images, lidar point clouds, IMU measurements, and joint kinematics form the measurement streams used by the estimator.IMU data include proper acceleration and rotational velocity, while kinematics include joint positions and velocities.
  • Measurement Streams: Camera and lidar measurements are synchronized before integration because they arrive at different times and frequencies.
  • Factor Graph: The factor graph contains prior, visual, lidar-plane, lidar-line, preintegrated IMU, preintegrated velocity, and ICP-based lidar-odometry factors.State nodes and landmarks are represented separately in the graph.
  • Optimization: The estimator maximizes measurement likelihood by solving a least-squares problem under conditional independence and white Gaussian noise assumptions.
  • Optimization: Each newly created keyframe adds IMU, kinematic-velocity, bias, lidar-odometry, and point, line, or plane landmark residuals, weighted by inverse covariance.The state prior is the exception to the rule that residuals are added with each new keyframe.
  • Sensor Factors: The formulation includes adapted bias and stereo-visual residuals plus lidar feature and novel lidar-registration residuals.
  • Sensor Factors: Preintegrated IMU factors constrain pose and velocity between keyframes while making high-frequency IMU fusion computationally feasible.Preintegration avoids recomputing integration whenever the linearization point changes.

B. Preintegrated Leg Odometry Factors

The preintegrated leg-odometry factor converts stance-leg kinematics into a combined noisy base-velocity measurement and uses it to constrain robot position. Stance selection relies on ground-reaction-force estimates and contact-state thresholding.

  • Factor Construction: The preintegrated velocity factor estimates robot position from high-frequency joint-kinematics measurements.
  • Factor Construction: Stance legs are identified, their kinematic odometries are fused into one velocity measurement, and contact nonlinearities are modeled with a linear velocity bias.
  • Stance Estimation: Ground-reaction forces are computed from the dynamics equation because direct force measurements are unavailable.The computation uses joint positions, Jacobians, torques, Coriolis terms, and IMU-derived accelerations.
  • Stance Estimation: Binary leg-contact states are obtained by thresholding each foot’s vertical ground-reaction force under a rigid, nonslipping-contact assumption.The stance set is a subset of the four legs LF, RF, LH, and RH.
  • Velocity Measurement: For a stance leg, base velocity is computed from joint kinematics, with encoder positions and velocities modeled as additive zero-mean Gaussian noise.
  • Velocity Measurement: Velocity measurements from multiple stance legs are fused by a weighted average using their information matrices instead of adding one graph factor per leg.This reduces graph complexity and avoids explicit contact-switch handling for every step.
  • Velocity Measurement: The fused velocity measurement is represented as a noisy observation with zero-mean Gaussian covariance Σv and supplied to the preintegrated velocity factor.

3) Velocity Bias:

Slippery or deformable terrain produces locally constant, gait- and terrain-dependent velocity drift in leg odometry. VILENS addresses it with a slowly varying bias that is included during preintegration and updated without recomputing the full measurement.

  • Velocity Bias: On slippery or deformable terrain, the rigid-contact constraint fails and creates locally constant velocity drift that depends on gait and terrain.
  • Velocity Bias: VILENS relaxes the rigid-contact model by adding a slowly varying linear velocity bias to the kinematic velocity measurement.
  • Velocity Bias: The bias represents drift caused by leg or terrain compression, slippage, and impacts at the contact point.
  • Preintegration: The preintegrated leg-odometry position measurement and noise depend on angular and linear velocity biases.
  • Preintegration: Assuming constant velocity between time steps, the method iteratively computes position and derives a relative measurement for consecutive states.
  • Bias Updates: A first-order approximation updates the preintegrated measurement when the bias changes, avoiding recomputation from scratch.

5) Residuals:

VILENS defines residuals for visual, lidar, and bias-related factors, while modeling fisheye distortion to represent feature uncertainty accurately.

  • Leg-odometry residuals: The leg-odometry residual covariance is propagated iteratively from an initial covariance and fixed sensor-noise covariance.
  • Bias residuals: The velocity-bias terms are modeled as slowly changing Gaussian random walks, with covariance determined by expected rates of change.
  • Visual residuals: Visual factors include reprojection error and an optional lidar-camera depth factor for visual features.
  • Visual residuals: Fisheye support uses a 150–165° diagonal field of view and undistorts tracked feature locations before graph insertion.
  • Visual residuals: Fisheye covariance is estimated by undistorting uncertainty samples and fitting an ellipse, requiring at least six points for generally rotated ellipses.

E. Plane and Line Factors

VILENS tracks lidar planes and lines and stabilizes their factor-graph optimization with local anchor frames and robust registration handling.

  • Feature tracking: Lidar processing extracts and tracks planes and lines across successive scans as geometric primitives.
  • Anchor frames: Anchor frames reduce convergence time and improve numerical stability when transforming lidar features in large-scale environments.
  • Plane and line residuals: Plane residuals compare measured features with estimated planes transformed into a local reference frame defined by the first observation pose.
  • Registration: Lidar registration combines full-frame-rate feature tracking with lower-frequency ICP factors between nonconsecutive keyframes.
  • Registration: ICP can fail in degenerate geometries such as long tunnels, so a robust cost function rejects unreliable registration factors.

V. IMPLEMENTATION

The implementation runs parallel sensor-processing threads with fixed-lag factor-graph optimization, synchronization, robust costs, and operational modes for contact and stationarity.

  • Architecture: Preintegration, camera, and lidar threads process measurements in parallel while a fourth thread performs optimization.
  • Architecture: IMU preintegration provides a forward-propagated state at 400 Hz for lidar motion correction, synchronization, and high-frequency control tasks.
  • Optimization: The factor graph uses iSAM2 fixed-lag smoothing with a 5 s lag, while visual and lidar factors use Dynamic Covariance Scaling.
  • Synchronization: High-frequency IMU and kinematic measurements are interpolated to camera timestamps, and lidar points are motion-compensated to the closest camera keyframe.
  • Contact handling: When three or four feet remain in contact for more than 200 ms, VILENS disables twist bias and uses relative pose factors.
  • Stationarity: Stationarity is detected by voting across sensor modalities, enabling zero-velocity constraints even when some inputs are unavailable.

VI. EXPERIMENTAL RESULTS

VILENS was evaluated on multiple ANYmal platforms and challenging environments using diverse sensor combinations, with the complete system achieving the strongest reported accuracy and outperforming CompSLAM.

  • Platforms and datasets: Experiments covered ANYmal B300 and C100 quadrupeds with IMU, lidar, joint encoders, and torque sensors across three sensor configurations.
  • Platforms and datasets: The datasets totaled 2 h and 1.8 km across concrete, gravel, grass, mud, darkness, dust, reflections, and long corridors.
  • Evaluation: Performance was measured using mean Relative Pose Error over 10 m traveled at 15 Hz across sensor-module combinations.
  • Results: Trajectory estimates closely matched ground truth in the FSC and LSM experiments without loop closures.
  • Results: The complete VILENS system achieved an overall mean RPE of 0.96 % in translation and 0.0956 deg /m in rotation.
  • Results: Multimodal fusion produced a smoother state estimate that improved lidar motion correction, strengthened ICP priors, and enabled rejection of inaccurate ICP estimates.
  • Results: VILENS outperformed the loosely coupled CompSLAM approach by an average of 62 % in translation and 51 % in rotation.

D. Twist Bias Ablation Study

Online velocity-bias estimation reduces drift from biased leg kinematics, while integrating kinematics inside the optimizer improves velocity tracking and position accuracy. VILENS state estimates also support high-frequency terrain mapping for path planning and perceptive control.

  • Twist Bias Ablation Study: 9.0% / 5.9% average reduction in translation / rotation RPE results from adding online velocity-bias estimation, even with cameras and lidar.The benefit is most apparent when exteroceptive sensors are degraded.
  • Internal Kinematics Comparison: 45% position-drift reduction follows from incorporating kinematics inside the factor graph instead of using an external kinematic-inertial filter.The comparison uses only kinematic-inertial inputs against motion-capture ground truth.
  • Local Path Planning: Over 30 min, the terrain map supported GBPlanner local path planning without failure during the LSM experiment.An elevation map clearly represented a ledge, allowing the planner to create routes around the obstacle.
  • Terrain Map Integration: 400 Hz, low-drift VILENS state estimates provide inputs for local terrain mapping and downstream perceptive control or local path planning.The mapping pipeline uses VILENS state estimates together with downward-facing depth-camera point clouds.
  • Perceptive Control: During a 5 min outdoor experiment, the terrain map enabled RLOC to plan and execute precise steps over grass, gravel, curbs, and slopes.Reduced state-estimate drift improved the map and enabled more accurate motion planning.

VII. DISCUSSION

VILENS maintains useful state estimates when visual or lidar sensing becomes degraded by combining leg odometry and online velocity-bias estimation. Its onboard outputs are accurate and frequent enough to support terrain mapping, control, and path planning.

  • Bias Estimation: Velocity-bias estimation tracks gravel-induced kinematic velocity error in the z-axis, demonstrating compensation for a major source of leg-odometry drift.The bias term models the difference between perceived kinematic velocity and true robot velocity.
  • Underconstrained Environments: VILENS handles degenerate sensing scenarios without the hard switches required by loosely coupled approaches.Partial lidar constraints can remain useful even when ICP becomes unstable or nearly divergent.
  • Underconstrained Environments: Leg odometry reduces angular error during visual underexposure, while online bias estimation further reduces drift on loose terrain.The comparison includes configurations without leg odometry and without online bias estimation.
  • Water Reflections: VILENS-VIK maintains a better pose estimate than VILENS-VI while crossing a puddle with drifting visual features on standing water.The improvement relies on leg odometry with bias estimation.
  • Onboard Integration: VILENS runs onboard with computationally lightweight visual and point-cloud features, while supporting terrain mapping, perceptive control, and local path planning.Experiments used several quadruped platforms in slippery, deformable, reflective, dark, and geometrically degenerate environments over more than two hours.
  • Output Characteristics: 400 Hz forward-propagated outputs closely track motion-capture ground truth between 10 Hz optimizations.The output is described as accurate, high-frequency, and low-latency for capturing behavior between optimizations.

APPENDIX PREINTEGRATED LEG ODOMETRY FACTOR

The preintegrated leg-odometry factor derives velocity measurements from joint kinematics and propagates their uncertainty for factor-graph optimization. Its formulation uses forward kinematics, Jacobians, IMU angular velocity, and stance-leg measurements.

  • Velocity Measurement: The factor computes robot linear velocity from foot position and velocity when a leg is in rigid, nonslipping ground contact.Multiple stance-leg measurements can be independently computed and fused by weighted averaging.
  • Noise Modeling: Forward kinematics fp(·) and its Jacobian Jp(·) separate the kinematic velocity measurement from joint-state noise.A Taylor expansion of Jp(α)α̇ ignores second-order terms, with independence assumed between joint positions and velocities.
  • Noise Modeling: The forward-kinematics Hessian Hp(·) captures higher-order structure in the noise expansion and is evaluated numerically in the implementation.Because forward kinematics is vector-valued, the Hessian is a rank-3 tensor.
  • Factor Construction: The derivation substitutes the separated noise terms into the velocity measurement and replaces rotational velocity with noisy IMU measurements.Cross-product inversion and distributive properties produce the factor’s working expression.

B. Iterative Noise Propagation

The factor introduces a velocity-bias state to relax the no-slip assumption and relate kinematic velocity to true velocity. Observability is supported empirically through multisensor fusion, but formal proof remains future work.

  • Velocity Bias: The velocity-bias term bv relaxes the non-slip contact relation in the leg-odometry model.It intuitively represents the velocity of the contact point.
  • Velocity Bias: Ignoring Gaussian noise, bv equals the difference between estimated kinematic velocity ˜vkin and true robot velocity v.The bias therefore captures contact motion or related systematic leg-odometry error.
  • Observability: Kinematic-inertial measurements alone estimate ˜vkin because absolute position and yaw remain unobservable under stationary-contact assumptions.Visual-inertial measurements share the same unobservable states, while combined measurements can expose the robot’s linear velocity.
  • Observability: VILENS’s combined kinematic, inertial, and camera measurements are expected to make bv observable, and experiments empirically confirm this behavior.An analytic proof of observability and convergence is left to future work.
Loading 2107.07243v3…