本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

第 53 章 WBC 分层优化——TSID 精读 + 轻量 WBC 对照¶

前置自测 ⭐¶

在开始阅读之前,请先尝试回答以下问题。如果有两个以上不确定,建议先复习足式/30_Pinocchio深度精读(Pinocchio)、足式/60_QP_NLP建模(QP 求解器)和足式/80_接触力学与约束优化(摩擦锥):

MPC 输出的是"未来 1 秒的质心轨迹",WBC 输出的是"当前时刻的关节扭矩"——这两层之间的信息接口是什么?WBC 需要哪些量作为输入?
给定全身动力学方程 $M\ddot{q} + h = S^T\tau + J_c^T\lambda$,如果决策变量选 $(\ddot{q}, \lambda)$ 而不是 $(\tau, \ddot{q}, \lambda)$,减少了多少维?为什么能这样做?
加权 QP 中设 $w_1 = 100, w_2 = 1$,为什么不能保证 Task 1 绝对优先于 Task 2?
EIGEN_RUNTIME_NO_MALLOC 开启后,哪些 Eigen 操作会触发 assert 失败?请至少列出 3 种。
legged_control 的 WBC 只有约 600 行代码,而 TSID 有数千行——它做了哪些简化来实现这一点?

本章目标 ⭐¶

学完本章,学员应能:

说清楚 WBC 在控制层级中的定位——MPC 的接力棒、关节扭矩的源头
手推 WBC 的 QP 公式——从全身动力学到矩阵组装,每一步都写得出来
理解 HQP 的数学思想——零空间投影的推导、为什么分层优于加权
读懂 TSID 源码并能为新机器人添加自定义 Task
对比 TSID 和 legged_control 的轻量 WBC——理解设计取舍
掌握实时控制的内存约束——EIGEN_RUNTIME_NO_MALLOC 的原理和实践
能独立实现一个四足 WBC 并在仿真中跑通平衡控制
了解前沿进展——VWBC、学习 WBC、Contact-Implicit WBC、Whole-Body MPPI

本章知识导航 ⭐¶

在深入内容之前,先用一张全景图展示本章的知识结构。本章包含 10 个核心知识点,它们之间存在明确的递进和依赖关系:

控制层级定位 (53.1)
      │
      ▼
WBC 数学形态 (53.2) ─────────────────────────────┐
      │                                            │
      ▼                                            ▼
HQP 分层优化 (53.3) ──→ TSID 架构设计 (53.4)    QP 求解器选择 (53.5.3)
      │                       │
      ▼                       ▼
TSID 核心公式 (53.5) ──→ 实时内存约束 (53.6)
      │
      ▼
轻量 WBC 对照 (53.7) ──→ MPC+WBC 集成 (53.8)
                               │
                               ▼
                    调参指南 (53.9) + 多形态适配 (53.10)
                               │
                               ▼
                    前沿进展 (53.11): VWBC / 学习WBC / Whole-Body MPPI

推荐阅读路径:

路径 A（理论导向）:53.1 $\to$ 53.2 $\to$ 53.3 $\to$ 53.5 $\to$ 53.4 $\to$ 53.11（研究前沿）
路径 B（工程导向）:53.1 $\to$ 53.2 $\to$ 53.7 $\to$ 53.6 $\to$ 53.8 $\to$ 53.9（调参实战）
路径 C（快速入门）:53.1 $\to$ 53.2 $\to$ 53.7 $\to$ 53.9（最小可行知识集）

知识点	依赖	与其他知识点的关系
53.1 控制层级	无	全章的逻辑起点,回答"为什么需要 WBC"
53.2 WBC 数学形态	53.1	核心理论,后续所有内容的数学基础
53.3 HQP 分层优化	53.2	53.2 的深化,解决"多任务冲突怎么办"
53.4 TSID 架构	53.3	将 53.3 的数学落地为软件框架
53.5 TSID 核心公式	53.2, 53.4	53.2 的工程实现细节
53.6 实时内存约束	53.4, 53.5	从"能跑"到"能上机器人"的关键
53.7 轻量 WBC	53.2	53.4 的简化替代,适合入门
53.8 MPC+WBC 集成	53.5, 53.7	系统集成,连接 MPC 和 WBC
53.9 调参指南	53.5, 53.8	实战经验,从理论到落地
53.10 多形态适配	53.2-53.5	横向扩展:四足/双足/人形

前置依赖 ⭐¶

基础前置:

章节	关联知识	在本章中的用途
02_C++基础与进阶/10_Eigen	Eigen 表达式模板 / 对齐 / SIMD	WBC 是高频矩阵运算,必须理解 Eigen 的内存行为
02_C++基础与进阶/30_并发与实时	并发与线程安全	WBC 在 SCHED_FIFO 实时线程中运行
02_C++基础与进阶/20_设计模式	设计模式 - Strategy	TSID 的 Task/Constraint/Solver 是 Strategy 模式的教科书案例
02_C++基础与进阶/40_内存管理	pmr 分配器	WBC 的无堆分配替代方案

本大纲前置:

章节	关联知识	在本章中的用途
足式/30_Pinocchio深度精读	Pinocchio 动力学计算	WBC 依赖 `aba()`, `crba()`, `computeJointJacobians()`
足式/50_空间向量与浮动基座动力学	接触 Jacobian	WBC 的接触约束 $J_c \ddot{q} = -\dot{J}_c \dot{q}$
足式/60_QP_NLP建模	QP 求解器(OSQP, ProxQP, qpOASES)	WBC 的核心计算引擎
足式/80_接触力学与约束优化	摩擦锥约束	WBC 的接触力约束

如果跳过本章会怎样¶

无法将 MPC 输出落地:MPC 给出的是质心轨迹和接触力参考——这些宏观量无法直接驱动电机。没有 WBC,你会卡在"有规划但无法执行"的尴尬境地,就像有了建筑蓝图却不知道每块砖该放哪里。
关节控制器沦为 PD 伺服:没有 WBC 的多任务协调,你只能用简单的 PD 控制器独立控制每个关节,无法同时满足"保持平衡""追踪足端""正则化姿态"等多个相互冲突的目标。

预计阅读时间 ⭐¶

阅读方式	时间	适合谁
精读（含推导和练习）	20-25 小时	需要深入理解并实现 WBC 的读者
速读（跳过代码细节）	8-10 小时	有相关经验,需要了解 WBC 架构和数学的读者
速查（只看表格和公式）	30-45 分钟	遇到具体 WBC 调参或实现问题时回来查

53.1 控制层级——规划到关节的四级火箭 ⭐¶

53.1.1 为什么需要分层?¶

动机:假设你要让一只四足机器人从 A 走到 B。最"简单"的方案是:把整个问题写成一个巨大的优化——同时决定步态、接触序列、质心轨迹、关节角度、关节扭矩。

这个"巨型优化"的问题维度有多大?以 Unitree Go2 为例:

量	维度	说明
关节位置 $q$	18 (6+12)	浮动基座 6 + 12 关节
关节速度 $\dot{q}$	18
关节扭矩 $\tau$	12	只有关节被驱动
接触力 $\lambda$	12 (4 足 $\times$ 3)
单时步决策变量	~60
MPC 预测步数	20-50	预测未来 0.5-1 秒
全时域决策变量	1200-3000	60 $\times$ 20~50

💡 概念澄清:全维优化不是不可能——Cafe-MPC (Li & Wensing, T-RO 2025) 就用全身动力学做 MPC。但它需要极其高效的求解器(定制 iLQR),而且预测步数受限。分层是工程上更可扩展的方案。

历史视角:控制分层不是新想法。Honda ASIMO (2000) 就已经使用了"步态规划 -> ZMP 控制 -> 关节伺服"的三层架构。但那时的 WBC 很简单——只是逆动力学,没有 QP 优化。现代 WBC 从 2010 年代开始引入 QP,代表工作包括:

Sentis & Khatib (2005):首次将操作空间控制(Operational Space Control)与优先级控制结合
Escande, Mansard & Wieber (2014):HQP 理论的完整数学框架
Kim et al. (2019):MIT Mini Cheetah 上的 MPC+WBC 实现,推动了小型四足机器人高动态 MPC+WBC 控制的广泛研究

本质洞察:控制分层的本质是将一个巨大的耦合问题分解为多个小的、可解的子问题。这种分解不是任意的,而是沿着物理系统的天然时间尺度进行:任务规划在秒级变化,运动规划在百毫秒级变化,扭矩控制在毫秒级变化。每一层只需要处理与自身时间尺度匹配的决策,从而将计算量从指数级降低到线性级。这与软件工程中的"关注点分离"（Separation of Concerns）原则完全一致——每个模块只负责一件事,通过清晰的接口通信。

53.1.2 时间尺度金字塔¶

┌──────────────────────────────────────────────────────────┐
│ 任务层(行为树、FSM)                                      │
│ "这是一个 pick-and-place 任务"                            │
│ 频率:1-10 Hz    状态维度:离散(任务 ID、阶段)            │
│ 输出:目标位姿、步态类型                                   │
└─────────────────────┬────────────────────────────────────┘
                      │ 目标位姿 / 步态模式
┌─────────────────────▼────────────────────────────────────┐
│ 全局规划层(A*、RRT*、Graph Search)                       │
│ "从 A 点走到 B 点,避开障碍物"                             │
│ 频率:0.1-1 Hz   状态维度:SE(2) 或 SE(3)                 │
│ 输出:路径点序列                                           │
└─────────────────────┬────────────────────────────────────┘
                      │ 路径点 + 速度命令
┌─────────────────────▼────────────────────────────────────┐
│ MPC 层(Crocoddyl、OCS2、Cafe-MPC)                       │
│ "未来 1 秒的 CoM 轨迹 + 接触力 + 步态序列"                │
│ 频率:10-100 Hz  状态维度:~30(质心 + 动量 + 足端)        │
│ 输出:质心轨迹、足端轨迹、接触力参考                       │
│ 模型:简化模型(LIPM / Centroidal / SRBD)                 │
└─────────────────────┬────────────────────────────────────┘
                      │ x_com_ref, f_ref, p_foot_ref
┌─────────────────────▼────────────────────────────────────┐
│ WBC 层(TSID、legged_wbc)  <-- 本章                       │
│ "把 MPC 参考翻译为关节扭矩"                               │
│ 频率:500-1000 Hz  状态维度:~60(全身 q, dq)              │
│ 输出:关节扭矩 tau                                        │
│ 模型:全身刚体动力学(Pinocchio)                           │
└─────────────────────┬────────────────────────────────────┘
                      │ 关节扭矩 tau
┌─────────────────────▼────────────────────────────────────┐
│ 关节伺服层(电机驱动器内部 FOC)                           │
│ "tau -> 电流 -> PWM"                                     │
│ 频率:10-40 kHz   状态维度:电流/位置                      │
│ 输出:电机电压                                             │
└──────────────────────────────────────────────────────────┘

53.1.3 频率分离原则¶

为什么不同层运行在不同频率? 这不是随意选择,而是由物理和计算共同决定的:

层	频率	决定因素
MPC	10-100 Hz	求解时间 10-100 ms;模型变化时间尺度 ~0.1 s
WBC	500-1000 Hz	关节动力学时间常数 ~1 ms;扭矩必须"紧跟"当前状态
伺服	10-40 kHz	电机电气时间常数 ~0.1 ms

⚠️ 编程陷阱:WBC 的 1 kHz 频率意味着每次调用必须在 1 ms 内完成。这包括 Pinocchio 动力学计算(~0.1 ms)+ QP 求解(~0.1-0.5 ms)+ 数据交换开销。任何堆分配、mutex 等待、缓存未命中都可能导致超时。

如果不分层会怎样?

┌─────────────────────────────────────────┐
│ 方案 A:巨型 MPC(不分层)              │
│                                         │
│ 优点:理论上最优(全局一致性)           │
│ 缺点:                                  │
│   1. 维度爆炸 -> 求解太慢               │
│   2. 全身模型非线性强 -> 收敛困难       │
│   3. 频率只能到 10-50 Hz               │
│      -> 关节响应延迟 20-100 ms          │
│      -> 走路时足底打滑                  │
│                                         │
│ 方案 B:分层 MPC + WBC                  │
│                                         │
│ MPC（简化模型,50 Hz）-> 参考轨迹       │
│ WBC（全身模型,1 kHz）-> 关节扭矩      │
│                                         │
│ 优点:                                  │
│   1. 各层独立优化,易于调试             │
│   2. WBC 高频 -> 即时响应扰动           │
│   3. MPC 用简化模型 -> 可预测更长时间   │
│ 缺点:                                  │
│   MPC 与 WBC 的模型不一致 -> 性能折损   │
└─────────────────────────────────────────┘

🧠 思维陷阱:不要认为"分层一定不如全局优化"。实际上分层的鲁棒性往往更好:MPC 求解失败时,WBC 仍能用上一次的参考维持平衡。全局方案一旦求解失败,整个控制链断裂。

53.1.4 分层与不分层的最新进展¶

2025 年的研究正在模糊"分层"与"不分层"的边界。一些标志性工作值得关注:

Whole-Body MPC with MuJoCo (Zhang et al., 2025):利用 MuJoCo 仿真器的高效并行化能力,直接用全身动力学模型做 MPC。通过 iLQR + MuJoCo 的有限差分导数,在标准桌面 CPU 上实现了实时全身 MPC,并在四足动态行走、四足双腿行走和全尺寸人形双足行走上进行了硬件实验验证。这表明随着算力提升和算法优化,全身 MPC 正在变得可行。

Whole-Body MPPI (Rapuano et al., 2024):首次成功将基于采样的全身 MPC(Model-Predictive Path Integral, MPPI)部署到真实四足机器人上。与基于梯度的方法不同,MPPI 通过大量并行采样来搜索最优控制序列,天然适合 GPU 加速,且对非凸代价函数和接触不连续性更鲁棒。

Cafe-MPC (Li & Wensing, T-RO 2025):提出了级联保真度(Cascaded-Fidelity)策略——预测时域近端使用高保真全身模型,远端使用简化模型,在时域内实现模型精度的渐进退化。这种设计兼顾了近端精度和远端覆盖范围。

这些工作的共同趋势是:传统的严格分层架构正在向"柔性分层"演进——MPC 层开始使用越来越完整的模型,WBC 层的独立性在降低。但在当前(2026 年)的工程实践中,MPC + WBC 的分层架构仍然是主流方案,特别是在嵌入式硬件和产品级系统中。

练习 53.1.A ⭐:画出你所熟悉的一个机器人系统的控制层级图。标注每层的频率、输入输出、使用的模型。如果某些层被合并了(例如没有独立的 WBC 层),分析这种设计的优劣。

练习 53.1.B ⭐:假设你有一个超强求解器能在 0.5 ms 内求解全身 MPC。你还需要 WBC 层吗?讨论取消 WBC 层的利弊(提示:考虑模型不确定性和传感器噪声)。

⚠️ 常见陷阱

💡 概念误区:认为 WBC 就是逆动力学

新手想法:"WBC 不就是已知期望加速度,用牛顿-欧拉反推扭矩吗?"

实际上:纯逆动力学(如 Pinocchio 的 rnea())只是 WBC 的一个子步骤。WBC 的核心在于多任务协调和约束满足——需要同时追踪质心、控制足端、满足摩擦锥、限制扭矩,这些目标通常相互冲突,必须通过 QP 优化来权衡。纯逆动力学没有"优先级"的概念,也无法处理不等式约束。

正确理解:WBC = 逆动力学 + 多任务优化 + 约束满足。逆动力学是计算工具,WBC 是决策框架。

🧠 思维陷阱:认为更高频率的 WBC 一定更好

新手想法:"既然 WBC 在 1 kHz 比 500 Hz 好,那 10 kHz 是不是更好?"

实际上:WBC 频率受到三个因素制约:（1）QP 求解时间必须小于控制周期;（2）传感器更新频率限制了状态估计的有效带宽;（3）电机驱动器的通信带宽通常在 1-10 kHz。将 WBC 提高到 2 kHz 以上通常收益递减,因为传感器噪声和通信延迟已经成为瓶颈。

正确思维:WBC 频率应匹配系统中最慢的环节,通常是传感器-执行器环路的带宽。

53.2 WBC 的数学形态——带约束的全身逆动力学 ⭐⭐¶

53.2.1 从动力学到 QP:完整推导¶

起点:我们有全身刚体动力学方程(足式/30_Pinocchio深度精读 Pinocchio 计算的核心):

\[M(q) \ddot{q} + h(q, \dot{q}) = S^T \tau + \sum_{c} J_c^T \lambda_c \tag{53.1}\]

其中: - $M(q) \in \mathbb{R}^{n_v \times n_v}$:广义质量矩阵(对称正定) - $h(q, \dot{q}) \in \mathbb{R}^{n_v}$:科氏力 + 重力项 - $S \in \mathbb{R}^{n_a \times n_v}$:选择矩阵,$S = [0_{n_a \times 6} \;\; I_{n_a}]$,选出驱动关节 - $\tau \in \mathbb{R}^{n_a}$:关节扭矩(决策变量) - $J_c \in \mathbb{R}^{3n_c \times n_v}$:接触 Jacobian(所有接触点堆叠) - $\lambda_c \in \mathbb{R}^{3n_c}$:接触力(决策变量)

