Home / 机器人经典基础 / 全身任务层级：够不到目标时，先决定什么不能牺牲

050｜全身任务层级：够不到目标时，先决定什么不能牺牲

论文：Luis Sentis、Oussama Khatib，Synthesis of Whole-Body Behaviors Through Hierarchical Control of Behavioral Primitives，International Journal of Humanoid Robotics 2(4)，2005，505–518。作者提供的期刊全文。以下页码为期刊页码，不混用作者网站另存的旧稿。

先接住前作的问题

操作空间已把任务力与动力学联系起来，多个任务发生冲突时还需明确让谁退让。本篇用动态一致层级保住高优先级。

五个前置概念

五个前置概念分别是：雅可比描述关节与任务速度的局部映射；零空间表示不会改变指定任务的方向；惯性矩阵把力矩和加速度联系起来；操作空间控制在任务坐标组织运动和力；秩亏意味着独立可控方向不足。

一、问题设定：手部目标与平衡冲突时怎样保证低优先级退让

想象机器人弯腰捡地上的东西。手希望继续向下，膝盖已经接近限位，身体又必须保持平衡。三个目标可能根本无法同时完成。若控制器只是把三个误差相加，就可能为了少一点手的位置误差，接受一些失衡。

背景：冗余不是无限的备用自由度

一个人形机器人有很多关节，但双手、重心、躯干、接触点和关节限位都在消耗运动自由度。增加关节不代表任何任务集合都能实现。更微妙的是，一个单独可行的手部运动，在加入重心保持和限位之后可能变得不可行。

早期“主任务加零空间姿态”的安排常把避障或关节约束放在次要位置：先把末端送到目标，再利用剩余自由度避开障碍。当没有剩余自由度时，约束反而失去控制能力。第 3 节的核心转换，是让这些约束优先，再把操作任务放进约束留下的空间。

本文采用关节动力学

$A(q)\ddot q+b(q,\dot q)+g(q)=\Gamma.$

$q$ 为关节坐标， $A$ 为惯性矩阵， $b$ 是科氏和离心项， $g$ 是重力项， $\Gamma$ 是力矩。任务坐标写作 $x_k(q)$ ，其雅可比 $J_k=\partial x_k/\partial q$ 描述关节速度怎样改变该任务。

论文以这套操作空间形式展开，不能直接当成已经完整解决任意浮动基座、欠驱动接触与摩擦锥的通用优化器。它讨论接触与柔顺，但关于多个约束的组织及进一步细节有明确留待后续研究的部分。阅读时应把本文的层级思想与后来全身动力学 QP 的完整约束集分开。

二、构造动态一致层级，区分约束任务和姿态

方法：为什么普通几何零空间还不够

若只控制速度，要求 $J\dot q=0$ 就能保持任务瞬时不动。但关节力矩经过惯性矩阵才产生加速度。一个几何上看似“与任务正交”的力矩，仍可能通过动力学耦合改变任务运动。

对于满行秩任务，动力学一致广义逆与投影可写成

$\Lambda=(JA^{-1}J^T)^{-1},\qquad \bar J=A^{-1}J^T\Lambda,\qquad N=I-\bar JJ.$

于是低优先级力矩以 $N^T\Gamma_{\rm low}$ 注入，满足

$JA^{-1}N^T\Gamma_{\rm low}=0.$

左侧就是这部分力矩对高优先级任务加速度的贡献。这给“不能干扰”一个明确的数学含义。它不是保证测量误差、外部冲击、模型偏差、采样延迟都不存在，也不是保证高优先级任务本身一定做得到。

第 3 节式（10）的组织为

$\Gamma=J_c^TF_c+N_c^T\Gamma_{\rm task},$

其中 $c$ 表示优先约束。第 4 节将它推广为多层，任务 $k$ 的雅可比先投影到所有更高层允许的空间：

$J_{k\mid\mathrm{prec}(k)}=J_kN_{\mathrm{prec}(k)}.$

