精读笔记

Problem Setting

【Proprioceptive Invariant State Estimation for Humanoid Robots on Non-Inertial Ground】（arXiv preprint / 2026）

这篇论文处理的不是一般 legged state estimation，而是一个更窄但很实际的设定：人形机器人站在/走在一个加速、旋转的支撑面上，并且不能假设支撑面上有 IMU、motion capture 或其他外部传感器。估计目标也不是世界系下的绝对状态，而是动态地面坐标系下的基座相对位置和速度，因为控制和规划真正关心的是机器人相对于移动平台、船舶、列车或动态地面的运动。

真正困难点在于经典 contact-aided estimation 的核心约束“stance foot 在惯性系速度为零”在这里失效了。站立足相对于地面可能静止，但地面本身在惯性系中有平动和转动加速度；如果继续按静止地面处理，足端约束会把地面运动误解释成机器人运动，导致位置/速度估计系统性偏差。已有动态地面 InEKF 可以解决非惯性项，但通常把地面运动当作已知输入或依赖地面 IMU，这在真实部署中正是最难满足的条件。因此本文的关键矛盾是：非惯性效应必须建模，但地面运动不能直接测。

Motivation

作者的出发点是：地面运动虽然没有被外部传感器直接测量，但在刚性接触期间，它会通过 stance foot 的 IMU 和足-基座运动学间接进入机器人本体传感器。换句话说，足端 IMU 在接触时不是普通冗余 IMU，而是一个随地面共同运动的传感点；只要接触约束足够可信，它就携带了动态地面角速度和加速度的信息。

已有路线的缺口很明确：静止地面 proprioceptive InEKF 在非惯性地面上模型错；动态地面相对状态估计需要地面 IMU，部署假设重；学习式方法可能能吸收一些扰动，但状态定义多在惯性系，且泛化到非惯性平台没有清晰结构保证。本文想补的是一个结构化、可观测性可分析、只依赖 onboard sensing 的动态地面 estimator。

Core Idea

核心思想是把“未知地面运动”从外部输入改写成“由接触足 IMU + 接触约束 + 运动学隐式恢复的量”。作者将状态定义在动态地面系下，同时把 stance foot 的相对姿态和位置纳入 Lie group 状态。这样一来，地面角速度和加速度引起的非惯性项不再作为不可得输入出现，而是通过足端 IMU 测量、足端相对地面的接触固定关系，以及足-基座 forward kinematics 被写进 process model。

这相当于改变了信息流：prior 的 DRS 是“地面传感器 → 地面运动 → 相对状态”，本文是“接触足传感器 → 隐式地面运动 → 相对状态”。这个 inductive bias 很强：它假设 stance foot 是动态地面的一个临时传感节点。相比直接做更复杂的滤波或学习残差，这个重参数化更可解释，也更容易分析可观测性。但它的 generalization 不是来自数据覆盖，而是来自刚性接触几何约束是否成立。

Method

方法层面最重要的不是滤波流程，而是三个建模选择。

第一，状态放在动态地面坐标系下：base orientation、relative velocity、relative position，加上 stance foot pose。这样估计结果直接对动态环境中的控制有意义，也避免把不可观/不必要的世界系绝对状态当成目标。

第二，用 stance foot IMU 消去地面运动输入。连续接触时，足端相对于地面的位置和姿态近似不变，因此足端 IMU 测到的角速度和加速度可用于构造地面系中的非惯性项。这个步骤解决的是 prior DRS 对外部地面 IMU 的依赖，也是本文最实质的传感配置创新。

第三，把 leg odometry measurement 写成 right-invariant observation。测量本质上是足-基座相对位移，但作者通过足端姿态运动学重写成 InEKF 友好的形式，使观测矩阵具有固定结构。它带来的核心变化是：大初始误差下，误差传播和更新更接近 trajectory-independent 的线性结构，而不是普通 EKF 那种随当前估计漂移的脆弱线性化。

接触切换时，用 forward kinematics 把旧 stance foot 状态切到新 stance foot；这对行走必要，但更像把 contact-aided InEKF 的已有机制迁移到动态地面坐标系下。

Key Insight / Why It Works

最关键的 insight 是：非惯性地面问题不是一定需要直接测地面，而是需要一个与地面刚性绑定的测量点；stance foot 在接触阶段正好可以临时扮演这个角色。足端 IMU 把 ground-induced rotation/acceleration 从“外部未知扰动”变成“接触约束下可写入模型的输入”。这是本文真正有效的原因。

第二个有效点是相对状态定义。若坚持估计世界系绝对状态，仅靠 proprioception 很容易陷入不可观或漂移；而动态地面系下的相对位置/速度正是接触和控制最能约束的量。本文不是让传感器变强，而是把问题投影到更可观、也更任务相关的状态空间。