Go2 的具体维度:

符号	含义	Go2 数值
$n_v$	广义速度维度	18 (6 基座 + 12 关节)
$n_a$	驱动关节数	12
$n_c$	接触点数	4 (四足着地)
$3n_c$	接触力维度	12

为了帮助建立直觉,我们可以将这个方程与日常经验联系起来。想象你站在冰面上推一面墙——你的手施加的力($\lambda_c$)通过你的身体传导,最终让你的脚在冰面上滑动。方程 (53.1) 描述的正是这种"力如何通过多体系统传导"的物理过程。$M\ddot{q}$ 是"惯性力"(越重的身体越难加速),$h$ 是"已经存在的内力"(重力和运动中产生的科氏力),$S^T\tau$ 是"你主动施加的力"(电机扭矩),$J_c^T\lambda_c$ 是"环境给你的力"(地面反力)。

53.2.2 欠驱动基座的处理¶

动力学方程 (53.1) 的前 6 行对应浮动基座(无驱动器):

\[M_{b}(q) \ddot{q} + h_{b}(q, \dot{q}) = J_{c,b}^T \lambda_c \tag{53.2}\]

注意没有 $\tau$——基座不受电机驱动,只通过接触力间接控制。这是 WBC 的核心约束之一:基座的加速度完全由接触力决定。

后 $n_a$ 行对应驱动关节:

\[M_{j}(q) \ddot{q} + h_{j}(q, \dot{q}) = \tau + J_{c,j}^T \lambda_c \tag{53.3}\]

💡 概念澄清:方程 (53.2) 不是"约束",而是物理规律——浮动基座必须通过地面反力来加速。这意味着 WBC 的解必须同时满足"足底力产生正确的基座加速度"和"摩擦锥限制足底力"——这就是 WBC 问题的核心张力。这相当于一位木偶师(WBC)只能通过拉绳子(接触力)来控制木偶身体(浮动基座)的运动,但每根绳子的拉力不能超过上限(摩擦锥),且绳子只能拉不能推(法向非负)。这个类比的边界在于:木偶的绳子是刚性的且方向固定,而接触力是通过关节扭矩间接产生的,方向随构型变化——这使得 WBC 比操纵木偶复杂得多。

53.2.3 约束集合¶

等式约束 (a)——动力学方程:

将 (53.1) 改写为关于决策变量 $(\ddot{q}, \lambda_c, \tau)$ 的线性约束:

\[\underbrace{[M \;\; -J_c^T \;\; -S^T]}_{A_{dyn}} \begin{bmatrix} \ddot{q} \\ \lambda_c \\ \tau \end{bmatrix} = \underbrace{-h}_{b_{dyn}} \tag{53.4}\]

等式约束 (b)——接触不滑动:

接触点的加速度为零(保持静止接触):

\[J_c \ddot{q} + \dot{J}_c \dot{q} = 0 \quad \Rightarrow \quad J_c \ddot{q} = -\dot{J}_c \dot{q} \tag{53.5}\]

写成标准形式:

\[\underbrace{[J_c \;\; 0 \;\; 0]}_{A_{contact}} \begin{bmatrix} \ddot{q} \\ \lambda_c \\ \tau \end{bmatrix} = \underbrace{-\dot{J}_c \dot{q}}_{b_{contact}} \tag{53.6}\]

阶段小结:到这里我们已经建立了两类等式约束——动力学约束确保解物理可行,接触约束确保着地脚不打滑。接下来要做的是加入不等式约束来限制接触力和扭矩。

不等式约束 (c)——摩擦锥:

回顾足式/80_接触力学与约束优化:库仑摩擦锥 $\|\lambda^t\| \le \mu \lambda^n$ 是二阶锥约束(SOC),为了在 QP 中使用,需要将其线性化为 $k$ 条边的多面体近似。这里我们采用 $k=4$ 的外切近似(即 box cone),误差约 27% 但计算最快。足式/80_接触力学与约束优化还分析了 $k=8$ 和内切近似的权衡——在 WBC 的 1kHz 频率需求下,$k=4$ 或 $k=8$ 是工程上的主流选择。

线性化后的摩擦锥约束(每个接触点 5 个不等式):

\[D_i \lambda_{c,i} \le 0, \quad i = 1, \ldots, n_c \tag{53.7}\]

其中 $D_i \in \mathbb{R}^{5 \times 3}$ 是摩擦锥的线性近似矩阵(足式/80_接触力学与约束优化详细推导了该矩阵的构造):

\[D = \begin{bmatrix} 1 & 0 & -\mu \\ -1 & 0 & -\mu \\ 0 & 1 & -\mu \\ 0 & -1 & -\mu \\ 0 & 0 & -1 \end{bmatrix}\]

第 1-4 行是摩擦锥的四棱锥近似:$|f_x| \le \mu f_z$ 和 $|f_y| \le \mu f_z$;第 5 行保证法向力非负:$f_z \ge 0$。

不等式约束 (d)——扭矩限制:

\[-\tau_{\max} \le \tau \le \tau_{\max} \tag{53.8}\]

代价函数 (e)——追踪 MPC 参考:

\[\min \sum_k w_k \| A_k \begin{bmatrix} \ddot{q} \\ \lambda_c \\ \tau \end{bmatrix} - b_k^{\text{ref}} \|^2 \tag{53.9}\]

典型追踪任务包括: - 质心加速度追踪:$\ddot{x}_G \approx \ddot{x}_G^{\text{ref}}$ - 足端追踪(摆动腿):$\ddot{p}_{foot} \approx \ddot{p}_{foot}^{\text{ref}}$ - 基座姿态追踪:$\ddot{\theta}_{base} \approx \ddot{\theta}_{base}^{\text{ref}}$ - 关节正则化:$\ddot{q}_{joint} \approx K_p(q^{ref} - q) + K_d(\dot{q}^{ref} - \dot{q})$

53.2.4 组装完整的 QP¶

将所有约束和代价函数组装为标准 QP:

\[\min_{x} \frac{1}{2} x^T H x + g^T x$$ $$\text{s.t.} \quad A_{eq} x = b_{eq}, \quad A_{ineq} x \le b_{ineq}\]

其中 $x = [\ddot{q}^T, \lambda_c^T, \tau^T]^T \in \mathbb{R}^{n_v + 3n_c + n_a}$。

Go2 四足的 QP 维度:

矩阵	维度	Go2 数值
决策变量 $x$	$n_v + 3n_c + n_a$	$18+12+12=42$
等式约束(动力学)	$n_v$	18
等式约束(接触不滑)	$3n_c$	12
不等式约束(摩擦锥)	$5n_c$	20
不等式约束(扭矩限制)	$2n_a$	24
QP 总规模		42 变量, 30 等式, 44 不等式

⚠️ 编程陷阱:这个 QP 只有 42 维,用 ProxQP/eiquadprog 求解只需 0.1-0.3 ms。千万不要用 Ipopt/SNOPT 等非线性求解器——它们的启动开销就超过 1 ms。足式/60_QP_NLP建模详细讨论了各 QP 求解器的性能特征。

如果不用摩擦锥约束会怎样?求解器会给出物理上不可能的接触力——例如水平摩擦力远超 $\mu \lambda_z$,机器人在仿真中"一步迈出"后瞬间打滑倒地。实物上更危险:电机会按照不可实现的力指令输出最大电流,导致关节过热甚至损坏减速器。足式/80_接触力学与约束优化的三大铁律正是 WBC 约束矩阵中不等式行的物理来源。

不同机器人的 QP 规模对比:

