精读笔记

Problem Setting

《Continuously Shaping Prioritized Jacobian Approach for Hierarchical Optimal Control With Task Priority Transition》（IEEE Transactions on Robotics / 2025）实际处理的是 redundant robot hierarchical optimal control 中的在线 priority transition。问题不是“多个任务怎么排序”，而是当排序本身需要在线改变时，如何避免 strict hierarchy 控制律中 prioritized Jacobian、dynamic consistent projection、task-space dynamics 和 torque 同时发生结构突变。

真正困难点在于 strict hierarchy 的好处来自离散结构：高优先级任务先占据 range space，低优先级任务被投到其动态一致零空间中。一旦任务从高层移到低层，或者插入/移除任务，某个任务前面突然多出一串 projection，伪逆对象、投影维度、rank 结构都会变。这不是简单 smoothing torque 可以解决的，因为 torque smoothing 会破坏层级最优跟踪的闭环结构。

以前方法大致卡在两个极端：soft priority 连续但层级语义弱，任务间互相污染；HQP / continuous inverse 能保持更强层级语义，但计算和稳定性分析都重；SVD shaping 连续但控制的是 singular-vector directions，不是任务方向，并且动态一致性不足。本文要解决的核心矛盾就是：既要 priority 可连续变化，又要在 strict periods 回到真正的 hierarchical optimal controller。

Motivation

作者的出发点很明确：动态环境中 priority stack 不可能固定，尤其 safety-related task 需要按场景升降优先级；但已有 strict hierarchy dynamic control 默认 priority ordering 不变。硬切换会导致 torque discontinuity 和切换边界振荡，这在 torque-controlled robot / HRI 场景里是实质问题。

关键观察是：不连续的源头不是任务误差，而是动态一致投影项 \(I-\bar J^T\bar J^{M+,T}\) 的突变。换句话说，priority transition 本质上是“某些任务方向逐渐进入或退出高优先级 range space”的过程。如果能连续塑形这个 projector，并且塑形变量有清晰 priority 语义，就可以保留 strict hierarchy 的端点性质，同时让中间过程连续。

因此本文缺的不是另一个 weighted sum controller，而是一套能表达“非严格过渡态”的 hierarchy parameterization，以及一个在任务方向上连续开关投影维度的算法。

Core Idea

核心思想是把离散任务栈提升为连续 priority network：用矩阵 \(\Psi\) 表示每个任务在每个 hierarchy level 上的占用程度。strict hierarchy 时 \(\Psi\) 是 0/1 阶梯矩阵；transition 时 \(\Psi\) 中部分元素连续变化。这样，priority transition 不再是 task order 的离散重排，而是各任务方向在不同层级上占用自由度的连续变化。

与 soft priority 的本质差异在于，本文不是把多个 task controller 的输出做权重混合，而是在 prioritized Jacobian 的构造中连续塑形 null-space projector。与 SVD shaping 的本质差异在于，它使用 QRD 对加权任务 Jacobian 行空间做正交化，activation 对应的是任务空间方向，而不是配置相关的右奇异向量。这个 inductive bias 更贴近 hierarchy control 的几何结构：优先级本来就是关于任务约束方向占据自由度的，而不是关于某个随构型漂移的 latent singular direction。

因此它重新组织的信息流是：priority parameter → projector shaping → continuous prioritized Jacobian → hierarchy-consistent error dynamics → torque。这个路径比直接 blend torque 更容易保留端点 strict optimality，也比枚举所有 task order 更 scalable。

Method

第一，priority parameterization \(\Psi\)。它解决离散 lexicographic ordering 无法描述 transition state 的问题。每列对应一个任务，每行对应一个层级，元素表示该任务在该层级的激活/占用程度。strict hierarchy 是 0/1，任务同级对应相同列模式，任务移除对应全 0 列。核心变化是：priority 变成连续可插值对象，而不是 permutation。

第二，QRD-based continuously shaping projection。对 \((J N^T L^{-1})^T\) 做 modified QRD，得到任务方向的正交基 \(Q\)，再通过 \(L^T(I-QAQ^T)L^{-T}\) 构造加权/动态一致的 shaping projection。这里 \(A\) 的对角元来自 \(\Psi\)，控制对应任务方向是否占据 range space。它解决的是投影项突然加入/移除导致的不连续，同时避免 SVD shaping 控制 singular directions 的错位。

