想象你正坐在音乐厅里，指挥家轻轻抬起双手。第一小提琴拉出旋律，圆号低声应和，定音鼓在最合适的瞬间加入。每位乐手都在演奏自己的部分，但真正让音乐成为整体的，是指挥家手中的那根指挥棒——他知道什么时候让某个声部响亮、什么时候让某个声部退后。机器人全身控制（Whole-Body Control，WBC）就是机器人的“指挥家”。传统方法让每个关节各管各的，而全身控制把手臂、腿、躯干看作一支完整的乐队，在同一个控制周期内同时算出所有关节该用的力，让机器人既能伸手够物、又能保持平衡、还能避免自碰撞。本文就用这支“乐队”贯穿始终，从运动学、动力学讲到优化与分层架构，把全身控制的里里外外讲明白。

岗位关联

全身控制是人形机器人实现“手脚并用、边动边稳”能力的关键技术。掌握它，意味着你有能力参与下一代具身智能机器人“大脑”的设计，这是当前机器人赛道最紧缺的算法方向之一。

目录
  1. 可视化演示
  2. 全身控制概念：一张总谱指挥整支乐队
  3. 运动学基础：音符与位置
  4. 动力学建模：该用多大的力
  5. 优化控制方法：指挥家的最优决策
  6. 分层控制架构：从总谱到分谱
  7. 多任务与平衡：同时进行的声部
  8. 工程实践挑战：从排练厅到舞台
  9. AI 应用场景
  10. Python 演示
  11. 常见误区
  12. 面试题

可视化演示

展示全身关节协同优化、雅可比矩阵关节空间与任务空间映射、平衡操作辅助优先级层次化控制及QP二次规划约束求解，含数据流背景动画。

本节配有交互动效（进入视口后自动加载，约 3KB）。

全身控制概念：一张总谱指挥整支乐队

一位指挥家接手新乐队，第一件事是看总谱：哪一段是主旋律、哪一段是伴奏、哪里渐强哪里收尾。他绝不会让每位乐手各拉各的调，而是让所有声部为一个统一的音乐目标服务。机器人全身控制就是机器人的“指挥家”——它不仅关心某个关节“自己想怎么动”，而是把手臂、腿、躯干看作一支乐队，统筹安排每个关节的力矩。

在机器人学里，一台人形机器人通常有二三十个以上的关节，每个关节称为一个自由度。传统做法像“独奏乐手各看各的谱”：给每个关节单独指定角度轨迹，彼此几乎没有沟通。可惜身体是耦合的——手伸出去拿东西，重心就跟着变，只顾自己角度的关节控制立刻失衡。全身控制把整个身体当作一个统一系统，在同一时刻、同一个优化框架里，决定所有关节该出多少力。

全身控制的定义可以浓缩为：在满足物理与安全约束的前提下，通过对全身关节力矩的协同优化，同时实现多个任务目标。输入是任务描述，比如“右手够到桌上的杯子”“躯干保持竖直”“双脚站稳定”；输出是每一个关节的力矩指令。核心目标分三层：完成任务（手到目标、身体到位）、维持稳定（重心始终落在支撑面内）、保护自身（关节角度、力矩上限、避免肢体自碰撞）。

总谱的意义正在于此：一首曲子的和谐不是靠某位乐手炫技，而是指挥家对整体约束的掌控。机器人的“总谱”就是任务误差与动力学约束的联合权衡——这是理解全身控制的第一块基石。人形机器人搬运、四足机器人爬坡、移动机械臂取物，都是这张总谱在不同乐队身上的演奏。