第三个有效点是 invariant structure。right-invariant measurement 让滤波器在大初值误差下有更好的收敛性质；这解释了为什么它不仅能工作，而且比一些 baseline 收敛更快。但实验中 FRS 相比 DRS 的增益归因不完全纯粹：它同时改变了传感来源、测量结构、运动学信息使用方式和噪声处理。文中说更快收敛可能来自同时利用位置和旋转 forward kinematics，这个解释合理，但增益来源不清。

哪些是核心贡献？足端 IMU 替代地面 IMU、并保持 invariant measurement structure 是核心。接触切换、低通滤波、冲击缓解、协方差调参是必要 engineering。walking case 中对足端冲击噪声的处理很关键，但这说明方法对足端 IMU 质量和接触冲击非常敏感；这部分性能不能完全归功于理论模型。

Relation To Prior Work

这篇最接近两条线：Hartley 等 contact-aided InEKF 的静止地面路线，以及 He/Gao/Gu 系列在非惯性地面坐标系下做 invariant filtering 的动态地面路线。它不是从零发明新的滤波理论，而是在已有 InEKF 框架中换了一个关键传感闭环：用足端 IMU + 接触约束替代 ground-mounted IMU。

相对静止地面 InEKF，实质差异是承认 stance foot 在惯性系不静止，并显式建模地面旋转/加速度诱导项；因此它不会把地面运动硬塞进 base motion。相对已有 DRS，实质差异是去掉地面外部仪器，把地面运动信息内生化。相对多 IMU proprioceptive 方法，它不是简单多加 IMU 降漂，而是给 foot IMU 一个明确的非惯性地面观测角色。

看似新的地方中，Lie group / right-invariant update / contact switch 很多是已有思想重组；真正新增的信息是：在动态地面问题中，足端 IMU 在接触时足以恢复一部分地面非惯性效应，并能导出相对位置/速度的观测性条件。

Dataset / Evaluation

评估的优点是真机 Digit、真实 IMU/编码器、动态 treadmill、站立/下蹲/行走、多初值误差，并且与静止地面 InEKF 和需要地面 IMU 的动态地面 InEKF 对比。它确实验证了本文最核心的 deployment claim：不装地面 IMU，也能在受控非惯性地面上估计相对位置和速度。

但 evaluation 覆盖仍偏窄。动态地面主要是低幅、低频、低维的周期 sway/pitch；这和船舶甲板、车辆平台、多轴随机运动、软接触地面之间还有明显鸿沟。行走实验只有单轴 rotating ground，且 foot IMU 冲击噪声需要额外机械/滤波缓解。观测性实验基本支持理论，但某些理论不可观分量在实验中也收敛，作者将其归因于非理想激励；这说明真实系统里存在额外未建模激励，但也意味着 benchmark 不能干净地区分理论可观性与实验偶然性。

总体上，实验足以证明“这个建模方向有效”，但不足以证明“在广义非惯性真实环境中鲁棒可部署”。

Limitation

最大限制是刚性接触假设。方法把 stance foot 当作地面运动的代理传感点；一旦存在显著滑移、滚动、软接触、局部形变或冲击导致足端 IMU 与地面运动不一致，核心机制就会退化。文中把滑移/接触不确定性作为噪声处理，但这不是从根上解决问题。

第二，观测性依赖足端角速度激励。若地面运动退化到某些单轴情形，部分位置分量理论上不可观；实验中的“实际收敛”来自非理想激励，不能当作保证。部署时如果平台运动维度不足，可能需要主动激励或额外传感。

第三，base orientation 在 nominal 配置下不可观。三 torso IMU augmentation 可以重建姿态，但这已经改变了传感配置；它证明了一个可行扩展，不等于原 FRS 天然 full-state observable。

第四，bias、同步、安装误差和足端冲击处理被低估。文中未显式建模 IMU bias，而是依赖离线校准和 covariance tuning；在长时部署中这会成为系统性误差源。FRS 的性能上限很可能由 foot IMU 安装质量、滤波预处理和接触检测决定，而不只是 InEKF 理论。

Takeaway

• 1. 非惯性地面状态估计的关键不是一定要估计世界系绝对状态，而是选对任务相关的动态地面相对状态；这个重参数化本身提高了可观性和控制相关性。
• 2. 接触足可以被看作一个临时地面传感节点。
• 这个 insight 可迁移到其他接触机器人问题：当环境不可直接测量时，稳定接触点可能提供环境运动的隐式观测。
• 3. InEKF 的价值在这里不是“更高级的 EKF”，而是让这个强几何约束模型在大初值误差下仍有合理误差结构。

一句话总结

这篇论文是 contact-aided InEKF 向非惯性地面场景的一次关键传感闭环重构：它把 stance foot IMU 变成隐式地面运动观测源，从而在不安装地面传感器的情况下估计动态地面系下的相对状态。