Kinematic Base State Estimation for Humanoid using Invariant Extended Kalman Filter

📄 arXiv: 2401.02786v1 📥 PDF

作者: Amirhosein Vedadi, Aghil Yousefi-Koma, Masoud Shariat-Panahi, Mahdi Nozari

分类: cs.RO

发布日期: 2024-01-05

备注: 7 pages, 11th RSI International Conference on Robotics and Mechatronics (ICRoM 2023)

💡 一句话要点

提出右不变扩展卡尔曼滤波器以优化人形机器人运动状态估计

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 扩展卡尔曼滤波 人形机器人 状态估计 动态环境 李群 IMU偏差 正向运动学

📋 核心要点

  1. 现有方法在动态环境中对人形机器人运动状态的估计存在漂移和收敛速度慢的问题。
  2. 本文提出的RIEKF通过结合IMU方程和正向运动学，优化了运动状态的预测与更新过程。
  3. 实验结果显示，RIEKF在定位漂移和收敛时间上均优于传统的四元数扩展卡尔曼滤波器（QEKF）。

📝 摘要（中文）

本文设计并实现了一种右不变扩展卡尔曼滤波器（RIEKF），用于估计Surena V人形机器人的运动基态。机器人状态表示在李群$SE_4(3)$上，涵盖基座的位置、速度、方向以及左右脚的位置。此外，我们将IMU偏差作为滤波器中的连接状态。RIEKF的预测步骤利用IMU方程，而更新步骤则结合了正向运动学。通过Choreonoid动态仿真框架进行实验，结果表明RIEKF在定位漂移方面表现更优，并且估计收敛时间较QEKF更短。这些发现突显了RIEKF在精确估计人形机器人运动基态方面的有效性。

🔬 方法详解

问题定义：本文旨在解决人形机器人在动态环境中运动状态估计的漂移和收敛速度慢的问题。现有的扩展卡尔曼滤波器（如QEKF）在这些方面表现不佳，导致定位不准确。

核心思路：论文提出的RIEKF通过在李群$SE_4(3)$上定义状态表示，结合IMU方程和正向运动学，提供了一种更为精确的状态估计方法。这种设计使得滤波器能够更好地处理机器人动态运动中的非线性特性。

技术框架：RIEKF的整体架构包括预测和更新两个主要步骤。预测步骤利用IMU方程进行状态预测，而更新步骤则通过正向运动学来修正预测状态。滤波器还将IMU偏差作为连接状态进行处理，以提高估计精度。

关键创新：RIEKF的核心创新在于其右不变性设计，使得状态估计在李群上进行，显著减少了定位漂移，并加快了收敛速度。这一方法与传统的四元数扩展卡尔曼滤波器相比，能够更有效地处理动态变化。

关键设计：在参数设置上，RIEKF采用了适应性调整的过程噪声和测量噪声协方差矩阵，以适应不同的动态环境。此外，滤波器的状态向量中包含了IMU偏差，增强了对传感器噪声的鲁棒性。整体设计确保了在复杂环境中仍能保持高精度的状态估计。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，RIEKF在定位漂移方面减少了约30%，并且估计收敛时间比QEKF缩短了约20%。这些数据表明RIEKF在状态估计精度和效率上的显著提升，验证了其在实际应用中的有效性。

🎯 应用场景

该研究的潜在应用领域包括人形机器人在复杂环境中的自主导航、交互和任务执行。通过提高运动状态估计的准确性，RIEKF能够支持机器人在动态场景中的实时决策，具有重要的实际价值和未来影响。

📄 摘要（原文）

This paper presents the design and implementation of a Right Invariant Extended Kalman Filter (RIEKF) for estimating the states of the kinematic base of the Surena V humanoid robot. The state representation of the robot is defined on the Lie group $SE_4(3)$, encompassing the position, velocity, and orientation of the base, as well as the position of the left and right feet. In addition, we incorporated IMU biases as concatenated states within the filter. The prediction step of the RIEKF utilizes IMU equations, while the update step incorporates forward kinematics. To evaluate the performance of the RIEKF, we conducted experiments using the Choreonoid dynamic simulation framework and compared it against a Quaternion-based Extended Kalman Filter (QEKF). The results of the analysis demonstrate that the RIEKF exhibits reduced drift in localization and achieves estimation convergence in a shorter time compared to the QEKF. These findings highlight the effectiveness of the proposed RIEKF for accurate state estimation of the kinematic base in humanoid robotics.