精读笔记

Problem Setting

《Whole-Body Impedance Model Predictive Control for Safe Physical Human--Robot Interaction on Floating-Base Platforms》（arXiv preprint / 2026）处理的不是一般的 loco-manipulation，而是一个更窄但更尖锐的问题：浮基机器人在保持刚性接触和平衡的同时，手臂末端要对持续人机接触力表现出可控阻抗并消除稳态误差。

关键矛盾是控制权限分配。传统 legged WBC/MPC 会优先满足接触、质心、动量和姿态，手臂交互力被视为需要压掉的 disturbance；而 pHRI 需要把这个 disturbance 变成受控交互通道。fixed-base impedance MPC 则解决了另一半问题：末端交互下的 predictive disturbance rejection，但它假设基座刚接地，操作空间惯量用 M^{-1}，在浮基和足端约束下会错估末端输入通道。

所以困难点不是“加一个手臂任务”，而是：在不破坏 balance stack 的情况下，给手臂留下一个物理一致、可预测、可高频求解的残差动力学模型。以前方法卡在两边：locomotion stack 没有零稳态 pHRI 通道；impedance MPC 没有 contact-consistent floating-base dynamics。

Motivation

作者的动机来自一个很实际的结构缺口：现有全身控制架构通常已经非常成熟，没必要为了 pHRI 推翻 centroidal MPC + WBC；但如果只在 WBC 中加 PD/阻抗手臂任务，持续外力下必然有偏置。如果引入积分项，又容易和接触切换、力矩饱和、浮基耦合纠缠。

论文的核心观察是：WBC 的高优先级任务其实可以先把接触和平衡“结算掉”，之后剩余 null space 仍可用于手臂交互；只要这个 residual plant 用 contact-consistent inertia 表达，fixed-base Impedance MPC 的核心结构就能复用。缺的不是一个更大的一体化 nonlinear MPC，而是一个能插进既有 balance stack、且不会污染高优先级任务的 predictive impedance layer。

这也是本文和很多 unified loco-manipulation MPC 的出发点差异：它不追求把所有东西放进一个大优化，而是承认 locomotion 和 pHRI 的时间尺度、约束性质不同，然后用 null-space 和模型线性化把二者隔离。

Core Idea

核心思想可以压缩为一句话：把浮基手臂末端在接触约束下的复杂动力学，通过 contact-consistent feedback linearization 变成接触模式内的局部双积分器，再在这个双积分器上跑带 disturbance state 的 Impedance MPC。

真正改变的是建模方式。fixed-base 方法里末端 apparent inertia 来自 (J M^{-1} J^T)^{-1}；本文把它替换为 (J \bar{M}^{-1} J^T)^{-1}，其中 \bar{M}^{-1} 已经投影掉接触约束方向。这样足端支撑对手臂末端惯量的影响进入输入矩阵 B_d，而状态转移 A_d 仍保持双积分器常矩阵。这个分解非常关键：复杂性被放进 B_d / mode index，MPC 的 lifted cost 结构基本不变。

它引入的 inductive bias 是“高优先级全身动力学由 WBC 解决，低优先级手臂交互只看 contact-consistent residual channel”。这比端到端 full-body NMPC 更可扩展，因为 QP 小、可预计算、可高频；也比传统 WBC 更强，因为它不只是瞬时反馈，而是把外力估计沿 horizon 传播，提前 preload corrective force。

Method

1. Contact-consistent residual plant：解决的是 fixed-base 操作空间模型在浮基接触下输入通道错误的问题。通过 \bar{M}^{-1} 定义 \Lambda_{arm}，末端动力学不再假设基座无限刚，而是显式包含支撑约束对 apparent inertia 的影响。核心变化是把“浮基耦合”从未知扰动的一部分变成预测模型的一部分。

2. Null-space placement：解决的是 pHRI 控制和 balance/contact 控制抢控制权的问题。MPC 力只通过高优先级任务的 contact-consistent null space 注入，因此理论上不干扰接触维护和平衡任务。这里的安全性主要来自任务优先级投影，而不是来自 cost 权重调得足够大。