第三，recursive continuous prioritized Jacobian。算法逐层处理，只选择 \(\Psi\) 在当前层发生变化的任务，累加 \(\alpha J_i N^T\)，并更新 shaped null-space projector。这个递归结构保留了传统 prioritized Jacobian 的 top-down 逻辑，但允许每层的占用是 fractional。严格 0/1 时可证明等价于传统动态一致 prioritized Jacobian。

第四，continuous control law。控制器沿用 hierarchical optimal tracking 形式，把 \(\bar J\) 替换为 \(\bar J^c\)，并重算 dynamics 和 hierarchy-consistent error。为了避免坐标变换 \(B\) 自身不连续，作者构造 \(B^c\) 在切换前后投影链之间插值。这个部分更像必要补丁：它保证 torque 连续，但过渡期并不等价于某个严格 hierarchy 的真实坐标变换。

第五，bounded stability during transition。strict periods 继承前作的渐近稳定/层级最优性；transition periods 因为非严格优先级产生双向耦合，作者不再证明收敛，而是将其视为 lumped disturbance，用 robust damping 证明闭环信号有界。这是合理但保守的理论边界。

Key Insight / Why It Works

最核心贡献是 projector-level continuation。优先级切换不应该在 torque 层或任务误差层做平滑，而应该在“哪些任务方向被投影掉”这个几何对象上做连续形变。因为 hierarchical control 的不连续正是由 projector rank / range 的突变导致的。只要 projector 连续，prioritized Jacobian、task-space inertia、反馈项和 torque 就有机会连续。

QRD 选择是关键，不只是工程替换 SVD。SVD shaping 的 activation 作用在右奇异向量上，这些方向依赖构型且接近奇异时可能剧烈变化；QRD 对任务行空间做正交化，更接近“任务方向占据自由度”的语义。这个 representation alignment 是本文比已有 continuous projection 方法更有说服力的地方。

另一个有效点是端点一致性：transition 中间允许非严格、允许耦合，但 \(\beta=0/1\) 时严格退化到传统 prioritized Jacobian。这使得论文不用重新发明整个 hierarchical optimal controller，而是把已有 strict controller 的有效区间扩展到动态 priority network。换句话说，真正的 optimality 来自前作的 strict hierarchical optimal control；本文新增的是让两个 optimal regimes 之间能连续连接。

需要直接判断的是：过渡期的“optimal control performance”表述有些强。transition 期间 \(\bar J^c\) 对应非严格层级，\(B^c\) 还是插值近似，作者证明的是 boundedness，不是严格意义上的 hierarchy-consistent optimality 或 asymptotic tracking。最实质的贡献是 continuous transition + strict endpoint equivalence，而不是 transition 期间也保持同等最优性。

哪些可能只是辅助：Algorithm 3 对 \(B^c\) 的构造、robust damping 项、仿真中 β 的五次多项式设计，更多是为了闭合控制律连续性和稳定性分析。它们重要但不是本质 insight。本质 insight 是“用 priority matrix 驱动 QRD task-direction activation，从而连续塑形动态一致 projection”。

Relation To Prior Work

这篇属于 null-space projection / operational-space dynamic hierarchical control 谱系，而不是 HQP 谱系。它继承了动态一致投影、prioritized Jacobian、hierarchy-consistent error 和 hierarchical optimal tracking controller。相对作者前作，新增的是 priority transition 的连续化机制；相对经典 strict projection，新增的是可连续变化的 activation；相对 soft priority，保留了 strict endpoints 的层级语义。

和 HQP 相比，本质差异不是是否能表达 hierarchy，而是控制律结构和稳定性可分析性。HQP 可以处理 inequality constraints，但切换和闭环分析复杂；本文牺牲一部分 constraint generality，换来解析 projector 和 Lyapunov-style boundedness 分析。

和 continuous inverse / intermediate desired value 方法相比，本文避免枚举所有 task order 或 task permutation，计算复杂度更接近线性递归。这个 scalability claim 相对可信，因为它不需要对所有组合求伪逆。

和 Dietrich-style SVD shaping 相比，实质创新是 shaping basis 从 singular vector directions 换成 QRD task-space directions，并且引入惯性权重以维持动态一致。这个变化不是简单数值优化，而是把 activation 的语义对齐到任务方向。

