WBC基础知识

人形机器人的全身控制（Whole-Body Control，WBC）听上去复杂，但它底层的物理其实只有几条主线。这篇笔记把 WBC 涉及的基础知识按「由浅入深」的顺序串起来：先建立平动与转动、力矩、运动学与雅可比矩阵这些基本工具，再写出机器人的动力学方程，最后说明 WBC 如何被形式化为一个带约束的优化问题，以及阻抗控制如何让关节表现出期望的柔顺性。全文以物理直觉为主，公式只用来把直觉说准确。

平动与转动

机器人的运动可以分解为两种最基本的形式：平动和转动。两者的物理量一一对应：

平动（Translation）	转动（Rotation）
位置 $x$	角度 $\theta$
速度 $v=\dot{x}$	角速度 $\omega =\dot{\theta }$
加速度 $a=\ddot{x}$	角加速度 $\alpha =\dot{\omega }$
质量 $m$	转动惯量 $I$
力 $F$	力矩 $\tau$
$F=ma$	$\tau =I\alpha$（固定轴简化形式）
动量 $p=mv$	角动量 $L=I\omega$（固定轴简化形式）

可以说，力矩之于转动，就像力之于平动。接下来我们专门看力矩。

力矩

力矩描述力使物体绕某个轴转动的能力：

$$\boxed{{\displaystyle \mathit{\tau }=\mathbf{r}\times \mathbf{F}}}$$

其大小为：

$$|\tau |=rF\sin \theta$$

其中：

• $r$：从转轴指向受力点的位置矢量的长度；
• $F$：力的大小；
• $\theta$：位置矢量与力之间的夹角；
• 单位：N·m。

正逆运动学

如果已知每个关节的旋转角度，就能计算出机器人脚（末端）的位置，这叫做正运动学（Forward Kinematics，FK）。反过来，如果希望脚移动到某个指定位置，就需要反算各个关节应该旋转多少角度，这叫做逆运动学（Inverse Kinematics，IK）。

正运动学 FK	逆运动学 IK
关节角度 $q$	期望末端位置 $p^{\ast }$
末端位置 $p$	关节角度 $q$
$p=f(q)$	$q=f^{-1}(p^{\ast })$

注意，FK 和 IK 都属于运动学（kinematics），描述的是「角度 ↔ 位置」之间的几何映射，不涉及力和质量；右侧的逆函数写法只是示意，实际 IK 不一定存在唯一解。

雅可比矩阵（Jacobian）

如果一个函数有多个输入和多个输出：

$$\mathbf{y}=f(\mathbf{x})$$

那么它的导数就是雅可比矩阵：

$$J=\frac{\partial f}{\partial \mathbf{x}}$$

Jacobian 的本质，是描述输入发生微小变化时输出会如何变化。在机器人里：

• 输入：各个关节的角度 $q$；
• 输出：机器人末端的位置 $p$。

因为：

$$p=f(q)$$

对时间求导，利用链式法则：

$$\boxed{{\displaystyle \dot{p}=J(q)\dot{q}}}$$

它表示：关节角速度经过 Jacobian 映射，就能得到机器人末端的线速度。

用 Jacobian 反求关节速度

前面得到：

$$\dot{p}=J\dot{q}$$

现在反过来：假设希望足部以某个速度 ${\dot{p}}^{\ast }$ 运动，各关节应该以什么速度运动？如果 $J$ 是可逆的方阵，直接求逆即可：

$$\dot{q}=J^{-1}{\dot{p}}^{\ast }$$

但真实机器人往往有冗余自由度，Jacobian 也可能不是方阵，因此更常见的做法是用伪逆：

$$\boxed{{\displaystyle \dot{q}=J^{+}{\dot{p}}^{\ast }}}$$

这就是速度层面的 IK。需要注意，伪逆给出的是最小范数解或最小二乘解，不一定满足全部关节限制。

用 Jacobian 把末端力映射到关节力矩

这是 WBC 中极为关键的公式：

$$\boxed{{\displaystyle \tau =J^{\mathrm{\top }}F}}$$