min⁡τ  ∑iwi ∥ei(q)∥2s.t.M(q)q¨+C(q,q˙)q˙+G(q)=τ\displaystyle \min_{\tau} \; \sum_{i} w_i \, \| e_i(q) \|^{2} \quad \text{s.t.} \quad M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau
这条公式是全身控制的“总谱”：左边是要最小化的任务误差加权和，右边是无论如何都不能违背的动力学物理法则。
  1. 目标函数 min指挥家要选一组力度方案，让整体的“不满意程度”降到最低。
    min⁡τ ∑iwi∥ei(q)∥2\displaystyle \min_{\tau} \, \sum_{i} w_i \|e_i(q)\|^{2}
  2. 任务误差 e_i(q)第 i 个任务离目标差多远，例如手掌与目标物体的距离偏差。
    ei(q)=fi(q)−xid\displaystyle e_i(q) = f_i(q) - x_i^{d}
  3. 权重 w_i主旋律优先级更高，优化器会优先消除它对应的误差。
    w平衡≫w姿态\displaystyle w_{\text{平衡}} \gg w_{\text{姿态}}
  4. 约束符号 s.t.后面是所有方案都不能跨越的物理底线，约束必须被严格遵守。
    s.t.M(q)q¨+C(q,q˙)q˙+G(q)=τ\displaystyle \text{s.t.} \quad M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau
  5. 动力学方程全部力矩必须与身体质量、速度效应、重力完全平衡，这是“不能造假”的铁律。
    M(q)q¨+C(q,q˙)q˙+G(q)\displaystyle M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q)

运动学基础：音符与位置

乐手拿到总谱后，第一件事是知道每个音符对应琴键的哪个位置。运动学就是机器人的“乐谱位置词典”：它回答“关节摆成什么角度，手和脚在哪里”，完全不涉及力与质量的概念。

自由度描述是基础。把每个关节的角度记作 q₁, q₂, …, qₙ，合并成一个向量 q，n 就是这台机器的自由度数量。人形机器人双腿双臂加腰，通常有 30 个左右的自由度。正向运动学（Forward Kinematics）解决“从关节到末端”：已知所有 q，求出手掌、脚掌在空间中的坐标 x。逆运动学（Inverse Kinematics）则反过来：已知希望手到达的位置，反推出每个关节应该转多少度。

连接两者的桥梁是雅可比矩阵 J。它是一张随当前姿态变化的“速度放大系数表”：关节哪怕只转一点点，末端也会运动，J 精确记录了这种从关节速度到末端速度的映射。更妙的是，力矩域存在对偶关系 τ = JᵀF：任务空间需要多大的力 F，都能借助 J 的转置折算成每个关节该出的力矩——这正是全身控制把“任务力”变成“关节力”的通道。

乐手看着谱子找到音，机器人看着 q 算出位置、看着 J 算出速度与力的关系。运动学这座“词典”，是后面所有控制算法都绕不开的地基——没有它，指挥家的任何手势都落不到具体的琴键上。

x=f(q),x˙=J(q) q˙\displaystyle x = f(q), \qquad \dot{x} = J(q)\,\dot{q}
第一行是“位置怎么来”，第二行是“速度怎么来”。雅可比矩阵就是连接关节空间与任务空间的那座桥。
  1. x 任务空间坐标手掌或脚掌在世界坐标系中的位置。
    x=[x,y,z]T\displaystyle x = [x, y, z]^{T}
  2. 正向运动学函数 f(q)把每个关节角度逐级拼接成末端位置的链条。
    f(q)=T1(q1) T2(q2)⋯Tn(qn)\displaystyle f(q) = T_1(q_1)\, T_2(q_2) \cdots T_n(q_n)
  3. q 关节向量所有关节角度组成的列向量，n 是自由度数量。
    q=[q1,q2,⋯ ,qn]T\displaystyle q = [q_1, q_2, \cdots, q_n]^{T}
  4. 雅可比矩阵 J(q)随姿态变化的“速度放大系数表”，是 f 对 q 的偏导数。
    J(q)=∂f∂q\displaystyle J(q) = \frac{\partial f}{\partial q}
  5. 速度映射关节怎么转（速度），末端就怎么动（速度）。
    x˙=J(q) q˙\displaystyle \dot{x} = J(q)\,\dot{q}

动力学建模：该用多大的力

乐谱只写了音高，没写力度。同一个音符，小提琴轻拉即可，低音提琴却要使出全身力气。动力学的任务，就是回答“这个关节到底该用多大的力矩”。

建模方法主要有两派。牛顿-欧拉法（Newton-Euler）从牛顿第二定律出发，像逐一检查乐手发力动作一样，从底座出发逐连杆递推速度和加速度，再反向递推每个关节承受的力与力矩，计算效率高，适合计算机实现。拉格朗日法（Lagrange）则换个角度：写出整台机器人的动能减势能，再对广义坐标求导，一气呵成得到运动方程，推导更优雅，但计算量通常更大。

