IEEE RAL 足式机器人鲁棒状态估计 精度较基线提升40%以上

清华大学李升波老师研究团队在IEEE Robotics and Automation Letters发表了题为《Robust State Estimation for Legged Robots With Dual Beta Kalman Filter》的研究论文。该研究提出了一种用于足式机器人鲁棒状态估计的新算法双β-卡尔曼滤波器（Dual β-KF），该模型考虑了机器人脚部打滑速度和腿长变化，展示出了显著优于最先进算法的鲁棒性。

NOKOV度量动作捕捉系统在真实实验验证中用于提供足式机器人的实际位姿，助力验证本文算法在真实环境下的精度与鲁棒性。

引用格式

T. Zhang, W. Cao, C. Liu, T. Zhang, J. Li and S. E. Li, "Robust State Estimation for Legged Robots With Dual Beta Kalman Filter," in IEEE Robotics and Automation Letters, vol. 10, no. 8, pp. 7955-7962, Aug. 2025, doi: 10.1109/LRA.2025.3579619.

研究背景

现有的依赖本体感知传感器的足式机器人状态估计算法，常常因为忽略了脚部打滑和腿部变形而导致较大误差。为消除这一限制，本文提出了双重鲁棒估计框架Dual β-KF，该框架包括一个能够独立于状态变量估计腿长参数的静力学参数滤波器，以及一个能够屏蔽脚部打滑引起的离群数据的状态滤波器β-KF。

本文贡献

为了准确获取腿长参数，研究团队提出了同时使用参数滤波器估计腿长、使用状态滤波器估计机器人的状态的双重估计框架。为防止双重结构中的误差累积，研究团队为参数滤波器构建了腿部静力学测量模型，该模型仅通过关节扭矩和脚部接触力来估计腿长，以免状态信息干扰参数估计。

研究团队将脚部打滑其视为测量数据中的离群值，将卡尔曼滤波器重新解释为基于Kullback-Leibler（KL）散度的最大后验概率（MAP）估计问题的解，通过使用更鲁棒的β-散度重新定义估计损失，开发出了β-卡尔曼滤波器（β-KF），减少了脚部打滑对估计精度的影响。

研究团队将双重估计框架与β-KF相结合，提出了一种用于足式机器人鲁棒状态估计的新算法，称为双β卡尔曼滤波器（Dual β-KF）。Gazebo仿真模拟结果和基于足式机器人的真实实验结果均表明，该算法估计精度显著高于最先进的本体感知状态估计算法。

图1：Dual β-KF框架概览。内部感知传感器数据输入模型后，与状态无关的参数滤波器首先估计腿长参数，估计结果随后被鲁棒状态滤波器用于确定机器人的状态。

实验验证

1. 实验方法

在实验中，研究团队首先在Gazebo仿真中测试算法，随后在宇树GO2机器人上进行真实实验。为公正且清晰地验证Dual β-KF的有效性，研究团队对其两个组成部分（状态滤波器β-KF、静力学参数滤波器）进行了彻底的验证。为验证状态滤波器β-KF，选择QEKF、UKFOR作为基线，并且引入IEKF，等价于迭代求解最佳MAP估计；引入Weight-KF模型开展消融实验，以证明β-KF的鲁棒性并非源于MAP估计或β散度的尺度。为验证腿部静力学参数过滤器的效果，研究团队将动态批量校准的QEKF（QEKF with [11]）、Dual QEKF和Dual β-KF评估结果进行对比。

图2：用于真实实验的足式机器人，推翻了不产生脚部打滑和腿长恒定的假设。

2. 真实实验

在真实实验中，足式机器人在平坦地面上以小跑步态移动，并在移动过程中收集传感器数据，包括1个惯性测量单元IMU、12个关节编码器和扭矩传感器及4个脚部接触力传感器。NOKOV度量动作捕捉系统负责提供真实实验中的机器人实际位姿，用于和预测位姿进行对比。

图3：NOKOV度量动作捕捉系统采集足式机器人实时位姿数据，与算法预测结果进行对比。

3. 实验结果

状态滤波器效果验证结果显示，基线QEKF由于缺乏对不准确测量模型的处理，表现出最大的估计误差；UKF-OR可以通过微调阈值有效抑制测量离群值，但某些时刻会因忽略测量更新而产生更大的误差；Dual β-KF的表现显著优于IEKF和Weight-KF，证明了其鲁棒性并非源于MAP估计或新散度的尺度。

腿部静力学参数滤波器验证结果显示，双重框架下Dual QEKF、Dual β-KF的表现不仅优于QEKF，而且优于动态批量校准的QEKF（QEKF with[11]），证明了研究团队提出的双重框架通过实时估计腿长变化的估计效果，优于使用预先校准过的固定腿长。值得注意的是，Dual β-KF实现了最佳的估计精度，与基线QEKF相比，精度提高了40%以上。

表1：不同环境下的实验结果

图4：仿真（左）和真实实验（右）中不同算法的位置估计结果。图中仅显示Dual β-KF算法和两个基线滤波器（QEKF、UKF-OR）的曲线。可以看出，Dual β-KF算法的估计轨迹更接近真实轨迹。

NOKOV度量动作捕捉系统在真实实验验证中用于提供足式机器人的实际位姿，助力验证本文算法在真实环境下的精度与鲁棒性。

作者简介

张天一，清华大学车辆与运载学院博士生，主要研究方向：机器人状态估计与控制，强化学习等。

曹文涵，新加坡国立大学博士后，主要研究方向：状态估计，机器人学习等。

刘畅，北京大学先机制造与机器人学院助理教授，博导。主要研究方向：具身智能，多机器人协同。

张涛，光象科技SunRisingAI Ltd CEO，主要研究方向：具身智能，机器人系统。

李江涛，光象科技SunRisingAI Ltd 首席科学家，主要研究方向：具身智能，机器人系统。

李升波（通讯作者），清华大学车辆与运载学院、人工智能学院教授，博导。主要研究方向：自动驾驶汽车、具身智能机器人、深度强化学习、神经网络设计、最优控制与估计等。