随后利用投影雅可比建立该层的有效惯性，再计算操作空间力，并映射回关节力矩。不同层不是随意把各自独立计算的普通投影相乘；递归必须对应先前任务的动力学一致空间。第 4.1 节通过归纳推导这个递归关系，其检验条件正是所有高层 $J_iA^{-1}N^T=0$ 。

这里数学上要实现的是任务参考加速度及层间不干扰。机制上新增的是优先级组织和投影后的任务动力学。实验中改善的则是特定高优先级重心误差，不能把三者都简称为“优化稳定性”。

动力学一致的“看不见”，要在加速度层检查

用 $M$ 表示关节惯性、 $J$ 表示高优先级任务雅可比，令 $\bar J=M^{-1}J^T(JM^{-1}J^T)^{-1}$ 、 $N=I-\bar JJ$ 。低优先级力矩取 $N^T\tau_0$ ，关键恒等式是 $JM^{-1}N^T=0$ ，所以它不改变高优先级瞬时任务加速度；只证明 $JN=0$ 还不足以说明力矩层解耦。

教学两关节例取 $M=\operatorname{diag}(1,4),J=[1\ 1]$ ， $\bar J=(0.8,0.2)^T$ 。令 $\tau_0=(1,0)^T$ ，投影后为 $(0.2,-0.8)^T$ ，加速度为 $(0.2,-0.2)^T$ ，任务加速度和为0。若误用欧氏力矩投影得到 $(0.5,-0.5)$ ，加速度和为0.375，低优先级便干扰了主任务。

本篇直接承接操作空间的动态一致逆，后来的质心与全身轨迹优化把多时刻接触、质心动力学与全身运动学联合求解，但不能把它误称为已经纳入全部关节力矩界的凸QP。原稿第2节也明确：投影后的任务不满秩时应修改或停止低优先级运动，实验中牺牲手部误差可能正是层级生效。

可行性：低优先级任务有权失败

若投影后的雅可比掉秩，意味着某些任务方向在保护高层后已经不可控制。第 4.3 节对

$J_{k\mid\mathrm{prec}}A^{-1}J_{k\mid\mathrm{prec}}^T$

做特征分解，保留非零特征值对应的可控方向，在那些方向计算有限控制。作者提出修改轨迹或移除不良任务，同时维持高层控制。

这个判断非常重要：手没到目标可能是正确的控制结果，而不是算法没有收敛。相反，如果一个控制器在任务冲突时让每个误差都小一点，却牺牲平衡，它可能在总平方误差上更好，在系统目标上更差。

最后一个“输出受限信息”是教学上的接口整理；本文主要给出运动可行性的数学判据，不提供一个涵盖所有故障的生产接口规范。

剩余方向可能不够完成低优先级任务，因此执行结果应允许它变差。下面用加权和的反例检查优先级含义，再读重心与手部的对照仿真。

三、执行剩余方向分配，用权重折中作对照

以下伪代码强调信息流，省略各层动力学补偿的完整实现；关节安全区、数值秩阈值和任务切换策略需要在具体系统中明确。

输入：q、qdot、机器人动力学、任务参考、规定的优先级
每个控制周期：
    计算当前惯性、重力、偏置项与各任务雅可比
    根据安全激活区识别当前关节限位或其他约束
    建立最高层约束控制及其动力学一致剩余空间
    按优先级依次处理重心、手部、姿态等任务：
        将任务雅可比投影到所有高层允许的空间
        检查有效惯性对应的秩与条件性
        若部分方向不可控：只保留可控方向，标记任务受限
        计算该层参考加速度、动力学补偿和力矩贡献
        递归更新留给后续任务的剩余空间
    合成关节力矩并发送到受控模型
输出：关节命令，以及哪些任务因层级冲突不能完成

自构例子：为什么调大权重与真正优先不同

考虑两个一维任务共享一个变量 $u$ ：重要任务要求 $u=0$ ，次要任务要求 $u=1$ 。加权目标 $100u^2+(u-1)^2$ 的最优解为 $u=1/101$ ，重要任务依然出现非零误差。若明确要求先满足 $u=0$ ，次要任务只能接受残差 1。