无论哪一派，最终得到的方程都有统一形式：τ = M(q)q̈ + C(q,q̇)q̇ + G(q)。M(q) 是惯性矩阵，描述质量分布如何“抗拒”加速度；C(q,q̇)q̇ 项包含科氏力与离心力，代表运动连杆之间互相“拉扯”的影响；G(q) 是重力项，每个连杆都要对抗地球引力。把这三项加起来，就是电机需要输出的净力矩。

指挥家下达的不是“大声点”，而是确切的力度记号：这段用中强、那一段渐强到极强。动力学方程就是全身控制这张总谱上的力度记号——每一项都精确到牛顿·米，让每个关节的发力有据可依。

τ=M(q)q¨+C(q,q˙)q˙+G(q)\displaystyle \tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q)
这条方程把“想要的运动”（加速度、速度、姿态）翻译成“需要的力矩”（电机的力）。它既是全身控制优化问题里的硬约束，也是仿真的物理引擎。
  1. τ 关节力矩向量每个关节电机需要输出的驱动力矩。
    τ=[τ1,τ2,⋯ ,τn]T\displaystyle \tau = [\tau_1, \tau_2, \cdots, \tau_n]^{T}
  2. M(q) 惯性矩阵质量分布决定了“推得动推不动”，而且随姿态改变。
    M(q)q¨ 是加速所需力矩\displaystyle M(q)\ddot{q} \ \text{是加速所需力矩}
  3. C(q,q̇)q̇ 科氏与离心项运动连杆间的相互拉扯，是速度的耦合产物。
    C(q,q˙)q˙ 是速度耦合产生力矩\displaystyle C(q,\dot{q})\dot{q} \ \text{是速度耦合产生力矩}
  4. G(q) 重力项地球引力对每个连杆的持续拉扯。
    G(q)=∑imig hi(q)\displaystyle G(q) = \sum_{i} m_i g \, h_i(q)
  5. 等号求和三股需求相加，就是稳稳演奏所需的全部力度。
    τ=M(q)q¨+C(q,q˙)q˙+G(q)\displaystyle \tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q)

优化控制方法：指挥家的最优决策

指挥家排练时总在权衡：主旋律要突出、和声要饱满、不能盖过独奏，还要照顾乐手的体力。全身控制的“排练”发生在每个控制周期内——典型 1kHz，也就是每 1 毫秒都要做一次这样的权衡。

这种权衡被数学家写成二次规划（Quadratic Programming，QP）。目标函数把“任务误差”和“控制能量”都写成二次项：误差越小越好，出力越省越好。约束则把“不能违反的底线”写进去：力矩不能超过电机极限、关节不能碰到机械限位。任务误差是“软目标”，靠目标函数最小化；物理极限是“硬约束”，必须严格遵守。

任务进入目标函数的方式是加权最小二乘：每个任务前乘一个权重 w，权重越大，优化器越优先照顾它。例如“保持平衡”权重设为 100，“手部微小抖动抑制”设为 1，那么控制器宁可牺牲后者也不让前者出错。真实系统中，QP 必须由实时求解器（如 OSQP、qpOASES）在 1 毫秒内解完，解出的加速度再转换回关节力矩下发。

指挥家的“试听-调整”如今被数学接管：每一毫秒，优化器都在心里默念一遍“误差最小、不越界”，然后果断给出所有关节的力度方案——这就是全身控制最核心的决策引擎。

min⁡q¨  ∑iwi ∥Jiq¨+J˙iq˙−x¨id∥2+λ ∥q¨∥2s.t.τ‾≤Mq¨+h≤τ‾\displaystyle \min_{\ddot{q}} \; \sum_{i} w_i \, \| J_i \ddot{q} + \dot{J}_i \dot{q} - \ddot{x}_i^{d} \|^{2} + \lambda \, \| \ddot{q} \|^{2} \quad \text{s.t.} \quad \underline{\tau} \le M\ddot{q} + h \le \overline{\tau}
这里 h = C(q,q̇)q̇ + G(q) 是动力学中的非线性项。整条公式的意思是：在电机力矩允许的范围内，找一组关节加速度，让所有任务误差尽量小、动作尽量省。
  1. 实际任务加速度第 i 个任务在当前状态下真实产生的加速度。
    Jiq¨+J˙iq˙\displaystyle J_i \ddot{q} + \dot{J}_i \dot{q}
  2. 期望加速度 ẍ^d上层轨迹规划想让手或脚达到的加速度。
    x¨id\displaystyle \ddot{x}_i^{d}
  3. 误差平方与权重任务误差平方和再乘权重 w，构成优化器“心疼程度”的度量。
    wi∥⋅∥2\displaystyle w_i \| \cdot \|^{2}
  4. 正则项 λ‖q̈‖²对剧烈动作收费，避免无意义的浪费与危险。
    λ∥q¨∥2\displaystyle \lambda \|\ddot{q}\|^{2}
  5. 力矩约束任何解出来的方案，最终力矩都必须落在电机允许区间内。
    τ‾≤Mq¨+h≤τ‾\displaystyle \underline{\tau} \le M\ddot{q} + h \le \overline{\tau}

