147、全身控制WBC：人形机器人多任务优先级控制调试间里那台双足平台又摔了，第七次。日志里清清楚楚写着——左腿髋关节力矩饱和，上身还在拼命执行一个“保持相机朝向”的任务。这不是控制算法没写对，是压根没想清楚优先级。人形机器人身上同时挂着十几个任务：别摔倒、别撞到、手别抖、头别歪、重心别偏……你让它们平起平坐，结果就是谁都干不好。今天这篇笔记，咱们把全身控制（Whole-Body Control，WBC）这层窗户纸捅破，从数学原理到代码实现，再到我踩过的那些坑，一次说透。为什么单关节PID不够用了先回到最朴素的直觉。你给每个关节写个PD控制器，目标角度来自运动学逆解，这活儿在机械臂上干得挺好。但人形机器人不一样——它是一个浮动基座系统，脚底跟地面没有固定螺栓，躯干和四肢之间存在强烈的动力学耦合。你单独调左腿的髋关节，右腿一迈步，重心一偏，左腿的力矩需求立刻变了。更麻烦的是，任务之间会打架：你想让手保持水平，同时让重心前移，这两个目标在关节空间里可能指向完全相反的方向。单关节PID根本没法协调这种冲突，它只会让每个关节各自为战，最后整个系统像喝醉了酒一样东倒西歪。WBC的核心思路就一句话：把所有任务放进一个统一的优化框架里，按优先级排序，高优先级的任务先满足，低优先级的任务在剩余的自由度里尽量做。这就像开会，老板（稳定性）先发言，中层（操作任务）等老板讲完再补充，基层（姿态优化）最后捡剩下的时间说两句。数学框架：从任务空间到关节力矩咱们把问题形式化。机器人广义坐标是 ( q \in \mathbb{R}^n )，浮动基座那6个自由度加上所有关节