机器人全身控制（WBC）原理详解

动力学方程是机器人身体运动的'牛顿定律'，我们来看看 WBC 的目标是什么？WBC 不只是让机器人'走'或'站'，而是要全身协调，比如要去抓杯子，脚要保持不滑，躯干要保持平衡，关节力矩不能超过电机限制。所以 WBC 的本质是解一个优化问题，找出一组关节力矩 $\tau$，既能完成任务目标，又满足动力学方程和约束。

优化问题

WBC 的核心思路是把机器人全身的目标任务转化为优化问题，在满足物理规律和约束条件的前提下，求出最合适的一组关节力矩 $\tau$。

具体一点在 WBC 中求解的决策变量通常是如下三个：

$$ x = \begin{bmatrix} \dot{v} \ \tau \ \lambda \end{bmatrix} $$

• 最优的关节力矩 $\tau$。
• 接触点反力 $\lambda$。
• 机器人下一步的加速度 $\dot{v}$。

优化问题转换为数学的目标函数如下

$$ \min_{x} | J_{\text{task}} \dot{v} - \dot{v}_{\text{des}} |^2 + | \tau |^2 + | \lambda |^2 $$

公式中 $J_{\text{task}} \dot{v} - \dot{v}{\text{des}}$ 表示实际加速度与期望加速度的误差，$J{\text{task}} \dot{v}$ 是在当前关节加速度下，末端/任务空的实际加速度，而 $\dot{v}_{\text{des}}$ 是我们期望任务空间实现的期望加速度（比如手往前加速 0.5 m/s²，质心保持 0 加速度）。

同时优化问题还要满足以下约束条件：

• 动力学约束：$M(q)\dot{v} + h(q,v) = S^T \tau + J_c^T \lambda$。这是硬约束，控制器必须遵守。
• 接触约束：$J_c v = 0$，接触点不能乱动（不滑、不穿透）。
• 摩擦约束：$\lambda \in \mathcal{K}_{\text{fric}}$，接触力必须符合摩擦模型（不能无限大）。
• 力矩限制：$\tau_{\min} \leq \tau \leq \tau_{\max}$。

总结一下优化问题的目标函数意思就是要满足任务误差最小化（手/身体/质心的加速度跟踪目标），同时要满足能量或力矩最小化（不能浪费力），同时满足接触力正则化（力要稳定不能乱跳）。

方程有了，怎么求解了？

这个目标函数是一个二次型，符合 QP，所以可以用现成的 QP 求解器来解，例如：OSQP、qpOASES、Gurobi（商业求解器）、CPLEX（商业求解器）、CVXPy（Python 封装，常用于原型），这里就不过多阐述了。

总结一下 WBC 核心就是要解决一个优化问题：二次目标（误差最小 + 力矩正则）+ 动力学/接触/摩擦/限幅约束。其求解的方式通常使用 QP 求解器（实时、高效、全局最优）。求解的结果是关节力矩 $\tau$（给电机执行），同时还得到加速度 $\dot{v}$ 和接触力 $\lambda$。

示例

接下来我们调用 cvxpy 库看看示例，直观体验一下。

import cvxpy as cp
import numpy as np

# ---- 机械臂参数 ----
l1, l2 = 1.0, 1.0
m1, m2 = 1.0, 1.0
theta1, theta2 = np.deg2rad(45), np.deg2rad(30)

# ---- 雅可比（末端位置对关节的导数）----
J_task = np.array([
    [-l1*np.sin(theta1)- l2*np.sin(theta1+theta2), -l2*np.sin(theta1+theta2)],
    [ l1*np.cos(theta1)+ l2*np.cos(theta1+theta2),  l2*np.cos(theta1+theta2)]
])

# ---- 动力学质量矩阵 M（简化版）----
M = np.array([
    [m1*l1**2+ m2*(l1**2+ l2**2+2*l1*l2*np.cos(theta2)), m2*(l2**2+ l1*l2*np.cos(theta2))],
    [m2*(l2**2+ l1*l2*np.cos(theta2)), m2*l2**2]
])
h = np.zeros(2) # 忽略重力/科氏项

# ---- 接触约束：假设末端 y 方向不能动（竖直方向约束）----
Jc = np.array([[0,1]]) @ J_task # 取末端 y 方向的行