和 generalized projection + PD control 相比，本文的优势在于反馈对象是 hierarchy-consistent error，而不是普通 task error 经 projection 间接控制。因此它更直接服务于 constrained optimal trajectory tracking。实验中 hierarchy-consistent error 更低并不意外，因为目标函数本身就对齐了这个指标。

Dataset / Evaluation

evaluation 覆盖了两个层面：仿真验证多任务冲突、重排、插入/移除，以及真机验证 torque-controlled Franka 上的实时可行性。任务设计包含 end-effector tracking、elbow avoidance、joint-limit avoidance，能体现 priority transition 在 HRI-like scenario 中的意义。真机 1 kHz 控制和多线程动力学计算说明实现不是纯离线演示。

实验确实支持本文最核心的两个 claim：一是 priority transition 期间 torque 连续；二是 strict hierarchy 区间 hierarchy-consistent error 能收敛或保持较小。与 generalized projection / successive projection 方法的对比也基本说明：如果 evaluation metric 是 hierarchy-consistent error，本文的控制结构更匹配。

但 evaluation 的边界也明显。切换时刻和 priority matrices 都是手工预设，并没有验证从 perception / planning / safety monitor 自动触发的复杂场景。β transition duration 较温和，未展示快速切换或频繁切换的极限。实验没有系统扫过奇异附近、rank 变化、模型误差、外力扰动或接触任务。因此 evaluation 支持“方法可行且比若干 baseline 更平滑/更对齐层级误差”，但没有完全支持“复杂动态环境中通用可靠”的强 claim。

Limitation

第一，理论保证的上限比较清楚：strict periods 有渐近 tracking / hierarchy-consistent optimality，transition periods 只有 boundedness。过渡期任务互相耦合是被接受并用 disturbance 处理的，不是被消除的。因此它没有真正解决“连续切换过程中仍严格层级最优”的问题，而是解决“两个严格层级之间连续且有界地过渡”。

第二，方法依赖常秩和非奇异假设。文中要求 \(\bar J^c\) 列满秩、子任务不穿越奇异；modified QRD 通过阈值处理相关方向，但接近 rank change 时连续性和数值稳定性仍是脆弱点。文中未充分说明在 rank threshold 抖动时如何避免 projector basis 跳变。

第三，\(B^c\) 是插值构造，不是 transition hierarchy 下的精确坐标变换。作者显式引入 \(\Delta B=B^*-B^c\)，然后把它放进 disturbance。这里本质上是把难点从“精确连续层级动力学”转移到“扰动有界且增益足够大”。这在控制上可接受，但削弱了过渡期最优性的解释。

第四，robust term 的可实现性不完全清晰。论文写 \(F_r=-d_{ns}-D_r\dot e_h\)，但 \(d_{ns}\) 是复杂 lumped disturbance，实际如何获得或近似文中未充分说明；增益条件依赖 \(\delta_s\)，其来源也偏理论。实际实验可能更多依赖足够阻尼和温和 transition，而不是精确 disturbance cancellation。

第五，scalability 是相对的。它比组合枚举方法更 scalable，但仍需实时计算 dynamics、weighted pseudoinverse、QRD、Jacobian 导数和 Coriolis 相关项。对高维 humanoid、多接触、多个 inequality safety constraints，是否仍能稳定 1 kHz 不明确。

第六，本文只处理 equality-like tracking tasks。真实 safety tasks 常是 inequality constraints、barrier constraints 或 contact force constraints。把 joint-limit avoidance 写成 tracking midpoint 是实验上的简化，不等价于约束安全控制。未来扩展到 force/position hybrid control 才能检验该框架的实际边界。

Takeaway

• 1. 优先级切换的核心不是平滑控制输入，而是连续化 prioritized projection 的 range/null-space 结构；这是比 torque blending 更可迁移的 insight。
• 2. 对 hierarchical control 来说，activation 应该作用在任务方向而不是奇异向量方向。
• QRD task-direction shaping 是本文最值得迁移的表示选择。
• 3. strict hierarchy 的端点等价性很重要：允许 transition 中间非严格，但必须保证两端回到原有理论可证明的 controller。

一句话总结

这篇论文把动态任务优先级切换重新表述为 QRD 任务方向上的动态一致投影连续塑形问题，实质贡献是让 strict hierarchical optimal control 在不同离散层级结构之间可连续、有界地过渡，而不是证明过渡期本身仍完全严格最优。