精读笔记

Problem Setting

论文标题：Hierarchical Incremental MPC for Redundant Robots: A Robust and Singularity-Free Approach（IEEE Transactions on Robotics / 2024）。

这篇论文实际处理的是冗余机械臂的多任务严格优先级 torque control：高优先级任务必须在任务冲突时保持优先，不允许被低优先级任务用权重折中掉；同时控制输入和关节状态必须满足物理约束；还要在动力学模型不准、Jacobian 奇异或 prioritized Jacobian 奇异时不崩。

关键矛盾是：严格层级通常依赖 null-space projection / dynamically consistent inverse，而这些东西天然引入矩阵逆和奇异性；约束处理通常依赖 QP/MPC，但如果保留真实机器人动力学和任务空间非线性，就会变成 NMPC/NLP，难以 1 kHz；鲁棒性通常需要模型学习、SMC 或 adaptive control，但这些又会破坏层级结构或增加计算复杂度。

因此这不是单纯“把 MPC 用到冗余机器人”问题，而是如何把严格层级、约束、鲁棒性和实时性放进同一个 torque-level 控制框架里。以前方法卡住的地方正是这四者无法同时满足：OSF 强在动态一致性但弱在约束和模型误差；HQP 强在层级和约束但短视且数值奇异；NMPC 强在 horizon 但实时性和模型依赖不可接受。

Motivation

作者的核心观察是：在高频控制下，与其在线计算复杂的非线性动力学，不如用上一采样时刻的输入/输出吸收未知项，直接构造一个 torque increment 到加速度变化的局部增量模型。这样可以把不确定动力学从显式建模问题转成短时连续性假设问题。

已有路线缺的不是某个单独模块，而是一个能同时满足以下条件的表示方式：1）不需要精确 inertia/Coriolis/gravity/friction；2）不需要 Jacobian 相关逆；3）预测模型线性，OCP 可变成 QP；4）任务层级是 hard constraint 而不是 weight tuning；5）能显式放入 torque、position、velocity constraints。

这解释了为什么作者选择 TDE + incremental MPC + hierarchical QP：TDE 负责弱模型依赖，incremental model 负责线性化和输入平滑，层级 QP 负责 hard priority，MPC horizon 负责摆脱 HQP 的单步局部性。

Core Idea

真正核心思想是把任务优先级控制从“在 null-space 中寻找低优先级动作”改成“要求低优先级动作在所有高优先级任务空间中产生与高优先级最优动作相同的作用”。形式上，高层求得 \Delta\tau_p^* 后，低层必须满足 J_p \bar{M}^{-1}\Delta\tau_i = J_p \bar{M}^{-1}\Delta\tau_p^*。这等价于保留高优先级任务的预测加速度增量效果，但不需要构造 null-space projector，也不需要求 prioritized Jacobian 的逆。

另一个核心改变是建模方式：不是用 nominal nonlinear dynamics 做预测，而是用 TDE 得到 \ddot{x}_i \approx \ddot{x}_{i,0}+J_i\bar{M}^{-1}\Delta\tau。这个 inductive bias 很强：系统在 1 ms 内未知动力学变化小，上一时刻未知项可以近似当前未知项。它把鲁棒性建立在高频短时平滑性上，而不是建立在模型辨识或学习泛化上。

本质区别在于信息流重组：过去是模型 -> operational-space dynamics -> inverse/projection -> torque；这里是历史测量和上一输入 -> 增量预测模型 -> 多层 QP -> torque increment。动态模型从主角变成一个被延迟信号补偿的残差项。

Method

1）TDE 增量任务动力学：解决模型不准和非线性预测复杂的问题。作者把未知动力学、扰动、\dot{J}\dot{q} 等项 lump 到 H 中，用上一采样时刻估计 H，再得到 torque increment 到任务加速度的线性关系。核心变化是预测模型不再需要 M,C,G,F，也不再需要计算 operational-space inertia。