分层控制架构：从总谱到分谱

一场交响乐不会只有指挥一个人对每位乐手喊话。指挥给声部长一个眼神，声部长再带领本声部，信息逐层传递。全身控制的工程实现同样如此：把所有决策都塞进一个优化器，往往既不实时也不可维护。

典型的分层架构从上到下分四层。外层任务规划层（Task Planning）决定“现在要做什么”，比如是走过去拿杯子还是先蹲下；中层运动生成层（Motion Generation）把任务翻译成参考轨迹，比如手的位置随时间怎么变、重心轨迹怎么规划；内层全身控制层（Whole-Body Control）就是我们前面讲的优化器，它接收参考轨迹，结合当前状态算出全身力矩；最底层关节伺服层（Joint Servo）真正驱动电机，并把编码器、力传感器的读数传回上层。

层与层之间靠清晰的数据流衔接：上层输出“参考轨迹”，中层输出“期望加速度与期望力”，底层反馈“实际状态”。这种接口让每一层都能独立测试、替换，甚至把某一层换成深度学习模块，而不会牵一发动全身。层级之间还有一个重要的数学接口——零空间投影，它保证上层（主任务）的指令不被下层（次要任务）干扰。

从总谱到分谱，指令一层层细化。分层不是为了炫技，而是让“指挥”这件复杂的事变成四件简单的事——这正是全身控制系统能稳定跑起来的工程秘诀。

q˙=J†x˙p+(I−J†J)q˙h\displaystyle \dot{q} = J^{\dagger} \dot{x}_{p} + \big( I - J^{\dagger} J \big) \dot{q}_{h}
主任务优先完成，次要任务只能在“不打扰主任务”的零空间里见缝插针。这就是分层架构中最关键的“互不干扰”机制。
  1. 伪逆 J†把主任务速度“最小二乘”地翻译回关节速度。
    J†=JT(JJT)−1\displaystyle J^{\dagger} = J^{T}(J J^{T})^{-1}
  2. 主任务速度例如手掌跟踪目标轨迹所对应的期望关节速度。
    q˙p=J†x˙p\displaystyle \dot{q}_{p} = J^{\dagger} \dot{x}_{p}
  3. 零空间投影矩阵把任何动作投影成“不影响主任务”的分量。
    N=I−J†J\displaystyle N = I - J^{\dagger} J
  4. 次要任务速度保持姿态、避免碰撞等低优先级任务的速度。
    q˙h\displaystyle \dot{q}_{h}
  5. 速度合成叠加之后，主任务纹丝不动，次要任务见缝插针。
    q˙=q˙p+Nq˙h\displaystyle \dot{q} = \dot{q}_{p} + N \dot{q}_{h}

多任务与平衡：同时进行的声部

交响乐的高潮往往是多声部同时进行：小提琴唱主旋律、大提琴铺低音、定音鼓敲节奏。机器人也一样，最常见的场景是“边动边稳”——手在干活，身体还得站得住。

多任务协调的第一要件是优先级。典型任务列表可能包括：保持平衡（最高优先级）、手部到达目标（高）、躯干保持竖直（中）、头部朝向舒服（低）。全身控制用两种方式体现优先级：一是软优先级，在 QP 里用权重 w 表达；二是硬优先级，用零空间投影把次要任务限制在“不影响主任务”的子空间里。

