Fast Decentralized State Estimation for Legged Robot Locomotion via EKF and MHE

📄 arXiv: 2405.20567v3 📥 PDF

作者: Jiarong Kang, Yi Wang, Xiaobin Xiong

分类: cs.RO

发布日期: 2024-05-31 (更新: 2024-10-11)

备注: 8 pages, accepted by RAL 2024

💡 一句话要点

提出基于EKF和MHE的快速去中心化状态估计框架，用于腿式机器人运动控制。

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

关键词: 状态估计 腿式机器人 扩展卡尔曼滤波器 移动 horizon 估计 去中心化 机器人控制 非线性优化

📋 核心要点

  1. 现有腿式机器人状态估计方法难以兼顾计算效率、估计精度和状态约束。
  2. 论文提出一种去中心化状态估计框架，利用EKF进行姿态估计，MHE进行线性速度估计，并采用边缘化方法加速计算。
  3. 实验表明，该方法能够以200Hz的频率为多种腿式机器人提供准确的状态估计。

📝 摘要（中文）

本文提出了一种快速且去中心化的状态估计框架，用于腿式运动控制。浮动基座状态的非线性估计被分解为：通过扩展卡尔曼滤波器（EKF）进行姿态估计，以及通过移动 horizon 估计（MHE）进行线性速度估计。EKF融合惯性传感器和视觉信息来估计浮动基座的姿态。MHE利用估计的姿态以及过去时间窗口内的所有传感器数据，基于感兴趣状态的时变线性动力学公式和状态约束来估计线性速度。更重要的是，提出了一种基于全信息滤波器（FIF）优化结构的边缘化方法，将等式约束的FIF转换为等效的MHE。这种状态估计的解耦促进了计算效率、估计精度和状态约束包含之间的平衡。所提出的方法能够为多种腿式机器人提供准确的状态估计，包括高动态跳跃机器人PogoX、双足机器人Cassie和四足机器人Unitree Go1，频率为200 Hz，窗口间隔为0.1秒。

🔬 方法详解

问题定义：腿式机器人需要准确且实时的状态估计来实现稳定的运动控制。然而，传统的非线性状态估计方法计算量大，难以满足实时性要求，并且难以有效处理状态约束。此外，集中式状态估计方法依赖于所有传感器的信息，一旦某个传感器出现故障，整个系统都会受到影响。

核心思路：论文的核心思路是将非线性状态估计问题分解为两个子问题：姿态估计和线性速度估计。姿态估计采用计算效率高的EKF，线性速度估计采用能够处理状态约束的MHE。通过这种解耦，可以兼顾计算效率、估计精度和状态约束。此外，采用去中心化的架构，提高了系统的鲁棒性。

技术框架：该框架主要包含两个模块：姿态估计模块和线性速度估计模块。姿态估计模块使用EKF融合惯性传感器和视觉信息来估计浮动基座的姿态。线性速度估计模块使用MHE，利用估计的姿态以及过去时间窗口内的所有传感器数据，基于感兴趣状态的时变线性动力学公式和状态约束来估计线性速度。为了提高计算效率，论文还提出了一种基于全信息滤波器（FIF）优化结构的边缘化方法，将等式约束的FIF转换为等效的MHE。

关键创新：该论文的关键创新在于提出了一种去中心化的状态估计框架，将非线性状态估计问题分解为姿态估计和线性速度估计两个子问题，并分别采用EKF和MHE进行求解。此外，论文还提出了一种基于FIF优化结构的边缘化方法，将等式约束的FIF转换为等效的MHE，从而提高了计算效率。

关键设计：在姿态估计模块中，EKF的状态向量包括浮动基座的姿态（四元数表示）和角速度。观测模型包括来自视觉传感器的位姿信息和来自惯性传感器的角速度信息。在线性速度估计模块中，MHE的状态向量包括浮动基座的线性速度。动力学模型基于腿式机器人的运动学和动力学方程。状态约束包括腿部与地面的接触约束和速度约束。时间窗口的长度设置为0.1秒，以平衡计算效率和估计精度。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，该方法能够为多种腿式机器人（包括PogoX、Cassie和Unitree Go1）提供准确的状态估计，频率达到200 Hz，窗口间隔为0.1秒。与传统的集中式状态估计方法相比，该方法在计算效率和鲁棒性方面具有显著优势。具体性能数据未知。

🎯 应用场景

该研究成果可应用于各种腿式机器人，包括双足机器人、四足机器人和跳跃机器人。通过提供准确且实时的状态估计，可以提高腿式机器人的运动控制性能，使其能够执行更复杂的任务，例如在崎岖地形上行走、跳跃和奔跑。此外，该方法还可以应用于无人驾驶车辆、增强现实和虚拟现实等领域。

📄 摘要（原文）

In this paper, we present a fast and decentralized state estimation framework for the control of legged locomotion. The nonlinear estimation of the floating base states is decentralized to an orientation estimation via Extended Kalman Filter (EKF) and a linear velocity estimation via Moving Horizon Estimation (MHE). The EKF fuses the inertia sensor with vision to estimate the floating base orientation. The MHE uses the estimated orientation with all the sensors within a time window in the past to estimate the linear velocities based on a time-varying linear dynamics formulation of the interested states with state constraints. More importantly, a marginalization method based on the optimization structure of the full information filter (FIF) is proposed to convert the equality-constrained FIF to an equivalent MHE. This decoupling of state estimation promotes the desired balance of computation efficiency, accuracy of estimation, and the inclusion of state constraints. The proposed method is shown to be capable of providing accurate state estimation to several legged robots, including the highly dynamic hopping robot PogoX, the bipedal robot Cassie, and the quadrupedal robot Unitree Go1, with a frequency at 200 Hz and a window interval of 0.1s.