它说明：末端受到的力，可以通过 Jacobian 的转置转换成关节上的广义力矩。这里的 $\tau$ 表示末端力对应的关节广义力；如果要计算电机需要施加的平衡力矩，还要考虑力的方向、重力及其他外力。

为什么是转置 $J^{\mathrm{\top }}$，而不是 $J$ 呢？用大学物理中的功就能推导。我们知道：

$$dW=F^{\mathrm{\top }}dp$$

同时有：

$$dp=Jdq$$

代入得：

$$dW=F^{\mathrm{\top }}Jdq$$

根据矩阵转置的性质：

$$dW=(J^{\mathrm{\top }}F{)}^{\mathrm{\top }}dq$$

而关节力矩做的虚功形式是：

$$dW={\tau }^{\mathrm{\top }}dq$$

因此：

$$\boxed{{\displaystyle \tau =J^{\mathrm{\top }}F}}$$

这就是虚功原理在机器人中的应用。

WBC 的动力学方程

WBC 真正要解决的问题是：如何让由多个刚体组成的机器人同时满足动力学规律、接触约束和运动目标。

机器人的动力学方程可以写成：

$$\boxed{{\displaystyle M(q)\dot{v}+h(q,v)=S^{\mathrm{\top }}\tau +J_c^{\mathrm{\top }}\lambda }}$$

其中：

• $q$：浮动基座位姿和关节角度；
• $v$：广义速度，包括基座线速度、角速度和关节速度；
• $\dot{v}$：广义加速度；
• $M(q)$：机器人全身质量矩阵；
• $h(q,v)$：重力、科里奥利力、离心力等项；
• $S^{\mathrm{\top }}\tau$：关节电机产生的广义力；
• $J_c^{\mathrm{\top }}\lambda$：地面接触产生的广义力。

之所以这里用 $v,\dot{v}$ 而不是统一写 $\dot{q},\ddot{q}$，是因为用四元数或旋转矩阵表示基座姿态时，广义速度不一定等于坐标的普通时间导数。右侧两项分别代表电机作用和地面接触作用，人形机器人的全身运动正是在这两类作用的共同影响下产生的。

优化式 WBC

考虑最简单的机器人站立任务。

我们希望机器人满足：

$${\ddot{p}}_{\mathrm{CoM}}=0$$

也就是质心保持静止。同时希望脚固定在地面上：

$$J_c\dot{v}+{\dot{J}}_{c}v=0$$

接触力还需满足：

$${\lambda }_n\ge 0$$$$\parallel {\lambda }_t\parallel \le \mu {\lambda }_n$$

同时电机输出力矩有限：

$${\tau }_{min}\le \tau \le {\tau }_{max}$$

因此我们不能任意选择关节力矩，必须同时考虑运动目标、接触条件、动力学方程以及电机的物理限制。

一个典型的优化式 WBC，可以把 $\dot{v},\tau ,\lambda$ 作为优化变量，例如最小化任务加速度的跟踪误差：

$$\underset{\dot{v},\tau ,\lambda }{min}{\Vert J_{\mathrm{task}}\dot{v}+{\dot{J}}_{\mathrm{task}}v-a_{\mathrm{des}}\Vert }^2$$

同时满足约束：

