Legged robots require knowledge of pose and velocity in order to maintain
stability and execute walking paths. Current solutions either rely on vision
data, which is susceptible to environmental and lighting conditions, or fusion
of kinematic and contact data with measurements from an inertial measurement
unit (IMU). In this work, we develop a contact-aided invariant extended Kalman
filter (InEKF) using the theory of Lie groups and invariant observer design.
This filter combines contact-inertial dynamics with forward kinematic
corrections to estimate pose and velocity along with all current contact
points. We show that the error dynamics follows a log-linear autonomous
differential equation with several important consequences: (a) the observable
state variables can be rendered convergent with a domain of attraction that is
independent of the system's trajectory; (b) unlike the standard EKF, neither
the linearized error dynamics nor the linearized observation model depend on
the current state estimate, which (c) leads to improved convergence properties
and (d) a local observability matrix that is consistent with the underlying
nonlinear system. Furthermore, we demonstrate how to include IMU biases,
add/remove contacts, and formulate both world-centric and robo-centric
versions. We compare the convergence of the proposed InEKF with the commonly
used quaternion-based EKF though both simulations and experiments on a
Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture,
while a LiDAR mapping experiment provides a practical use case. Overall, the
developed contact-aided InEKF provides better performance in comparison with
the quaternion-based EKF as a result of exploiting symmetries present in
system.

该研究开发了利用 Lie 群和不变观测器设计技术的接触辅助不变扩展卡尔曼滤波器（InEKF），该滤波器结合了接触惯性动力学和正向运动学校正，可估计姿态和速度以及所有当前接触点。研究表明，误差动力学遵循对数线性自治微分方程，从而提高了收敛性能，并产生了符合非线性系统的本地可观测矩阵。该过程在 Cassie 系列双足机器人上进行了模拟和实验，并表明其比基于四元数的 EKF 具有更好的性能。