清华大学团队IEEE RAL 研究｜足式机器人鲁棒状态估计，精度较基线提升40%以上

清华大学李升波教授团队在IEEE Robotics and Automation Letters发表论文《Robust State Estimation for Legged Robots With Dual Beta Kalman Filter》。研究提出双β-卡尔曼滤波器（Dual β-KF），用于解决足式机器人状态估计中脚部打滑与腿长变化带来的误差问题。

在真实实验验证阶段，NOKOV度量动作捕捉系统提供足式机器人真实位姿数据，用于精度对比与鲁棒性验证。

一、研究背景：足式机器人状态估计为何需要鲁棒算法？

现有依赖本体感知传感器的足式机器人状态估计算法，常因忽略脚部打滑和腿部变形而产生较大误差。为消除这一限制，研究团队提出双重鲁棒估计框架Dual β-KF：

• 独立估计腿长参数的静力学参数滤波器
• 屏蔽脚部打滑离群数据的β-KF状态滤波器

该方法面向足式机器人状态估计核心难题展开。

二、方法创新：双β卡尔曼滤波器（Dual β-KF）如何提升足式机器人状态估计精度？

1、双重估计框架：参数滤波器 + 状态滤波器

研究团队提出双重估计框架：

• 参数滤波器估计腿长
• 状态滤波器估计机器人位姿

 

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

为避免误差累积，参数滤波器构建腿部静力学测量模型，仅通过关节扭矩和脚部接触力估计腿长，不受状态信息干扰。

该设计增强了足式机器人状态估计鲁棒性。

2、β-卡尔曼滤波器如何抑制脚部打滑误差

研究团队将脚部打滑视为测量离群值。

将卡尔曼滤波器重新解释为基于KL散度的MAP估计问题，并使用更鲁棒的β-散度重新定义损失函数，开发出β-卡尔曼滤波器（β-KF）。

该算法有效降低脚部打滑对四足机器人位姿获取精度的影响。

最终形成双β卡尔曼滤波器（Dual β-KF）。

仿真与真实实验均验证其优于最先进本体感知状态估计算法。

三、实验设计：仿真与真实机器人位姿验证方法

1、Gazebo仿真验证

研究团队首先在Gazebo仿真环境中验证算法有效性。

为验证状态滤波器β-KF，选取：QEKF，UKFOR，IEKF，Weight-KF

为验证腿部静力学参数滤波器，对比：

• QEKF
• QEKF with [11]
• Dual QEKF
• Dual β-KF

2、宇树GO2真实实验与NOKOV度量动作捕捉位姿对比

真实实验在宇树GO2机器人上进行。

机器人在平坦地面小跑，采集数据包括：

• 1个IMU
• 12个关节编码器与扭矩传感器
• 4个脚部接触力传感器

 

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

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

NOKOV度量动作捕捉系统提供机器人真实位姿数据，用于与算法预测结果进行对比，完成足式机器人状态估计精度验证。

在真实实验阶段，NOKOV度量动作捕捉作为高精度位姿基准系统，验证Dual β-KF算法在真实环境下的鲁棒性与精度提升效果。

四、实验结果：Dual β-KF相比主流状态估计算法提升多少？

状态滤波器验证结果：

• QEKF误差最大
• UKF-OR在某些时刻忽略测量更新产生误差
• Dual β-KF显著优于IEKF和Weight-KF

腿部参数滤波验证结果：

• Dual QEKF、Dual β-KF优于QEKF
• 优于动态批量校准QEKF
• Dual β-KF精度最高

与基线QEKF相比，Dual β-KF精度提升超过40%。

仿真与真实实验轨迹对比中，Dual β-KF估计轨迹更接近真实轨迹。

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

 

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

真实轨迹由NOKOV度量动作捕捉系统提供高精度位姿基准。

五、NOKOV度量动作捕捉在足式机器人状态估计验证中的作用

在本研究中，NOKOV度量动作捕捉系统承担核心角色：

• 提供真实实验位姿基准
• 作为鲁棒状态估计算法验证标准
• 支持仿真与真实场景对比

对于从事足式机器人状态估计、四足机器人位姿验证、鲁棒卡尔曼滤波算法研究的科研团队而言，动作捕捉系统是算法精度验证的重要基础设施。

六、FAQ：足式机器人状态估计与实验验证关键问题解析

Q1：足式机器人状态估计为什么容易出现误差？

足式机器人状态估计依赖IMU、关节编码器、扭矩传感器与足端接触力等本体传感器数据。但传统算法通常基于两个隐含假设：

• 足端不打滑
• 腿长保持恒定

真实运动过程中，脚部打滑会产生测量离群值，腿部结构微变形会导致模型参数变化，从而破坏滤波器的状态观测模型，导致位姿漂移。

因此，鲁棒状态估计成为足式机器人控制与导航的关键问题。

Q2：Dual β-KF如何提升足式机器人状态估计鲁棒性？

Dual β-KF通过“双重估计框架”解决误差来源问题：

• 参数滤波器：实时估计腿长变化
• 状态滤波器（β-KF）：抑制脚部打滑引起的离群测量

其中β-卡尔曼滤波器通过引入β-散度替代传统KL散度，使滤波器对异常值不敏感。

实验结果表明，在仿真与真实场景中，Dual β-KF相比QEKF精度提升超过40%。

Q3：对于研究足式机器人状态估计的团队来说，动作捕捉系统的价值是什么？

对于开展以下研究方向的团队：

• 足式机器人状态估计
• 四足机器人位姿获取
• 鲁棒卡尔曼滤波算法
• 具身智能机器人控制

高精度动作捕捉系统不仅是验证工具，更是算法研发阶段的“外部参考真值平台”。

它可以：

• 发现模型假设失效场景
• 定位误差来源
• 评估不同滤波结构差异

在本文研究中，在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.

作者简介

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

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

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

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

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

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