平衡判据这块，最常用的概念是重心（Center of Mass, COM）与零力矩点（ZMP）。把每个连杆的质量乘位置累加再除以总质量，得到全身质心；把质心投影到地面，只要投影落在双脚围成的支撑多边形内，机器人就不会倒。慢速运动时 ZMP 判据退化为这个简单规则。此外，人形机器人的自由度远多于任务所需的 6 个，多出来的冗余度正好用来完成“装饰音”——调整姿态、避开障碍、实现省力，这就是冗余度利用。

各声部再怎么交错，音乐的“重心”始终在指挥家手里。机器人的重心也一样：只要它乖乖落在脚掌画出的多边形里，手再忙、身体再扭，整体依然稳稳当当——这就是全身控制“边动边稳”的底气。

xcom=∑imixi∑imi  ∈  [xmin⁡, xmax⁡]\displaystyle x_{\mathrm{com}} = \frac{\sum_i m_i x_i}{\sum_i m_i} \;\in\; \big[ x_{\min},\, x_{\max} \big]
全身质量的平均位置就是重心。只要这个平均位置的地面投影落在双脚围成的多边形里，机器人就稳如泰山。
  1. 分子加权和每个连杆的质量乘以其位置，再全部相加。
    ∑imixi\displaystyle \sum_i m_i x_i
  2. 分母总质量把质量加权和归一化成“平均位置”。
    ∑imi\displaystyle \sum_i m_i
  3. 重心位置 x_com全身质量在地面的投影点，是平衡的“定盘星”。
    xcom\displaystyle x_{\mathrm{com}}
  4. 属于符号 ∈判断投影点是否落在安全区间内。
    ∈[xmin⁡,xmax⁡]\displaystyle \in \big[ x_{\min}, x_{\max} \big]
  5. 支撑多边形双脚脚印围出的凸区域，重力落在其中就不会产生倾倒力矩。
    xmin≤xcom≤xmax\displaystyle x_{min} \le x_{com} \le x_{max}

工程实践挑战：从排练厅到舞台

排练再完美，现场演出总有意外：琴弦跑音、观众掌声、舞台震动。工程实践就是处理这些“现场事故”，全身控制从仿真走向实物，有四大关要过。

第一关是实时计算瓶颈。控制频率 1kHz，意味着求解器加通信总共只有 1 毫秒预算；超时一次，就只能使用上一周期的旧指令，系统可能失稳。优化器要精心裁剪约束规模、使用避免动态内存分配的求解器、甚至用 FPGA 加速。第二关是模型不确定性。负载变了、关节磨损了、地面软硬不同，动力学方程里的 M、C、G 都不再准确，必须靠鲁棒控制、自适应参数或学习型模型兜底。

第三关是状态估计。真实机器人看不到自己的重心，只有编码器、IMU、力传感器这些“间接证据”，需要用卡尔曼滤波把多路信号融合出可靠的位姿、速度与接触力。第四关是安全与鲁棒。力矩限幅、碰撞检测、自碰撞避免、急停与遥操作切换，都是量产机器人必须内置的“安全气囊”。

现场指挥的每个拍点都必须卡准，迟到的指令就像迟到的鼓点，整首曲子都会乱套。全身控制从论文到产品，拼的正是这些“现场功夫”——实时、鲁棒、安全，一个都不能少。

Tsolve+Tcomm≤Ts,Ts=1fs\displaystyle T_{\mathrm{solve}} + T_{\mathrm{comm}} \le T_s, \qquad T_s = \frac{1}{f_s}
解算时间加通信时间必须小于控制周期。控制频率越高，留给每个周期的预算越少，实时性的弦就绷得越紧。
  1. 求解时间 T_solveQP 求解器算出全身力矩所花的时间。
    Tsolve\displaystyle T_{\mathrm{solve}}
  2. 通信时间 T_comm读写传感器与下发指令的网络延迟。
    Tcomm\displaystyle T_{\mathrm{comm}}
  3. 控制周期 T_s两次控制之间的间隔，1kHz 对应 1 毫秒。
    Ts=1fs\displaystyle T_s = \frac{1}{f_s}
  4. 不等式 ≤总时长一旦超周期，指令就过期，系统可能失稳。
    Tsolve+Tcomm≤Ts\displaystyle T_{\mathrm{solve}} + T_{\mathrm{comm}} \le T_s
  5. 控制频率 f_s必须覆盖机器人最重要的运动模态，否则高频振荡会被漏掉。
    fs≥2fmax\displaystyle f_s \ge 2 f_{\mathrm{max}}

