Contact Forces Preintegration for Estimation in Legged Robotics using Factor Graphs

Abstract

State estimation, in particular estimation of the base position, orientation and velocity, plays a big role in the efficiency of legged robot stabilization. The estimation of the base state is particularly important because of its strong correlation with the underactuated dynamics, i.e. the evolution of center of mass and angular momentum. Yet this estimation is typically done in two phases, first estimating the base state, then reconstructing the center of mass from the robot model. The underactuated dynamics is indeed not properly observed, and any bias in the model would not be corrected from the sensors. While it has already been observed that force measurements make such a bias observable, these are often only used for a binary estimation of the contact state. In this paper, we propose to simultaneously estimate the base and the underactuation state by using all measurements simultaneously. To this end, we propose several contributions to implement a complete state estimator using factor graphs. Contact forces altering the underactuated dynamics are pre-integrated using a novel adaptation of the IMU pre-integration method, which constitutes the principal contribution. IMU pre-integration is also used to measure the positional motion of the base. Encoder measurements are then participating to the estimation in two ways: by providing leg odometry displacements, contributing to the observability of IMU biases; and by relating the positional and centroidal states, thus connecting the whole graph and producing a tightly-coupled whole-body estimator. The validity of the approach is demonstrated on real data captured by the Solo12 quadruped robot.

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

“…Fourmy et al [23] proposed a factor-graph-based state estimation system for the Solo12 quadruped robot [24]. It fused both kinematic and dynamic information to estimate the base frame of the robot and performed online calibration of the offset between the base frame and center of mass.…”

IMU preintegration for 2D SLAM problems using Lie Groups

“…We follow the generalized preintegration theory described in [3,Chapter 4] and exploited in [12], [13], which is sketched below.…”

On the Use of Torque Measurement in Centroidal State Estimation

“…One of the most widely used choices for base state estimation is the Extended Kalman Filter (EKF) [1], [2], [3]. While there have been recent attempts to use more advanced approaches based on the use of factor graphs [4] and invariant Kalman Filters [5], the EKF framework is commonly used due to its compromise between simplicity, efficiency, and performance [6].…”

“…Force measurement at the endeffectors of legged robots is another useful source of information that can be used to better estimate [7] and handle contact switch events [8], [9] as well as aid in estimation of the centroidal states [10], [4]. However, since it is highly important for legged robots to have low leg inertia for agile movements, using sensorized feet [9] can drastically degrade the range of dynamic movements they can perform, especially for quadrupeds.…”

“…However, since it is highly important for legged robots to have low leg inertia for agile movements, using sensorized feet [9] can drastically degrade the range of dynamic movements they can perform, especially for quadrupeds. Thus, most of works in this area first estimate the contact forces without direct sensing, and then use the estimated forces for state estimation [8], [4].…”