当传统卡尔曼滤波遇上Transformer，人形机器人站得更稳了！

编辑：陈萍萍的公主@一点人工一点智能

论文地址：https://arxiv.org/pdf/2511.16306

引言

人形机器人在工业、家庭、医疗和搜救等领域具有广泛的应用前景，然而其双足运动在不同环境中的稳定性和动态控制仍面临挑战。状态估计在这一过程中扮演关键角色，它为运动控制器提供机器人浮动基座状态的快速准确反馈。传统状态估计方法如卡尔曼滤波器虽广泛应用，但需要专家经验进行噪声参数调优。近年来，随着机器学习的发展，深度学习方法逐渐被引入状态估计任务中。

本文提出了一种新型混合状态估计方法——InEKFormer，该方法结合了不变扩展卡尔曼滤波与Transformer网络，旨在提升人形机器人状态估计的准确性与鲁棒性。

本文的主要贡献包括三个方面：

首先，提出了InEKFormer这一新型混合状态估计架构；

其次，构建并公开了一个包含多种运动类型的人形机器人数据集，用于Transformer网络的离线训练；

最后，通过大量实验将InEKFormer与InEKF和KalmanNet进行对比分析。

相关工作

状态估计方法大致可分为三类：模型驱动方法、数据驱动方法以及混合方法。模型驱动方法如扩展卡尔曼滤波器及其改进版本不变扩展卡尔曼滤波器，利用系统模型信息进行状态估计，但在存在模型失配或非线性较强时性能受限。InEKF通过李群对称性提升估计的一致性，但在引入IMU偏差等状态变量后会失去其理论上的“不变性”。

数据驱动方法近年来受到广泛关注，尤其是基于循环神经网络和Transformer的序列建模方法。例如，有研究尝试用Transformer结合Koopman算子处理非线性系统，也有理论分析表明Transformer能够逼近卡尔曼滤波器。然而，这些方法多局限于低维系统，且对数据量和计算资源要求较高。

混合方法则试图结合模型驱动与数据驱动的优势，通常分为外部耦合与内部耦合两类。外部耦合方法如将学习到的惯性里程计或接触事件信息输入到InEKF中，而内部耦合方法如KalmanNet使用RNN学习卡尔曼增益。然而，现有方法多针对四足机器人或低维系统，尚未充分探索在人形机器人上的应用。本文提出的InEKFormer属于内部耦合方法，首次将InEKF与Transformer深度融合，并应用于全尺寸人形机器人状态估计任务中。

方法

3.1 不变扩展卡尔曼滤波

InEKFormer的核心是基于不变扩展卡尔曼滤波的框架。InEKF利用机器人状态所在的李群结构，使得线性化过程不依赖于当前状态估计，从而提升滤波器的收敛性和鲁棒性。状态传播步骤通过IMU测量值和strapdown IMU模型对当前状态进行预测：

状态校正步骤则利用李指数映射从传播状态计算校正后的状态：

其中，Kt为卡尔曼增益矩阵，Yt为观测向量，

为选择矩阵。由于本文的混合方法不涉及协方差估计，因此省略了协方差矩阵的传播与更新方程。

3.2 混合状态估计算法

与传统卡尔曼滤波器依赖手工调优的噪声协方差矩阵不同，InEKFormer通过从状态与观测差的历史序列中隐式提取信息来估计卡尔曼增益。该方法适用于存在模型失配和传感器噪声的各类机器人平台。为适应Transformer的固定输入维度，状态矩阵被永久性地扩展为包含左右脚接触信息的SE₄(3)李群形式：

其中，

，

和

分别表示浮动基座在世界坐标系中的旋转、速度与位置，

和

表示左右脚接触点位置。接触状态通过力传感器数据经Sigmoid函数计算得到：

该连续型接触状态表示便于未来引入概率型接触估计器。

3.3 增益估计模型

增益估计模型采用具有编码器-解码器结构的Transformer网络，输入特征包括五类：观测差、新息差、前向演化差、前向更新差以及接触状态。前四类特征在世界坐标系中计算，具有右不变性。特征经过缩放、嵌入后分别输入编码器和解码器，编码器输出观测噪声的隐表示，解码器输出估计不确定性的隐表示，最终通过全连接层解码为初步卡尔曼增益矩阵。接触状态通过后处理方式与增益矩阵结合：

该设计使得模型能够灵活处理不同接触状态，同时保持结构统一。

实验评估

4.1 机器人数据集

为训练和测试InEKFormer模型，研究团队构建了一个包含仿真与真实机器人实验数据的数据集。实验平台为RHS全尺寸人形机器人，配备IMU、关节编码器和六维力/力矩传感器。浮动基座坐标系位于机器人腰部，与世界坐标系通过运动捕捉系统对齐。数据集包含30条真实机器人运动轨迹和14条仿真轨迹，涵盖行走、下蹲、单腿平衡等多种动作，总样本量达575,258个，时间跨度约50分钟。所有数据均公开提供，便于后续研究使用。

4.2 优化与训练

增益估计模型采用监督学习方式训练，损失函数为校正后状态与真实状态之间的Frobenius范数：

训练采用基于时间截断的反向传播与计划采样策略，结合Adam优化器和OneCycleLR学习率调度器。超参数优化使用Tree-structured Parzen Estimator方法。模型性能最终通过在滤波闭环中的估计精度进行评估，包括绝对误差和均方根误差：

4.3 实验结果

实验分为三类：基线测试、单轨迹测试与多轨迹测试。基线测试表明，InEKFormer能够在短序列上有效过拟合，并在后续样本中表现出一定的泛化能力，而KalmanNet在高维数据上表现不佳。自回归训练模式显著优于教师强制模式，误差可降低三个数量级。

单轨迹测试中，InEKFormer在多数状态维度上优于KalmanNet，但在z方向位置和速度估计上仍不及模型驱动的InEKF。KalmanNet在所有高维测试中均未能有效学习运动模式。多轨迹测试进一步验证了数据量对模型性能的提升作用，模型在更多训练数据上表现出更低的RMSE。然而，仅使用仿真数据训练的模型由于仿真-现实差距存在明显的横向振荡现象。

4.4 讨论

实验中发现，自回归训练在大规模数据集上难以稳定进行，因此多数模型采用教师强制模式训练。测试阶段采用一步预测方式以避免暴露偏差累积。此外，传感器采样频率的不一致性可能导致估计轨迹振荡，建议将来将时间间隔作为模型输入，并引入时空注意力机制。在计算效率方面，模型在标准CPU上的推理时间在50–350 ms之间，未来可通过轻量化Transformer结构进一步提升实时性。

结论与未来工作

本文提出了InEKFormer这一结合InEKF与Transformer的混合状态估计方法，并在人形机器人RHS上进行了验证。实验表明，InEKFormer在短轨迹全反馈和长轨迹真值反馈条件下均优于KalmanNet，并在部分任务中超越InEKF。尽管自回归训练尚未完全实现，但该方法展示了Transformer在高维状态估计中的潜力。

未来工作将集中于实现稳定的自回归训练，以支持在线机器人部署；通过改进仿真模型与数据集质量减小仿真-现实差距；并将方法推广至其他机器人平台，如四足机器人、空中机器人等。