人形机器人 Whole-Body Control（WBC）控制方法介绍
1. 基本概念
Whole-Body Control（WBC）是一种基于任务优先级的实时全身运动与力控制框架，用于协调人形机器人多个自由度（DoF），同时满足运动学/动力学约束与多任务目标（如平衡、手部操作、足部接触等）。
其核心思想是：
在满足物理约束的前提下，通过分层优化或加权融合，使机器人同时完成多个控制任务。
典型应用场景包括：
- 双足行走中的质心跟踪与ZMP稳定；
- 手臂抓取物体的同时保持躯干平衡；
- 抗外部扰动时的全身协调响应。
2. 系统建模基础
设机器人具有 n 个关节，广义坐标为 q ∈ ℝⁿ（通常包含基座6D位姿和关节角）。其前向运动学和动力学模型为：
- 运动学：ẋ = J(q) · q̇
- 动力学：M(q) · q̈ + h(q, q̇) = Sᵀτ + J_cᵀ · F
其中：
- x 为任务空间变量（如手部位姿、质心位置等）；
- J(q) 为对应任务的雅可比矩阵；
- M(q) 为质量矩阵；
- h(q, q̇) 包含科里奥利力、离心力和重力项；
- S 为关节选择矩阵（通常 S = [0₆ₓ₆ Iₙₓₙ]）；
- τ 为关节力矩；
- F 为环境接触力（如足底GRF）；
- J_c 为接触点雅可比矩阵。
3. WBC 的典型实现方式
（1）分层任务优先级法（Hierarchical Quadratic Programming, HQP）
将控制任务按优先级排序（如 P₁ > P₂ > P₃），依次求解带约束的二次规划（QP）问题：
第 k 层优化问题 ：
min ‖Jₖ q̈ − (ẍₖ^d − ḊJₖ q̇)‖²
s.t. Aᵢ q̈ ≤ bᵢ, ∀ i < k（高优先级任务的等效约束）
动力学约束、摩擦锥、关节限位等
其中 ẍₖ^d 为第 k 个任务的期望加速度。
优点：严格保证高优先级任务（如平衡）不被低优先级任务（如手势）干扰。
缺点：计算量大，需多层QP求解。
（2）加权融合法（Weighted QP）
将所有任务合并为单一目标函数：
min Σᵢ wᵢ · ‖Jᵢ q̈ − (ẍᵢ^d − ḊJᵢ q̇)‖²
s.t. M(q) q̈ + h = Sᵀτ + J_cᵀ F
F ∈ FrictionCone
τₘᵢₙ ≤ τ ≤ τₘₐₓ
q̈ₘᵢₙ ≤ q̈ ≤ q̈ₘₐₓ
其中 wᵢ 为任务权重，反映相对重要性。
优点：单次QP求解，效率高；
缺点：任务间存在耦合，可能牺牲关键任务性能。
4. 关键约束处理
WBC 必须显式处理以下物理约束：
-
接触约束：
- 单向力：F_z ≥ 0（不能"吸附"地面）；
- 摩擦锥：√(Fₓ² + Fᵧ²) ≤ μ · F_z（μ 为摩擦系数）。
-
执行器限制：
- 关节力矩：τₘᵢₙ ≤ τ ≤ τₘₐₓ；
- 关节速度/加速度限幅。
-
稳定性约束（可选）：
- ZMP 或 Capture Point 位于支撑多边形内；
- 零力矩点（ZMP）计算：
x_zmp = x_com − (z_com / g) · ẍ_com。
5. 控制流程（典型 WBC 循环）
- 感知输入：获取当前状态 q, q̇，接触状态（哪只脚接触），外部扰动估计；
- 任务定义：设定各任务目标（如 CoM 轨迹、手部位姿、头部朝向）；
- 构建优化问题：根据 HQP 或加权 QP 形式建立目标函数与约束；
- 求解 QP：使用实时求解器（如 OSQP、qpOASES、eiquadprog）计算最优 q̈ 或 τ；
- 输出控制量：发送关节力矩 τ 或关节加速度指令至底层控制器；
- 循环更新：以 100--1000 Hz 频率重复上述过程。
6. 与 MPC 的关系
- WBC 侧重实时性与模块化，适合高频（>200 Hz）底层控制；
- MPC 侧重预测与全局优化，通常运行频率较低（10--100 Hz）；
- 实际系统常采用 MPC+WBC 分层架构 ：
- MPC 规划 CoM/ZMP/步态等高层轨迹；
- WBC 跟踪这些轨迹并处理全身协调与约束。
7. 代表系统与开源框架
- HyQ、TALOS、HRP-4、Atlas 等均采用 WBC 实现复杂行为；
- 开源工具：
- iDynTree + WBI-Toolbox（Istituto Italiano di Tecnologia）；
- ocs2（ETH Zurich，支持 WBC 与 MPC 融合）；
- Ginkgo （MIT）、LIPM-Walking（LAAS-CNRS）。