$$\{\begin{array}{l}M\dot{v}+h=S^{\mathrm{\top }}\tau +J_c^{\mathrm{\top }}\lambda \\ J_c\dot{v}+{\dot{J}}_{c}v=0\\ {\lambda }_n\ge 0\\ \parallel {\lambda }_t\parallel \le \mu {\lambda }_n\\ {\tau }_{min}\le \tau \le {\tau }_{max}\end{array}$$

这里假设 $J_{\mathrm{task}}$ 对应某个需要控制的任务空间变量，$a_{\mathrm{des}}$ 是其期望加速度；接触约束针对当前处于稳定支撑状态的接触点。实际 WBC 可能采用 QP、层级优化或其他控制算法，目标函数也未必是上面这个。但这个例子已经能说明 WBC 的核心：让机器人在满足物理约束的前提下，尽可能实现我们希望的运动。

阻抗控制（Impedance Control）

前文说的都是「把位置或力跟踪到目标」，属于位置/力控制的思路。阻抗控制则是另一种视角：不硬性追踪某个位置，而是让关节在与环境交互时表现出期望的弹簧—阻尼性质。下面先分别看刚度与阻尼这两个要素。

刚度：关节像一根弹簧

机器人关节也可以模拟弹簧。假设膝关节的目标角度是 $q_d$，当前角度是 $q$，我们希望关节像扭转弹簧一样，偏离目标角度后自动产生恢复力矩：

$$\boxed{{\displaystyle {\tau }_{\mathrm{spring}}=K_p(q_d-q)}}$$

这和胡克定律完全类似，只是：

• 直线位移 $x$ 变成关节角度 $q$；
• 力 $F$ 变成力矩 $\tau$；
• 线性弹簧刚度 $k$ 变成关节旋转刚度 $K_p$。

旋转刚度的单位是 $\mathrm{N}\cdot \mathrm{m}/\mathrm{rad}$。

阻尼：耗散能量、抑制振荡

阻尼可以理解成一种阻碍相对运动、耗散机械能的作用，最经典的模型是黏性阻尼：

$$\boxed{{\displaystyle F_d=-b\dot{x}}}$$

其中：

• $b$：阻尼系数，单位 N·s/m；
• $\dot{x}$：物体运动速度；
• $F_d$：阻尼力。

与弹簧不同，阻尼力取决于速度而不是位置。对机器人关节而言，阻尼力矩可以表示为：

$$\boxed{{\displaystyle {\tau }_{\mathrm{damping}}=-K_d\dot{q}}}$$

如果希望跟踪一个具有目标角速度 ${\dot{q}}_d$ 的轨迹，则可以写成：

$${\tau }_{\mathrm{damping}}=K_d({\dot{q}}_d-\dot{q})$$

注意这里的 $K_d$ 与前面的刚度 $K_p$ 完全不同：

• $K_p$ 负责纠正角度偏差；
• $K_d$ 负责纠正速度偏差、抑制振荡。

两者结合，就得到经典的 PD 控制：

$$\boxed{{\displaystyle \tau =K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})}}$$

关节阻抗控制与 PD

对于机器人关节，我们希望它在偏离目标角度时表现得像一个虚拟弹簧—阻尼系统，于是让电机产生：

$$\boxed{{\displaystyle {\tau }_{\mathrm{motor}}=K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})}}$$

假设目标角度固定不变（${\dot{q}}_d=0$），并暂时忽略重力和关节之间的动力学耦合，关节满足：

$$I\ddot{q}={\tau }_{\mathrm{motor}}+{\tau }_{\mathrm{ext}}$$

代入 PD 控制律：

$$I\ddot{q}=K_p(q_d-q)-K_d\dot{q}+{\tau }_{\mathrm{ext}}$$

整理得：

$$\boxed{{\displaystyle I\ddot{q}+K_d\dot{q}+K_p(q-q_d)={\tau }_{\mathrm{ext}}}}$$

它已经变成了质量—弹簧—阻尼方程，解这个微分方程就能得到响应。其中：

• $I$：关节及负载的有效转动惯量；
• $K_d$：虚拟阻尼；
• $K_p$：虚拟弹簧刚度；
• $q_d$：虚拟弹簧的平衡角度。

如果希望关节表现出特定的二阶响应（自然频率 ${\omega }_n$、阻尼比 $\zeta$），可以按下面的关系选择增益：

$$\boxed{{\displaystyle \begin{array}{rl}{K}_{p,j}& =I_j{\omega }_n^2\\ K_{d,j}& =2I_j\zeta {\omega }_n\end{array}}}$$

所以 PD 控制实际上能让关节表现出某种弹簧—阻尼性质，而机器人的实际惯性则与这些虚拟参数共同决定关节的完整响应。严格来说，这只是关节级阻抗控制的一种常见实现，并非阻抗控制理论的全部。

高阻抗与低阻抗

在传统的高增益 Motion Tracking 中，机器人的关节可能表现得非常僵硬：Policy 给定一个设定点，低层控制器会产生较大的力矩，强行把关节拉到设定点。这种方式更容易跟踪关节位置，但也带来一些问题：

	高阻抗	相对低阻抗