3. Constant-A_d Impedance MPC：解决的是实时性问题。反馈线性化后 A_d 是双积分器常矩阵，B_d 吸收 \Lambda_{arm}^{-1}。这让 QP Hessian / lifted matrices 可按 contact mode 预计算，在线只更新线性项和约束。1 kHz 的主要来源就是这个结构压缩。

4. Kalman disturbance augmentation：解决的是持续外力下稳态误差问题。论文把 pHRI force、leg momentum variation、SRBD approximation error 都塞进一个慢变/常值 disturbance state，并在 horizon 内传播。机制上等价于在预测模型中加入积分扰动通道，而不是事后加 I gain。

5. Contact switch protocol：解决的是 B_d、\Lambda_{arm}、\bar{M}^{-1} 在接触切换时跳变的问题。保留 disturbance estimate、膨胀 covariance，让滤波器快速重收敛。这个部分更偏工程，但对于把固定接触理论扩展到多接触模式是必要的。

Key Insight / Why It Works

最核心的有效性来自两个结构性事实，而不是来自调参。

第一，contact-consistent inertia 让末端输入通道对了。对于浮基机器人，末端力和加速度之间的关系不是 fixed-base 的 J M^{-1} J^T，而是受足端约束投影后的 J \bar{M}^{-1} J^T。D3 fixed-base MPC 在仿真中退化，说明模型错配会直接污染 Kalman disturbance estimate：滤波器无法区分真实人力和错误动力学造成的残差，最后所谓 disturbance rejection 反而补偿错方向。这一点是本文最实质的理论贡献。

第二，常 A_d 双积分器结构把高频 MPC 从“全身动态优化”降维成“小规模残差力优化”。这不是 scaling 或 data coverage，而是更强的 inductive bias：只保留与 pHRI 相关的误差状态，把姿态、接触、质心这些高维变量交给上层。这样做的代价是依赖上层真的能稳定隔离这些变量；收益是 QP 可预计算、更新率高、disturbance state 可干净地进入 prediction。

Kalman augmentation 是零稳态误差的关键，但不是全新思想；它本质上是 internal model principle / disturbance observer / offset-free MPC 在 contact-consistent residual plant 上的应用。真正新增的信息在于：这个 offset-free MPC 被放进了 floating-base WBC residual null space，并且用 \bar{M}^{-1} 保证 disturbance estimate 不被接触约束模型错误污染。

contact-mode-indexed library 和 covariance inflation 更像辅助机制。它们能降低接触切换瞬态，但不是性能主因；在固定接触场景中 D6 和 D7 等价也说明 inflation 只对 mode switch 有边际作用。论文的主要增益不来自更大 horizon 或 solver trick，而来自 representation alignment：让 MPC 看到的低维模型和真实接触约束下的末端动力学对齐。

Relation To Prior Work

这篇论文处在三条谱系的交叉处：Khatib/Sentis 操作空间与层级 WBC，Di Carlo/Kim 式 centroidal MPC + inner WBC locomotion stack，以及作者自己 fixed-base Impedance MPC / offset-free MPC 的延伸。

和 locomotion-centric WBC/MPC 的本质差异是控制目标的角色变化：prior 把外部手臂力看成破坏 balance 的扰动，本文把它作为 residual null space 中需要主动调节的交互变量。它不是把 locomotion MPC 做得更强，而是在 locomotion stack 停止之后再接一个 pHRI optimizer。

和 unified loco-manipulation MPC 的差异是优化边界。Sleiman 等路线倾向于把 locomotion 和 manipulation 一起放进 receding-horizon full-body formulation；本文则刻意避免大优化，把 pHRI 降成 contact-consistent double-integrator QP。因此它牺牲了一部分全局协调能力，换来高频、结构清晰和易接入现有 WBC。