AI 应用场景

仓储物流：人形机器人抱箱转身

阶段：运动规划与全身控制阶段

解决的问题：传统机械臂固定在基座上，抱起重物并转身时重心容易越出底座，导致倾覆。全身控制把“抱稳箱子”和“保持平衡”写成两个任务，在力矩分配中同时满足，让机器人像叉车司机一样用腿和腰协同发力。

具体案例：某物流仓库的人形机器人从货架抱起 15kg 纸箱，转身放到 AGV 上。控制器实时估计重心，将支撑腿后撤与躯干前倾的补偿力矩加入 QP，全程保持重心投影不离开双脚围成的支撑多边形。

家庭服务：机器人双手整理桌面

阶段：任务规划-运动生成-整机控制阶段

解决的问题：桌面整理需要“右手够杯、左手扶持、躯干微侧”同时进行。分层架构把任务拆解到不同层级，零空间投影确保次要任务（保持躯干姿态）不干扰主任务（手部精确到达）。

具体案例：服务机器人在厨房把盘子从桌面端到洗碗机。任务规划层决定先后顺序，运动生成层输出手的平滑轨迹，全身控制层在保证手部精度的同时调整髋部与膝部，既保持视线舒适又维持重心稳定。

灾害救援：四足机器人爬碎石坡

阶段：接触力优化与姿态控制阶段

解决的问题：复杂地形下四足机器人需要多足协同分配支撑力。全身控制将接触力优化与机体姿态控制结合，每毫秒重新分配四条腿的支撑力，同时保持机体水平、避免打滑。

具体案例：在震后废墟演练场，四足机器人通过松散的碎石坡时，全身控制器动态调整四足落点与支撑力比例，即便某只脚打滑，也会立刻把支撑力转移到其余三足，维持机身稳定。

Python 演示

python36 行
import numpy as np # 全身控制中的关节力矩分配(简化示例)# 原理: 已知任务需要的广义力 F(竖直支撑 + 水平推进)# 求解: min ||tau||^2  s.t. J tau = F, 即 KKT 方程# 用途: 把全身任务力"分配"到踝/膝/髋/肩各关节 class TorqueAllocator:    def __init__(self, jacobian, tau_limit):        self.A = np.asarray(jacobian, dtype=float)  # (m, n) 力矩到任务力的映射        self.tau_max = np.asarray(tau_limit, dtype=float)        self.n_joints = jacobian.shape[1]     def allocate(self, f_desired):        n = self.n_joints        m = self.A.shape[0]        kkt = np.zeros((n + m, n + m))        kkt[:n, :n] = 2.0 * np.eye(n)   # 目标: 全身力矩平方和最小        kkt[:n, n:] = self.A.T          # 等式约束的转置块        kkt[n:, :n] = self.A            # 等式约束: J tau = F        rhs = np.zeros(n + m)        rhs[n:] = f_desired             # 任务目标放在右端项        sol = np.linalg.solve(kkt, rhs)        tau = sol[:n]        return np.clip(tau, -self.tau_max, self.tau_max)  # 硬限幅保护电机 if __name__ == "__main__":    J = np.array([        [1.0, 0.8, 0.5, 0.2],  # 竖直支撑方向: 腿部贡献更大        [0.1, 0.2, 0.3, 0.6]   # 水平推进方向: 手臂贡献更大    ])    alloc = TorqueAllocator(J, tau_limit=[80, 150, 120, 50])    F = np.array([150.0, 30.0])  # 需要 150N 支撑 + 30N 推进    tau = alloc.allocate(F)    print("关节力矩(踝/膝/髋/肩):", np.round(tau, 2))    print("实际广义力:", np.round(J @ tau, 2))
目的：演示全身控制中最核心的“力矩分配”思想——当机器人需要同时产生竖直支撑力和水平推进力时，如何用最小二乘优化把这些任务力分配到每一个关节上。
  1. kkt[:n, :n] = 2.0 * np.eye(n)把“全身总共最省力”写成优化目标（力矩平方和最小）
  2. kkt[n:, :n] = self.A把“任务力必须达到”写成等式约束，这是任务进入控制器的关键
  3. rhs[n:] = f_desired等号右边放着机器人要实现的任务目标（支撑力与推进力）
  4. sol = np.linalg.solve(kkt, rhs)一次矩阵求解同时放出所有关节力矩，相当于指挥家的“一锤定音”
  5. np.clip(tau, -self.tau_max, self.tau_max)把超限力矩压回电机安全范围，这是硬约束的工程兜底
  6. print("实际广义力:", np.round(J @ tau, 2))用映射矩阵反向验证分配结果是否真的产生了目标任务力

