Part IV · 第 13 章 反馈控制 · 13.7

全身控制（QP）

人形和四足机器人要同时做好几件事：保持平衡、稳住躯干、伸手去拿东西，而且脚只能推不能拉、不能打滑、电机力矩有上限。全身控制把这一切写成一个二次规划：每个控制周期求一次关节加速度、关节力矩和接触力，让各个任务尽量满足，同时严格遵守浮动基座动力学、接触、摩擦锥和力矩限制。这一节在一个平面人形机器人上实时求解这个 QP，看它怎样分配重量、抬脚、在湿滑的沟里撑住、扶墙前倾，以及怎样用严格的优先级保护平衡。

约 45 分钟 前置：6.7 约束优化与 KKT · 6.8 线性规划与二次规划 · 8.3 质量矩阵与科氏力 · 8.4 接触与摩擦 · 9.2 足式与人形机器人基础 · 13.6 操作空间控制 5 个交互

    1为什么机器人需要它

    13.6 节的操作空间控制处理的是一只固定在桌上的手臂：一个主任务（手爪），再把第二个任务投影到它的零空间里。换成一个用两只脚站着的人形机器人，问题一下子多了起来：

    • 任务很多，而且会打架。质心要待在脚的上方，躯干要保持直立，手要伸向目标，摆动脚要抬起来，剩下的自由度还要保持一个"像样"的姿态。目标够不着的时候，必须有人让步：谁让？
    • 基座没有电机。9.2 节说过，浮动基座运动方程的前 6 行（平面里是 3 行）没有关节力矩，躯干的加速度只能靠接触力和摆动肢体的惯性耦合来改变。
    • 接触力受不等式约束。脚只能推地，不能拉地；切向力必须在摩擦锥里（8.4 节）；压力中心必须在脚掌内。这些都是不等式，零空间投影处理不了。
    • 电机有上限。力矩不能超过额定值。如果先算出"理想力矩"再截断，截断后的力矩已经和接触力对不上了，脚会滑，机器人会倒。

    全身控制（whole-body control，WBC）的答案是：每个控制周期解一个二次规划（6.8 节）。决策变量是关节加速度 \(\ddot{\mathbf q}\)、关节力矩 \(\boldsymbol\tau\) 和接触力 \(\mathbf f\)；目标函数是各任务误差的加权平方和；约束是物理：动力学方程、接触不动、摩擦锥、力矩上下限。QP 是凸的，有唯一解，几十个变量的问题解一次远不到 1 毫秒，所以可以放进 500 Hz 到 1 kHz 的控制回路。

    本节用的机器人

    一个侧视（矢状面）的平面人形：躯干（含头）28 kg，每条腿是大腿 6 kg、小腿 3 kg、脚 1 kg，一条手臂 2.5 + 1.5 kg，共 51.5 kg。广义坐标 \(\mathbf q=(x,y,\theta,q_1,\dots,q_8)\in\R^{11}\)：前 3 个是骨盆的位置和躯干俯仰角（浮动基座，没有电机），后 8 个是两侧的髋、膝、踝和手臂的肩、肘。每只脚用脚跟和脚尖两个接触点。脚下的接触是真正仿真出来的（刚硬的弹簧阻尼接触加库仑静摩擦，0.5 ms 步长），全身控制器每 2 ms（500 Hz）解一次 QP，并不知道接触是怎么仿真的。

    2直觉：任务是愿望，物理是法律

    把控制器要决定的量想成一个点 \(\mathbf x\)（真实问题里它有几十维，下图只画两维，可以想成两个关节加速度）。每个任务是一个愿望："希望 \(A_k\mathbf x=\mathbf b_k\)"，愿望之间可能互相矛盾，所以只能"尽量"满足，惩罚 \(\|A_k\mathbf x-\mathbf b_k\|^2\)。约束是法律：方框外的点（比如超出力矩上限）根本不能选。剩下的问题是：两个愿望冲突时听谁的？有两种回答：

    • 加权：把愿望加权求和，\(\min\ w\,\|A_1\mathbf x-\mathbf b_1\|^2+\|A_2\mathbf x-\mathbf b_2\|^2\)。权重越大，任务 1 越被照顾，但只要 \(w\) 有限，它就永远不会被完全满足：两个任务在讨价还价。
    • 严格优先级（分层、字典序）：先只管任务 1，求出它能做到的最好结果；再在"不让任务 1 变差"的所有点里，尽量满足任务 2。任务 2 一点也影响不了任务 1。

    下图中红线是任务 1（"落在这条线上"），蓝色手柄是任务 2 的目标点，青色方框是约束。拖动红线的两个端点和蓝色目标，改变权重，在右上角切换两种方式。

    FIG 1两个冲突的任务：加权还是严格优先
    决策空间 (x₁, x₂)：拖动红色端点和蓝色目标
    琥珀色实心点是当前方式的解；空心绿环是严格优先级的解；紫色虚线是权重 \(w\) 从 0.01 增加到 10⁴ 时加权解走过的路径：它从蓝色目标附近出发，逐渐靠近绿环，但要 \(w\to\infty\) 才到达。方框不起作用时，加权解离红线的距离正好是"蓝点到红线的距离"的 \(1/(1+w)\) 倍。方框半宽 \(L\) 代表执行器的上限。
    试一试
    • 在"加权"模式下，把任务 1 的残差（解到红线的距离）压到 0.01 以下。需要多大的 \(w\)？如果要压到 0.0001 呢？
    • 切换到"严格优先"，把红线整条拖到方框外面（离方框至少 0.05）。现在任务 1 根本做不到：严格优先级给出的是哪个点？任务 2 还有没有发言权？
    核心洞见

    权重表达的是"多重要"，不是"谁优先"：加权解永远是一个折中，权重比要非常大才接近严格优先，而极端的权重比又会让问题病态。严格优先级把问题拆成一串 QP，每一层只在上一层的最优解集里挑选。两种方式都把物理当作硬约束：不论任务怎么排，方框外的点都不会被选中。

    3动手玩：每 2 毫秒一个 QP，让机器人站住

    下面是完整的全身控制器在实时运行。每个周期它读取机器人的状态 \((\mathbf q,\dot{\mathbf q})\)，把四个任务写成关于 \(\ddot{\mathbf q}\) 的线性方程，再加上物理约束，解出 \((\ddot{\mathbf q},\mathbf f)\) 和由它们决定的 \(\boldsymbol\tau\)，把 \(\boldsymbol\tau\) 发给电机：

    • 质心（权重 10）：质心加速度跟踪一个 PD 规律，把质心拉向绿色空心圆（可拖动）。
    • 躯干（权重 5）：躯干保持直立。
    • 手（权重 1）：手跟踪红色空心圆（可拖动）。
    • 姿态（权重 0.001）：关节轻轻地回到初始角度，只用来消耗"剩下的"自由度。

    琥珀色箭头是仿真器里地面真正作用在脚上的力，青色楔形是控制器使用的摩擦锥；某个摩擦约束在 QP 里处于"有效"（力压在锥的边上）时，楔形变成红色。右侧是 8 个关节的力矩，竖线是上限。

    FIG 2全身控制实验室：平面人形
    拖动绿色质心目标和红色手目标
    琥珀色箭头：仿真接触力（1 m ↔ 800 N），画在地面以下、指向脚。绿色实心圆是质心，竖直虚线是它在地面上的投影；青色横条是支撑区间（最后方的脚跟到最前方的脚尖）。蓝色是前腿，紫色是后腿，红色是手臂。控制器给摩擦锥留了 10% 的安全余量（用 \(0.9\mu\)）。"推一下"在颈部施加 150 N、持续 0.1 s 的向前推力。
    试一试
    • 两脚着地，把质心目标往前拖，让前脚承担至少 75% 的竖直力，并等质心停稳（速度小于 3 cm/s）。把目标一直拖到最前面时，质心为什么停在离目标还差一点的地方？（看后腿。）
    • 先把质心移到前脚上方，再按"抬起后脚"，单脚站稳 2 秒（后脚跟离地超过 5 cm）。如果质心不在前脚上方就抬脚，会发生什么？
    • 把腿部力矩上限降到 45 N·m 或更低，让至少一个力矩约束连续 1 秒处于有效状态（力矩条变红），而机器人仍然站着。QP 把负担转给了谁？
    核心洞见

    控制器从不直接决定"前脚出多少力"：它只说"质心往这里加速"，QP 自己在两脚之间分配力，并且保证每只脚的力都在摩擦锥里、压力中心在脚掌内、每个关节的力矩不超限。抬脚只是把一只脚的接触约束从 QP 里拿掉、换成一个"脚往上走"的任务；同一套代码，约束集合变了而已。力矩上限一旦起作用，QP 会改用别的关节、别的力分配，而不是像"先算后截断"那样把力矩硬削掉。

    4第二个视角：接触力住在锥里，内力是免费的

    两只脚站在地上时，"支撑体重"并不能唯一决定两只脚各自的力：你可以让两只脚互相向外蹬（或者互相向里夹），这种内力大小相等、方向相反，合力为零，不改变身体的运动。在平地上内力没什么用；但在斜面上它是救命的。

    下图让机器人站在一条 V 形沟里，前脚在右坡，后脚在左坡，坡度 \(\alpha\)。如果每只脚的力都是竖直的（"只撑住重量"），力和坡面法线的夹角就是 \(\alpha\)，需要 \(\mu\ge\tan\alpha\) 才不打滑。但两只脚可以向外蹬两侧坡面：内力把两个力都转向各自的法线，于是摩擦系数小得多也能站住，代价是关节要多出一些力矩。爬烟囱的人用的就是这一招。

    右上角切换控制器："QP" 把摩擦锥写成约束；"无锥约束"解的是同一个 QP，但把所有不等式（摩擦锥和力矩上限）都去掉，再把力矩截断到上限再发出，相当于"先求最小二乘解，再截断"。

    FIG 3湿滑的 V 形沟：摩擦锥约束与内力
    琥珀色箭头是仿真的接触力（1 m ↔ 800 N），青色楔形是控制器使用的摩擦锥（半角 \(\arctan0.9\mu\)），在 QP 里有效时变红。脚开始打滑时，那个接触点上画一个红色叉。改变坡度会把机器人重新放回沟里。"内力"读数是前脚受力的水平分量（向外蹬时为负，即坡面把前脚往回推）。
    试一试
    • 用 QP 控制器，把 \(\mu\) 降到 0.25 或更低（远小于 \(\tan\alpha\)），让机器人站满 3 秒，两只脚的滑移都小于 1 cm。内力读数是多少？
    • 切换到"无锥约束"，在 \(\mu<\tan\alpha\) 下看着某只脚滑出超过 5 cm。再把 \(\mu\) 调到 \(\tan\alpha\) 以上并重置：这次还滑吗？

    5第三个视角：多一个接触，多一片天地

    9.2 节的结论是：在平地上静止站立，质心必须在支撑多边形上方。但这个结论只对"所有接触都在同一个水平面、只靠摩擦"成立。一旦手撑在墙上，墙可以水平地推身体，平衡条件就变了：静止时，所有接触力必须平衡重力，即 \(\sum_i\mathbf f_i=(0,mg)\)，且对任一点的力矩之和为零。对给定的接触点和摩擦系数，能做到这一点的质心横坐标构成一个区间（Bretl 与 Lall 称之为静平衡区域），求它的两端各是一个线性规划。

    下图在机器人前面立了一面墙。打开"手扶墙"，手会先靠到墙上，碰到之后控制器把它当作第三个接触（法线指向 \(-x\)，有自己的摩擦锥）。再把质心目标往前拖：绿色区间是当前接触集合下的静平衡区域，它远远越过了脚尖。

    FIG 4扶墙前倾：多接触的静平衡区域
    拖动绿色质心目标
    地面上的绿色横条是当前接触集合的静平衡区域（只检查力和摩擦，不检查力矩上限和够不够得着，那是 QP 的事），青色细条是只有两只脚时的区域。区域越过墙面时，横条在墙处截断并画一个箭头，数值见右侧读数。墙在 \(x=0.6\) m。躯干前倾超过 45° 时算作摔倒。这个场景里躯干任务的权重降到 0.1，身体可以前倾。
    试一试
    • 打开"手扶墙"，等手接触墙之后，把质心保持在前脚尖前方至少 4 cm 处，持续 2 秒。这时手上有多大的力？质心为什么跟不上目标？（看哪个锥是红的。）
    • 质心还在脚尖前方时，关掉"手扶墙"。绿色区间立刻缩回到两只脚上，质心落在了外面：机器人会怎样？
    一个概念，三个视角

    优化视角（FIG 1）：全身控制是"在物理允许的集合里，按优先顺序满足愿望"。力的视角（FIG 2、FIG 3）：QP 的大部分工作是分配接触力：在两只脚之间分配重量，在需要时加入内力，同时让每个力留在各自的锥里。接触集合的视角（FIG 4）：机器人能做什么，取决于它此刻碰着哪些东西；每加一个接触，QP 就多一组变量和约束，能达到的质心区域、能产生的加速度就变大。规划层（落脚点、扶哪里）决定接触集合，全身控制在给定的接触集合里把每一毫秒做对。

    6再看数学

    提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（FIG 1 的加权路径和严格解，FIG 2 的浮动基座、接触点、摩擦锥、力矩条和质心）。

    任务：加速度层面的线性方程

    任务坐标 \(\mathbf x_k=\boldsymbol\phi_k(\mathbf q)\)（质心位置、手的位置、躯干角、某个关节角……）的加速度对 \(\ddot{\mathbf q}\) 是仿射的：\(\ddot{\mathbf x}_k=J_k\ddot{\mathbf q}+\dot J_k\dot{\mathbf q}\)（7.4 节）。期望加速度用和 13.6 节一样的 PD 规律给出，于是每个任务是 \(\ddot{\mathbf q}\) 的一个线性最小二乘项：

    $$ \hl{com}{e_k(\ddot{\mathbf q})=\big\|J_k\ddot{\mathbf q}+\dot J_k\dot{\mathbf q}-\ddot{\mathbf x}_k^{\mathrm{des}}\big\|^2},\qquad \ddot{\mathbf x}_k^{\mathrm{des}}=\ddot{\mathbf x}_k^{\mathrm{ref}}+K_{d}\big(\dot{\mathbf x}_k^{\mathrm{ref}}-\dot{\mathbf x}_k\big)+K_{p}\big(\mathbf x_k^{\mathrm{ref}}-\mathbf x_k\big). $$

    质心的雅可比就是各连杆质心雅可比按质量的加权平均，\(J_{\mathrm{com}}=\tfrac1m\sum_i m_iJ_{c,i}\)。本课的增益都按临界阻尼取：质心 \(\omega=10\) rad/s（\(K_p=100,\ K_d=20\)），躯干 14 rad/s，手 10 rad/s，姿态 6 rad/s。

    一个 QP：变量、目标与物理约束

    把所有任务加权相加，再写上物理（第一行是目标，下面四行是约束）：

    $$ \begin{aligned} \min_{\ddot{\mathbf q},\,\boldsymbol\tau,\,\mathbf f}\quad & \hl{wsum}{\textstyle\sum_k w_k\, e_k(\ddot{\mathbf q})}+\varepsilon_a\|\ddot{\mathbf q}\|^2+\varepsilon_f\|\mathbf f\|^2\\ & \hl{dyn}{M(\mathbf q)\ddot{\mathbf q}+\mathbf h(\mathbf q,\dot{\mathbf q})=S^{\top}\boldsymbol\tau+J_c(\mathbf q)^{\top}\mathbf f}\\ & \hl{contact}{J_c\ddot{\mathbf q}+\dot J_c\dot{\mathbf q}=\mathbf 0}\\ & \hl{cone}{\mathbf f_i\in\mathcal K_i:\ |f_{t,i}|\le\mu f_{n,i}}\\ & \hl{tau}{\boldsymbol\tau_{\min}\le\boldsymbol\tau\le\boldsymbol\tau_{\max}} \end{aligned} $$

    逐行看：

    • 动力学：9.2 节的浮动基座方程，本节沿用 9.2 节的记号：\(\mathbf h\) 就是 8.3 节和 13.6 节的偏置项 \(\mathbf b\)（科氏/离心项与重力之和），\(S=[\,0\ \ I\,]\) 选出有电机的关节，\(\mathbf f\) 是接触力。8.4 节把同一个方程写成 \(S\boldsymbol\tau\) 和 \(\boldsymbol\lambda\)：那里的 \(S\) 就是这里的 \(S^\top\)；本节把 \(\lambda\) 留给 QP 的乘子。它是关于 \((\ddot{\mathbf q},\boldsymbol\tau,\mathbf f)\) 的线性等式：\(M,\mathbf h,J_c\) 在当前状态下都是已知的数。
    • 接触：着地的脚（或扶墙的手）不加速。脚是一个刚体，平面里约束它的 3 个加速度分量（脚跟点的 2 个加上脚的角加速度）；手是一个点接触，约束 2 个分量。本课实际用的是 \(J_c\ddot{\mathbf q}+\dot J_c\dot{\mathbf q}=-20\,J_c\dot{\mathbf q}\)，多出的一项把接触点残余的速度阻尼掉。
    • 摩擦锥：8.4 节的库仑锥。平面里它恰好是两个半平面 \((\pm\mathbf t-\mu\mathbf n)^{\top}\mathbf f_i\le0\)，是精确的线性约束（两条不等式合起来已经蕴含 \(f_n\ge0\)，只要 \(\mu>0\)）；三维里圆锥用 8.4 节的 \(k\) 棱锥内接近似，每个接触 4 到 8 条不等式。每只脚在脚跟、脚尖各有一个接触点，两个法向力都非负，正好等价于"压力中心在脚掌内"（9.2 节的 ZMP 条件）。
    • 力矩上限：盒约束。

    目标是二次的，约束全是线性的：这是一个凸 QP。两个很小的正则项 \(\varepsilon_a=10^{-4}\)、\(\varepsilon_f=10^{-3}\)（力以 100 N 为单位）让 Hessian 严格正定、解唯一，也决定了"什么都不影响任务时"内力取最小。

    消去 \(\boldsymbol\tau\)：只剩基座的那几行

    把动力学方程按"基座行"（下标 u，没有电机）和"关节行"（下标 a）拆开：

    $$ M_u\ddot{\mathbf q}+\mathbf h_u=J_{c,u}^{\top}\mathbf f,\qquad \boldsymbol\tau=M_a\ddot{\mathbf q}+\mathbf h_a-J_{c,a}^{\top}\mathbf f . $$

    第一式是 3 行（三维里 6 行）。关节行里每个 \(\tau_j\) 都可以由 \((\ddot{\mathbf q},\mathbf f)\) 直接算出，所以 \(\boldsymbol\tau\) 不必当作变量：力矩上限变成关于 \((\ddot{\mathbf q},\mathbf f)\) 的线性不等式。本课的实现就是这样做的。两脚着地时变量是 \(\ddot{\mathbf q}\in\R^{11}\) 加 4 个接触点 × 2 个力分量，共 19 个；等式 9 条（3 条基座行 + 2 只脚 × 3 条接触行）；不等式 24 条（4 个点 × 2 条摩擦 + 8 个关节 × 2 条力矩）。抬起一只脚后变成 15 个变量、6 条等式、20 条不等式。基座行正是"躯干只能靠接触力加速"这句话的方程形式：去掉所有接触，\(J_{c,u}^\top\mathbf f=0\)，质心只能自由落体。

    严格优先级：一串 QP

    把任务分成层 \(1,\dots,p\)。第 \(k\) 层解

    $$ \hl{lex}{\mathbf x_k^{*}=\arg\min_{\mathbf x}\ \|A_k\mathbf x-\mathbf b_k\|^2},\qquad \mathbf x\in\mathcal P,\qquad A_j\mathbf x=A_j\mathbf x_j^{*}\ \ (j\lt k), $$

    其中 \(\mathbf x=(\ddot{\mathbf q},\mathbf f)\)，\(\mathcal P\) 是满足上面全部物理约束的集合。因为 \(\|A_j\mathbf x-\mathbf b_j\|^2\) 对 \(A_j\mathbf x\) 是严格凸的，第 \(j\) 层的最优值 \(A_j\mathbf x_j^*\) 是唯一的，把它冻结下来，后面的层就只能在"不让第 \(j\) 层变差"的点里挑。只有等式任务、没有不等式时，这和 13.6 节的零空间投影（Nakamura 等，1987；Siciliano 与 Slotine，1991）给出同样的结果；一旦有不等式（摩擦锥、力矩上限，或者"关节角不超过某值"这类不等式任务），零空间公式就失效，必须用分层 QP（Kanoun、Lamiraux 与 Wieber，2011；Escande、Mansard 与 Wieber，2014）。本课的实现有两个工程细节：冻结的等式写成 \(\pm10^{-6}\) 的细带，防止数值误差让下一层被判为不可行；低层加了更大的阻尼 \(\varepsilon=10^{-2}\)，避免够不着的目标在奇异位形附近要求极大的加速度（与 13.6 节的阻尼同一个道理）。

    加权和分层的关系：把加权目标写成 \(w\,e_1+e_2\)，令 \(w\to\infty\)，解收敛到分层解（FIG 1 的紫色路径）。但有限的 \(w\) 总是折中，而太大的 \(w\) 让 Hessian 的条件数随 \(w\) 增长，数值上不可靠。实践中两者常常混用：少数几层严格优先级，每层内部用权重。

    求解器：从无约束最小值出发的对偶有效集法

    本页所有的 QP 都由一个内嵌的 Goldfarb–Idnani 对偶有效集法求解（1983，也是 quadprog 和 eiquadprog 用的算法）：先求无约束最小值 \(\mathbf x=-H^{-1}\mathbf g\)，然后每次把一条违反的约束加入有效集，沿着保持 KKT 条件的方向走，如果某条有效约束的乘子要变负，就把它移出。它不需要可行的初始点，而且每一步结束时都满足 6.7 节的对偶可行性。站立时它通常 0 到 6 次迭代就结束。返回的乘子 \(\lambda\) 有明确含义：某个力矩上限的 \(\lambda\) 是"这个上限每放宽 1 N·m，求解器的目标 \(\tfrac12\mathbf x^\top H\mathbf x+\mathbf g^\top\mathbf x\) 的最优值下降多少"（一阶近似），FIG 2 用它判断哪些约束在起作用。

    7机器人中的它：保护平衡的优先级

    真实的人形机器人要伸手去够一个远处的东西时，最重要的是不要摔倒。下图让手去追一个可以拖到很远的目标，比较两种写法：

    • 加权：质心 10，手 \(w_{\mathrm{h}}\)（滑块），躯干 0.1，姿态 0.001。
    • 严格优先：第 1 层质心（加上抬脚时的摆动脚），第 2 层手，第 3 层躯干和姿态。

    两种写法都有同样的物理约束，QP 都不会让脚打滑或抬起。区别在于：加权时，手的权重越大，QP 越愿意牺牲质心去换手的距离，质心可能被拖到脚尖附近，失去了承受扰动的余量。

    FIG 5伸手够远处：加权与分层 QP
    拖动红色手目标
    绿色空心圆是固定的质心目标（初始质心位置），绿色实心圆是实际质心；青色横条是支撑区间。"推一下"在颈部施加 200 N、持续 0.1 s 的向前推力。分层模式每个周期解 3 个 QP。在较慢的设备上仿真会自动放慢，但时间步长不变。
    试一试
    • 在"加权"模式下把 \(w_{\mathrm{h}}\) 调到 100 或更大，把手目标拖到 \(x\ge1.1\) m（手臂根本够不着）。机器人会怎样？
    • 切换到"严格优先"，同样把手目标放在 \(x\ge1.1\) m：机器人站满 3 秒，质心离目标不到 5 mm，而手离目标还很远。
    • 严格优先模式下，手目标放在 \(x\ge0.9\) m，按"推一下"，推完后站稳 2 秒。再在加权模式（\(w_{\mathrm{h}}=100\)）下对目标 \((1.0,\,0.8)\) 做同样的事。
    真实系统里

    DARPA 机器人挑战赛的 Atlas：多支队伍都用 QP 全身控制驱动 Boston Dynamics 的液压人形 Atlas。MIT 队的控制器每个周期解一个 QP，用一个利用稀疏性、并从上一周期的有效集热启动的定制有效集求解器，使整个控制周期达到 1 kHz（Kuindersma、Permenter 与 Tedrake，2014；Kuindersma 等，2016）；IHMC 队采用基于动量的 QP 框架（Koolen 等，2016），WPI–CMU 队采用全身优化控制（Feng 等，2015）。四足：ANYmal 上用分层 QP 做全身控制（Bellicoso 等，2016）；MIT Mini Cheetah 把凸 MPC（13.4 节）给出的地面反力交给"全身冲量控制"WBIC，由它算出关节力矩（Kim 等，2019）：这就是 9.2 节提到的"MPC + 全身控制"分层结构，MPC 负责往前看几百毫秒，WBC 负责把每一毫秒做对。扭矩控制的人形：Sarcos 人形下半身上的分层逆动力学与动量控制（Herzog 等，2016），力矩控制下的接触力优化分配（Righetti 等，2013）。软件：TSID（基于 Pinocchio，Stack of Tasks 系列）、OpenSoT、IHMC 的开源控制栈；常用 QP 求解器有 eiquadprog、qpOASES、OSQP 和 ProxQP。

    8常见误区

    误区 1：用权重表达优先级

    FIG 1 里，方框不起作用时，任务 1 的残差是 \(d/(1+w)\)：要比 \(d\) 小 100 倍，\(w\) 就要约 100；小 10⁴ 倍就要约 10⁴。权重比太大时 Hessian 的条件数也跟着变大，数值误差会污染低权重的任务。FIG 5 里，\(w_{\mathrm{h}}=100\) 的机器人站着，却被一次 200 N 的推力推倒，而分层 QP 承受住了同样的推力：权重的问题常常不在静止时暴露，而在扰动下暴露。安全相关的任务（平衡、接触）应该放在更高的层里，或者干脆写成约束。

    误区 2：先求解，再截断

    把不等式丢掉、求一个最小二乘解、再把力矩截断到上限、把力"投影"回锥里，看上去省事，但截断后的力矩和计划的接触力已经不满足动力学方程了：实际的接触力会自己"找"一个解，往往就在锥外。FIG 3 的"无锥约束"就是这样：\(\mu<\tan\alpha\) 时脚顺着坡面滑下去。约束必须放在优化里面，求解器才能用内力、换关节、改分配来满足它们。

    误区 3：用真实的 \(\mu\)、硬接触且不留后路

    QP 会把力推到锥的边上（FIG 3 里红色的锥），而真实的 \(\mu\) 只是估计值，三维的棱锥近似又会改变边界，所以控制器要用比估计值小的 \(\mu\)（本课用 0.9 倍，工程上常常更保守）。另一方面，硬约束之间可能互相矛盾，例如力矩上限太低、根本撑不住体重：QP 无解。控制器必须有后备方案，常见做法是给接触或任务约束加松弛变量。本课的后备方案是：QP 无解时，把接触约束改成权重很大的任务再解一次。

    误区 4：突然切换接触

    本课的"抬起后脚"是突然把一只脚的接触约束拿掉。如果那一刻那只脚还承担着体重，QP 的解会跳变：力矩突变，机器人可能摔倒（FIG 2 的第二个任务）。真实控制器会在抬脚前用几十到几百毫秒把这只脚的力降到零（例如对它的法向力加一个随时间收紧的上限），落脚时同样逐渐加载；接触检测本身也会有误差，需要容错。

    误区 5：指望全身控制"往前看"

    WBC 的 QP 只管当前这一个周期的加速度。它保证这一刻的力是可行的，却不保证一秒以后还有可行解：质心以很大的速度冲向脚尖时，每一步的 QP 都可行，最后仍然会倒。要避免这类情况，需要能预测未来的层：MPC（13.4 节）、捕获点式的落脚规划（9.2 节），或者学习得到的策略。WBC 是执行层，不是规划层。

    9小练习

    1. 浮动基座运动方程的前 3 行（平面）里没有关节力矩。这意味着什么？
    \(S^\top\boldsymbol\tau\) 的前几个分量为零，所以 \(M_u\ddot{\mathbf q}+\mathbf h_u=J_{c,u}^\top\mathbf f\)。这正是 QP 里最关键的等式：没有它，控制器会假装能直接"推躯干"。删掉它（选项 A）就丢掉了物理；它并不要求加速度为零（B）；电机也无法直接作用在基座上（D），只能通过接触力间接作用。
    2. 任务 1 的权重 \(w=1000\)，任务 2 的权重为 1，两者冲突、约束不起作用。下面哪个说法对？
    FIG 1 里残差正好是 \(d/(1+w)\)，\(w=1000\) 时约为 \(d/1000\)，不是零（A 错）。任务 2 仍在起作用，它决定了解沿着红线方向的位置，也让解稍微偏离红线（C 错）。权重不影响可行性（D 错）。
    3. FIG 3 里，\(\mu=0.2\)、坡度 25°（\(\tan25^\circ\approx0.47\)）。QP 控制器站住了，"无锥约束"控制器滑倒了。为什么？
    两只脚的内力合力为零，不改变身体的运动，所以任务代价对它几乎无所谓；只有把摩擦锥写成约束，QP 才"知道"需要它。QP 用的是 \(0.9\mu\)，比真实值还小（B 错）；仿真的接触只能推不能拉（C 错）；在沟里站立需要的力矩不大，截断并不是原因（D 错）。
    4. 本课的平面机器人有 11 个广义坐标、8 个电机，两只脚各有 2 个接触点（每点 2 个力分量）。如果把 \(\ddot{\mathbf q}\)、\(\boldsymbol\tau\)、\(\mathbf f\) 都当作变量，QP 有多少个变量？消去 \(\boldsymbol\tau\) 之后呢？
    \(11+8+4\times2=27\)。关节行把每个 \(\tau_j\) 写成 \((\ddot{\mathbf q},\mathbf f)\) 的函数，消去 8 个力矩后剩 \(11+8=19\) 个变量，力矩上限变成 16 条关于它们的不等式。
    5. 在三维里，为什么全身控制常把摩擦圆锥换成一个内接的四棱锥？
    圆锥 \(\|\mathbf f_t\|\le\mu f_n\) 是凸的（C 错），但它是二阶锥约束，不是线性的，需要 SOCP 求解器；四棱锥 \(|f_x|\le\tfrac{\mu}{\sqrt2}f_n,\ |f_y|\le\tfrac{\mu}{\sqrt2}f_n\)（内接）或 \(|f_x|,|f_y|\le\mu f_n\)（外切）是 4 条线性不等式。内接的那个比圆锥小（A 错），保证安全；任何一种都要求 \(f_n\ge0\)（D 错）。

    10延伸阅读

    • L. Sentis & O. Khatib, Synthesis of Whole-Body Behaviors through Hierarchical Control of Behavioral Primitives, Int. J. Humanoid Robotics 2(4), 2005 — 把操作空间控制推广到带优先级的全身控制
    • Y. Nakamura, H. Hanafusa & T. Yoshikawa, Task-Priority Based Redundancy Control of Robot Manipulators, Int. J. Robotics Research 6(2), 1987; B. Siciliano & J.-J. E. Slotine, A General Framework for Managing Multiple Tasks in Highly Redundant Robotic Systems, ICAR 1991 — 零空间投影的任务优先级
    • O. Kanoun, F. Lamiraux & P.-B. Wieber, Kinematic Control of Redundant Manipulators: Generalizing the Task-Priority Framework to Inequality Task, IEEE Trans. Robotics 27(4), 2011 — 带不等式的任务优先级
    • A. Escande, N. Mansard & P.-B. Wieber, Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation, Int. J. Robotics Research 33(7), 2014 — 分层 QP 的高效求解
    • L. Saab, O. E. Ramos, F. Keith, N. Mansard, P. Souères & J.-Y. Fourquet, Dynamic Whole-Body Motion Generation under Rigid Contacts and Other Unilateral Constraints, IEEE Trans. Robotics 29(2), 2013 — 动力学层面的分层控制与接触约束
    • L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan & S. Schaal, Optimal Distribution of Contact Forces with Inverse-Dynamics Control, Int. J. Robotics Research 32(3), 2013 — 接触力分配与内力
    • S. Kuindersma, F. Permenter & R. Tedrake, An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion, ICRA 2014; S. Kuindersma et al., Optimization-Based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot, Autonomous Robots 40(3), 2016 — Atlas 上的实时 QP 控制
    • T. Koolen et al., Design of a Momentum-Based Control Framework and Application to the Humanoid Robot Atlas, Int. J. Humanoid Robotics 13(1), 2016 — IHMC 的动量 QP 框架
    • T. Bretl & S. Lall, Testing Static Equilibrium for Legged Robots, IEEE Trans. Robotics 24(4), 2008 — 多接触的静平衡区域（FIG 4）
    • D. Goldfarb & A. Idnani, A Numerically Stable Dual Method for Solving Strictly Convex Quadratic Programs, Mathematical Programming 27, 1983 — 本页使用的 QP 算法
    • P. M. Wensing, M. Posa, Y. Hu, A. Escande, N. Mansard & A. Del Prete, Optimization-Based Control for Dynamic Legged Robots, IEEE Trans. Robotics 40, 2024 — 足式机器人 MPC 与全身控制的综述