2）关节空间增量模型：解决 MPC 中状态约束预测的问题。任务空间模型用于优化任务误差，关节空间模型用于预测 q、\dot{q} 是否越界。这里的关键是约束仍然能保持线性，因为 joint prediction 也用增量模型写成线性系统。

3）stage cost 设计为目标误差动力学 + torque increment 正则：解决跟踪、输入平滑和 Hessian 正定性。这里不是简单 regularization，而是利用 incremental input 让 R 项有清晰意义：抑制 torque jump，同时保证 QP Hessian 正定，避免 Jacobian 奇异导致半正定 Hessian 的数值问题。

4）多层 OCP 顺序求解：解决严格任务优先级。每一层求一个 QP，低层继承所有高层的“任务空间增量作用”等式约束。任务优先级不是靠权重大小，也不是靠 soft inequality，而是 hard equality。

5）上一时刻预测 Jacobian 固定化：解决实时性和凸性。若 horizon 内实时更新 Jacobian，问题会变成非线性甚至非凸；作者用上一次最优 joint prediction 近似当前 horizon 内 Jacobian，使 OCP 保持 QP。这是一个明确的工程-理论折中：牺牲一部分预测精度换取凸性、可解性和实时性。

Key Insight / Why It Works

最重要的有效性来源是高频增量建模，而不是 MPC 本身。TDE 使未知动力学以“慢变化残差”的形式被上一时刻抵消；incremental input 使控制问题局部线性化；高频 1 kHz 让这个近似通常足够好。也就是说，论文的鲁棒性不是来自 robust optimization，而是来自 fast feedback + time-delay cancellation。

第二个核心贡献是任务层级的表达方式。传统动态一致性依赖精确 M 和 null-space projector；HIMPC 用 \bar{M} 和 equality constraints 保留高优先级任务的输入-输出增量作用。它绕开了 inversion singularity，但并没有神奇地消除物理奇异性：在 kinematic singularity 处可控自由度仍会丢失，只是优化问题不会因为求逆或 Hessian 退化而数值爆炸。论文中“singularity-free”应理解为 controller formulation singularity-free，而不是机器人任务能力在奇异点不受影响。

第三个有效性来源是 convexification。把 horizon 内 Jacobian 固定为上一轮预测值，本质上是 RTI/SQP 精神的一种保守线性化，但作者没有迭代求解，而是每步求一个 QP。这个近似使问题非常稳定，也解释了为什么 NHIMPC 未必更好：非凸 NLP 更新 Jacobian 虽然模型更准，但更容易陷入局部解且计算慢。

哪些部分可能只是辅助：MPC horizon 带来的 smoother torque 和较低平均 cost 是合理但不是根本创新；N=5 在真机上有效更多是工程尺度合适。QP Hessian 正定证明也主要依赖 R_i \succ 0，是必要保证但不是深层理论突破。真正值得迁移的是“用增量输入作用等式表达层级”，而不是具体 cost 或实验参数。

这篇不是 scaling/data-driven 工作，能力不来自数据覆盖、retrieval 或隐式记忆；它属于 better inductive bias + test-time optimization。核心 inductive bias 是短时动力学连续性和高频反馈可补偿模型误差。

Relation To Prior Work

最接近的谱系有三条：TDE-based operational space control、HQP/lexicographic task priority control、MPC for redundant manipulators。HIMPC 实际上是这三条线的重组，但重组点很关键。

相对 OSF：它保留了动态一致性的目标，但放弃 null-space projection 和 operational-space inverse。OSF 中低优先级任务通过投影进入高优先级 null space；HIMPC 中低优先级任务通过等式约束匹配高优先级任务作用。这是本质差异。

相对 HQP：它同样用多层 QP 表达优先级，但 HQP 通常是 one-step 或 instantaneous optimization，容易局部短视且在 Jacobian 退化时 Hessian/约束数值问题明显。HIMPC 的新增信息是 horizon prediction 和 TDE 增量模型。严格地说，多层 QP + equality priority 并非全新思想，新的是把它与 TDE incremental prediction 结合到 torque-level MPC 中。