# ---- 变量 ----
ddq = cp.Variable(2) # 关节加速度
tau = cp.Variable(2) # 力矩
lam = cp.Variable(1) # 接触力 (竖直反作用力)

# ---- 期望任务加速度（末端 x 方向=1.0, y 方向=0.0）----
xddot_des = np.array([1.0, 0.0])

# ---- 目标函数：末端任务 + 力矩/接触力正则 ----
objective = cp.Minimize(
    cp.sum_squares(J_task @ ddq - xddot_des) +
    0.01*cp.sum_squares(tau) +
    0.01*cp.sum_squares(lam)
)

# ---- 约束 ----
constraints = [
    M @ ddq + h == tau + Jc.T @ lam, # 动力学方程
    Jc @ ddq == 0,                   # 接触点不加速
    tau >= -10, tau <= 10,           # 力矩范围
    lam >= 0                         # 接触力必须向上推
]

# ---- 求解 ----
prob = cp.Problem(objective, constraints)
prob.solve()

print("Optimal joint accelerations:", ddq.value)
print("Optimal torques:", tau.value)
print("Optimal contact force λ:", lam.value)
print("End-effector acc achieved:", J_task @ ddq.value)
print("Desired end-effector acc :", xddot_des)

上面的示例中可以分为几部分：

（1）任务

末端（手）在水平 x 方向产生 1.0 m/s² 的加速度。在垂直 y 方向不要加速（因为手撑在桌子上，不应该离开桌面）。

数学写法：

$$ \ddot{x}_{des} = [1.0, 0.0]^T $$

（2）决策变量

优化器要决定的量是

$$ \ddot{q} = [\ddot{\theta}_1, \ddot{\theta}_2]^T, \quad \tau = [\tau_1, \tau_2]^T, \quad \lambda $$

（3）要优化的目标函数

最小化

$$ \min_{\ddot{q},;\tau,;\lambda}| J_{\text{task}} \ddot{q} - \ddot{x}_{des} |^2+ 0.01 |\tau|^2+ 0.01 |\lambda|^2 $$

（4）约束条件

• 动力学约束：$M(q)\ddot{q} + h(q,\dot{q}) = \tau + J_c^T \lambda$
• 接触约束：$J_c \ddot{q} = 0$
• 力矩范围：$-10 \leq \tau_i \leq 10$
• 接触力非负：$\lambda \geq 0$

（5）输出结果

最后调用

prob = cp.Problem(objective, constraints)
prob.solve()

求解得出结果：

Optimal joint accelerations: [0.50615867 -1.88900987]
Optimal torques: [-1.12977187 -0.94450493]
Optimal contact force λ: [1.29515155e-23]
End-effector acc achieved: [9.77823461e-01 -5.00938335e-17]
Desired end-effector acc : [1.0.]

QP 解出来的就是

• 最优关节加速度：$\ddot{q}$
• 最优关节力矩：$\tau$
• 接触反力：$\lambda$
• 实际末端加速度：$\ddot{x} = J_{\text{task}} \ddot{q}$

并与期望值 $\ddot{x}_{des}$ 对比。

上面的代码中只截取了关键部分，下面是绘制图像的效果如下，可以直观体会看看：

WBC 与 MPC

上一篇文章我们分析了 MPC，那么 MPC 与 WBC 什么关系了？

MPC 在 WBC 之上，MPC 作为决策层做'未来几步的规划'，比如预测未来 1S 内，质心应该怎么移动，脚该放哪里，其输出的是期望的任务轨迹。WBC 是在下层，拿到 MPC 给的任务（目标加速度/姿态/接触序列）在动力学和接触约束下，求解 QP 得到当下这一瞬间的关节力矩 $\tau$。简而言之 MPC 决定'机器人未来要往哪走'，WBC 决定'当前每个关节该怎么出力'。

举个例子：双足机器人走路

• MPC：优化未来 1 秒的 质心轨迹、摆腿位置、支撑相切换。输出：期望的 $\ddot{x}_{des}$（质心加速度）、脚的落点计划。
• WBC：把这些期望当作任务输入，解 QP，输出关节力矩 $\tau$，同时计算接触力 $\lambda$，保证机器人在每一步不摔倒。