PythonAI算法
机器人全身控制（WBC）原理详解
机器人全身控制（WBC）的概念与原理。WBC 将多任务目标与物理约束转化为优化问题，在动力学方程基础上求解关节力矩。核心涉及雅可比矩阵、动力学方程及二次规划（QP）求解器。文中提供了基于 Python cvxpy 的代码示例，并对比了 WBC 与 MPC 的关系，阐述了其在双足机器人运动控制中的应用逻辑。
机器人全身控制（WBC）的概念与原理。WBC 将多任务目标与物理约束转化为优化问题，在动力学方程基础上求解关节力矩。核心涉及雅可比矩阵、动力学方程及二次规划（QP）求解器。文中提供了基于 Python cvxpy 的代码示例，并对比了 WBC 与 MPC 的关系，阐述了其在双足机器人运动控制中的应用逻辑。
WBC（Whole-Body Control，全身控制）是什么？机器人是由'各关节'组成的，其不是'各关节各玩各的'而是一个耦合的整体。在某个时刻可能要做很多事情，比如保持平衡（重心别出圈）、手/脚要动作到目标位置、躯干姿态不能乱、关节不能超限、脚下不能打滑。这些都是一系列任务的组合。
WBC 的核心就是把这些任务（目标）和约束（物理/安全）写进一个小型优化问题，在每个控制周期（几百 Hz~1kHz）求解，得到**'当下这毫秒，各关节应该怎么动/用多大力'**。
一句话总结就是 WBC 就是用优化的方法求解出要给'关节多少力'，以便让机器的各个关节一起配合完成多个目标，且不违反物理与安全约束。
要解释 WBC 的原理，那必须绕不开动力学方程，这里就先对动力学方程做个简单介绍。
$$ M(q)\dot{v} + h(q,v) = S^T \tau + J_c^T \lambda $$
配合接触约束：
$$ J_c v = 0, \quad \lambda \in \text{摩擦锥} $$
通俗理解公式就是：'惯性 × 加速度 + 自然出现的力 = 电机能给的力 + 地面/物体的反力'。
直观记忆就是类比现实生活中的推车子：
还要满足接触力约束 $J_c v = 0, \lambda \in \text{摩擦锥}$，其中 $J_c v = 0$ 意义是接触点速度为 0，比如机器人脚贴在地上不滑、不穿透；$\lambda \in \text{摩擦锥}$ 意义是接触力必须满足摩擦模型，脚不会不穷大摩擦，力要在摩擦椎范围内。
假设有一个机械臂：
那么问题就是关节角度动一动，末端的位置会怎么动了？这个'关节空间的微小变化'影响到'末端空间的微小变化'。这样一个映射关系，就是雅可比矩阵 $J$。
图中红色箭头表示关节角度的小变化 $\Delta \theta_1, \Delta \theta_2$。红色箭头的变换导致绿色箭头末端位置的变化：$\Delta x, \Delta y$。雅可比矩阵 $J$ 就是把
$$ \begin{bmatrix}\Delta \theta_1 \ \Delta \theta_2\end{bmatrix} \longrightarrow \begin{bmatrix}\Delta x \ \Delta y\end{bmatrix} $$
因此如果想要末端动多少就用 $J$，想算末端力传回关节多少就用 $J^T$。
总结一下雅可比 $J$ 就是关节空间和任务空间的桥梁，作用就是我们关节动多少，末端/接触点动多少。
动力学方程是机器人身体运动的'牛顿定律'，我们来看看 WBC 的目标是什么？WBC 不只是让机器人'走'或'站'，而是要全身协调，比如要去抓杯子，脚要保持不滑，躯干要保持平衡，关节力矩不能超过电机限制。所以 WBC 的本质是解一个优化问题，找出一组关节力矩 $\tau$，既能完成任务目标，又满足动力学方程和约束。
WBC 的核心思路是把机器人全身的目标任务转化为优化问题，在满足物理规律和约束条件的前提下，求出最合适的一组关节力矩 $\tau$。
具体一点在 WBC 中求解的决策变量通常是如下三个：
$$ x = \begin{bmatrix} \dot{v} \ \tau \ \lambda \end{bmatrix} $$
$$ \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 加速度）。
同时优化问题还要满足以下约束条件：
总结一下优化问题的目标函数意思就是要满足任务误差最小化（手/身体/质心的加速度跟踪目标），同时要满足能量或力矩最小化（不能浪费力），同时满足接触力正则化（力要稳定不能乱跳）。
这个目标函数是一个二次型，符合 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)
上面的示例中可以分为几部分：
末端（手）在水平 x 方向产生 1.0 m/s² 的加速度。在垂直 y 方向不要加速（因为手撑在桌子上，不应该离开桌面）。
数学写法：
$$ \ddot{x}_{des} = [1.0, 0.0]^T $$
优化器要决定的量是
$$ \ddot{q} = [\ddot{\theta}_1, \ddot{\theta}_2]^T, \quad \tau = [\tau_1, \tau_2]^T, \quad \lambda $$
最小化
$$ \min_{\ddot{q},;\tau,;\lambda}| J_{\text{task}} \ddot{q} - \ddot{x}_{des} |^2+ 0.01 |\tau|^2+ 0.01 |\lambda|^2 $$
最后调用
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{x}_{des}$ 对比。
上面的代码中只截取了关键部分，下面是绘制图像的效果如下，可以直观体会看看：
上一篇文章我们分析了 MPC，那么 MPC 与 WBC 什么关系了？
MPC 在 WBC 之上，MPC 作为决策层做'未来几步的规划'，比如预测未来 1S 内，质心应该怎么移动，脚该放哪里，其输出的是期望的任务轨迹。WBC 是在下层，拿到 MPC 给的任务（目标加速度/姿态/接触序列）在动力学和接触约束下，求解 QP 得到当下这一瞬间的关节力矩 $\tau$。简而言之 MPC 决定'机器人未来要往哪走'，WBC 决定'当前每个关节该怎么出力'。
举个例子：双足机器人走路
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。 在线工具，curl 转代码在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online