相对 NMPC：它不是更强大的模型预测，而是更可部署的近似。作者选择牺牲非线性预测精度，换取 QP、凸性和 1 kHz。与学习型 MPC 相比，它不追求在线辨识模型，而是用时间延迟信号做局部补偿，计算量和调参复杂度低很多。

实质创新在于：TDE 增量模型不仅用于鲁棒控制，还被用来构造线性 MPC 预测和层级等式约束，使 robust、hierarchical、constraint-aware、singularity-avoidant 四个目标在一个 QP 框架下闭合。

Dataset / Evaluation

评估设计基本围绕核心 claim 展开。3-DoF planar 真机验证实时 torque-level 控制、模型误差鲁棒性、任务冲突时层级保持、输入约束处理，以及穿越运动学奇异点时控制器不崩。7-DoF KUKA 仿真验证更高 DoF 和三层任务层级，并补充 horizon、Jacobian approximation、测量分辨率影响。

真机部分有价值，因为论文 claim 很依赖采样率、传感器噪声和求解器实时性；只做仿真不足以说明 TDE-MPC 可部署。对比对象也基本合理：OSF/HQP/TDEOSF 分别对应模型型层级、优化型层级、TDE 型层级。

但 evaluation 也有明显边界：任务主要是自由空间轨迹跟踪，没有系统验证接触突变、强外扰、payload 快速变化或低采样率退化；严格状态约束并未被完全支持，实验中 velocity constraint 有轻微违反；7-DoF 情况下计算时间已经超过 1 ms，说明 scalability claim 需要谨慎理解。论文证明了该机制在中低维/短 horizon 下有效，但没有证明它可直接扩展到 humanoid 级高维多接触任务。

Limitation

最大限制是 TDE 误差。整个框架把未知动力学和外扰都压进上一采样时刻可近似的残差中，这要求采样足够快、状态测量足够干净、外扰变化足够慢。遇到冲击接触、摩擦突变、通信延迟或粗编码器时，TDE error 会直接破坏预测和层级约束。

第二，约束保证不是严格 robust。OCP 中使用 nominal incremental system，未显式考虑 uncertainty tube；因此状态约束可能违反。作者也承认 tube-based MPC 不直接适用，因为任务空间优化和关节空间约束之间缺少 joint reference。未来若要用于安全关键场景，需要 stochastic/tube/data-driven uncertainty handling，而不是只依赖高频近似。

第三，\bar{M} 的选择仍是手调。文中给了稳定性直觉和调参流程，但不是系统化设计；过小会放大 TDE 误差，过大会噪声响应。增益来源不清，实际部署时仍需要经验。

第四，scalability 有上限。任务层数越多，QP 串行求解越多；horizon 越长，计算时间快速上升。7-DoF 三层任务的普通实现已经不能 1 kHz，说明“linear MPC”不等于高维实时无忧。

第五，“singularity-free”表述容易过强。它避免的是算法中的逆和数值奇异，而不是让机器人在奇异构型拥有不存在的控制自由度。奇异点附近任务误差仍会上升，只是控制器不会因求逆发散。

Takeaway

1）值得记住的不是 TDE 本身，而是 TDE 把复杂不确定动力学变成可用于 QP-MPC 的线性增量模型；这给 robust MPC for robots 提供了一条非学习、低计算的路径。
2）严格任务优先级可以不通过 null-space projection 表达；用“高优先级任务空间作用不变”的等式约束表达层级，是可迁移到其他 hierarchical optimization/control 问题的关键 insight。
3）这篇推动的是一种工程上更可部署的层级 MPC：牺牲全非线性预测精度，换取凸性、实时性和奇异性鲁棒。
未来方向不应是把模型再做复杂，而应是给 TDE/MPC 加上可计算的不确定性边界和严格约束保证。

一句话总结

HIMPC 是一篇把 TDE 增量建模、层级 QP 和 MPC horizon 组合成可实时 torque-level 冗余机器人控制器的工作，真正贡献在于用线性增量作用约束替代 null-space/inverse 来实现鲁棒、约束感知且算法奇异性规避的严格任务优先级控制。