和 fixed-base impedance MPC 的差异最实质：M^{-1} → \bar{M}^{-1}，以及 contact-mode-indexed B_d。其他如 Kalman augmentation、QP 预测、零稳态误差证明，很大程度是已有 fixed-base 思想迁移。真正新增的是证明这种迁移在浮基刚性接触下仍可通过 contact-consistent linearization 保持常 A_d 结构。

因此本文看似有很多模块，其实是已有思想的有约束重组；实质创新在于模型接口，而不是 MPC 形式本身。

Dataset / Evaluation

评估是纯仿真，MuJoCo 中包含一个简化 17-DOF 双足和 Unitree G1 官方模型。任务覆盖主要是固定双支撑下的 step pHRI、周期冲击，以及 G1 固定站立复现实验；没有真实硬件，也没有真正动态行走中的接触序列验证。

这些实验足够支持两个局部 claim：contact-consistent dynamics 比 fixed-base dynamics 更适合浮基支撑场景；Kalman disturbance augmentation 对恒定外力稳态误差有效。尤其几个 ablation 的排序很干净：无 Kalman 不能归零，用 M^{-1} 会错，hierarchy+PD 仍有稳态误差。

但评估没有充分支持更强的 claim：比如“动态 walking base 下也保证零稳态误差”、“contact transition protocol 对真实足端冲击足够鲁棒”、“1 kHz torque-MPC 在真实 G1 上可达到简化模型性能”。G1 实验反而暴露执行器接口问题：用位置执行器近似力矩后，稳态误差仍有毫米级，改善幅度远小于简化双足。这里不能把仿真中的高频 QP 成功直接外推到真机 torque mode。

Limitation

最大限制是成立前提很硬。零稳态误差证明依赖固定接触模式、bounded/asymptotically constant disturbance、\Lambda_{arm} 非奇异、输入约束不把系统推出 invariant set、支撑多边形有效、接触刚性且无滑移。只要其中任何一个破坏，理论结论会迅速变成经验性控制策略。

第二，论文把很多复杂耦合压进 disturbance state，但这也可能是在转移问题。pHRI force、leg momentum error、SRBD approximation error 被同一个 3D disturbance 表示；当这些误差不是常值、而是和 gait phase、base acceleration、人动作耦合时，Kalman state 未必能分辨来源。所谓 disturbance isolation 在动态场景下文中未充分说明。

第三，null-space guarantee 是理想动力学和精确投影下的。实际机器人存在 torque limits、latency、state estimation noise、contact compliance、model error，投影并不一定保证 arm MPC 完全不影响 balance。论文对这一层的鲁棒性讨论不足。

第四，contact transition 只处理了已知 mode switch 后的 matrix reload 和 covariance inflation，没有处理接触检测错误、冲击动力学、短暂多接触/滑移、摩擦锥 violation。这个协议更像工程 heuristic，有用但不等于完整 switched hybrid stability proof。

第五，1 kHz 性能主要来自强结构化简化和预计算，而不是求解器或控制理论本身的突破。对于更高维末端任务、多臂协作、手部接触、多点环境接触，B_d library 和 residual double-integrator 假设的 scalability 上限不清楚。

Takeaway

• 1. 最值得迁移的 insight 是：不要把 pHRI 直接塞进全身大 MPC；先用高优先级 WBC 解决接触和平衡，再在 residual null space 中构造低维、物理一致的 offset-free MPC，可能是更可部署的路线。
• 2. 对浮基操作任务，contact-consistent apparent inertia 不是细节。
• 只要末端控制器的输入通道仍用 fixed-base M^{-1}，后端再强的 Kalman/MPC 都可能在补偿模型错误。
• 3. 零稳态误差的来源不是“阻抗参数自适应”，而是 internal-model/disturbance-state 被放进了正确的 residual plant。

一句话总结

这篇论文的价值在于把 fixed-base offset-free Impedance MPC 通过 contact-consistent operational-space modeling 接入浮基 WBC residual null space，是一次结构上很干净的模型接口升级，而不是一个全新的全身 MPC 框架。