机器人	$n_v$	$n_a$	$n_c$(最大）	决策变量	求解时间(典型)
Go2 四足	18	12	4	42	0.1-0.3 ms
A1 四足	18	12	4	42	0.1-0.3 ms
Atlas 人形	36	30	2	72	0.3-0.8 ms
H1 人形	25	19	2	50	0.2-0.5 ms
Panda 臂	7	7	0	7	<0.05 ms

⚠️ 常见陷阱

💡 概念误区:认为 QP 的等式约束是"额外施加"的限制

新手想法:"动力学方程作为约束放进 QP,是不是在限制优化的自由度?"

实际上:动力学方程不是我们施加的约束,而是物理世界的基本规律。在 WBC 中它作为等式约束的作用是保证解的物理可行性——任何违反牛顿第二定律的 $(\ddot{q}, \tau, \lambda)$ 组合在现实世界中根本不可能实现。"约束"是数学形式的叫法,物理本质是"定律"。

🧠 思维陷阱:认为约束越多 QP 越难解

新手想法:"加了这么多约束,QP 会不会变得很慢?"

实际上:对于 active-set 方法(如 eiquadprog),真正影响求解速度的是活跃约束(active constraints)的数量,而非总约束数。大部分不等式约束在最优解处是非活跃的(远离边界),对计算量几乎没有贡献。WBC 的 QP 通常只有 5-15 个活跃约束,即使总约束数有 44 个。

练习 53.2.A ⭐⭐:为平面三连杆机器人(2D,3 个关节,无浮动基座)手写 WBC 的 QP 矩阵。决策变量是 $(\ddot{q}, \tau)$(没有接触力),约束只有动力学方程和关节扭矩限制。用 Eigen + osqp-eigen 求解,验证扭矩是否满足限制。

练习 53.2.B ⭐⭐⭐:在 53.2.A 的基础上,加入一个"足尖接触地面"的约束。将机器人变为浮动基座,增加接触力 $\lambda$ 和摩擦锥约束。对比有无摩擦锥约束时,解的差异。

53.3 "为什么要分层而不是加权"——HQP 核心洞察 ⭐⭐⭐¶

53.3.1 加权 QP 的根本缺陷¶

朴素做法:把多个目标加权求和做成单个 QP:

\[\min \; w_1 \|\ddot{x}_G - \ddot{x}_G^{\text{ref}}\|^2 + w_2 \|\tau\|^2 + w_3 \|\ddot{p}_{ee} - \ddot{p}_{ee}^{\text{ref}}\|^2 \tag{53.10}\]

问题 1:权重与物理量级耦合

设 $w_1 = 10, w_2 = 1$,看起来"追踪重要、扭矩次要"。但实际行为取决于各项的数值量级:

项	典型量级	加权后
$\\|\ddot{x}_G\\|^2$	$(1 \text{ m/s}^2)^2 = 1$	$10 \times 1 = 10$
$\\|\tau\\|^2$	$(50 \text{ Nm})^2 = 2500$	$1 \times 2500 = 2500$

结果:扭矩项主导代价函数,与直觉完全相反!要修正这个问题,你需要把权重设为 $w_1 = 2500, w_2 = 1$——但这个"2500"完全取决于当前运动状态,没有泛化能力。

问题 2:不能表达"绝对优先级"

"保持平衡"和"末端追踪"之间的关系不是"重要性差 10 倍",而是质的差别:平衡丢了,机器人摔倒,末端追踪毫无意义。用权重表达这种"绝对优先"需要 $w_1 / w_2 \to \infty$——但这会导致 QP 的 Hessian 矩阵条件数退化,求解器不收敛。

⚠️ 工程陷阱：加权 QP 的"污染"问题

在 WQP（Weighted QP）中，不同任务通过权重区分优先级。但一个低权重任务如果其雅可比矩阵范数较大或瞬时误差较大，仍可能在优化过程中"污染"高优先级任务的执行——因为 QP 求解器在最小化加权代价时会折中所有任务。

这是 WQP 相对于严格分层 HQP 的本质缺陷：WQP 没有严格的优先级保证。如果高优先级任务的执行质量不可妥协（如支撑腿约束），应使用 HQP 或 NSP（零空间投影）。

🧠 思维陷阱:很多人认为"把权重设得很大就能保证优先级"。数学上不成立。设 $w_1 = 10^6$,那么 Hessian 矩阵的条件数 $\kappa(H) \ge 10^6$,在 64 位浮点下只剩 ~10 位有效数字,QP 求解可能给出垃圾结果。

问题 3:条件数退化的定量分析

为了更精确地理解这个问题,考虑一个简化的双任务加权 QP,Hessian 矩阵为:

\[H = w_1 J_1^T J_1 + w_2 J_2^T J_2\]

当 $w_1 \gg w_2$ 时,$H$ 的最大特征值约为 $w_1 \|J_1\|^2$,最小特征值约为 $w_2 \|J_2\|^2$(假设 $J_1, J_2$ 的零空间不完全重叠)。条件数:

\[\kappa(H) \approx \frac{w_1 \|J_1\|^2}{w_2 \|J_2\|^2}\]

在 IEEE 754 双精度浮点(约 15.9 位有效十进制数字)下,当 $\kappa(H) > 10^{12}$ 时,数值解的有效精度不到 4 位十进制数字,已经不可信。这意味着 $w_1/w_2$ 最多只能到 $10^{10}$ 量级(假设 $\|J_1\| \approx \|J_2\|$),远不是"无穷大"。

53.3.2 HQP 的数学框架¶

HQP (Hierarchical Quadratic Programming) 的核心思想:将任务按优先级排列,逐层求解,低层不破坏高层的最优性。

关键论文:Escande A., Mansard N., Wieber P.-B. (2014) "Hierarchical quadratic programming: Fast online humanoid-robot motion generation", IJRR, 33(7):1006-1028。该论文提出了一种完整的方法来求解包含等式和不等式约束的多层最小二乘问题,比传统的迭代投影方法快 10 倍以上。

形式化定义:给定 $p$ 层优先级,每层有任务 $(A_k, b_k)$ 和约束 $(C_k, d_k)$:

\[\text{lexmin}_{x} \left( \|A_1 x - b_1\|^2, \|A_2 x - b_2\|^2, \ldots, \|A_p x - b_p\|^2 \right)\]

\[\text{s.t.} \quad C_k x \le d_k, \quad k = 1, \ldots, p\]

"lexmin"(字典序最小化）的含义:先最小化第 1 层,在所有使第 1 层最优的解中最小化第 2 层,以此类推。这好比医院急诊的分级诊疗:危重病人(Level 1)无条件优先,普通病人(Level 2)只能在不影响危重病人的前提下得到治疗。不同于加权方案(给每个病人打一个"紧急程度分数"然后排序),分级制度保证了危重病人的绝对优先权,不会因为普通病人数量多而被"稀释"。这个类比在"任务完全冲突"时成立——当资源(自由度)不足以同时满足所有任务时,高优先级任务获得绝对保护。但与医院不同的是,HQP 中的低优先级任务不是简单地"等待",而是在不影响高优先级任务的前提下尽可能执行——这更像是"在不影响危重病人的前提下,用剩余的医疗资源尽可能多地救治普通病人"。

53.3.3 零空间投影——HQP 的数学引擎¶

Level 1:求解最高优先级任务:

\[x_1^* = \arg\min_{x} \|A_1 x - b_1\|^2 \quad \text{s.t.} \; C_1 x \le d_1\]

如果无不等式约束,解为最小二乘解 $x_1^* = A_1^+ b_1$,其中 $A_1^+$ 是 Moore-Penrose 伪逆。

零空间投影矩阵:

\[N_1 = I - A_1^+ A_1 \tag{53.11}\]

$N_1$ 将任何向量投影到 $A_1$ 的零空间。在这个子空间里改变 $x$ 不会影响 $A_1 x$ 的值——这是整个 HQP 理论的基石。

本质洞察:零空间投影的意义不是让次要任务完美执行,而是在完美保护主任务的前提下,尽最大努力去近似次要任务。当零空间维度不足时,低优先级任务会被部分甚至完全牺牲——这不是缺陷,而是"优先级控制"的本质:有限的自由度必须分配给最重要的目标。

直觉理解:假设 $A_1$ 是一个 $3 \times 5$ 矩阵(3 个方程,5 个未知数)。最小二乘解确定了解空间中的一个 2 维平面(5 - 3 = 2 维零空间)。Level 2 只能在这个 2 维平面上优化,不能"跳出"平面。

Level 2:在不破坏 Level 1 的前提下优化 Level 2:

\[x_2 = x_1^* + N_1 \delta_2 \tag{53.12}\]

代入 Level 2 的目标:

\[\|A_2 x_2 - b_2\|^2 = \|A_2(x_1^* + N_1 \delta_2) - b_2\|^2 = \|\underbrace{A_2 N_1}_{\tilde{A}_2} \delta_2 - \underbrace{(b_2 - A_2 x_1^*)}_{\tilde{b}_2}\|^2\]

这是一个关于 $\delta_2$ 的新最小二乘问题:

\[\delta_2^* = \tilde{A}_2^+ \tilde{b}_2 = (A_2 N_1)^+ (b_2 - A_2 x_1^*) \tag{53.13}\]

Level 3:用累积零空间 $N_2 = N_1 (I - \tilde{A}_2^+ \tilde{A}_2)$ 继续投影,以此类推。

关键性质的证明:Level 2 不破坏 Level 1:

\[A_1 x_2 = A_1 (x_1^* + N_1 \delta_2) = A_1 x_1^* + A_1 N_1 \delta_2\]

\[= A_1 x_1^* + A_1 (I - A_1^+ A_1) \delta_2 = A_1 x_1^* + (A_1 - A_1) \delta_2 = A_1 x_1^* \quad \square\]

💡 几何直觉:想象 3D 空间中,Level 1 约束 $x$ 在一条线上(1D 子空间)。Level 2 的自由度只有沿这条线滑动的方向。每增加一层任务,可用自由度减少。当自由度耗尽时,更低层的任务完全被忽略——这是设计优先级时必须考虑的。

53.3.3b 为什么 Moore-Penrose 伪逆是最优的——拉格朗日乘子法证明¶

上文直接使用了伪逆 $A^+ = A^T(AA^T)^{-1}$ 来获取最小二乘解，但一个自然的问题是：对于欠定方程 $Ax = b$（$A$ 是 $m \times n$ 矩阵，$m < n$），存在无穷多个右逆矩阵 $G$ 满足 $AG = I$，为什么偏偏选 $A^+$？

答案来自能量最优性：在满足任务约束的前提下，我们希望找到范数最小的解——即用最小的"动作量"完成任务，避免不必要的剧烈运动。这可以形式化为一个带等式约束的优化问题：

\[\min_{x} \frac{1}{2} \|x\|^2 \quad \text{s.t.} \quad Ax = b\]

引入 $m \times 1$ 的拉格朗日乘子向量 $\boldsymbol{\lambda}$，构造拉格朗日函数：

\[\mathcal{L}(x, \boldsymbol{\lambda}) = \frac{1}{2} x^T x - \boldsymbol{\lambda}^T (Ax - b)\]

对 $x$ 和 $\boldsymbol{\lambda}$ 分别求偏导并令其为零：

\[\nabla_x \mathcal{L} = x - A^T \boldsymbol{\lambda} = 0 \quad \Longrightarrow \quad x = A^T \boldsymbol{\lambda}\]

\[\nabla_{\boldsymbol{\lambda}} \mathcal{L} = -(Ax - b) = 0 \quad \Longrightarrow \quad Ax = b\]

将第一式代入第二式：$A(A^T \boldsymbol{\lambda}) = b$，即 $(AA^T)\boldsymbol{\lambda} = b$。当 $A$ 行满秩时（非奇异构型下冗余机器人的雅可比矩阵满足此条件），$AA^T$ 可逆，解得：

\[\boldsymbol{\lambda} = (AA^T)^{-1} b\]

代回第一式：

\[x^* = A^T (AA^T)^{-1} b = A^+ b\]

这就证明了：满足 $Ax = b$ 约束的唯一最小范数解，正是由 Moore-Penrose 伪逆给出的。

更深刻的数学性质：$A^+ b$ 位于 $A$ 的行空间 $\mathcal{R}(A^T)$ 中——而行空间与零空间 $\mathcal{N}(A)$ 正交互补（秩-零度定理）。因此最小范数解天然地不包含任何零空间分量，是"纯粹完成任务"的那部分运动。这也是零空间投影 $N = I - A^+ A$ 能将任意向量正交投影到 $\mathcal{N}(A)$ 的根本原因——投影后的向量与原始向量之间的欧氏距离最小。

⚠️ 工程注意：当 $A$ 接近奇异（接近工作空间边界）时，$AA^T$ 的行列式趋近于零，直接求逆会导致数值爆炸。工程中常用阻尼最小二乘法（DLS）替代：$A^+ \approx A^T(AA^T + \lambda^2 I)^{-1}$。微小的阻尼项 $\lambda^2$ 保证了矩阵始终可逆，但代价是解不再精确满足 $Ax = b$——这是"精度 vs 数值稳定性"的经典权衡。

53.3.3c 零空间维度递减的定量分析 ⭐⭐⭐¶

零空间投影逐层递推的过程中,可用自由度单调递减。为了帮助理解这一过程,我们对四足 WBC 的典型场景做一个定量分析。

设总决策变量维度为 $n$（Go2: $n = 42$）,第 $k$ 层任务的雅可比矩阵 $A_k$ 的秩为 $r_k$。经过前 $k$ 层后,累积零空间的维度为:

\[\dim(\mathcal{N}_k) = n - \sum_{i=1}^{k} r_i\]

这里假设每层新增的任务行与前面的任务行线性无关（在非奇异构型下通常成立）。

Go2 四足 trot 步态的维度追踪:

层级	任务	新增行数 $r_k$	累积约束维度	剩余自由度
初始	—	0	0	42
Level 0	浮基动力学	6	6	36
Level 0	接触不滑(2 脚)	6	12	30
Level 1	基座位姿追踪	6	18	24
Level 1	摆动腿追踪(2 腿)	6	24	18
Level 2	关节正则化	12	36	6
Level 2	接触力正则化	6	42	0

当剩余自由度降为零时,更低优先级的任务完全无法执行。这揭示了一个重要的工程设计原则:不要设置太多层级或太多任务,否则低优先级任务的零空间维度为零,形同虚设。实践中 3-4 层已经是上限。

53.3.4 级联 QP 实现¶

实际的 HQP 实现并不直接计算零空间投影(伪逆的数值稳定性差)。Escande et al. (2014) 提出了级联 QP方法:

算法:级联 QP (Cascaded QP)
输入:p 层任务 {(A_k, b_k, C_k, d_k)}

1. 解 Level 1 QP:
   x1* = argmin ||A1*x - b1||^2  s.t. C1*x <= d1
   w1* = ||A1*x1* - b1||^2  (最优代价)

2. for k = 2, ..., p:
   解 Level k QP:
   xk* = argmin ||Ak*x - bk||^2
   s.t. Ck*x <= dk                     (本层约束)
        ||Aj*x - bj||^2 <= wj* + eps   (保持上层最优, j < k)

   wk* = ||Ak*xk* - bk||^2

3. 输出: xp*

⚠️ 编程陷阱:约束 $\|A_j x - b_j\|^2 \le w_j^* + \varepsilon$ 是二次约束,不能直接放进标准 QP。TSID 的实现将其转化为线性等式约束:固定上层的残差方向,只允许在零空间中移动。这种线性化是 solver-HQuadProg-fast.cpp 的核心技巧。

53.3.5 三种 QP 策略对比¶

策略	优先级保证	计算复杂度	调参难度	典型实现
加权 QP	无(软优先级)	1 次 QP	高(权重敏感)	简单手写
严格 HQP	数学保证	$p$ 次 QP	低(只排优先级)	TSID HQuadProg
加权 + 优先级约束	近似保证	1 次 QP + 约束	中等	legged_control

四足典型优先级设置(3 层):

Level 1 (硬约束):
  - 动力学方程:M*ddq + h = S^T*tau + Jc^T*lambda
  - 接触不滑:Jc*ddq = -dJc*dq
  - 摩擦锥:D*lambda <= 0
  - 扭矩限制:-tau_max <= tau <= tau_max

Level 2 (高优先级软目标):
  - 基座姿态追踪(保持水平)
  - 质心位置追踪(防摔倒)
  - 摆动腿足端追踪(步态执行)

Level 3 (低优先级软目标):
  - 关节姿态正则化(接近 nominal pose)
  - 接触力正则化(||lambda - lambda_ref||^2)

🧠 思维陷阱:有人认为"HQP 一定比加权 QP 好"。不一定。HQP 需要解 $p$ 次 QP,计算量更大。对于实时性要求极高的场景(如 1 kHz WBC),有时用加权 QP + 合理权重比 HQP 更实用。legged_control 同时提供两种实现:HierarchicalWbc 是 3 层严格 HQP(零空间逐层投影),而 WeightedWbc 则是把所有任务用权重叠加成单个 QP 的加权备选方案。

53.3.6 四足机器人的经典四任务分层与 NSP 递推公式¶

在基于零空间投影（NSP）的经典 WBC 实现中，四足机器人的控制任务通常分为以下四个优先级。这种分层的物理直觉是：四足依靠支撑腿与地面的稳定接触来实现身体控制，支撑腿的约束是一切运动的前提；机身姿态（保持水平）对平衡至关重要；机身位置（高度控制）次之；摆动腿轨迹只要大致正确即可。

优先级	任务	物理含义
P1 (最高)	支撑腿足端静止	触地脚在世界系中保持不动（$\dot{\mathbf{x}}_{\text{sup}} = \mathbf{0}$），防止打滑导致摔倒
P2	机身转动控制	控制 Roll/Pitch/Yaw 角跟随期望姿态
P3	机身平动控制	控制质心位置（尤其是高度）跟随期望轨迹
P4 (最低)	摆动腿足端追踪	控制空中腿沿规划轨迹运动至下一个落脚点

对应的 NSP 递推公式在加速度级别的完整形式为：

\[\ddot{\mathbf{q}}_1^{\text{cmd}} = J_1^+ (-\dot{J}_1 \dot{\mathbf{q}})\]

\[\ddot{\mathbf{q}}_i^{\text{cmd}} = \ddot{\mathbf{q}}_{i-1}^{\text{cmd}} + (J_i N_{i-1}^A)^+ \left( \ddot{\mathbf{x}}_i^{\text{cmd}} - \dot{J}_i \dot{\mathbf{q}} - J_i \ddot{\mathbf{q}}_{i-1}^{\text{cmd}} \right), \quad i = 2, 3, 4\]

其中 $\ddot{\mathbf{x}}_i^{\text{cmd}} = \ddot{\mathbf{x}}_i^d + K_p^i (\mathbf{x}_i^d - \mathbf{x}_i) + K_d^i (\dot{\mathbf{x}}_i^d - \dot{\mathbf{x}}_i)$ 是带 PD 反馈的期望加速度，$N_{i-1}^A = I - (J_{i-1}^A)^+ J_{i-1}^A$ 是前 $i-1$ 个任务的累积零空间投影矩阵，$J_{i-1}^A = [J_1^T, \ldots, J_{i-1}^T]^T$ 是堆叠雅可比矩阵。最终输出 $\ddot{\mathbf{q}}_4^{\text{cmd}}$ 即为融合了全部四个优先级任务的关节加速度指令。

NSP vs 优化 WBC 的取舍：上述 NSP 递推公式有解析形式，计算极快，但无法处理不等式约束（如摩擦锥、力矩限幅）。工程中更常见的做法是将上述优先级思想转化为 HQP 或加权 QP 形式（如 TSID），在保留分层精神的同时获得约束处理能力。

53.3.7 不同优先级策略在工业界的应用情况 ⭐⭐¶

不同的机器人公司和研究机构根据自身需求选择了不同的 WBC 策略。2025 年的 QP 基准测试论文（Stark et al., 2025, arXiv:2502.01329）系统比较了不同 QP 公式和求解器在四足动态行走中的表现,提供了选择依据:

策略	典型用户	选择理由
严格 HQP	LAAS-CNRS (TSID)、IIT (iCub)	人形机器人安全关键,需要绝对优先级保证
加权 QP	Unitree (部分场景)、MIT Mini Cheetah	四足稳定性裕度大,加权 QP 足够且更快
加权 + 硬约束混合	legged_control、OCS2 WBC	物理约束硬编码,软目标用权重,兼顾安全和灵活
NSP (零空间投影)	Raibert 经典架构、Stanford Doggo	极简实现,适合教学和原型验证
RL 替代 WBC	Agility Robotics (部分)、多数 sim-to-real	端到端 RL 直接输出扭矩,跳过 WBC 层

本质洞察:WBC 策略的选择不是"哪个数学上更优雅",而是"当前系统的安全裕度和计算预算允许什么"。四足机器人有 4 个支撑点,即使 WBC 略有偏差也不会立刻摔倒——因此可以使用更简单的加权 QP。双足/人形机器人只有 2 个支撑点(甚至单脚支撑),安全裕度极小,必须用严格 HQP 来保证平衡任务绝对优先。

练习 53.3.A ⭐⭐:用 Python + numpy 实现零空间投影法。构造一个 3 层优先级问题(2D 空间):Level 1 约束 $x_1 + x_2 = 1$,Level 2 最小化 $\|x - [3,3]^T\|^2$,Level 3 最小化 $\|x\|^2$。验证每层是否不破坏上层。

练习 53.3.B ⭐⭐⭐:对同一个问题,用加权 QP 方法,尝试不同的权重比($w_1:w_2:w_3 = 100:10:1$ 和 $10000:100:1$),观察解如何变化。与 HQP 的解对比,讨论在什么情况下加权 QP 的解"足够接近" HQP。

⚠️ 常见陷阱

💡 概念误区:认为零空间投影只用于 WBC

新手想法:"零空间投影是 WBC 的专有技术。"

实际上:零空间投影是线性代数中的通用工具,在机器人学中应用广泛。冗余机械臂的逆运动学（在满足末端追踪的前提下优化关节构型）、力/位混合控制（在力控制方向上施加力,在位置控制方向上追踪位置）、甚至强化学习中的安全约束（在安全子空间内搜索策略）都使用了相同的数学工具。理解零空间投影的本质——"在不影响已有约束的前提下利用剩余自由度"——是掌握多任务控制的关键。

53.4 TSID 的 Task / Constraint / Solver 三元分离 ⭐⭐¶

上一节从数学层面回答了"为什么分层优于加权"。但数学上的优美并不自动转化为工程上的可用性——如何将 HQP 的零空间投影、多层 QP 封装成一个可扩展、可维护的软件框架?这正是 TSID 要解决的问题。

53.4.1 设计哲学¶

TSID (Task Space Inverse Dynamics) 是 Pinocchio 生态的工业级 WBC 实现,由 LAAS-CNRS 的 Andrea Del Prete 等人开发。当前版本为 1.9.x,同时提供 C++ 和 Python 接口(通过 EigenPy 和 Boost.Python 绑定)。

TSID 的核心设计思想是 Strategy Pattern (02_C++基础与进阶/20_设计模式) 的教科书级应用:将"描述要做什么"(Task)、"描述必须满足什么"(Constraint)、"如何求解"(Solver) 三者完全分离:

┌─────────────────────────────────────────────────────┐
│              InverseDynamicsFormulation              │
│                                                     │
│  ┌───────────┐  ┌──────────────┐  ┌──────────────┐  │
│  │ TaskBase  │  │ConstraintBase│  │ SolverBase   │  │
│  │           │  │              │  │              │  │
│  │ ┌───────┐ │  │ ┌──────────┐ │  │ ┌──────────┐ │  │
│  │ │TaskSE3│ │  │ │Equality  │ │  │ │HQuadProg │ │  │
│  │ ├───────┤ │  │ ├──────────┤ │  │ ├──────────┤ │  │
│  │ │TaskCoM│ │  │ │Inequality│ │  │ │ProxQP    │ │  │
│  │ ├───────┤ │  │ ├──────────┤ │  │ ├──────────┤ │  │
│  │ │Posture│ │  │ │Bound     │ │  │ │eiquadprog│ │  │
│  │ └───────┘ │  │ └──────────┘ │  │ └──────────┘ │  │
│  └───────────┘  └──────────────┘  └──────────────┘  │
│                                                     │
│  addMotionTask(task, weight, priority_level)         │
│  addContactTask(contact)                             │
│  computeProblemData(t, q, v) -> HQPData              │
└─────────────────────────────────────────────────────┘

为什么这个分离很重要:这好比乐高积木的标准化接口——每块积木(Task/Constraint/Solver)的内部结构可以完全不同,但只要接口形状一致就能自由组合。TSID 的 Strategy Pattern 正是让 WBC 的各个"积木"可以独立替换而不影响整体:换一个 QP 后端就像换一块积木,不需要重建整座城堡。这个类比的边界在于:乐高积木的接口是物理上的凸凹结构,不会因为使用方式不同而"行为变化";而 TSID 的不同 Solver 后端虽然接口相同,但在数值精度、收敛速度、鲁棒性上可能有显著差异——换求解器后需要重新验证,不能像换乐高积木那样无脑替换。

场景	需要修改的部分	不需要改的部分
添加新任务(如"保持末端水平")	继承 TaskBase	Solver, Formulation
换 QP 后端(如 qpOASES -> ProxQP)	继承 SolverBase	Task, Formulation
换机器人(如 Go2 -> H1)	换 URDF + 调参	所有代码
增加约束(如关节速度限制)	继承 ConstraintBase	Task, Solver

53.4.2 TaskBase 接口详解¶

// tsid/include/tsid/tasks/task-base.hpp (simplified)
class TaskBase {
 public:
  // Compute task residual and Jacobian at current time
  virtual const ConstraintBase& compute(
      double t,
      const Eigen::VectorXd& q,   // joint positions
      const Eigen::VectorXd& v,   // joint velocities
      pinocchio::Data& data       // Pinocchio cache
  ) = 0;

  // Return the constraint object (equality/inequality/bound)
  virtual const ConstraintBase& getConstraint() const = 0;

  // Task name (for debug output)
  const std::string& name() const;

  // Task type determines which block of decision variables
  // TaskMotion:    target is J*ddq = desired_acc
  // TaskForce:     target is lambda = desired_force
  // TaskActuation: target is tau = desired_torque
  enum TaskType { MOTION, FORCE, ACTUATION };
};

关键方法 compute() 的职责(以 TaskSE3Equality 为例):

调用 Pinocchio 的 getFrameJacobian() 获取末端 Jacobian $J_{ee}$
调用 getFrameAcceleration() 获取 $\dot{J}_{ee}\dot{q}$(Jacobian 时间导数项)
计算位姿误差:$e = \text{log}(R_{ref}^T R_{cur})$（旋转部分使用 SE3 对数映射）
计算期望加速度:$\ddot{x}_{des} = \ddot{x}_{ref} + K_d \dot{e} + K_p e$
填入约束矩阵:$J_{ee} \ddot{q} = \ddot{x}_{des} - \dot{J}_{ee}\dot{q}$

⚠️ 编程陷阱:compute() 内部不能有堆分配,因为 TSID 默认开启 EIGEN_RUNTIME_NO_MALLOC(53.6 节详述)。所有临时矩阵必须预分配为成员变量。如果你继承 TaskBase 实现自定义 Task,这一点至关重要。

53.4.3 主要 Task 类型¶

Task 类	追踪目标	Jacobian 来源	典型应用
`TaskSE3Equality`	末端 6D 位姿	`getFrameJacobian()`	机械臂末端追踪、四足足端
`TaskComEquality`	质心位置	`jacobianCenterOfMass()`	四足/人形平衡
`TaskJointPosture`	关节角度	$I_{n_a}$ (单位矩阵)	关节正则化
`TaskAMEquality`	角动量	质心动量矩阵	人形步行保持角动量
`TaskJointBounds`	关节限位	边界约束	安全
`TaskCopEquality`	压力中心(CoP)	接触力 Jacobian	人形全脚掌平衡

53.4.4 ContactModel6d vs ContactPoint¶

TSID 提供两种接触模型:

接触模型	力维度	适用场景	约束类型
`ContactPoint`	3D 力	点接触(四足足尖)	线性摩擦锥($5 \times 3$ 矩阵)
`Contact6d`	6D 力矩	面接触(人形全脚掌)	摩擦锥 + CoP 在脚掌内 + 力矩约束

💡 概念澄清:四足机器人通常用 ContactPoint(每足 3 维力);人形机器人用 Contact6d(每脚 6 维力+力矩),因为脚掌是面接触,需要约束 CoP(压力中心)不超出脚掌边缘,否则脚会"翻倒"。Contact6d 的约束矩阵更大(17 行 vs 5 行),QP 也更大。

如何添加自定义 Task(分步指南):

// Step 1: Inherit TaskBase (or TaskMotion for motion tasks)
class TaskEndEffectorVertical : public tsid::tasks::TaskMotion {
 public:
  TaskEndEffectorVertical(const std::string& name,
                          tsid::robots::RobotWrapper& robot,
                          const std::string& frame_name);

  // Step 2: Implement compute()
  const ConstraintBase& compute(double t,
                                const Eigen::VectorXd& q,
                                const Eigen::VectorXd& v,
                                pinocchio::Data& data) override {
    // Get end-effector rotation matrix from Pinocchio
    auto frame_id = m_robot.model().getFrameId(m_frame_name);
    const auto& oMf = data.oMf[frame_id];
    Eigen::Vector3d z_axis = oMf.rotation().col(2);

    // Error: z_axis should be [0, 0, 1] (vertical)
    Eigen::Vector3d e = z_axis - Eigen::Vector3d::UnitZ();

    // Desired angular acceleration (PD control)
    // J_angular * ddq = -Kp * e - Kd * de
    m_constraint.setMatrix(m_J_angular);    // pre-allocated
    m_constraint.setVector(m_desired_acc);  // pre-allocated

    return m_constraint;
  }

 private:
  std::string m_frame_name;
  // Pre-allocated matrices (NO runtime malloc!)
  Eigen::MatrixXd m_J_angular;
  Eigen::VectorXd m_desired_acc;
  tsid::math::ConstraintEquality m_constraint;
};

53.4.5 典型使用代码¶

#include <tsid/tasks/task-se3-equality.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/contacts/contact-point.hpp>
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
#include <tsid/solvers/solver-proxqp.hpp>

// 1. Load Go2 model
tsid::robots::RobotWrapper robot(
    "go2.urdf",
    std::vector<std::string>(),  // package search paths
    pinocchio::JointModelFreeFlyer()  // floating base
);

// 2. Create Formulation
tsid::InverseDynamicsFormulationAccForce formulation(
    "go2_wbc", robot, false  // false = no debug print
);
formulation.computeProblemData(0.0, q0, v0);

// 3. Add 4 foot contacts
const std::array<std::string, 4> foot_names =
    {"FL_foot", "FR_foot", "RL_foot", "RR_foot"};
for (int i = 0; i < 4; ++i) {
    auto contact = std::make_shared<tsid::contacts::ContactPoint>(
        "foot_" + std::to_string(i), robot,
        foot_names[i],
        Eigen::Vector3d::UnitZ(),  // contact normal
        0.6,                       // friction coefficient mu
        1.0, 400.0                 // min/max normal force [N]
    );
    contact->Kp(1000 * Eigen::Vector3d::Ones());
    contact->Kd(100 * Eigen::Vector3d::Ones());
    formulation.addRigidContact(*contact, 1e-5);
}

// 4. Add CoM tracking task (Level 1, high priority)
auto com_task = std::make_shared<tsid::tasks::TaskComEquality>(
    "com_task", robot
);
com_task->Kp(300 * Eigen::Vector3d::Ones());
com_task->Kd(50 * Eigen::Vector3d::Ones());
formulation.addMotionTask(*com_task, 1.0, 1);

// 5. Add joint regularization task (Level 2, low priority)
auto posture_task = std::make_shared<tsid::tasks::TaskJointPosture>(
    "posture_task", robot
);
posture_task->Kp(10 * Eigen::VectorXd::Ones(robot.nv() - 6));
posture_task->Kd(2 * Eigen::VectorXd::Ones(robot.nv() - 6));
formulation.addMotionTask(*posture_task, 0.1, 2);

// 6. Create Solver
tsid::solvers::SolverProxQP solver("solver");

// 7. Main loop (1 kHz)
while (running) {
    // Update reference trajectories
    com_task->setReference(com_ref_trajectory.computeNext());
    posture_task->setReference(q_nominal);

    // Assemble QP and solve
    const auto& qp_data = formulation.computeProblemData(t, q, v);
    const auto& sol = solver.solve(qp_data);

    if (sol.status != tsid::solvers::HQP_STATUS_OPTIMAL) {
        std::cerr << "WBC solver failed!" << std::endl;
        // Fallback: use last valid torque or zero torque
    }

    // Extract joint torques
    Eigen::VectorXd tau = formulation.getActuatorForces(sol);

    // Send to robot
    robot.sendTorque(tau);
    t += dt;
}

⚠️ 编程陷阱:addMotionTask() 的第三个参数是优先级 level,不是"权重比例因子"。Level 0 是最高优先级(硬约束级别),Level 1 和 Level 2 是软约束的不同优先级。如果你把所有任务都放在 Level 1,那退化为加权 QP,HQP 的优先级保证消失。

练习 53.4.A ⭐⭐:编译 TSID + example-robot-data,运行 Python 示例 exercizes/ex_1_forward_dynamics.py。修改 PD 增益(Kp, Kd),观察末端追踪精度和振荡行为的变化。记录至少 3 组增益的效果。

练习 53.4.B ⭐⭐⭐:继承 TaskBase 实现一个自定义 Task——"末端竖直任务"(强制末端 z 轴与世界 z 轴对齐)。提示:误差 $e = R_{ee}[:,2] - [0,0,1]^T$,Jacobian 需要用 Pinocchio 的角速度 Jacobian。添加到已有 TSID 问题中,观察行为变化。

53.5 `InverseDynamicsFormulationAccForce`——TSID 的核心公式 ⭐⭐⭐¶

53.5.1 决策变量的选择:为什么不用 tau?¶

TSID 的决策变量是 $(\ddot{q}, \lambda)$ 而不是 $(\tau, \ddot{q}, \lambda)$。这是一个精妙的工程选择。

推导:从动力学方程 (53.1),一旦 $(\ddot{q}, \lambda)$ 确定,$\tau$ 可以直接反推:

\[\tau = S (M \ddot{q} + h - J_c^T \lambda) \tag{53.14}\]

其中 $S = [0_{n_a \times 6} \;\; I_{n_a}]$ 选出驱动关节的行。

为什么这是合法的? 因为 $S$ 选出的是方程 (53.1) 的后 $n_a$ 行,这些行的左边就是 $M_j \ddot{q} + h_j$,右边就是 $\tau + J_{c,j}^T \lambda$。一旦 $\ddot{q}$ 和 $\lambda$ 确定,左右两边都确定了,$\tau$ 就是唯一确定的。

维度节省:

方案	决策变量维度	Go2 数值
$(\tau, \ddot{q}, \lambda)$	$n_a + n_v + 3n_c$	$12+18+12=42$
$(\ddot{q}, \lambda)$	$n_v + 3n_c$	$18+12=30$
节省	$n_a$	12 维 (28.6%)

QP 求解时间大致与 $n^{2}$ 到 $n^{3}$ 成正比(取决于求解算法和稀疏性),30 维 vs 42 维可以快 30-50%——在 1 kHz 的约束下,这就是"能不能跑起来"的差别。

本质洞察:决策变量的消元技巧揭示了一个更深层的原则——好的数学建模直接影响工程性能。同一个物理问题,换一种数学表述可以让 QP 维度减少近 30%。这不仅是理论上的优雅,更是工程上的生死线:在 1 kHz 实时约束下,30 维 QP 求解时间约 0.15 ms,42 维约 0.3 ms。看似微小的差异,在加上 Pinocchio 动力学计算、数据交换和其他开销后,可能决定了系统是否能稳定运行。

53.5.2 完整 QP 矩阵构建¶

等式约束——欠驱动基座的动力学方程:

消去 $\tau$ 后,只剩基座的 6 行方程(因为基座没有驱动器):

\[M_{b} \ddot{q} + h_{b} = J_{c,b}^T \lambda \tag{53.15}\]

改写为标准形式:

\[\underbrace{[M_b \;\; -J_{c,b}^T]}_{A_{eq,1}} \begin{bmatrix} \ddot{q} \\ \lambda \end{bmatrix} = \underbrace{-h_b}_{b_{eq,1}} \tag{53.16}\]

维度:$6 \times (n_v + 3n_c) = 6 \times 30$。

等式约束——接触不滑:

\[\underbrace{[J_c \;\; 0]}_{A_{eq,2}} \begin{bmatrix} \ddot{q} \\ \lambda \end{bmatrix} = \underbrace{-\dot{J}_c \dot{q}}_{b_{eq,2}} \tag{53.17}\]

维度:$3n_c \times (n_v + 3n_c) = 12 \times 30$。

不等式约束——摩擦锥:

\[\underbrace{[0 \;\; D_{stack}]}_{A_{ineq,1}} \begin{bmatrix} \ddot{q} \\ \lambda \end{bmatrix} \le 0 \tag{53.18}\]

维度:$5n_c \times 30 = 20 \times 30$。

不等式约束——扭矩限制(通过 (53.14) 反推):

\[-\tau_{max} \le S(M\ddot{q} + h - J_c^T \lambda) \le \tau_{max}\]

展开为两组不等式:

\[SM \ddot{q} - SJ_c^T \lambda \le \tau_{max} - Sh \tag{53.19}\]

\[-SM \ddot{q} + SJ_c^T \lambda \le \tau_{max} + Sh \tag{53.20}\]

维度:$2n_a \times 30 = 24 \times 30$。

代价函数——任务追踪:

对每个 Motion Task $k$(权重 $w_k$,Jacobian $J_k$,期望加速度 $\ddot{x}_k^{des}$):

\[\text{err}_k = J_k \ddot{q} - (\ddot{x}_k^{des} - \dot{J}_k \dot{q})\]

堆叠所有任务后的 Hessian 和梯度:

\[H = \sum_k w_k \begin{bmatrix} J_k^T J_k & 0 \\ 0 & 0 \end{bmatrix}, \quad g = -\sum_k w_k \begin{bmatrix} J_k^T \\ 0 \end{bmatrix} (\ddot{x}_k^{des} - \dot{J}_k \dot{q}) \tag{53.21}\]

如果还有 Force Task(如接触力正则化 $\|\lambda - \lambda^{ref}\|^2$),则 $H$ 的右下角也会有对应项。

完整 QP 汇总(Go2 四足,4 足着地):

\[\min_{\ddot{q}, \lambda \in \mathbb{R}^{30}} \frac{1}{2} x^T H x + g^T x\]

约束类型	矩阵维度	行数
基座动力学(等式)	$6 \times 30$	6
接触不滑(等式)	$12 \times 30$	12
摩擦锥(不等式)	$20 \times 30$	20
扭矩限制(不等式)	$24 \times 30$	24
总计		18 等式 + 44 不等式

💡 与 53.2 的对比:53.2 节用 $(\tau, \ddot{q}, \lambda)$ 时是 42 维 + 30 等式 + 44 不等式。消去 $\tau$ 后变为 30 维 + 18 等式 + 44 不等式。等式约束也减少了(不需要显式写驱动关节的动力学),QP 更紧凑。这就是"好的数学建模直接影响工程性能"的范例。

53.5.3 QP 求解器选择¶

基于近期的 QP 求解器基准测试(arXiv:2510.21773, 2025; arXiv:2502.01329, Stark et al., 2025),对 WBC 这类密集小规模 QP 的推荐:

求解器	算法	WBC 典型求解时间	鲁棒性	推荐场景
eiquadprog	Active-set	<0.1 ms	良好	默认首选,最快
qpmad	Active-set	<0.1 ms	良好	低依赖,嵌入式
ProxQP	Augmented Lagrangian	0.1-0.3 ms	优秀	接触丰富/鲁棒性优先
qpOASES	Active-set	0.1-0.3 ms	良好	小问题,经典选择
OSQP	ADMM	0.2-0.5 ms	中等	稀疏 MPC,不推荐 WBC

💡 概念澄清:OSQP 虽然流行,但它是一阶方法(ADMM),对 WBC 这种小密集 QP 不如二阶方法(active-set)快。OSQP 更适合 MPC 中的大规模稀疏 QP。足式/60_QP_NLP建模有详细的算法对比。

Stark et al. (2025) 的基准测试还引入了一个有价值的跨硬件对比指标——Solve Frequency per Watt (SFPW),用于评估 QP 求解器在不同硬件平台(桌面 CPU、NVIDIA Jetson Orin、LattePanda)上的能效表现。结果显示:Jetson Orin 的 SFPW 是桌面 PC 的约 3 倍,这意味着在嵌入式场景下选择正确的求解器-硬件组合至关重要。对于 WBC 场景,eiquadprog 在所有硬件平台上都是最快的;对于 MPC 场景,HPIPM 利用稀疏结构表现最优。

练习 53.5.A ⭐⭐⭐:用 Eigen 手动构建上述 QP 矩阵(使用 Pinocchio 计算 M, h, J_c),然后用 ProxQP 和 eiquadprog 分别求解。对比求解时间和精度差异。

练习 53.5.B ⭐⭐:在 TSID 中切换不同的 Solver 后端(SolverHQuadProgFast vs SolverProxQP),跑同一个追踪任务,记录求解时间和追踪精度。你能复现基准测试中的性能排名吗?

53.6 `EIGEN_RUNTIME_NO_MALLOC`——实时控制的硬约束 ⭐⭐¶

前两节解剖了 TSID 的架构设计和 QP 公式构造。然而,即使数学公式完全正确、软件架构清晰优雅,如果 QP 求解过程中触发了一次堆内存分配,整个 1kHz 控制循环就可能超时。内存管理是 WBC 从"能跑"到"能上机器人"之间最容易被忽视的鸿沟。

53.6.1 为什么 malloc 是实时控制的"死刑"¶

背景:WBC 在实时线程(SCHED_FIFO)中运行,控制周期是 1 ms。假如没有 EIGEN_RUNTIME_NO_MALLOC 保护,一个看似无害的 Eigen 表达式(如 MatrixXd C = A * B 而非预分配的 C.noalias() = A * B)就会在 QP 组装的热路径中触发 malloc。在实验室环境下可能 999 次都没事,但第 1000 次恰好碰到内核页面回收,控制周期从 0.8 ms 跳到 5 ms——机器人在那一瞬间失去平衡控制,轻则踉跄、重则倒地。一次 malloc 调用可能带来以下延迟:

情况	耗时	原因
最好情况(free list 命中)	0.1-1 us	只操作用户态链表
一般情况(sbrk)	1-10 us	系统调用扩展堆
最坏情况(mmap)	10-1000 us	内核分配虚拟内存页
极端情况(页面回收)	1-10 ms	内核 OOM killer 或 swap

1 ms 的控制周期,10-1000 us 的 malloc 延迟,最坏情况直接超时。更糟的是,malloc 的延迟不可预测——这就是实时系统最忌讳的"非确定性延迟"。

53.6.2 Linux 实时调度基础¶

为了理解 WBC 的运行环境,需要了解 Linux 的实时调度:

┌─────────────────────────────────────────────┐
│ SCHED_FIFO / SCHED_RR (实时策略)            │
│ 优先级 1-99,抢占式调度                      │
│ WBC 线程: SCHED_FIFO, priority=49           │
│ -> 不被普通进程抢占                         │
│ -> 但 malloc 的内核路径可能阻塞!            │
├─────────────────────────────────────────────┤
│ SCHED_OTHER (普通策略,CFS 调度器)           │
│ 优先级 0,时间片轮转                         │
│ 非实时进程运行在这里                         │
└─────────────────────────────────────────────┘

SCHED_FIFO 的关键规则: - 实时线程不会被普通进程抢占 - 同优先级的实时线程按 FIFO 顺序运行(先到先服务) - 更高优先级的实时线程可以抢占低优先级的

但 SCHED_FIFO 不能保护你免受 malloc 的伤害:当你调用 malloc,内核需要操作页表——这发生在内核态,实时线程必须等待。更糟的是,如果触发了优先级反转(priority inversion):

时间 ->
┌──────────────────────────────────────────┐
│ 高优先级(WBC):   运行...调 malloc...等   │
│ 低优先级(日志):   持有 malloc 内部锁     │
│ 中优先级(网络):   抢占了低优先级         │
│                                          │
│ 结果:WBC 等待低优先级释放锁             │
│       但低优先级被中优先级抢占           │
│       -> WBC 间接被中优先级阻塞!        │
└──────────────────────────────────────────┘

PREEMPT_RT 内核通过优先级继承(priority inheritance)可以缓解这个问题,但最根本的解决方案是:在实时路径上不调用 malloc。

53.6.3 TSID 的内存安全机制¶

TSID 的 CMakeLists.txt 默认开启两个 Eigen 宏:

option(EIGEN_RUNTIME_NO_MALLOC
       "If ON, Eigen will assert on runtime malloc" ON)
option(EIGEN_NO_AUTOMATIC_RESIZING
       "If ON, Eigen will not auto-resize matrices" ON)

EIGEN_RUNTIME_NO_MALLOC 的工作机制:

// Eigen/src/Core/util/Memory.h (simplified)
#ifdef EIGEN_RUNTIME_NO_MALLOC
static bool is_malloc_allowed_flag = true;

inline void set_is_malloc_allowed(bool allowed) {
    is_malloc_allowed_flag = allowed;
}

inline void check_that_malloc_is_allowed() {
    eigen_assert(is_malloc_allowed_flag &&
        "Heap allocation in real-time code path!");
}
#endif

使用方式:

// During initialization (before real-time loop)
Eigen::internal::set_is_malloc_allowed(true);
// ... allocate all matrices here ...

// Enter real-time loop
Eigen::internal::set_is_malloc_allowed(false);
while (running) {
    // Any Eigen allocation here will trigger assert failure
    wbc.solve(q, v);  // must be malloc-free
}

53.6.4 Eigen 何时会偷偷 malloc?¶

这是最容易犯错的地方。以下操作在 EIGEN_RUNTIME_NO_MALLOC 下会触发 assert:

操作	原因	修复方法
`MatrixXd A; A = B * C;`	矩阵乘法创建临时变量	`A.noalias() = B * C;`
`MatrixXd A(n, m);` 在循环内	每次构造都分配	移到循环外作为成员变量
`M.resize(10, 10)`	改变尺寸 = 重分配	预分配正确尺寸
`A * B * C`	链式乘法的中间结果	分步计算或 `.noalias()`
`A.inverse()`	需要临时矩阵	用 `A.llt().solve(I)`
`svd.compute(A)`	SVD 需要工作空间	预分配 SVD 对象
`v.conservativeResize(n+1)`	保守调大 = malloc + copy	预分配最大尺寸

正确示范:

class WBC {
  // Pre-allocate ALL matrices as member variables
  Eigen::MatrixXd A_;       // constraint matrix
  Eigen::VectorXd b_;       // constraint vector
  Eigen::VectorXd x_;       // solution
  Eigen::MatrixXd temp_;    // temporary for A*B

 public:
  WBC(int n, int m) : A_(n, m), b_(n), x_(m), temp_(n, m) {}

  void solve(const Eigen::VectorXd& q, const Eigen::VectorXd& v) {
    // GOOD: use pre-allocated matrices, noalias avoids temporaries
    temp_.noalias() = M_ * J_.transpose();
    A_.block(0, 0, 6, nv_) = M_.topRows(6);

    // BAD examples (would crash with EIGEN_RUNTIME_NO_MALLOC):
    // Eigen::MatrixXd temp = M_ * J_.transpose();  // malloc!
    // A_.resize(new_n, new_m);                      // malloc!
  }
};

⚠️ 编程陷阱:noalias() 并非万能。当目标矩阵出现在表达式右侧时,使用 noalias() 会产生错误结果:
// WRONG: C appears on both sides
C.noalias() += A * C;  // overwrites C mid-computation!

// CORRECT: use pre-allocated temporary
temp.noalias() = A * C;
C += temp;

53.6.5 pmr 分配器替代方案¶

如果你确实需要在实时路径上分配内存(例如接触点数量动态变化),可以使用 02_C++基础与进阶/40_内存管理中介绍的 std::pmr 方案:

#include <memory_resource>

class RealtimeWBC {
    // Stack-based memory pool: 64 KB on stack, no malloc
    alignas(64) std::byte buffer_[65536];
    std::pmr::monotonic_buffer_resource pool_{
        buffer_, sizeof(buffer_)};

    // Containers use pool instead of malloc
    std::pmr::vector<Eigen::Vector3d> contact_forces_{&pool_};

 public:
    void update(int num_contacts) {
        pool_.release();  // O(1) "free all" - no malloc
        contact_forces_.reserve(num_contacts);
        // ... use contact_forces_ ...
    }
};

53.6.6 调试 malloc 违规¶

当你在 TSID 中添加新 Task 后遇到 assert 失败时,如何定位?

# Method 1: GDB backtrace
gdb ./wbc_node
(gdb) run
# ... assert failure ...
(gdb) bt
# Shows the exact line that triggered malloc

# Method 2: Valgrind + massif (offline profiling)
valgrind --tool=massif ./wbc_node
ms_print massif.out.12345

# Method 3: LD_PRELOAD malloc interception
LD_PRELOAD=./libmalloc_tracker.so ./wbc_node

💡 实践技巧:在开发阶段,用 EIGEN_RUNTIME_NO_MALLOC + Debug 模式运行所有单元测试。所有 malloc 违规都会在测试中暴露,而不是在机器人上暴露(那时已经来不及了)。

练习 53.6.A ⭐⭐:写一个小程序,开启 EIGEN_RUNTIME_NO_MALLOC,然后故意触发 5 种不同的 Eigen 堆分配。逐一修复,使程序在 set_is_malloc_allowed(false) 下不触发 assert。

练习 53.6.B ⭐⭐⭐:在你的 WBC 实现中加入 EIGEN_RUNTIME_NO_MALLOC 保护。用 std::chrono 测量加保护前后的求解时间分布(均值、最大值、p99)。预分配是否消除了时间分布的长尾?

53.7 legged_control 的轻量 WBC——相对 TSID 的简化 ⭐⭐¶

TSID 提供了一个功能完备、可扩展的 WBC 框架,但它的通用性也带来了代码量和接口复杂度。在实际四足机器人开发中,许多团队选择了一条更务实的路径:只保留最核心的 QP 组装逻辑,砍掉不需要的抽象层。legged_control 的 WBC 就是这种工程取舍的典型代表。

53.7.1 legged_control 概述¶

legged_control 由 Qiayuan Liao 开发(GitHub: qiayuanl/legged_control),是基于 OCS2 + ros-control 的四足机器人控制框架。它的 WBC 模块(legged_wbc/)是一个极简但完整的 WBC 实现,非常适合入门学习。

代码规模对比:

项目	核心 WBC 代码量	支持的机器人	求解器
TSID	~5000 行(多文件)	任意(通用框架)	多后端
legged_wbc	~600 行(3 文件)	四足(专用)	qpOASES

53.7.2 核心类结构¶

legged_wbc/
├── include/legged_wbc/
│   ├── WbcBase.h          // Base class: dynamics computation
│   ├── HierarchicalWbc.h  // 3-level HQP implementation
│   └── WeightedWbc.h      // Weighted QP alternative
└── src/
    ├── WbcBase.cpp         // ~270 lines: task formulation
    ├── HierarchicalWbc.cpp // ~22 lines: 仅装配 task0/1/2,递推逻辑在 HoQp
    └── WeightedWbc.cpp     // ~100 lines: single QP

53.7.3 WbcBase 的 7 个任务公式¶

WbcBase 实现了 7 个任务公式化方法,每个返回一个 (A, b) 对(等式约束)或 (D, f) 对(不等式约束):

方法	功能	约束类型
`formulateFloatingBaseEomTask()`	浮动基座动力学方程	等式
`formulateNoContactMotionTask()`	接触足零加速度	等式
`formulateFrictionConeTask()`	摩擦锥约束	不等式
`formulateBaseAccelTask()`	基座加速度追踪	等式(软)
`formulateSwingLegTask()`	摆动腿足端追踪	等式(软)
`formulateTorqueLimitsTask()`	关节扭矩限制	不等式
`formulateContactForceTask()`	接触力参考追踪	等式(软约束)

决策变量结构(与 TSID 相同的消元思路):

x = [ddq (18), lambda (12), tau (12)]^T    // 42 维

注意 legged_wbc 没有消去 $\tau$——它保留了全部三组决策变量。这使代码更直观(直接读出 $\tau$),但 QP 维度更大(42 vs 30)。

53.7.4 HierarchicalWbc 的三层结构¶

HierarchicalWbc 实现了一个三层严格 HQP——即 HoQp(task2, HoQp(task1, HoQp(task0))),逐层在前一层的零空间内求解(与足式/240_legged_control精读一致):

Level 0 (硬约束 -> HoQp 0):
  等式约束:
    - formulateFloatingBaseEomTask()   // 浮基动力学
    - formulateNoContactMotionTask()   // 接触足零加速度(非接触运动)
  不等式约束:
    - formulateTorqueLimitsTask()      // 力矩限制
    - formulateFrictionConeTask()      // 摩擦锥
  含义:物理定律,不可违反

Level 1 (运动追踪 -> HoQp 1, 在 Level 0 的零空间内):
  代价:
    - formulateBaseAccelTask()         // 基座追踪
    - formulateSwingLegTask()          // 摆动腿追踪
  约束:不破坏 Level 0

Level 2 (力分配 -> HoQp 2, 在 Level 0+1 的零空间内):
  代价:
    - formulateContactForceTask()      // 接触力参考(MPC 期望力)
  约束:不破坏 Level 0+1

核心代码片段(简化):

// HierarchicalWbc.cpp (simplified)
Eigen::VectorXd HierarchicalWbc::solve(
    const Eigen::VectorXd& q, const Eigen::VectorXd& v) {

  // Level 0 (硬约束): dynamics + contact constraints
  auto [A1, b1] = formulateFloatingBaseEomTask();
  auto [A2, b2] = formulateNoContactMotionTask();

  Eigen::MatrixXd A_eq;      // stack A1, A2
  Eigen::VectorXd b_eq;      // stack b1, b2
  stackTasks(A1, b1, A2, b2, A_eq, b_eq);

  auto [D, f] = formulateFrictionConeTask();
  auto [D2, f2] = formulateTorqueLimitsTask();
  stackTasks(D, f, D2, f2, D_ineq, f_ineq);

  // Solve HoQp 0: min ||x||^2 s.t. A_eq*x=b_eq, D*x<=f
  solveQP(H_identity, g_zero, A_eq, b_eq, D_ineq, f_ineq, x0);

  // Compute null-space of Level 0
  // N0 = I - A_eq^+ * A_eq
  Eigen::MatrixXd N0 = computeNullSpace(A_eq);

  // Level 1 (运动追踪): tracking tasks in null-space of Level 0
  auto [A3, b3] = formulateBaseAccelTask();
  auto [A4, b4] = formulateSwingLegTask();
  stackTasks(A3, b3, A4, b4, A_track, b_track);

  // Project into null-space: A_track_proj = A_track * N0
  Eigen::MatrixXd A_proj = A_track * N0;
  Eigen::VectorXd b_proj = b_track - A_track * x0;

  // Solve HoQp 1: min ||A_proj*delta1 - b_proj||^2
  solveQP(A_proj.transpose() * A_proj,
          -A_proj.transpose() * b_proj,
          ..., delta1);
  Eigen::VectorXd x1 = x0 + N0 * delta1;

  // Level 2 (力分配): contact-force reference in null-space of Level 0+1
  Eigen::MatrixXd N01 = computeNullSpace(/* stack(Level0, Level1) */);
  auto [A5, b5] = formulateContactForceTask();
  Eigen::MatrixXd A_cf = A5 * N01;
  Eigen::VectorXd b_cf = b5 - A5 * x1;
  solveQP(A_cf.transpose() * A_cf,
          -A_cf.transpose() * b_cf,
          ..., delta2);

  // Final solution
  return x1 + N01 * delta2;
}

53.7.5 TSID vs legged_wbc 对比¶

维度	TSID	legged_wbc
代码量	~5000 行	~600 行
优先级层数	任意	3 层
Task 类型	10+ 种(SE3, CoM, AM, ...)	6 种(硬编码)
约束抽象	通用(Equality/Inequality/Bound)	硬编码
求解器	多后端(ProxQP, eiquadprog, ...)	只用 qpOASES
决策变量	$(\ddot{q}, \lambda)$ 消元	$(\ddot{q}, \lambda, \tau)$ 全保留
机器人类型	任意	四足专用
实时安全	`EIGEN_RUNTIME_NO_MALLOC`	无
学习曲线	1-2 周	半天-1 天
适合场景	产品级/研究级	教学/快速原型

🧠 思维陷阱:不要因为 legged_wbc 代码少就认为它"不好"。它的简化是有目的的——去掉了通用性,换取了可读性和开发速度。如果你只做四足控制,legged_wbc 完全够用;如果你要做人形或多平台产品,则需要 TSID 级别的框架。

推荐学习路径:

第 1 天:读 legged_wbc (600 行,建立直觉)
  -> 理解 WBC 的基本结构
  -> 理解"任务公式化"的套路

第 2-3 天:读 TSID 的 TaskSE3Equality (核心 Task)
  -> 对比 legged_wbc 的 formulateSwingLegTask()
  -> 理解"通用框架 vs 专用实现"的设计差异

第 4-5 天:读 TSID 的 InverseDynamicsFormulationAccForce
  -> 理解 QP 组装流程
  -> 理解决策变量消元技巧

第 6-7 天:自己实现一个极简 WBC
  -> 融合两者的优点
  -> 在 MuJoCo/Gazebo 中验证

练习 53.7.A ⭐⭐:精读 legged_wbc/src/WbcBase.cpp 的 formulateFloatingBaseEomTask()。用文字描述:它如何用 Pinocchio 计算 $M$、$h$、$J_c$?动力学方程如何转化为 $Ax = b$ 的形式?

练习 53.7.B ⭐⭐⭐:精读 HierarchicalWbc.cpp,画出它的三层 QP 求解流程图。标注每步的矩阵维度(以 Go2 为例)。

53.8 MPC + WBC 集成——腿足控制的完整图景 ⭐⭐⭐¶

本质洞察:MPC 和 WBC 的分工不是"粗糙规划 + 精细执行"那么简单。MPC 用简化模型在长时域上搜索全局一致的运动策略(解决"做什么"),WBC 用全身模型在当前时刻满足所有物理约束(解决"怎么做")。两者的模型不一致恰恰是设计意图——简化模型让 MPC 跑得快,全身模型让 WBC 跟得准。这种"有意的模型不一致 + 高频修正"是分层控制鲁棒性的来源,而非缺陷。

53.8.1 数据流¶

┌──────────────────┐     triple buffer / atomic ptr     ┌──────────────────┐
│   MPC Thread     │ ─────────────────────────────────> │   WBC Thread     │
│                  │                                     │                  │
│ freq: 50 Hz      │  data: {com_traj, foot_traj,       │ freq: 1000 Hz    │
│ model: SRBD/     │         contact_forces,             │ model: full-body │
│   Centroidal     │         contact_schedule,           │   dynamics       │
│ solver: iLQR/    │         timestamp}                  │ solver: QP       │
│   DDP            │                                     │                  │
│ time: 10-20 ms   │                                     │ time: 0.3-0.8 ms │
└──────────────────┘                                     └──────────────────┘

关键数据接口:

数据	类型	维度	说明
`com_trajectory`	分段多项式	$3 \times N_{steps}$	质心位置轨迹
`foot_trajectory`	分段多项式	$3 \times 4 \times N_{steps}$	四足位置轨迹
`contact_forces`	离散序列	$12 \times N_{steps}$	接触力参考
`contact_schedule`	布尔序列	$4 \times N_{steps}$	每足是否着地
`timestamp`	double	1	MPC 解对应的起始时间

53.8.2 Triple Buffer 机制¶

为什么不用 mutex? 因为 mutex 会导致 WBC 线程阻塞(等待 MPC 线程释放锁),这在 1 kHz 控制中不可接受。

Triple buffer 的工作原理:

┌─────────────────────────────────────────────────────┐
│ Buffer A: [MPC 正在写入新解]                         │
│ Buffer B: [WBC 正在读取当前解]                       │
│ Buffer C: [空闲,等待交换]                           │
│                                                     │
│ MPC 写完 -> atomic swap(A, C) -> MPC 开始写 C       │
│ WBC 需要新解 -> atomic swap(B, C) -> WBC 读 C       │
│                                                     │
│ 关键:所有 swap 操作都是原子的,无锁                 │
│ 代价:WBC 可能读到"前一次"而非"最新"的 MPC 解      │
│       (延迟最多一个 MPC 周期)                       │
└─────────────────────────────────────────────────────┘

// Simplified triple buffer implementation
template <typename T>
class TripleBuffer {
    std::array<T, 3> buffers_;
    std::atomic<int> write_idx_{0};  // MPC writes here
    std::atomic<int> read_idx_{1};   // WBC reads here
    std::atomic<int> free_idx_{2};   // ready for swap

 public:
    // MPC thread calls this after finishing a solve
    T& getWriteBuffer() { return buffers_[write_idx_.load()]; }

    void publishWrite() {
        int old_free = free_idx_.exchange(write_idx_.load());
        write_idx_.store(old_free);
    }

    // WBC thread calls this to get latest MPC solution
    const T& getReadBuffer() {
        int old_free = free_idx_.exchange(read_idx_.load());
        read_idx_.store(old_free);
        return buffers_[read_idx_.load()];
    }
};

53.8.3 时间插值算法¶

MPC 输出的是离散时间步的轨迹(例如 20 个点,每点间隔 50 ms)。WBC 运行在 1 kHz,需要在任意时刻查询参考值。

线性插值(最简单):

\[x_{ref}(t) = x_k + \frac{t - t_k}{t_{k+1} - t_k} (x_{k+1} - x_k), \quad t_k \le t < t_{k+1}\]

三次样条插值(更平滑,用于足端轨迹):

\[x_{ref}(t) = a_0 + a_1(t - t_k) + a_2(t - t_k)^2 + a_3(t - t_k)^3\]

系数由位置和速度的边界条件确定(Hermite 插值）。

⚠️ 编程陷阱:时间戳同步是 MPC+WBC 集成中最容易出错的地方。MPC 解的时间戳是"MPC 开始求解时的时间",但 WBC 使用时已经过了 10-20 ms。如果 WBC 不补偿这个延迟,追踪会有滞后。正确做法:WBC 查询 $t_{current}$ 对应的参考值,而不是 $t_{MPC\_start}$。

53.8.4 失败模式与恢复¶

失败模式	检测方法	恢复策略
MPC 求解超时	超过预设时间阈值	WBC 用上一次 MPC 解 + 外推
MPC 求解失败	求解器返回 INFEASIBLE	切换到安全站立姿态
WBC 求解失败	QP 返回 INFEASIBLE	降级:放松摩擦锥约束或切到 PD 控制
通信丢包	时间戳不更新	用上一次有效解,计数告警

// Robust MPC+WBC integration (pseudocode)
void wbc_loop() {
    while (running) {
        // Try to get latest MPC solution
        auto& mpc_sol = triple_buffer.getReadBuffer();

        if (mpc_sol.timestamp < t_current - mpc_timeout) {
            // MPC solution is stale
            stale_count++;
            if (stale_count > max_stale) {
                // Switch to safe standing mode
                tau = pd_controller(q, v, q_safe, v_zero);
                continue;
            }
            // Use stale solution with time extrapolation
        }

        // Normal WBC solve
        auto status = wbc.solve(q, v, mpc_sol);
        if (status != QP_OPTIMAL) {
            // WBC failed: relax friction cone or use last tau
            tau = last_valid_tau;
            continue;
        }

        tau = wbc.getTorques();
        last_valid_tau = tau;
        stale_count = 0;
    }
}

💡 工程洞察:足式/110_OCS2完整栈与双线程MPC OCS2 框架的双线程架构(MPC_MRT_Interface)就是上述 MPC+WBC 集成的工业级实现。它使用 std::condition_variable + triple buffer 实现线程间通信,并包含完整的失败恢复逻辑。

53.8.5 MPC 与 WBC 模型不一致的影响与缓解 ⭐⭐⭐¶

MPC 使用简化模型（如 SRBD 或 Centroidal 模型）,WBC 使用全身模型（Pinocchio RNEA/ABA）。这种模型不一致是分层架构的固有特征,会产生以下影响:

接触力分配不一致:MPC 基于简化模型计算的最优接触力 $\lambda^{MPC}$,在全身模型下可能不满足扭矩限制——因为简化模型没有考虑腿部惯量和关节约束。WBC 必须在满足物理约束的前提下"修正"这些力,修正量越大意味着 MPC 的参考越不可信。

动量追踪误差:SRBD 假设腿部质量集中在髋关节,因此高速摆腿时 MPC 预测的角动量与实际不符。WBC 无法在单步内修正这种系统性偏差,只能依靠 MPC 在下一个周期重新优化来逐步收敛。

缓解策略:

策略	原理	代表工作
模型匹配	让 MPC 模型尽量贴近 WBC 模型	Centroidal 模型比 SRBD 更精确
反馈修正	WBC 将实际状态反馈给 MPC	OCS2 的 MPC-MRT 接口
VWBC	用 MPC 的 Q 函数替代 WBC 权重	Cafe-MPC (2024)
全身 MPC	取消分层,直接用全身模型做 MPC	MuJoCo WBC-MPC (2025)

练习 53.8.A ⭐⭐:实现一个 triple buffer 模板类,并用两个线程测试:生产者每 20 ms 写入一个递增计数器,消费者每 1 ms 读取。验证消费者是否从不阻塞,且读到的值单调递增(允许重复但不允许跳回)。

练习 53.8.B ⭐⭐⭐:在 MPC+WBC 集成中,如果 MPC 求解时间不确定(有时 10 ms,有时 50 ms),WBC 应如何处理?设计一个自适应插值方案:当 MPC 解新鲜时用线性插值,当 MPC 解陈旧时用外推 + 衰减。

53.9 WBC 调参实用指南 ⭐⭐¶

53.9.1 PD 增益调参¶

WBC 中每个 Task 都有 PD 增益 $(K_p, K_d)$,它们决定了任务追踪的刚度和阻尼。

基本原则:

\[\ddot{x}_{des} = \ddot{x}_{ref} + K_d(\dot{x}_{ref} - \dot{x}) + K_p(x_{ref} - x)\]

这等效于一个二阶系统:

\[\ddot{e} + K_d \dot{e} + K_p e = 0\]

特征方程:$s^2 + K_d s + K_p = 0$

临界阻尼条件:$K_d = 2\sqrt{K_p}$（不振荡、最快收敛）

场景	推荐 $K_p$	推荐 $K_d$	理由
质心追踪	200-500	30-50	中等刚度,防止过冲导致失稳
摆动腿足端	300-1000	50-100	高刚度,精确落脚
关节正则化	5-20	1-5	低刚度,不干扰主要任务
基座姿态	100-300	20-50	中等,保持水平但允许倾斜

⚠️ 编程陷阱:$K_p$ 设太大(如 $>2000$)会导致 QP 的 Hessian 矩阵条件数恶化,求解器可能不收敛或给出振荡解。如果你发现机器人"抖动",第一件事是降低 $K_p$。

53.9.2 权重与优先级配置¶

权重的选择原则:

量纲归一化:不同任务的误差量纲不同(位置 m、角度 rad、力 N）。先将各任务误差归一化到相近量级,再用权重调节相对重要性。
对角阵权重:对 SE3 任务,通常位置分量和旋转分量的权重不同:

Eigen::VectorXd weight(6);
weight << 1.0, 1.0, 1.0,    // position (x, y, z)
          0.5, 0.5, 0.5;    // orientation (roll, pitch, yaw)
task.setWeight(weight);

渐进调参法:

Step 1: 只开 Level 1 硬约束,确认机器人能站稳
Step 2: 加 CoM 追踪(Level 2),调 Kp/Kd 直到平衡良好
Step 3: 加足端追踪(Level 2),确认步态正常
Step 4: 加关节正则化(Level 3),防止关节漂移
Step 5: 微调权重比,优化能量效率

53.9.3 常见故障诊断¶

现象	可能原因	排查方法
机器人抖动	$K_p$ 过大 / 控制延迟	降低 $K_p$,检查通信延迟
机器人"软"	$K_p$ 过小 / 权重不当	提高追踪任务权重
QP 不可行	约束冲突(如摩擦锥太紧)	放松摩擦系数或检查接触模式
关节扭矩饱和	$\tau_{max}$ 设置不当	检查电机参数,降低追踪要求
足底打滑	摩擦系数过高 / 力分配不均	降低 $\mu$,加力正则化

53.9.4 接触切换时的平滑过渡 ⭐⭐¶

WBC 调参中最棘手的问题之一是接触状态切换(足从空中着地、或从地面抬起)时的扭矩跳变。这个问题的根源是:接触约束的突然激活/去激活会导致 QP 的约束集瞬间变化,求解出的 $\tau$ 在切换瞬间可能出现不连续。

问题的物理理解:想象你正在搬一张桌子,突然有人抽走了你一只手下面的支撑——你需要瞬间用另一只手承担所有重量。WBC 在足从着地变为腾空时面临同样的问题:接触约束消失,力必须在剩余支撑足之间重新分配。

平滑过渡策略:

力渐变窗口:在接触切换前后各留一个时间窗口(通常 20-50 ms),将接触力的上下界线性过渡:

\[f_z^{max}(t) = f_z^{max,0} \cdot \sigma\left(\frac{t - t_{switch}}{\Delta t}\right)\]

其中 $\sigma(\cdot)$ 是 sigmoid 函数或线性斜坡。

摩擦锥渐变:着地瞬间摩擦系数从 0 线性增加到 $\mu$,避免刚接触时要求过大的水平力。
双重参考混合:在切换窗口内,WBC 同时追踪"旧接触模式"和"新接触模式"的参考,用时间权重混合。

练习 53.9.A ⭐⭐:在四足仿真中,从"全部默认参数"开始,按上述渐进法调参。记录每步的调参过程和效果。

⚠️ 常见陷阱

💡 概念误区:认为 WBC 增益可以从 MPC 增益直接复制

新手想法:"MPC 已经调好了 PD 增益,WBC 直接用同样的值就行。"

实际上:MPC 的增益是在简化模型下针对较低频率(10-100 Hz)优化的。WBC 运行在 1 kHz,如果直接使用 MPC 的增益,等效闭环带宽可能过高,导致振荡。WBC 的增益通常需要比 MPC 低 2-5 倍,以匹配实际的执行器带宽和通信延迟。

53.10 WBC 在不同机器人形态的适配 ⭐⭐¶

53.10.1 四足 vs 双足 vs 人形¶

维度	四足	双足	人形(上肢+腿）
自由度	12-18	10-12	20-40+
接触模型	4 点接触	2 面接触	2 面接触 + 手
稳定性	高(4 点支撑)	低(2 点支撑)	低 + 操作需求
关键 Task	CoM + 足端	CoM + ZMP + 角动量	全部 + 操作
计算负担	中(42 维 QP)	中(50 维 QP)	高(80+ 维 QP)

双足特有的挑战:

角动量管理:双足只有 2 个接触点,稳定性裕度小。通过 TaskAMEquality 控制角动量变化率,防止侧翻。
ZMP 约束:ZMP(Zero Moment Point)必须在支撑多边形内——这是一个依赖接触力的非线性约束,需要用 Contact6d 处理。

人形特有的挑战:

操作与平衡的冲突:上肢操作物体会改变质心位置和角动量,WBC 必须在操作和平衡之间分配优先级。
自碰撞避免:关节数多,需要额外的 Task 或 Constraint 防止身体各部分碰撞。

53.10.2 bipedal_locomotion_framework 的 TSID 变体¶

IIT 的 robotology/bipedal-locomotion-framework (BLF) 为 iCub 人形机器人实现了一个 TSID 变体。与 stack-of-tasks/TSID 的主要区别:

维度	TSID (stack-of-tasks)	BLF QPTSID
动力学库	Pinocchio	iDynTree (转向 Pinocchio)
接触模型	ContactPoint / Contact6d	ContactWrench
QP 求解器	多后端	OSQP / qpOASES
角动量 Task	TaskAMEquality	CentroidalMomentumTask
目标平台	通用	iCub / ergoCub

💡 趋势观察:随着 Pinocchio 3.x 的发展和 TSID 的成熟,越来越多的人形机器人项目开始统一使用 Pinocchio + TSID 生态,而非各自开发 WBC 框架。

53.10.3 WBC 在工业界主流平台的应用 ⭐⭐¶

2025-2026 年,WBC 技术在工业界的应用呈现出两极分化的趋势:传统模型基控制（Model-Based Control）和强化学习（RL）两条路线并行发展。

Unitree 系列:Unitree Go2/B2 四足和 H1/G1 人形是当前 WBC 研究最活跃的硬件平台。Unitree H1 的 WBC 实验证实了基于 SRBD 模型的 MPC+WBC 架构在全尺寸人形上的可行性。值得注意的是,大量 RL 研究(约 12/18 个已发表的全身控制策略)运行在 Unitree 硬件上,表明 RL 正在成为传统 WBC 的强力替代方案。

Agility Robotics Digit:Digit V3 是目前唯一真正进入商业部署的人形机器人(2026 年 2 月与 Toyota Motor Manufacturing Canada 签署 Robots-as-a-Service 协议)。Digit 的控制架构结合了传统 WBC 和学习组件,在物流场景中展示了可靠的全身运动控制。

Boston Dynamics Atlas:Atlas 从液压驱动转向电驱动后,WBC 的设计面临新的挑战——电机的扭矩带宽和峰值扭矩不如液压,需要更精细的力分配和更高效的 QP 求解。Atlas 的控制系统被认为使用了 MPC + WBC 的分层架构,但具体实现细节未公开。

RL 替代 WBC 的趋势:越来越多的工作表明,端到端 RL 策略可以直接从感知输入映射到关节扭矩,跳过整个 MPC+WBC 管道。但 RL 策略的安全保证和可解释性仍然是未解决的挑战。当前的混合方案——RL 高层决策 + WBC 底层执行——可能是近期最务实的路线。

53.11 研究前沿与论文阅读（博士预备视角） ⭐⭐⭐¶

53.11.1 经典必读¶

Escande A., Mansard N., Wieber P.-B. (2014) "Hierarchical quadratic programming: Fast online humanoid-robot motion generation" — IJRR, 33(7):1006-1028. HQP 理论的奠基论文。提出了同时处理等式和不等式约束的完整层级求解框架。
Kim D., Jorgensen S. J., Lee J., et al. (2020) "Dynamic locomotion for passive-ankle biped robots and humanoids using whole-body locomotion control" — IJRR. WBC 应用于高动态腿足运动的标杆工作。

53.11.2 近期重要进展（2024-2026）¶

Cafe-MPC / VWBC (Li & Wensing, T-RO 2025): 提出了VWBC (Value-function-based WBC):用 MPC 反向扫描得到的 action-value 函数 $Q(\delta x, \delta u)$ 作为 WBC 的代价函数,消除了 WBC 层的手动调参需求。$Q$ 函数编码了长时域代价-到-走(cost-to-go)信息,WBC 直接最小化 $Q$ 函数展开,在全身动力学和不等式约束下求解。实验在 MIT Mini Cheetah 上实现了跑步空翻(barrel roll)。

VWBC 的数学形式可以写成:

\[\min_{\ddot{q}, \lambda} \; \frac{1}{2} [\delta x, \delta u]^T \begin{bmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} \quad \text{s.t.} \; \text{动力学 + 约束}\]

其中 $Q_{xx}, Q_{xu}, Q_{uu}$ 是 MPC 反向扫描(backward pass)计算的二次近似矩阵。这些矩阵自动编码了"追踪质心重要还是正则化扭矩重要"的信息,无需人工设定权重。

Herzog A., Rotella N., Mason S., et al. (2016) "Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid" — Autonomous Robots. Momentum-based HQP 变体,通过控制质心动量而非质心位置来实现更鲁棒的人形平衡。
QP 求解器基准(2025) arXiv:2510.21773 "Real-Time QP Solvers: A Concise Review and Practical Guide Towards Legged Robots". 系统比较了 ProxQP、qpOASES、eiquadprog、qpmad、OSQP 在 WBC 和 MPC 场景下的性能。结论:eiquadprog/qpmad 在 WBC 场景最快(sub-ms),ProxQP 最鲁棒。
Whole-Body MPC with MuJoCo (Zhang et al., 2025): 利用 MuJoCo 仿真器的高效并行化和 iLQR 算法,在标准桌面 CPU 上实现了实时全身 MPC。在四足动态行走、四足双腿行走和全尺寸人形双足行走上进行了硬件验证。这项工作表明,随着算力提升,全身 MPC 正在从"理论可行"走向"工程可用",传统 MPC+WBC 分层架构的必要性正在被重新审视。
Whole-Body MPPI (Rapuano et al., 2024): 首次成功将基于采样的全身 MPC(MPPI)部署到真实四足机器人上。与基于梯度的 iLQR/DDP 不同,MPPI 通过大量 GPU 并行采样搜索最优控制序列,对非凸代价函数和接触不连续性天然鲁棒。但其计算量大,目前仅在四足上验证,人形场景的实时性仍是挑战。
Benchmarking QP Formulations (Stark et al., 2025): arXiv:2502.01329。系统比较了不同 QP 公式化方案（稠密/稀疏）和多种求解器在四足动态行走中的计算效率。引入了 SFPW（Solve Frequency per Watt）指标,为嵌入式场景下的求解器选型提供了定量依据。结论:eiquadprog 在 WBC 场景全面领先;HPIPM 在 MPC 场景利用稀疏结构最优。

53.11.3 学习型 WBC——RL 与经典控制的融合 ⭐⭐⭐⭐¶

学习型 WBC 是当前最活跃的研究方向之一,核心问题是:能否用数据驱动方法替代或增强传统 WBC 中的手工设计?

Deep Whole-Body Control (Fu, Cheng & Pathak, CoRL 2022):提出了统一的策略网络,同时控制四足行走和机械臂操作。核心创新包括 Regularized Online Adaptation(在线自适应弥合 sim-to-real gap）和 Advantage Mixing（利用动作空间的因果依赖克服局部极小）。这项工作证明了 RL 可以学到超越手工 WBC 的控制策略。

LeVERB (Xue et al., 2025): 首个面向人形机器人的视觉-语言全身控制框架。高层的视觉-语言策略学习潜在动作词汇,低层的 RL 策略消费这些潜在指令执行全身控制。在仿真中实现了 80% 的简单视觉导航成功率和 58.5% 的总体成功率,比朴素的层级化全身 VLA 实现高出 7.8 倍。

WholeBodyVLA (OpenDriveLab, ICLR 2026): 统一的潜在 VLA（Vision-Language-Action）框架,用于人形全身行走-操作控制。将视觉-语言理解与全身运动控制统一到单一的潜在空间中,通过合成数据训练实现 sim-to-real 迁移。

RL 替代 WBC 的优劣分析:

维度	传统 WBC	RL 策略
物理可解释性	高(基于动力学方程)	低(黑箱)
安全保证	约束硬编码	无显式保证(需 reward shaping)
泛化能力	取决于模型精度	取决于训练分布
调参工作量	高(权重/增益手调)	低(reward 设计后自动优化)
极端性能	受限于模型简化	可能超越模型极限(如空翻)
计算要求(推理)	CPU 1 核	GPU 或 NPU

本质洞察:RL 和 WBC 并非二选一的对立关系,而是在不同层次上互补。当前最务实的混合方案是:RL 学习高层策略（步态选择、落脚点规划）,WBC 执行底层控制（满足物理约束、生成扭矩）。这种分工让 RL 负责"做什么"（利用其泛化能力和自适应能力）,WBC 负责"怎么做"（利用其物理可解释性和安全保证）。

53.11.4 ProxNLP 与 Aligator 在 WBC 中的应用 ⭐⭐⭐⭐¶

背景：ProxNLP 是 Pinocchio 3.x 生态（由 LAAS-CNRS / INRIA 的 Jallet, Bambade, Mansard 等人开发）中的新一代约束求解框架。它将 ProxQP 的近端增广拉格朗日思想推广到非线性约束场景，核心思想是：将 NLP 转化为一系列 proximal 子问题，每个子问题是一个更容易求解的 QP。

Aligator 是基于 Pinocchio 3.x 的下一代轨迹优化库。其核心算法 ProxDDP 将 DDP（微分动态规划）与近端增广拉格朗日方法结合,能够处理带不等式约束的轨迹优化问题。ProxDDP 算法论文于 2025 年 3 月发表在 IEEE Transactions on Robotics (vol. 41, pp. 2605-2624)。

为什么 WBC 需要 ProxNLP？ 传统 WBC 将所有约束线性化后求 QP。但某些约束天然是非线性的： - 流形约束：浮动基座的姿态在 $SO(3)$ 上，不是欧几里得空间。传统做法是"假装"姿态角是欧几里得变量，这在大角度时引入不可忽略的误差。 - 非线性摩擦锥：线性化摩擦锥（多面体近似）引入保守性。ProxNLP 可以直接处理二阶锥约束 $\|\mathbf{f}_t\| \leq \mu f_n$。 - 自碰撞约束：$\phi_{\text{self}}(q) \geq d_{\min}$ 是关节角度的非线性函数。

ProxNLP 的 WBC 集成架构：

传统 WBC 流程:
  动力学 + 约束 → 线性化 → QP → 求解 → τ

ProxNLP WBC 流程:
  动力学 + 约束 → NLP (含流形约束) → ProxNLP 求解 → τ
  内部: NLP → proximal 子问题(QP) → ProxQP 求解 → 更新乘子 → 迭代

TSID 2.0 (Pinocchio 3.x) API 更新：Pinocchio 3.x 引入了统一的 proximal 求解框架，TSID 2.0 的主要变化：

特性	TSID 1.x (Pinocchio 2.x)	TSID 2.0 (Pinocchio 3.x)
约束求解	eiquadprog / ProxQP (QP)	ProxNLP (NLP)
流形	欧几里得近似	原生 $SE(3)$ / $SO(3)$ 流形
摩擦锥	线性化多面体	直接二阶锥
自动微分	手写导数	CppAD / 符号微分可选
Python API	`tsid.TaskSE3Equality`	`aligator.StageModel` 统一接口
并行	无	OpenMP parallel rollout

实践建议：对于 2026 年的新项目，推荐直接使用 Pinocchio 3.x + Aligator 的 StageModel 接口。如果你的 WBC 需要处理非线性约束（如人形机器人的自碰撞），ProxNLP 是目前唯一在 Pinocchio 生态中提供原生支持的方案。对于纯 QP 场景（四足 trot/walk），传统 TSID 1.x + ProxQP 仍然足够且更成熟。

53.11.5 Contact-Implicit WBC 与 MIQP ⭐⭐⭐⭐¶

传统 WBC 假设接触模式（哪些脚着地、哪些腾空）由上层 MPC 或步态调度器预先决定。Contact-Implicit WBC 则将接触模式也纳入优化变量,让 WBC 自主决定"何时何地接触"。

数学形式:引入二值变量 $z_i \in \{0, 1\}$ 表示第 $i$ 个接触点是否激活:

\[z_i = 0: \quad \lambda_i = 0 \text{ (无接触力)}$$ $$z_i = 1: \quad J_i \ddot{q} = -\dot{J}_i \dot{q} \text{ (接触约束激活)}\]

这将原来的 QP 变成了混合整数二次规划（MIQP）。MIQP 的求解时间在最坏情况下是 $O(2^{n_c})$（需要枚举所有接触组合）,对 4 个接触点需要最多 $2^4 = 16$ 次 QP——虽然比单次 QP 慢,但在现代硬件上仍可能实时。

2025 年的研究进展表明,通过定制求解器(如基于信赖域/dogleg 的方法)和并行化 MIQP 求解,Contact-Implicit MPC 已经可以在 24 自由度人形机器人上以 50 Hz 运行(Esteban et al., 2025)。但在 1 kHz WBC 频率下直接使用 MIQP 目前仍不实际,更可行的方案是:MPC 层使用 Contact-Implicit 优化来确定接触模式,WBC 层接收确定的接触模式后用标准 QP 执行。

53.11.6 开放研究问题¶

学习 WBC:能否用强化学习自动调 WBC 权重?最新工作通过 RL 策略网络预测最优权重组合,根据地形和步态自适应调整。
Contact-Implicit WBC:当前 WBC 假设接触模式由 MPC 预先决定。能否让接触模式也成为 WBC 的决策变量?这会将 QP 变成混合整数规划(MIQP),计算量大增,但能处理意外接触。
GPU WBC:单个 WBC 的 QP 只有 30-80 维,GPU 加速收益不大。但并行环境训练(1000+ 个机器人同时仿真)需要批量 WBC——这是 RL sim-to-real 训练的瓶颈之一。
多模态感知驱动的自适应 WBC:结合视觉、触觉和力觉感知,在线估计地形属性(摩擦系数、刚度)并动态调整 WBC 约束参数。
WBC 的形式化安全保证:结合控制障碍函数(CBF)和 WBC,在约束层面提供可证明的安全保证。

本章常见误解汇总¶

误解	正确理解
"WBC 就是逆动力学"	WBC = 逆动力学 + 多任务优化 + 约束满足。逆动力学只是计算工具
"权重大就能保证优先级"	权重比过大会导致 Hessian 条件数退化,数值解不可信。只有 HQP 能提供数学保证
"HQP 一定比加权 QP 好"	HQP 计算量更大($p$ 次 QP),对实时性要求极高的场景可能不如加权 QP 实用
"TSID 比 legged_wbc 好"	TSID 更通用,legged_wbc 更简洁。选择取决于需求:教学/四足用后者,产品/人形用前者
"QP 约束越多越慢"	Active-set 方法的计算量取决于活跃约束数,大部分不等式约束在最优解处非活跃
"OSQP 是最好的 QP 求解器"	OSQP 是一阶方法,适合大规模稀疏 QP(如 MPC)。WBC 的小密集 QP 用 eiquadprog 更快
"WBC 频率越高越好"	频率受传感器带宽和通信延迟限制,超过 2 kHz 通常收益递减
"RL 会完全替代 WBC"	RL 和 WBC 互补:RL 擅长高层策略,WBC 提供物理保证和可解释性

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
QP 求解返回 NaN	约束矛盾(摩擦锥+力矩限同时激活)	1. 检查约束松弛项是否启用 2. 打印 KKT 矩阵条件数 3. 检查接触状态是否与实际一致	足式/80
扭矩跳变/振荡	优先级切换时零空间投影不连续	1. 检查任务激活/去激活的平滑过渡 2. 对比 HQP vs 加权 QP 3. 降低 $K_p$	足式/90 53.3, 53.9
接触力分配不均	摩擦锥权重设置不当	1. 可视化四足各脚接触力 2. 检查 $\mu$ 值与实际地面匹配 3. 加力正则化项	足式/80
浮动基座加速度异常	动力学一致性约束松弛过大	1. 检查基座动力学等式约束的残差 2. 对比期望 vs 实际基座加速度 3. 检查 Pinocchio 数据更新	足式/50
TSID 实时超限	任务数过多或 QP 维度过大	1. 用 `std::chrono` profiling QP 求解时间 2. 减少低优先级任务 3. 考虑 eiquadprog 替换 OSQP	足式/60, 53.5
`EIGEN_RUNTIME_NO_MALLOC` assert 失败	实时路径上存在堆分配	1. GDB backtrace 定位触发行 2. 检查 `noalias()` 使用 3. 预分配所有矩阵	53.6

53.12 本章小结 ⭐¶

核心概念回顾¶

概念	一句话总结
WBC	把 MPC 的宏观参考翻译为关节扭矩的 QP 优化器
HQP	按优先级分层求解,数学保证低层不破坏高层
零空间投影	$N = I - A^+A$,在不影响高优先级任务的子空间中优化
AccForce 消元	用 $(\ddot{q}, \lambda)$ 代替 $(\tau, \ddot{q}, \lambda)$,减少 QP 维度
EIGEN_RUNTIME_NO_MALLOC	实时循环中禁止堆分配,开发期 assert 捕获违规
Triple buffer	无锁线程间数据交换,MPC/WBC 异步运行
VWBC	用 MPC 的 Q 函数代替手调 WBC 权重
Contact-Implicit	将接触模式纳入优化变量,代价是 QP 变为 MIQP
Whole-Body MPPI	基于采样的全身 MPC,GPU 并行友好

关键公式速查¶

公式	编号	用途
$M\ddot{q} + h = S^T\tau + J_c^T\lambda$	(53.1)	全身动力学
$J_c\ddot{q} = -\dot{J}_c\dot{q}$	(53.5)	接触不滑约束
$N_1 = I - A_1^+A_1$	(53.11)	零空间投影
$\tau = S(M\ddot{q} + h - J_c^T\lambda)$	(53.14)	扭矩反推(消元)
$H = \sum_k w_k J_k^T J_k$	(53.21)	QP Hessian 构造

符号表¶

符号	含义	首次出现
$M(q)$	广义质量矩阵(对称正定)	53.2.1
$h(q, \dot{q})$	科氏力 + 重力项	53.2.1
$S$	选择矩阵,$[0 \;\; I_{n_a}]$	53.2.1
$\tau$	关节扭矩	53.2.1
$J_c$	接触 Jacobian	53.2.1
$\lambda_c$	接触力	53.2.1
$n_v$	广义速度维度	53.2.1
$n_a$	驱动关节数	53.2.1
$n_c$	接触点数	53.2.1
$N_k$	第 $k$ 层零空间投影矩阵	53.3.3
$A^+$	Moore-Penrose 伪逆	53.3.3
$D$	摩擦锥线性近似矩阵	53.2.3
$\mu$	摩擦系数	53.2.3
$K_p, K_d$	PD 增益(比例/微分)	53.9.1
$\kappa(H)$	Hessian 矩阵条件数	53.3.1
$w_k$	第 $k$ 个任务的权重	53.2.4

定理速查表¶

定理/公式	一句话说明	对应节
零空间投影保护定理	$A_1(x_1^* + N_1 \delta) = A_1 x_1^*$:低优先级修正不影响高优先级	53.3.3
伪逆最小范数性	$A^+ b$ 是满足 $Ax=b$ 的唯一最小范数解	53.3.3b
AccForce 消元	动力学方程的后 $n_a$ 行可消去 $\tau$,QP 维度减少 $n_a$	53.5.1
临界阻尼条件	$K_d = 2\sqrt{K_p}$ 时二阶系统无振荡、最快收敛	53.9.1

跨章综合练习 ⭐⭐⭐¶

综合题：从接触力学约束到 WBC-QP 再到关节力矩的完整链路

本题需要综合足式/80_接触力学与约束优化（摩擦锥）、足式/60_QP_NLP建模（QP 求解）和本章（WBC 公式推导）三章知识。

问题描述：为 Go2 四足机器人在 trot 步态下（左前+右后着地，右前+左后腾空）手动推导并组装完整的 WBC-QP。

决策变量与维度分析：写出决策变量 $x = [\ddot{q}^T, \lambda^T, \tau^T]^T$ 的维度。trot 步态下 $\lambda$ 只包含 2 只着地脚的力（$\lambda \in \mathbb{R}^6$），$\ddot{q} \in \mathbb{R}^{18}$，$\tau \in \mathbb{R}^{12}$，总维度 = ?
等式约束（本章 53.2 + 53.5）：
全身动力学：$M\ddot{q} + h = S^T\tau + J_c^T\lambda$（18 行）
接触点零加速度：$J_c\ddot{q} + \dot{J}_c\dot{q} = 0$（6 行，2 脚 x 3D）
堆叠为 $A_{eq} x = b_{eq}$，写出 $A_{eq}$ 的分块形式
不等式约束（足式/80_接触力学与约束优化 52.2）：
$k=8$ 棱线性化摩擦锥（$\mu=0.6$），每只脚 8 行 + 1 行（$\lambda_z \geq 0$），2 只脚共 18 行
关节力矩限位：$\tau_{\min} \leq \tau \leq \tau_{\max}$（24 行）
目标函数（本章 53.2 + 足式/60_QP_NLP建模 50.2）：
Task 1（高优先级）：跟踪 MPC 给定的 CoM 加速度 $\ddot{r}_G^{\text{des}}$
Task 2（低优先级）：跟踪默认关节姿态 $q_{\text{ref}}$
用加权 QP 实现（而非 HQP），写出 $H$ 和 $g$
求解与验证：用 ProxQP 求解，检查约束满足度。将求解的 $\tau^*$ 代入 Pinocchio 的 ABA 正向动力学，验证产生的加速度与 $\ddot{q}^*$ 一致。

累积项目:四足控制器——WBC 扭矩生成模块 ⭐⭐⭐¶

在前几章中,你已经实现了: - 足式/30_Pinocchio深度精读: Pinocchio 模型加载与动力学计算 - 足式/50_空间向量与浮动基座动力学: 接触 Jacobian 计算 - 足式/60_QP_NLP建模: QP 求解器封装 - 足式/80_接触力学与约束优化: 摩擦锥约束构建

本章的累积项目目标:实现完整的 WBC 模块,接收 MPC 参考,输出关节扭矩。

// Target interface for cumulative project
class QuadrupedWBC {
 public:
  QuadrupedWBC(const pinocchio::Model& model, const WBCConfig& config);

  // Main solve function: called at 1 kHz
  // Input: current state (q, v), MPC reference
  // Output: joint torques (12-dim for Go2)
  Eigen::VectorXd solve(
      const Eigen::VectorXd& q,
      const Eigen::VectorXd& v,
      const MPCReference& mpc_ref,
      const ContactSchedule& contacts);

 private:
  // Task formulation (following legged_wbc pattern)
  Task formulateFloatingBaseEomTask();
  Task formulateContactNoSlipTask();
  Task formulateFrictionConeTask();
  Task formulateComTrackingTask();
  Task formulateSwingLegTask();
  Task formulatePostureTask();
  Task formulateTorqueLimitsTask();

  // QP solver (from 足式/60_QP_NLP建模)
  QPSolver solver_;

  // Pre-allocated matrices (EIGEN_RUNTIME_NO_MALLOC safe)
  Eigen::MatrixXd M_, Jc_, H_, A_eq_, A_ineq_;
  Eigen::VectorXd h_, b_eq_, b_ineq_, g_, x_;
};

实现步骤:

Step 1 (1-2 小时):实现 formulateFloatingBaseEomTask() 和 formulateContactNoSlipTask(),用 Pinocchio 计算 $M$, $h$, $J_c$。
Step 2 (1-2 小时):实现 formulateFrictionConeTask() 和 formulateTorqueLimitsTask()(复用足式/80_接触力学与约束优化代码)。
Step 3 (2-3 小时):实现追踪任务(CoM, 足端, 姿态),组装 QP 并求解。
Step 4 (2-3 小时):在 MuJoCo 仿真中测试四足站立平衡。
Step 5 (进阶):添加 triple buffer,与 MPC 模块集成(足式/110_OCS2完整栈与双线程MPC 衔接)。

项目精读点 ⭐⭐¶

项目	文件路径	精读重点	类型
TSID	`include/tsid/tasks/task-base.hpp`	`TaskBase` 抽象基类	代码阅读
TSID	`include/tsid/tasks/task-se3-equality.hpp` + `.cpp`	最常用的末端 Task 实现	代码阅读(核心)
TSID	`include/tsid/tasks/task-com-equality.hpp`	质心追踪 Task	代码阅读
TSID	`include/tsid/tasks/task-joint-posture.hpp`	关节姿态 Task(正则化)	代码阅读
TSID	`formulations/inverse-dynamics-formulation-acc-force.hpp`	HQP 构建核心	代码阅读(核心)
TSID	`include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp`	实时 HQP 求解器	代码阅读
TSID	`include/tsid/solvers/solver-proxqp.hpp`	ProxQP 后端集成	代码阅读
TSID	`CMakeLists.txt`	`EIGEN_RUNTIME_NO_MALLOC` 配置	代码阅读
TSID	`exercizes/ex_4_LIPM_to_TSID.py`	LIPM -> WBC 双足行走	代码实战
legged_control	`legged_wbc/src/WbcBase.cpp`	6 个任务公式化方法	代码阅读(核心)
legged_control	`legged_wbc/src/HierarchicalWbc.cpp`	轻量 HQP 实现	代码阅读(对比)
legged_control	`legged_controllers/src/LeggedController.cpp`	WBC 在控制主循环中的调用	代码阅读
BLF	`src/TSID/`	iCub 生态的 TSID 变体	代码阅读

延伸阅读 ⭐¶

资源	类型	难度	推荐理由
TSID GitHub	代码	⭐⭐	官方仓库,含 Python 教程
legged_control GitHub	代码	⭐⭐	最好的四足 WBC 入门代码
Pinocchio 文档	文档	⭐⭐	WBC 依赖的动力学库
Escande et al. (2014) HQP PDF	论文	⭐⭐⭐	HQP 理论原文
Cafe-MPC (2024) arXiv	论文	⭐⭐⭐	VWBC 方法,消除调参
eigenrealtime GitHub	代码	⭐⭐	Eigen 实时安全的最佳实践
QP Solver Review (2025) arXiv	论文	⭐⭐	QP 求解器基准测试
QP Benchmarking (2025) arXiv	论文	⭐⭐	四足 QP 公式与求解器对比
Whole-Body MPC MuJoCo (2025) arXiv	论文	⭐⭐⭐	全身 MPC 的实时实现
Deep Whole-Body Control (CoRL 2022) PDF	论文	⭐⭐⭐	RL + WBC 统一策略
Aligator GitHub	代码	⭐⭐⭐⭐	下一代轨迹优化库
LeVERB (2025) arXiv	论文	⭐⭐⭐⭐	视觉-语言全身控制

本章与后续章节的关系¶

后续章节	与本章的关系	本章哪个知识点为其铺垫
足式/110_OCS2完整栈与双线程MPC	WBC 与 MPC 的集成实现	53.8 MPC+WBC 集成
足式/120_足式强化学习	RL 替代/增强 WBC	53.11.3 学习型 WBC
足式/240_legged_control精读	legged_wbc 的完整源码分析	53.7 轻量 WBC

知识点总表¶

编号	知识点	核心要点	对应节	难度
1	控制层级	WBC 在 MPC 与关节伺服之间,频率 500-1000 Hz	53.1	⭐
2	WBC QP 公式	全身动力学 + 接触约束 + 摩擦锥 → 标准 QP	53.2	⭐⭐
3	HQP 分层优化	零空间投影保证严格优先级,加权 QP 无此保证	53.3	⭐⭐⭐
4	TSID 架构	Task/Constraint/Solver 三元分离,Strategy 模式	53.4	⭐⭐
5	AccForce 消元	$(\ddot{q}, \lambda)$ 替代 $(\tau, \ddot{q}, \lambda)$,减少 28.6% QP 维度	53.5	⭐⭐⭐
6	实时内存安全	EIGEN_RUNTIME_NO_MALLOC 禁止热路径堆分配	53.6	⭐⭐
7	轻量 WBC	legged_wbc 600 行极简实现,3 层 HQP	53.7	⭐⭐
8	MPC+WBC 集成	Triple buffer 无锁通信,失败恢复策略	53.8	⭐⭐⭐
9	WBC 调参	PD 增益、权重归一化、渐进调参法	53.9	⭐⭐
10	多形态适配	四足/双足/人形的 WBC 差异与挑战	53.10	⭐⭐
11	VWBC	用 MPC Q 函数替代手调权重,消除调参	53.11	⭐⭐⭐
12	学习型 WBC	RL 替代/增强 WBC,Deep WBC/LeVERB	53.11	⭐⭐⭐⭐
13	ProxNLP/Aligator	非线性约束 WBC,Pinocchio 3.x 生态	53.11	⭐⭐⭐⭐

研究实践建议 ⭐¶

给新手的建议:

从 legged_wbc 的 600 行代码开始,不要一上来就读 TSID。理解"任务公式化"的模式比理解框架设计更重要。
在 MuJoCo 中实现一个最简 WBC:只追踪质心高度 + 关节正则化。先让机器人站稳,再加摆动腿追踪。
不要在调参上花太多时间。如果你发现需要反复调权重才能让 WBC 工作,可能是任务设计有问题(例如任务之间物理上矛盾)。

给有经验者的建议:

关注 VWBC (Cafe-MPC):它从根本上解决了 WBC 调参的痛点。如果你的项目中 WBC 权重调参占用了大量时间,建议研究 VWBC 的实现。
考虑 Pinocchio 3.x + Aligator 的迁移。ProxNLP 对流形约束的原生支持在人形机器人场景下有显著优势。
跟踪 Whole-Body MPC 的进展。如果你的硬件算力足够(如配备 Orin 的人形机器人),全身 MPC 可能让你完全省去 WBC 层。

版本信息速查 ⭐¶

工具/库	版本	说明
Pinocchio	2.7.x (稳定) / 3.x (开发中)	WBC 核心动力学库
TSID	1.9.x	基于 Pinocchio 2.x 的 WBC 框架
ProxQP (proxsuite)	0.6.x	鲁棒 QP 求解器
eiquadprog	1.2.x	最快的 active-set QP 求解器
qpOASES	3.2.1	经典 QP 求解器
OSQP	0.6.x	ADMM 一阶 QP 求解器
qpmad	1.1.x	轻量 active-set,嵌入式友好
Aligator	0.6.x	下一代轨迹优化库
MuJoCo	3.x	高效物理仿真器

项	典型量级	加权后
\(\\|\ddot{x}_G\\|^2\)	\((1 \text{ m/s}^2)^2 = 1\)	\(10 \times 1 = 10\)
\(\\|\tau\\|^2\)	\((50 \text{ Nm})^2 = 2500\)	\(1 \times 2500 = 2500\)

方案	决策变量维度	Go2 数值
\((\tau, \ddot{q}, \lambda)\)	\(n_a + n_v + 3n_c\)	\(12+18+12=42\)
\((\ddot{q}, \lambda)\)	\(n_v + 3n_c\)	\(18+12=30\)
节省	\(n_a\)	12 维 (28.6%)

量	维度	说明
关节位置 \(q\)	18 (6+12)	浮动基座 6 + 12 关节
关节速度 \(\dot{q}\)	18
关节扭矩 \(\tau\)	12	只有关节被驱动
接触力 \(\lambda\)	12 (4 足 \(\times\) 3)
单时步决策变量	~60
MPC 预测步数	20-50	预测未来 0.5-1 秒
全时域决策变量	1200-3000	60 \(\times\) 20~50

符号	含义	Go2 数值
\(n_v\)	广义速度维度	18 (6 基座 + 12 关节)
\(n_a\)	驱动关节数	12
\(n_c\)	接触点数	4 (四足着地)
\(3n_c\)	接触力维度	12

矩阵	维度	Go2 数值
决策变量 \(x\)	\(n_v + 3n_c + n_a\)	\(18+12+12=42\)
等式约束(动力学)	\(n_v\)	18
等式约束(接触不滑)	\(3n_c\)	12
不等式约束(摩擦锥)	\(5n_c\)	20
不等式约束(扭矩限制)	\(2n_a\)	24
QP 总规模		42 变量, 30 等式, 44 不等式

优先级	任务	物理含义
P1 (最高)	支撑腿足端静止	触地脚在世界系中保持不动（\(\dot{\mathbf{x}}_{\text{sup}} = \mathbf{0}\)），防止打滑导致摔倒
P2	机身转动控制	控制 Roll/Pitch/Yaw 角跟随期望姿态
P3	机身平动控制	控制质心位置（尤其是高度）跟随期望轨迹
P4 (最低)	摆动腿足端追踪	控制空中腿沿规划轨迹运动至下一个落脚点

约束类型	矩阵维度	行数
基座动力学(等式)	\(6 \times 30\)	6
接触不滑(等式)	\(12 \times 30\)	12
摩擦锥(不等式)	\(20 \times 30\)	20
扭矩限制(不等式)	\(24 \times 30\)	24
总计		18 等式 + 44 不等式

数据	类型	维度	说明
`com_trajectory`	分段多项式	\(3 \times N_{steps}\)	质心位置轨迹
`foot_trajectory`	分段多项式	\(3 \times 4 \times N_{steps}\)	四足位置轨迹
`contact_forces`	离散序列	\(12 \times N_{steps}\)	接触力参考
`contact_schedule`	布尔序列	\(4 \times N_{steps}\)	每足是否着地
`timestamp`	double	1	MPC 解对应的起始时间

现象	可能原因	排查方法
机器人抖动	\(K_p\) 过大 / 控制延迟	降低 \(K_p\),检查通信延迟
机器人"软"	\(K_p\) 过小 / 权重不当	提高追踪任务权重
QP 不可行	约束冲突(如摩擦锥太紧)	放松摩擦系数或检查接触模式
关节扭矩饱和	\(\tau_{max}\) 设置不当	检查电机参数,降低追踪要求
足底打滑	摩擦系数过高 / 力分配不均	降低 \(\mu\),加力正则化

公式	编号	用途
\(M\ddot{q} + h = S^T\tau + J_c^T\lambda\)	(53.1)	全身动力学
\(J_c\ddot{q} = -\dot{J}_c\dot{q}\)	(53.5)	接触不滑约束
\(N_1 = I - A_1^+A_1\)	(53.11)	零空间投影
\(\tau = S(M\ddot{q} + h - J_c^T\lambda)\)	(53.14)	扭矩反推(消元)
\(H = \sum_k w_k J_k^T J_k\)	(53.21)	QP Hessian 构造

符号	含义	首次出现
\(M(q)\)	广义质量矩阵(对称正定)	53.2.1
\(h(q, \dot{q})\)	科氏力 + 重力项	53.2.1
\(S\)	选择矩阵,\([0 \;\; I_{n_a}]\)	53.2.1
\(\tau\)	关节扭矩	53.2.1
\(J_c\)	接触 Jacobian	53.2.1
\(\lambda_c\)	接触力	53.2.1
\(n_v\)	广义速度维度	53.2.1
\(n_a\)	驱动关节数	53.2.1
\(n_c\)	接触点数	53.2.1
\(N_k\)	第 \(k\) 层零空间投影矩阵	53.3.3
\(A^+\)	Moore-Penrose 伪逆	53.3.3
\(D\)	摩擦锥线性近似矩阵	53.2.3
\(\mu\)	摩擦系数	53.2.3
\(K_p, K_d\)	PD 增益(比例/微分)	53.9.1
\(\kappa(H)\)	Hessian 矩阵条件数	53.3.1
\(w_k\)	第 \(k\) 个任务的权重	53.2.4

定理/公式	一句话说明	对应节
零空间投影保护定理	\(A_1(x_1^* + N_1 \delta) = A_1 x_1^*\):低优先级修正不影响高优先级	53.3.3
伪逆最小范数性	\(A^+ b\) 是满足 \(Ax=b\) 的唯一最小范数解	53.3.3b
AccForce 消元	动力学方程的后 \(n_a\) 行可消去 \(\tau\),QP 维度减少 \(n_a\)	53.5.1
临界阻尼条件	\(K_d = 2\sqrt{K_p}\) 时二阶系统无振荡、最快收敛	53.9.1

第 53 章 WBC 分层优化——TSID 精读 + 轻量 WBC 对照¶

前置自测 ⭐¶

本章目标 ⭐¶

本章知识导航 ⭐¶

前置依赖 ⭐¶

如果跳过本章会怎样¶

预计阅读时间 ⭐¶

53.1 控制层级——规划到关节的四级火箭 ⭐¶

53.1.1 为什么需要分层?¶

53.1.2 时间尺度金字塔¶

53.1.3 频率分离原则¶

53.1.4 分层与不分层的最新进展¶

53.2 WBC 的数学形态——带约束的全身逆动力学 ⭐⭐¶

53.2.1 从动力学到 QP:完整推导¶

53.2.2 欠驱动基座的处理¶

53.2.3 约束集合¶

53.2.4 组装完整的 QP¶

53.3 "为什么要分层而不是加权"——HQP 核心洞察 ⭐⭐⭐¶

53.3.1 加权 QP 的根本缺陷¶

53.3.2 HQP 的数学框架¶

53.3.3 零空间投影——HQP 的数学引擎¶

53.3.3b 为什么 Moore-Penrose 伪逆是最优的——拉格朗日乘子法证明¶

53.3.3c 零空间维度递减的定量分析 ⭐⭐⭐¶

53.3.4 级联 QP 实现¶

53.3.5 三种 QP 策略对比¶

53.3.6 四足机器人的经典四任务分层与 NSP 递推公式¶

53.3.7 不同优先级策略在工业界的应用情况 ⭐⭐¶

53.4 TSID 的 Task / Constraint / Solver 三元分离 ⭐⭐¶

53.4.1 设计哲学¶

53.4.2 TaskBase 接口详解¶

53.4.3 主要 Task 类型¶

53.4.4 ContactModel6d vs ContactPoint¶

53.4.5 典型使用代码¶

53.5 InverseDynamicsFormulationAccForce——TSID 的核心公式 ⭐⭐⭐¶

53.5.1 决策变量的选择:为什么不用 tau?¶

53.5.2 完整 QP 矩阵构建¶

53.5.3 QP 求解器选择¶

53.6 EIGEN_RUNTIME_NO_MALLOC——实时控制的硬约束 ⭐⭐¶

53.6.1 为什么 malloc 是实时控制的"死刑"¶

53.6.2 Linux 实时调度基础¶

53.6.3 TSID 的内存安全机制¶

53.6.4 Eigen 何时会偷偷 malloc?¶

53.6.5 pmr 分配器替代方案¶

53.6.6 调试 malloc 违规¶

53.7 legged_control 的轻量 WBC——相对 TSID 的简化 ⭐⭐¶

53.7.1 legged_control 概述¶

53.7.2 核心类结构¶

53.7.3 WbcBase 的 7 个任务公式¶

53.7.4 HierarchicalWbc 的三层结构¶

53.7.5 TSID vs legged_wbc 对比¶

53.8 MPC + WBC 集成——腿足控制的完整图景 ⭐⭐⭐¶

53.8.1 数据流¶

53.8.2 Triple Buffer 机制¶

53.8.3 时间插值算法¶

53.8.4 失败模式与恢复¶

53.8.5 MPC 与 WBC 模型不一致的影响与缓解 ⭐⭐⭐¶

53.9 WBC 调参实用指南 ⭐⭐¶

53.9.1 PD 增益调参¶

53.9.2 权重与优先级配置¶

53.9.3 常见故障诊断¶

53.9.4 接触切换时的平滑过渡 ⭐⭐¶

53.10 WBC 在不同机器人形态的适配 ⭐⭐¶

53.10.1 四足 vs 双足 vs 人形¶

53.10.2 bipedal_locomotion_framework 的 TSID 变体¶

53.10.3 WBC 在工业界主流平台的应用 ⭐⭐¶

53.11 研究前沿与论文阅读（博士预备视角） ⭐⭐⭐¶

53.11.1 经典必读¶

53.11.2 近期重要进展（2024-2026）¶

53.11.3 学习型 WBC——RL 与经典控制的融合 ⭐⭐⭐⭐¶

53.11.4 ProxNLP 与 Aligator 在 WBC 中的应用 ⭐⭐⭐⭐¶

53.11.5 Contact-Implicit WBC 与 MIQP ⭐⭐⭐⭐¶

53.11.6 开放研究问题¶

本章常见误解汇总¶

🔧 故障排查手册¶

53.12 本章小结 ⭐¶

核心概念回顾¶

关键公式速查¶

符号表¶

定理速查表¶

跨章综合练习 ⭐⭐⭐¶

53.5 `InverseDynamicsFormulationAccForce`——TSID 的核心公式 ⭐⭐⭐¶

53.6 `EIGEN_RUNTIME_NO_MALLOC`——实时控制的硬约束 ⭐⭐¶