这是自构静态例子，不是论文实验，也省略了机器人动力学。它说明有限权重通常表达折中，严格层级表达词典式优先。真实机器人通过动力学一致投影实现类似的不干扰关系，但还必须检查高层可行性。

四、读重心与手部任务同级或分级的仿真

Fig. 5：同时看重心误差和右手位置：提高平衡优先级后，重心接近目标，手仍保留明显残差 — 原论文结果图／表摘引：Luis Sentis、Oussama Khatib（2005），Fig. 5，PDF 第 12 页；核对原文。原版权保留。

读图：同时看重心误差和右手位置：提高平衡优先级后，重心接近目标，手仍保留明显残差。这张图最重要的是误差被分配到低优先级任务，而非所有误差都消失。

实验：重心与手部同级，还是重心优先

定量比较主要来自模拟人形机器人的同一任务；优先级投影保证的是模型内不干扰，不能抹去关节限位和力矩饱和。

项目	原文设置	要验证的关系
平台	24 自由度人形模型，身高 1.65 米、质量 71 千克	多部位任务共享关节的冲突
任务	右手追踪目标、重心保持在双脚区域中心、对称姿态	任务和姿态是否能组合
高层约束	关节进入预定义激活区后，控制回安全位置	操作命令是否继续挤压关节限位
对照	重心和手同级，对比重心高于手	优先顺序怎样分配不可避免的误差
控制参考	各任务使用带速度饱和的 PD 型参考	控制目标不是学习得到的策略
指标	重心水平误差、手部离目标距离、关节角时序	哪些目标被保护、哪些被牺牲
证据范围	同一示例的轨迹及对比图	不提供大样本实机成功率

第 5 节将重心写成各连杆质心位置的质量加权平均，其雅可比也按相同质量加权。姿态任务把关节往中间位置组织，手部任务使用笛卡尔位置。关节约束的激活区在硬限位之前；如果等到高速关节撞上机械止挡才启动控制，层级投影本身无法撤销已经发生的冲击。

论文没有用多个随机种子构成统计性能排名，也没有在这组对照里量化传感延迟、关节力矩饱和和模型参数误差。这里的图是机制验证：相同任务发生冲突时，换一个层级安排，误差确实流向不同任务。

五处证据：哪些结论来自图，哪些依赖假设

图 1，第 507 页：任务和姿态可以区分。 抓箱子的手、重心及躯干姿态同时出现，直观展示多个行为基本单元的组合。这是功能演示，不能据此得出真实搬运负载范围或碰撞安全统计。

原论文 Figure 1：手部目标、躯干姿态与重心任务分解 — 原论文图摘引：Sentis、Khatib（2005），Figure 1，PDF 第 3 页；核对原文。点击图片可放大。原版权保留。

读图：箭头把手部位置、躯干竖直和重心落在足部支撑区三项要求分开；并行提出目标以后，还需要优先级处理互相冲突的方向。

图 2，第 510 页：障碍约束改变全身运动。 障碍被交互移动到身体附近时，机器人尝试保持右手位置，并协调重心及姿态。它支持局部约束与任务协调的可表达性，不等于全局规划完备性：控制器可能在局部无法完成目标。

避障任务与保持手部位置同时激活的三帧全身姿态 — 原论文图摘引：Luis Sentis、Oussama Khatib，Synthesis of Whole-Body Behaviors Through Hierarchical Control of Behavioral Primitives，International Journal of Humanoid Robotics 2(4)，2005，505–518，图 2，PDF 第 6 页；原文与图注。局部裁切；原版权保留。点击图片可放大。

读图：三面板中障碍改变了躯干和腿部姿态，而手的目标位置保持固定；这是任务优先级协调的具体例子，不证明全局规划完备。

式（17）及第 4.1 节，第 511–512 页：层级的理论依据。 不干扰条件是低层力矩在高层任务加速度中的贡献为零。归纳证明支持递归投影结构；前提包括模型与投影计算的适用性。把这个等式直接解释成“任何物理约束永远不会违反”，会跳过执行器和约束自身可行性的条件。