常见误区

误区我一开始以为全身控制就是所有关节一起使劲，全身都发力，这样反应才快、动作才够猛。

正解全身控制的精髓是“需要时出力、不需要时不动”。它在一个优化问题里权衡任务优先级和物理约束，常常有些关节故意不发力，甚至反向制动——比如伸手拿东西时，膝盖微微后屈来把重心拉回脚掌中央。指挥家绝不会让所有乐手同时发出最大声。

为何常见：“全身”两个字容易让人误以为所有部位都要参与，忽视了“协同优化”其实包含“选择哪些关节出力”。

误区只要把质量、惯量都精确测出来，全身控制照着模型算就行，传感器和状态估计可以省掉。

正解模型永远有误差：负载变化、关节磨损、地面软硬都会让 M、C、G 偏离真实值。真实系统必须用卡尔曼滤波等估计器融合 IMU、编码器、力传感器，把模型输出和测量值互相校正。现场演出永远有意外，光靠排练谱子远远不够。

为何常见：教科书上的方程都是精确的，让人误以为实机也如此，忽略了“建模”和“真实世界”之间的缝隙。

误区1kHz 不够快，干脆跑到 10kHz，感觉频率越高反应越快、系统越稳。

正解频率不是免费的午餐。每个周期要完成状态估计、优化求解和通信，频率过高会让 QP 求解超时，反而得到“过期指令”。执行器本身也有带宽限制，盲目提高频率只会让系统在噪声里发抖。工程上选择能覆盖最重要运动模态的最小够用频率。指挥家的拍点再准，也要给乐手呼吸的时间。

为何常见：直觉上“越快越好”很合理，却忽略了计算开销和硬件执行带宽的约束。

误区既然次要任务在零空间里，那它就不重要，随便写写就行。

正解零空间任务在真实系统中的价值常被低估：保持关节居中可以防止撞限位，保持姿态能为下一步动作铺路，避碰任务更是安全底线。它虽然不改变主任务的结果，却是舒适性与安全性的关键——就像伴唱声部不抢主旋律，但少了它歌曲就单薄了。

为何常见：名字叫“次要”“零空间”，容易让人误以为等于“无所谓”，忽略了它在安全和舒适上的作用。

误区只有双足人形机器人才需要全身控制，普通机械臂或轮式机器人用不上。

正解移动机械臂、四足机器人、外骨骼都需要全身控制。任何“多个任务 + 多个关节 + 物理约束”的系统都适用。轮式底座加机械臂在伸出重臂搬运时，同样需要重心协调和力矩分配，只是支撑多边形的形状不同而已。

为何常见：全身控制的教科书例子大多是人形机器人，导致初学者误以为它是人形的专属技术。

面试题

Q1. 全身控制与传统“各关节独立控制”最本质的区别是什么？

单选题 · easy

  • A.采用更高精度的电机元件
  • B.在全系统范围做协同决策优化← 正确答案
  • C.完全不依赖任何传感器反馈
  • D.只适用于双足人形机器人

答案：B。独立关节控制只追求“每个关节角度到达指令值”，互相之间没有协调；全身控制把全部关节作为决策变量，在同一个优化问题里权衡任务、平衡与约束。它更像指挥家而非散装乐手。B 正确，A、C、D 与定义无关或过于片面。

Q2. 正向运动学解决的问题是什么？

单选题 · easy

  • A.根据关节角度求末端位置← 正确答案
  • B.根据末端位置反推关节角
  • C.计算电机的温度变化
  • D.估计地面的摩擦系数

答案：A。正向运动学指已知关节角度求末端位置，对应 x = f(q) 的方向；B 描述的是逆运动学，C、D 与运动学无关。就像乐手看到指法符号就知道按哪个键，正向运动学把“角度”翻成“位置”。

Q3. 雅可比矩阵 J 的核心作用之一是？

单选题 · mid

  • A.把关节速度映射为末端速度← 正确答案
  • B.把视觉图像映射为音频
  • C.直接输出关节的力矩上限
  • D.替代重力补偿的所有计算

