Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs

Abstract

The factor graph framework is a convenient modeling technique for robotic state estimation where states are represented as nodes, and measurements are modeled as factors. When designing a sensor fusion framework for legged robots, one often has access to visual, inertial, joint encoder, and contact sensors. While visual-inertial odometry has been studied extensively in this framework, the addition of a preintegrated contact factor for legged robots has been only recently proposed. This allowed for integration of encoder and contact measurements into existing factor graphs, however, new nodes had to be added to the graph every time contact was made or broken. In this work, to cope with the problem of switching contact frames, we propose a hybrid contact preintegration theory that allows contact information to be integrated through an arbitrary number of contact switches. The proposed hybrid modeling approach reduces the number of required variables in the nonlinear optimization problem by only requiring new states to be added alongside camera or selected keyframes. This method is evaluated using real experimental data collected from a Cassie-series robot where the trajectory of the robot produced by a motion capture system is used as a proxy for ground truth. The evaluation shows that inclusion of the proposed preintegrated hybrid contact factor alongside visual-inertial navigation systems improves estimation accuracy as well as robustness to vision failure, while its generalization makes it more accessible for legged platforms.

arXiv commentDetailed derivations are provided in the supplementary material document listed under "Ancillary files"

References in corpus (3)

• On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
• State Estimation for a Humanoid Robot
• Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation

Cited by in corpus (12)

• VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots
• Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation
• Preintegrated Velocity Bias Estimation to Overcome Contact Nonlinearities in Legged Robot Odometry
• Toward Safety-Aware Informative Motion Planning for Legged Robots
• A Factor-Graph Approach for Optimization Problems with Dynamics Constraints
• Tightly-Coupled LiDAR-IMU-Leg Odometry with Online Learned Leg Kinematics Incorporating Foot Tactile Information
• Adaptive Continuous Visual Odometry from RGB-D Images
• Learning Inertial Odometry for Dynamic Legged Robot State Estimation
• Continuous Direct Sparse Visual Odometry from RGB-D Images
• A Unified Method for Solving Inverse, Forward, and Hybrid Manipulator Dynamics using Factor Graphs
• WALK-VIO: Walking-motion-Adaptive Leg Kinematic Constraint Visual-Inertial Odometry for Quadruped Robots
• Legged Robot State Estimation in Slippery Environments Using Invariant Extended Kalman Filter with Velocity Update