关节偏离设定点	较小	可以较大
外界推动时	抵抗较强	更容易顺应
关节位置误差放大	更强	相对较弱
碰撞时	可能产生较大反作用力峰值	有机会改善冲击吸收
精确位置伺服	通常更容易	对控制策略要求更高
训练重点	容易较依赖位置伺服	需要学会利用动力学和反馈

为什么低阻抗可能有助于 RL？因为在低增益下，Policy 不能只依赖 PD 控制器强行把每个关节拉到参考角度，它必须学会考虑当前姿态、关节速度、惯性和接触状态，提前产生合适的控制作用。例如，机器人准备落地时，Policy 需要在着地前就协调好腿部姿态，而不是等落地之后才依赖很大的关节位置误差反馈来纠正动作。

不过这里必须区分清楚：低阻抗本身不会自动让机器人学会更好的动作，它改变的只是底层执行器的动态特性。RL 能否学会稳定控制，还取决于奖励设计、训练数据、动作空间、仿真精度和策略优化。

电机转子惯量与反射惯量（Armature）

为什么需要减速器？

机器人电机往往转速较高，但直接输出的力矩有限，因此关节通常使用减速器（Gearbox）：

$${\omega }_{\mathrm{motor}}=g{\omega }_{\mathrm{joint}}$$

其中 $g$ 是减速比，定义为电机转速与关节输出转速之比。若忽略能量损失：

$${\tau }_{\mathrm{joint}}=g{\tau }_{\mathrm{motor}}$$

例如减速比为 10：电机转 10 圈，关节转 1 圈；理想情况下，关节侧力矩放大 10 倍。

为什么转子惯量要乘上减速比的平方？

电机内部旋转的部件称为转子（Rotor），假设其转动惯量为 $I_{\mathrm{rotor}}$，转速满足：

$${\dot{q}}_{\mathrm{motor}}=g{\dot{q}}_{\mathrm{joint}}$$

根据转动动能公式：

$$E_k=\frac{1}{2}{I}_{\mathrm{rotor}}{\dot{q}}_{\mathrm{motor}}^2$$

代入减速比关系：

$$E_k=\frac{1}{2}{I}_{\mathrm{rotor}}{g}^2{\dot{q}}_{\mathrm{joint}}^2$$

如果从关节一侧观察，希望把它等效为：

$$E_k=\frac{1}{2}{I}_{\mathrm{reflected}}{\dot{q}}_{\mathrm{joint}}^2$$

那么：

$$\boxed{{\displaystyle I_{\mathrm{reflected}}=g^2{I}_{\mathrm{rotor}}}}$$

这就是反射惯量：电机自身的转子惯量经过减速器之后，从关节输出侧看，表现为放大了 $g^2$ 倍的等效惯量。

举例，若：

$$I_{\mathrm{rotor}}=0.0001\mathrm{kg}\cdot {\mathrm{m}}^2$$$$g=20$$

那么：

$$I_{\mathrm{reflected}}=400\times 0.0001=0.04\mathrm{kg}\cdot {\mathrm{m}}^2$$

这个量可能明显影响关节加速和减速时所需的力矩。实际上，完整的关节等效惯量不仅包含电机转子，也包含传动机构及机器人连杆的贡献。Isaac Sim 等仿真器中的 Armature 是真实的物理惯性贡献，而不仅仅是仿真器里为了数值稳定随意调节的参数。

小结

回顾整条线索：平动与转动 → 力矩 → 正逆运动学 → 雅可比矩阵 → 动力学方程 → 优化式 WBC → 阻抗控制。WBC 的核心思想可以浓缩为一句话：在满足动力学、接触和驱动等物理约束的前提下，让机器人尽可能实现期望的运动。阻抗控制则提供了另一种视角——不硬性追踪某个位置，而是让关节表现出指定的弹簧—阻尼性质，从而在与环境交互时更柔顺。掌握这些基础后，再去读 QP、层级 WBC 或 RL 落地控制的相关论文，会顺畅很多。