Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors
share
State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and fail when visual tracking is lost due to lighting or scarcity of features. In addition, such systems are formulated such that the system is independent of the operating platform. This paper presents state-estimation technique for legged robots that takes into account the robot's kinematic model as well as its contact with the environment. This is achieved by introducing forward kinematic factors and preintegrated contact factors into a factor graph framework that can incrementally solved in real-time. The forward kinematics factor relates the robot's base pose to a contact frame through noisy encoder measurements. The preintegrated contact factor provides odometry measurements of this contact frame, while accounting for possible foot slippage. Taken together, the two factors constrain the graph optimization problem allowing the robot's trajectory to be estimated. The paper evaluates the method using simulated and real sensory IMU and kinematic data from experiments with a Cassie-series robot designed by Agility Robotics. These experiments show that using the proposed method in addition to IMU significantly decreases drift and improves localization accuracy, suggesting that its use can enable successful recovery from loss of visual tracking.
READ FULL TEXT