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 extended Kalman filter (EKF) through 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.

中文翻译：

---

用于机器人状态估计的接触辅助不变扩展卡尔曼滤波

腿式机器人需要姿势和速度知识才能保持稳定性并执行行走路径。当前的解决方案要么依赖于易受环境和光照条件影响的视觉数据，要么依赖于运动学和接触数据与惯性测量单元 (IMU) 的测量值的融合。在这项工作中，我们使用李群理论和不变观测器设计开发了一种接触辅助不变扩展卡尔曼滤波器 (InEKF)。该滤波器将接触惯性动力学与正向运动学校正相结合，以估计姿势和速度以及所有当前接触点。我们表明误差动力学遵循对数线性自治微分方程，具有几个重要的后果：(a) 可观察的状态变量可以收敛于一个独立于系统轨迹的吸引域；(b) 与标准 EKF 不同，线性化误差动力学和线性化观测模型都不依赖于当前状态估计，这 (c) 导致收敛特性的改善和 (d) 与底层非线性系统一致的局部可观察性矩阵. 此外，我们演示了如何包含 IMU 偏差、添加/删除联系人以及制定以世界为中心和以机器人为中心的版本。我们通过对 Cassie 系列双足机器人的模拟和实验，比较了所提出的 InEKF 与常用的基于四元数的扩展卡尔曼滤波器 (EKF) 的收敛性。使用运动捕捉分析过滤器精度，而 LiDAR 映射实验提供了一个实际用例。总体而言，由于利用系统中存在的对称性，开发的接触辅助 InEKF 与基于四元数的 EKF 相比提供了更好的性能。