图 4，第 515 页：同级目标发生折中。 文本报告重心水平稳态误差在两个方向分别为 1 厘米和 3 厘米，手停在距目标约 22 厘米处。关节限位相继激活后，重心与手无法同时实现。这证明同级组织会分担误差，不能把它简单说成未使用任何约束的失败基线。

原论文 Figure 4：重心与手部同级时的四组响应 — 原论文图摘引：Sentis、Khatib（2005），Figure 4，PDF 第 11 页；核对原文。点击图片可放大。原版权保留。

读图：同时查看平衡误差、手部高度及膝肘角度：关节限制激活后，平衡误差仍会偏离零，可与已有图5的优先级设置对照。

图 5，第 516 页：把误差集中到较低层。 重心优先时，正文报告两个方向最大重心误差只有 2 毫米，手离目标约 24 厘米。注意前组是稳态误差、后组是最大误差，不宜直接算一个统一百分比改善。手的残差反而增加约 2 厘米，正是优先级选择的代价。作者据此解释避免倾倒；这仍不是摩擦不确定与强动态扰动下的完整稳定性验证。

五、检查可行性、模型误差与剩余自由度

本文把约束、操作任务、姿态纳入统一的动力学层级，把“什么更重要”从模糊权重变成结构性约束。它延续操作空间控制和冗余零空间方法，新的价值在于多层组合与对约束地位的重新安排，不是从零发明雅可比或 PD 控制。

严格层级会牺牲低优先级性能。若人为优先级不符合实际任务，控制器会忠实执行一个错误取舍。第 4 节明确表示如何组织优先级的准则留待后续讨论，因此不能把本文说成自动理解任务重要性的方法。

任务切换和秩变化也有实际代价。投影矩阵可能变化很快，数值逆可能在近奇异方向放大命令。截断小特征值可以避免部分数值问题，但引入阈值选择和行为变化。对摩擦锥、力矩界限、速度过大导致制动距离不足的场景，仅靠理想投影不够，这属于由模型假设推导的适用边界。

此外，“重心投影在支撑区域”与一般动态稳定不是同义词。本文示例的平衡目标适合说明层级冲突，却不能代替捕获点、角动量和接触力可行性分析。后续腿足控制文献将把这些条件表达得更完整。

六、追问：一个任务被投到高优先级零空间后，它的秩应按哪个雅可比判断？

一个任务被投到高优先级零空间后，它的秩应按哪个雅可比判断？

参考答案：应检查投影后的有效映射，而非原任务雅可比。即使原 $J_2$ 满行秩， $J_2N_1$ 也可能降秩，因为高优先级已占用它需要的方向。这个局部线性检查先解释“还剩几种可做动作”，再进入动力学一致的具体投影；不能把普通几何投影直接换作原文力矩层公式。

七、比较全身约束的联合优化路线

平行路线：质心动力学与全身运动学：怎样少算方程而不丢掉身体。用联合优化组织身体运动学、质心动力学与接触，另查驱动力矩可行性。

八、自测与参考答案

1．高优先级误差始终不为零，是否说明零空间投影失效？

不一定。投影保证的是特定低层力矩不引起额外的高层加速度，而不是高层没有初始误差、扰动或模型偏差。应比较加入低层前后的高层响应，并检查高层本身是否可行、执行器是否能实现控制力矩。

2．手本来够得到的位置，加入重心保持后够不到，是算法变差了吗？

单独可达与在层级中可达不同。重心保持占用了部分自由度，投影后的手部雅可比可能掉秩。若重心确实更重要，停下手是符合目标的结果；应调整目标或身体支撑条件，而不是强迫低层破坏高层。

3．为什么不能把所有普通零空间投影随意相乘，就宣称实现了论文？

任务在经过前层约束后，有效雅可比和惯性都变了。独立计算的投影未必彼此兼容，更未必满足每个高层的 $J_iA^{-1}N^T=0$ 。正确递归必须跟踪已消耗的空间，并在掉秩时处理可控方向；验证这个动力学不干扰条件比观察矩阵名字更可靠。