答案：A。雅可比矩阵将关节速度映射为末端速度，即 ẋ = Jq̇；其转置还用于力映射 τ = JᵀF。它就像乐谱里的“移调表”，把关节空间的运动翻译成任务空间的运动。B、C、D 均与雅可比的数学定义无关。

Q4. 以下哪些属于全身控制中的典型约束？

多选题 · mid

  • A.关节力矩不能超过电机极限← 正确答案
  • B.关节角度不能超过机械限位← 正确答案
  • C.肢体之间不能发生自碰撞← 正确答案
  • D.电池电量必须始终显示在 UI

答案：ABC。全身控制的硬约束包含力矩上限、关节机械限位、自碰撞避免等，它们保护机器人的物理安全；D 属于上层 UI 功能，不是运动控制层面的约束。约束就像指挥家给乐手划定的“不能越过的音量红线”。

Q5. 动力学方程 τ=M(q)q̈+C(q,q̇)q̇+G(q) 中，C(q,q̇)q̇ 代表什么？

单选题 · mid

  • A.地球重力对连杆的拉力
  • B.科氏力和离心力的耦合← 正确答案
  • C.电机内阻发热损耗
  • D.关节编码器的噪声

答案：B。C(q,q̇)q̇ 项是科氏力与离心力的合成，来自连杆间相对运动引起的耦合效应；A 是 G(q)，C、D 与动力学模型无关。它就像多个乐器同奏时空气振动互相影响，属于“运动中的互相牵拉”。

Q6. 关于 QP 中的任务权重 w，下列说法正确的是？

多选题 · hard

  • A.主任务权重通常设置得更大← 正确答案
  • B.权重一旦设定就不能改变
  • C.权重表达任务之间的相对优先级← 正确答案
  • D.所有任务权重必须完全相等

答案：AC。权重 w 表达任务的相对优先级，主任务（如保持平衡）权重更大，优化器会优先消除它的误差；权重可以根据场景动态调整，并非一成不变；D 让所有任务平等反而失去了主次。权重如同指挥家对声部音量比例的现场调度。

Q7. “重心投影必须落在支撑多边形内”这条判据主要用于？

单选题 · easy

  • A.判断机器人是否会摔倒← 正确答案
  • B.判断机器人是否联网
  • C.判断关节是否过热
  • D.判断电量是否充足

答案：A。这条判据判定静态或准静态稳定：重心投影落在双脚围成的多边形内，重力线不会产生倾倒力矩，机器人就不会倒；慢速运动时 ZMP 判据也退化为它。B、C、D 与平衡稳定性无关。

Q8. 零空间投影 (I−J†J)q̇_h 的作用是什么？

单选题 · mid

  • A.让次要任务不影响主任务← 正确答案
  • B.让主任务完全无法执行
  • C.把所有任务都变成硬约束
  • D.消除重力对系统的全部影响

答案：A。(I−J†J) 是零空间投影矩阵，把它乘上次要任务速度 q̇_h，得到的运动对主任务没有任何影响，这就是“不打扰主旋律的装饰音”。B、C、D 是对零空间概念的曲解。

Q9. 为什么真实机器人需要状态估计而不能只靠动力学模型？

多选题 · hard

  • A.模型参数存在不确定性误差← 正确答案
  • B.传感器测量自身有噪声漂移← 正确答案
  • C.环境扰动很难被提前建模← 正确答案
  • D.为了让代码看着更加复杂

答案：ABC。模型参数误差、传感器噪声与漂移、环境扰动三者都是真实世界的常态，状态估计用多路测量融合来修正模型预测；D 没有实际意义。就像现场演出的音准，不能只靠排练时的谱子，必须实时听、实时调。

Q10. 控制频率从 1kHz 提高到 10kHz 后，下列说法最合理的是？

单选题 · extreme

  • A.性能必定线性提升十倍
  • B.求解超时风险上升并可能失稳← 正确答案
  • C.模型误差会自动消失
  • D.电机发热问题必然消失

答案：B。频率提高十倍意味着每个周期预算缩到 0.1ms，QP 求解极易超时，超时后使用旧指令反而破坏稳定性；性能不会线性提升，模型误差与电机发热也不会因频率升高而消失。频率的选择是工程权衡，而非越大越好。