第72章：浮动基座+臂统一动力学——质心动量矩阵¶
| 元信息 | 值 |
|---|---|
| 难度 | ⭐⭐⭐（核心动力学推导 + 代码实现） |
| 预计时间 | 2 周（30-35 小时） |
| 前置依赖 | 足式/30_Pinocchio深度精读(Pinocchio CRTP), 足式/50_空间向量与浮动基座动力学(空间向量/浮基动力学), 足式/60_QP_NLP建模(WBC/TSID), 足式/110_OCS2完整栈与双线程MPC(RNEA/CRBA/ABA) |
| 下游章节 | 复合/30_多模态MPC(多模态MPC), 复合/40_RL全身控制基础(RL全身控制), 复合/120_底盘臂联合规划(复合应用) |
本章目标¶
学完本章后，你应能把四足、机械臂和浮动基座写进同一个动力学方程，解释质量矩阵分块、CMM、ZMP 偏移和零空间投影如何描述腿臂耦合，并能用 Pinocchio/OCS2 风格的代码提取这些量用于 WBC、MPC 和状态估计。
72.0 前置自测¶
在开始本章前，请独立回答以下五个问题。如果有两个以上答不出，建议先回顾对应章节。
| # | 问题 | 来源 |
|---|---|---|
| 1 | Pinocchio 中 model.nq 与 model.nv 的区别是什么？为什么浮动基座时 nq = nv + 1？ |
足式/30_Pinocchio深度精读 |
| 2 | 写出质量矩阵 \(\mathbf{M}(\mathbf{q})\) 的 \(2\times2\) 分块结构（浮基 vs 关节），并说明对角块和耦合块的物理含义。 | 足式/50_空间向量与浮动基座动力学 |
| 3 | 质心动量矩阵（Centroidal Momentum Matrix, CMM）\(\mathbf{A}_G\) 的定义是什么？它的维度和含义？ | 足式/50_空间向量与浮动基座动力学/足式/60_QP_NLP建模 |
| 4 | WBC 中"任务优先级"的核心思想是什么？说出至少三个优先级层次。 | 足式/60_QP_NLP建模 |
| 5 | 复合/10_复合机器人全景 定义了几种复合机器人？Go2+Z1 的广义坐标维度 \(n_q / n_v\) 分别是多少？ | 复合/10_复合机器人全景 |
自测标准： - 5/5 正确：直接阅读本章 - 3-4/5 正确：边读边查阅前置章节 - 0-2/5 正确：建议先完成 足式/30_Pinocchio深度精读 + 足式/50_空间向量与浮动基座动力学 + 足式/60_QP_NLP建模
72.1 从足式到复合——URDF 维度爆炸 ⭐¶
动机：为什么维度爆炸是核心挑战？¶
在 足式/30_Pinocchio深度精读-足式/110_OCS2完整栈与双线程MPC 中，我们处理的是纯四足机器人：12 个腿关节加 6-DOF 浮动基座，总共 \(n_v = 18\)。质量矩阵 \(\mathbf{M} \in \mathbb{R}^{18 \times 18}\)，CRBA 算法在微秒级完成。
但复合机器人的维度急剧上升。让我们看三个典型平台：
| 平台 | 浮基 | 腿 DOF | 臂 DOF | 腰 DOF | \(n_q\) | \(n_v\) | \(\mathbf{M}\) 维度 |
|---|---|---|---|---|---|---|---|
| Go2（纯四足） | 7/6 | 12 | 0 | 0 | 19 | 18 | \(18\times18\) |
| Go2+Z1 6-DOF | 7/6 | 12 | 6 | 0 | 25 | 24 | \(24\times24\) |
| Go2+Z1 7-DOF | 7/6 | 12 | 7 | 0 | 26 | 25 | \(25\times25\) |
| H1（人形） | 7/6 | 12 | 14 | 3 | 36 | 35 | \(35\times35\) |
| G1（人形） | 7/6 | 12 | 14 | 3 | 36 | 35 | \(35\times35\) |
| CENTAURO（双臂四足） | 7/6 | 24 | 14 | 2 | 47 | 46 | \(46\times46\) |
注意：\(n_q = n_v + 1\) 的 "+1" 来自浮动基座用四元数（4 维）表示方位而速度用角速度（3 维）。这是 Lie 群 \(SE(3)\) 的基本性质。
维度爆炸的计算后果¶
CRBA 算法的复杂度为 \(O(n)\)（链式结构）但质量矩阵求逆为 \(O(n^3)\)。MPC 中每一步需要求解一个 QP，其计算量与 \(n_v^3\) 成正比：
| 平台 | \(n_v\) | \(n_v^3\) | 相对纯四足倍数 | 500Hz WBC 可行？ |
|---|---|---|---|---|
| Go2 | 18 | 5,832 | 1.0x | 轻松 |
| Go2+Z1 | 24 | 13,824 | 2.4x | 可行 |
| H1 | 35 | 42,875 | 7.4x | 紧张 |
| CENTAURO | 46 | 97,336 | 16.7x | 需要简化模型 |
结论：从纯四足到人形，MPC 的单步计算量增长近一个数量级。这正是为什么 centroidal dynamics 简化模型如此重要——将 \(O(n_v^3)\) 降为 \(O(1)\)（质心模型只有 6 维）。
Pinocchio 统一加载¶
不论系统多复杂，Pinocchio 的加载接口始终一致：
import pinocchio as pin
import numpy as np
# ========== 方式1：从 URDF 加载 ==========
# Go2+Z1 的统一 URDF 中，臂通过 fixed joint 连接在 base_link 上
model = pin.buildModelFromUrdf(
"go2_z1.urdf",
pin.JointModelFreeFlyer() # 浮动基座关节类型
)
data = model.createData()
print(f"nq = {model.nq}") # 25：7(浮基) + 12(腿) + 6(臂)
print(f"nv = {model.nv}") # 24：6(浮基) + 12(腿) + 6(臂)
# ========== 方式2：打印关节树（用于理解拓扑） ==========
# Pinocchio 内部的关节树结构：
# universe -> root_joint(FreeFlyer) -> FL_hip -> FL_thigh -> FL_calf
# -> FR_hip -> ...
# -> RL_hip -> ...
# -> RR_hip -> ...
# -> arm_joint1 -> arm_joint2 -> ... -> arm_joint6
for i in range(model.njoints):
parent = model.parents[i]
print(f"Joint {i}: {model.names[i]}, parent={parent}")
质量矩阵的增长可视化¶
import matplotlib.pyplot as plt
# 计算不同构型下的质量矩阵
q = pin.neutral(model)
pin.crba(model, data, q)
M = data.M.copy()
M = (M + M.T) / 2 # CRBA 只计算上三角，手动对称化
# 可视化 M 的稀疏结构
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# 绝对值热力图
im0 = axes[0].imshow(np.log10(np.abs(M) + 1e-10), cmap='hot')
axes[0].set_title("log10(|M(q)|): 质量矩阵结构")
axes[0].set_xlabel("速度维度 (nv=24)")
axes[0].set_ylabel("速度维度 (nv=24)")
# 标注分块边界
for ax in axes:
for pos in [6, 18]: # 浮基|腿 边界, 腿|臂 边界
ax.axhline(pos - 0.5, color='cyan', linewidth=2)
ax.axvline(pos - 0.5, color='cyan', linewidth=2)
ax.set_xticks([3, 12, 21])
ax.set_xticklabels(['浮基', '腿', '臂'])
ax.set_yticks([3, 12, 21])
ax.set_yticklabels(['浮基', '腿', '臂'])
# 耦合项强度
M_coupling = M.copy()
M_coupling[:6, :6] = 0 # 清除基座自耦合
M_coupling[6:18, 6:18] = 0 # 清除腿自耦合
M_coupling[18:, 18:] = 0 # 清除臂自耦合
im1 = axes[1].imshow(np.abs(M_coupling), cmap='Reds')
axes[1].set_title("交叉耦合项 |M_coupling|")
plt.colorbar(im0, ax=axes[0])
plt.colorbar(im1, ax=axes[1])
plt.tight_layout()
plt.savefig("mass_matrix_structure.png", dpi=150)
plt.show()
易错点：CRBA 输出只有上三角
Pinocchio 的
crba()函数为了性能，只填充data.M的上三角部分。如果直接使用data.M做矩阵运算，下三角全是零，会导致错误的求逆或分解结果。必须手动对称化：M = (data.M + data.M.T) / 2或在 C++ 端data.M.triangularView<Eigen::Lower>() = data.M.transpose()。
练习 72.1¶
[A] ⭐ 加载 Go2 的 URDF（12-DOF）和 Go2+Z1 的 URDF（18-DOF 关节），分别计算质量矩阵并打印形状。验证 \(\mathbf{M}\) 的维度符合预期。
[B] ⭐⭐ 将 Go2+Z1 的质量矩阵可视化为热力图，标注 3x3 分块结构的边界。观察哪些耦合块最强、哪些几乎为零。解释原因。
[C] ⭐⭐ 测量 Pinocchio crba() 在不同 \(n_v\) 下的计算时间（Go2 / Go2+Z1 / H1），绘制 \(n_v\) vs 计算时间的散点图，验证是否接近 \(O(n)\)。
72.2 统一动力学方程——三类力完整推导 ⭐⭐⭐¶
动机：为什么需要完整推导？¶
复合/10_复合机器人全景 给出了统一动力学方程的最终形式。但对于 MPC 和 WBC 的设计者来说，理解每一项从何而来是至关重要的——否则在实际调参时，面对振荡、漂移、跌倒等问题将无从下手。
本节从 Lagrange 力学出发，一步步推导出完整的统一动力学方程，并详细分析质量矩阵的 \(3\times3\) 分块结构。
从 Lagrangian 出发¶
第一步：定义广义坐标与广义速度¶
对于 Go2+Z1 系统，广义坐标和广义速度分别为：
其中 \(\mathbf{q}_b \in SE(3)\)（位置 + 四元数），\(\mathbf{v}_b \in \mathbb{R}^6\)（线速度 + 角速度），\(\mathbf{q}_\ell \in \mathbb{R}^{12}\)（四条腿各 3 关节），\(\mathbf{q}_a \in \mathbb{R}^{6}\)（臂 6 关节）。
符号约定：本章使用 \(\mathbf{v}\)（而非 \(\dot{\mathbf{q}}\)）表示广义速度，因为浮动基座的速度 \(\mathbf{v}_b\) 不是 \(\dot{\mathbf{q}}_b\) 的简单时间导数——它们之间通过 \(\dot{\mathbf{q}}_b = \mathbf{E}(\mathbf{q}_b)\mathbf{v}_b\) 联系，其中 \(\mathbf{E}\) 是与四元数表示相关的映射矩阵。这是 Lie 群流形上微分方程的基本特征。
第二步：构造系统 Lagrangian¶
系统的总动能为所有刚体（基座 + 腿连杆 + 臂连杆）的动能之和：
其中 \(\mathbf{M}(\mathbf{q})\) 是联合质量矩阵（joint-space inertia matrix），由所有连杆的惯性参数通过 CRBA 算法递归计算得到。
系统的总势能为：
Lagrangian 为 \(\mathcal{L} = T - V\)。
第三步：Euler-Lagrange 方程¶
对 Lagrangian 应用 Euler-Lagrange 方程。浮动基座严格地生活在 \(SE(3)\) 上，因此不能把姿态当成普通欧氏向量随意相减；工程实现通常在当前姿态的切空间中使用广义速度 \(\mathbf{v}\) 做局部坐标化。这样做的含义是：配置变量 \(\mathbf{q}\) 负责描述全局位姿，速度变量 \(\mathbf{v}\) 负责描述切空间中的 twist，变分推导在这个局部坐标中得到与固定基座操纵器同形的方程：
展开后得到经典的操纵器方程形式：
其中： - \(\mathbf{M}(\mathbf{q})\dot{\mathbf{v}}\)：惯性力项 - \(\mathbf{C}(\mathbf{q}, \mathbf{v})\mathbf{v}\)：科里奥利力 + 离心力项 - \(\mathbf{g}(\mathbf{q})\)：重力项
通常将后两项合并为 \(\mathbf{h}(\mathbf{q}, \mathbf{v}) = \mathbf{C}(\mathbf{q}, \mathbf{v})\mathbf{v} + \mathbf{g}(\mathbf{q})\)，即非线性力项。在 Pinocchio 中，\(\mathbf{h}\) 由 rnea(model, data, q, v, 0) 一次计算得到（RNEA 输入零加速度）。
第四步：广义力的三类分解¶
\(\boldsymbol{\tau}_{\text{gen}}\) 是所有外力在广义坐标上的投影。对于复合机器人，外力来自三个源头：
源头1：关节力矩（Joint Torques）
关节电机只能驱动铰链关节，无法直接驱动浮动基座。用选择矩阵 \(\mathbf{S}\) 描述这一约束：
对于 Go2+Z1：\(\mathbf{S} \in \mathbb{R}^{18 \times 24}\)，前 6 列为零（浮基不可驱动），后 18 列为单位阵。
关节力矩对广义力的贡献为 \(\mathbf{S}^T \boldsymbol{\tau}\)，其中 \(\boldsymbol{\tau} \in \mathbb{R}^{18}\) 是所有可驱动关节的力矩向量。
源头2：足端接触力（Foot Contact Forces）
四足机器人的每只脚可能与地面接触。设第 \(i\) 只脚的接触力为 \(\boldsymbol{\lambda}_i \in \mathbb{R}^3\)（三维空间力），接触雅可比为 \(\mathbf{J}_{c,i} \in \mathbb{R}^{3 \times n_v}\)，则所有接触力对广义力的贡献为：
其中 \(\mathcal{C}_{\text{foot}}\) 是当前接触足集合（随步态相位变化）。
源头3：臂末端力/力矩（End-Effector Wrench）
这是复合机器人相比纯四足的核心新增项。臂末端可能： - 主动施力（推门、搬运物体） - 承受被动外力（碰撞、被抓物体重力）
设末端 wrench（6D 力螺旋）为 \(\mathbf{f}_{\text{ee}} \in \mathbb{R}^6\)，末端雅可比为 \(\mathbf{J}_{\text{ee}} \in \mathbb{R}^{6 \times n_v}\)，则其对广义力的贡献为：
第五步：合并得到统一动力学方程¶
将三类力代入 Euler-Lagrange 方程：
这是本章（及后续 复合/30_多模态MPC, 复合/40_RL全身控制基础）的核心方程。三类力的对比总结：
| 力类型 | 符号 | 维度（投影后） | 物理来源 | 可控性 |
|---|---|---|---|---|
| 关节力矩 | \(\mathbf{S}^T\boldsymbol{\tau}\) | \(n_v\) | 电机输出 | 完全可控（决策变量） |
| 足端接触力 | \(\mathbf{J}_{c,i}^T\boldsymbol{\lambda}_i\) | \(n_v\) | 地面反力 | 间接可控（通过步态规划） |
| 末端 wrench | \(\mathbf{J}_{\text{ee}}^T\mathbf{f}_{\text{ee}}\) | \(n_v\) | 操作力/外力 | 部分可控（力控）或不可控（扰动） |
质量矩阵的 \(3\times3\) 分块结构¶
将 \(\mathbf{M}(\mathbf{q})\) 按浮基(b)、腿(\(\ell\))、臂(a) 三个子空间分块：
各分块的物理含义：
| 分块 | 维度 | 物理含义 | 数值特征 |
|---|---|---|---|
| \(\mathbf{M}_{bb}\) | \(6 \times 6\) | 基座锁关节惯量（locked inertia）：所有关节锁定时基座的等效惯量 | 最大，主导 |
| \(\mathbf{M}_{\ell\ell}\) | \(12 \times 12\) | 腿关节的等效惯量矩阵 | 近对角，4 条腿之间耦合弱 |
| \(\mathbf{M}_{aa}\) | \(6 \times 6\) | 臂关节的等效惯量矩阵 | 构型依赖强 |
| \(\mathbf{M}_{b\ell}\) | \(6 \times 12\) | 基座-腿耦合：腿运动对基座的惯性反力矩 | 中等 |
| \(\mathbf{M}_{ba}\) | \(6 \times 6\) | 基座-臂耦合：臂运动对基座的惯性反力矩（本章重点） | 构型依赖极强 |
| \(\mathbf{M}_{\ell a}\) | \(12 \times 6\) | 腿-臂耦合：臂运动通过基座间接影响腿 | 通常较小 |
关键洞察：\(\mathbf{M}_{ba}\) 是理解"臂扰动基座"的核心。当臂加速度 \(\ddot{\mathbf{q}}_a \neq 0\) 时，统一动力学方程的基座行为：
\[\mathbf{M}_{bb}\dot{\mathbf{v}}_b + \mathbf{M}_{b\ell}\ddot{\mathbf{q}}_\ell + \mathbf{M}_{ba}\ddot{\mathbf{q}}_a + \mathbf{h}_b = \sum_i \mathbf{J}_{c,i,b}^T \boldsymbol{\lambda}_i + \mathbf{J}_{\text{ee},b}^T \mathbf{f}_{\text{ee}}\]其中 \(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\) 项正是臂加速度对基座产生的惯性反力矩。忽略此项就是 复合/10_复合机器人全景 所述的"独立控制"方案失败的根源。
Pinocchio 中提取分块¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
# 计算质量矩阵
pin.crba(model, data, q)
M = data.M.copy()
M = (M + M.T) / 2 # 对称化（CRBA 只输出上三角）
# 分块索引
idx_b = slice(0, 6) # 浮基：前 6 行/列
idx_l = slice(6, 18) # 腿：第 6-17 行/列
idx_a = slice(18, 24) # 臂：第 18-23 行/列
# 提取 3x3 分块
M_bb = M[idx_b, idx_b] # (6, 6) 基座惯量
M_ll = M[idx_l, idx_l] # (12,12) 腿惯量
M_aa = M[idx_a, idx_a] # (6, 6) 臂惯量
M_bl = M[idx_b, idx_l] # (6, 12) 基座-腿耦合
M_ba = M[idx_b, idx_a] # (6, 6) 基座-臂耦合（关键！）
M_la = M[idx_l, idx_a] # (12, 6) 腿-臂耦合
print("=== 质量矩阵分块结构 ===")
print(f"M_bb (基座惯量): Frobenius范数 = {np.linalg.norm(M_bb):.3f}")
print(f"M_ll (腿惯量): Frobenius范数 = {np.linalg.norm(M_ll):.3f}")
print(f"M_aa (臂惯量): Frobenius范数 = {np.linalg.norm(M_aa):.3f}")
print(f"M_bl (基座-腿耦合): Frobenius范数 = {np.linalg.norm(M_bl):.3f}")
print(f"M_ba (基座-臂耦合): Frobenius范数 = {np.linalg.norm(M_ba):.3f}")
print(f"M_la (腿-臂耦合): Frobenius范数 = {np.linalg.norm(M_la):.3f}")
# 计算耦合强度比：衡量耦合项相对主对角块的强度
print(f"\n=== 耦合强度比 ===")
print(f"臂-基座耦合/基座惯量 = {np.linalg.norm(M_ba)/np.linalg.norm(M_bb)*100:.1f}%")
print(f"腿-臂耦合/腿惯量 = {np.linalg.norm(M_la)/np.linalg.norm(M_ll)*100:.1f}%")
三类力的 Pinocchio 计算¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
v = np.zeros(model.nv)
a = np.zeros(model.nv)
# === 非线性力 h(q,v) = C(q,v)v + g(q) ===
# RNEA 在零加速度下等于非线性力
h = pin.rnea(model, data, q, v, a) # h ∈ R^24
print(f"h(q,v) shape: {h.shape}") # 包含重力 + 科里奥利力
# === 选择矩阵 S ===
n_actuated = model.nv - 6 # 18 个可驱动关节
S = np.zeros((n_actuated, model.nv)) # (18, 24)
S[:, 6:] = np.eye(n_actuated)
print(f"S shape: {S.shape}")
# === 接触雅可比 ===
# 假设 FL_foot 是左前脚的 frame
FL_foot_id = model.getFrameId("FL_foot")
pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)
J_FL = pin.getFrameJacobian(
model, data, FL_foot_id, pin.LOCAL_WORLD_ALIGNED
)
print(f"J_FL shape: {J_FL.shape}") # (6, 24)
# 只取线性部分（3D 接触力，忽略力矩）
J_FL_linear = J_FL[:3, :] # (3, 24)
# === 末端雅可比 ===
ee_frame_id = model.getFrameId("gripper_link")
J_ee = pin.getFrameJacobian(
model, data, ee_frame_id, pin.LOCAL_WORLD_ALIGNED
)
print(f"J_ee shape: {J_ee.shape}") # (6, 24)，完整 6D wrench
易错点：雅可比的参考坐标系
Pinocchio 支持三种雅可比参考系：
LOCAL（body-fixed），WORLD（空间固定），LOCAL_WORLD_ALIGNED（位于 body 原点但轴平行于世界系）。WBC/MPC 中通常使用LOCAL_WORLD_ALIGNED，因为它保持了世界系的物理直觉，同时避免了WORLD参考系在远离原点时数值不稳定的问题。选错参考系是最常见的 WBC 调试陷阱之一——症状是末端跟踪看似正确但力方向完全错误。
练习 72.2¶
[A] ⭐⭐ 写出 Go2+Z1 系统中选择矩阵 \(\mathbf{S}\) 的显式形式。验证 \(\mathbf{S}\mathbf{S}^T = \mathbf{I}_{18}\) 且 \(\mathbf{S}^T\mathbf{S}\) 是一个什么矩阵？它的物理含义是什么？
[B] ⭐⭐⭐ 手动推导：将统一方程按浮基/腿/臂分块展开为三个子方程。特别关注浮基方程（第一行）——为什么左侧没有 \(\mathbf{S}^T\boldsymbol{\tau}\) 的贡献？这一性质对 WBC 设计意味着什么？
[C] ⭐⭐⭐ 在 Pinocchio 中，给定 \(\mathbf{q}, \mathbf{v}, \boldsymbol{\tau}\)，使用 ABA（pin.aba()）直接计算 \(\dot{\mathbf{v}}\)。与通过 \(\dot{\mathbf{v}} = \mathbf{M}^{-1}(\boldsymbol{\tau}_{\text{gen}} - \mathbf{h})\) 的结果对比，验证数值一致性并比较计算时间。
72.3 臂反力矩对基座稳定性 ⭐⭐⭐¶
动机：1kg 就能让四足摔倒？¶
Unitree Go2 的总质量约 15 kg，Z1 臂的自重约 4.3 kg（含 gripper）。当臂完全外伸并抓取 1 kg 物体时，臂末端相对基座中心的力臂约 0.5 m。这意味着臂产生的附加力矩为：
而 Go2 的步态稳定裕度（support polygon 内的 ZMP 余量）通常只有几厘米。26 Nm 的力矩足以将 ZMP 推到支撑多边形边缘，导致倾覆*。
这就是为什么"臂反力矩"是复合机器人的第一个也是最重要的稳定性挑战。
ZMP 偏移推导¶
回顾 ZMP 定义¶
ZMP（Zero Moment Point）是地面上使得绕该点的合力矩的水平分量为零的点。对于受重力和接触力作用的刚体系统，ZMP 坐标的实用简化形式为：
这是经典的 LIPM（Linear Inverted Pendulum Model） 近似。它假设所有质量集中在一个质点，且忽略角动量效应。
引入臂外力的完整推导¶
当臂末端施加或承受外力时，系统的角动量平衡方程需要修正。从牛顿-欧拉方程在质心处展开：
取 \(y\) 分量（pitch 方向），并利用 ZMP 定义（绕 ZMP 的水平力矩为零），化简得到：
其中 \(\tau_{\text{arm},y}\) 是臂绕 \(y\) 轴（pitch 方向）对质心产生的力矩。第三项 \(\frac{\tau_{\text{arm},y}}{m_{\text{total}} \cdot g}\) 就是臂引起的 ZMP 偏移量。
推导注记：完整推导需要考虑 \(\dot{\mathbf{L}}_G\)（角动量变化率）的贡献。在准静态近似下（\(\dot{\mathbf{L}}_G \approx 0\)），上式是精确的。动态情况下需加入 \(\frac{\dot{L}_{G,y}}{m g}\) 修正项。
Go2+Z1 数值案例¶
| 参数 | 值 | 来源 |
|---|---|---|
| \(m_{\text{total}}\) | 15 + 4.3 + 1.0 = 20.3 kg | Go2 + Z1 + payload |
| \(z_{\text{CoM}}\) | 0.3 m | 站立高度 |
| \(g\) | 9.81 m/s\(^2\) | |
| \(\tau_{\text{arm},y}\) | 26 N*m | 上面计算 |
| 支撑多边形半长 | ~0.15 m | Go2 前后足距 |
ZMP 偏移量（准静态，\(\ddot{x}_{\text{CoM}} = 0\)）：
结论：ZMP 偏移 13.1 cm，而支撑多边形的半长只有约 15 cm。ZMP 已接近边界，极易倾覆。
不同臂姿态下的完整对照：
| 臂姿态 | \(\tau_{\text{arm},y}\) (N*m) | \(\Delta x_{\text{ZMP}}\) (m) | 占支撑半长比 | 风险等级 |
|---|---|---|---|---|
| 收拢（Home） | 2.1 | 0.011 | 7% | 安全 |
| 半伸展（45度） | 12.5 | 0.063 | 42% | 注意 |
| 全伸展，空载 | 21.1 | 0.106 | 71% | 危险 |
| 全伸展，1 kg | 26.0 | 0.131 | 87% | 临界 |
| 全伸展，2 kg | 30.9 | 0.155 | >100% | 倾覆 |
摩擦锥收紧¶
除了 ZMP 偏移，臂操作还会导致摩擦锥约束收紧。当臂末端施加水平力 \(f_{\text{ee},x}\) 时，足端需要产生额外的水平反力来平衡。牛顿第二定律在水平方向：
足端的可用水平力受摩擦锥约束 \(|\lambda_{i,x}| \leq \mu \lambda_{i,z}\)。当 \(f_{\text{ee},x}\) 消耗了部分摩擦裕度后，等效摩擦系数降为：
| 场景 | \(\mu\) | \(|f_{\text{ee},x}|\) | \(\sum \lambda_{i,z}\) | \(\mu_{\text{eff}}\) | 打滑风险 | |------|-------|---------------------|---------------------|-------------------|---------| | 无臂力 | 0.7 | 0 | 200 N | 0.70 | 极低 | | 推门 10N | 0.7 | 10 | 200 N | 0.65 | 低 | | 推重物 50N | 0.7 | 50 | 200 N | 0.45 | 中 | | 拉拽 80N | 0.7 | 80 | 200 N | 0.30 | 高 |
三种补偿策略¶
| 策略 | 方法 | 响应速度 | 适用场景 | 实现复杂度 |
|---|---|---|---|---|
| 主动补偿（Proactive） | MPC 提前规划质心/步态轨迹以抵消已知臂扰动 | 最快（前馈） | 已知操作任务（搬运） | 高 |
| 反应补偿（Reactive） | WBC 实时检测 ZMP 偏移并调整足端力分配 | 中等（反馈） | 未知外力扰动 | 中 |
| 被动补偿（Passive） | 增大支撑多边形（张开双腿）、降低重心 | 最慢（姿态调整） | 重载静态操作 | 低 |
主动补偿的 Pinocchio 实现¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
def compute_zmp_shift(model, data, q, v, f_ee):
"""
计算臂末端力导致的 ZMP 偏移。
参数:
model, data: Pinocchio 模型和数据
q: 广义坐标 (nq,)
v: 广义速度 (nv,)
f_ee: 末端 wrench [fx, fy, fz, mx, my, mz]，世界系
返回:
delta_zmp: (dx, dy) ZMP 偏移量，单位 m
"""
# 1. 计算质心位置
pin.centerOfMass(model, data, q, v)
com = data.com[0] # 质心世界坐标
m_total = data.mass[0] # 系统总质量
# 2. 获取末端位置
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
ee_id = model.getFrameId("gripper_link")
p_ee = data.oMf[ee_id].translation
# 3. 计算臂力对质心的力矩
r_ee = p_ee - com # 末端相对质心的力臂
tau_arm = np.cross(r_ee, f_ee[:3]) + f_ee[3:] # 力矩 = r x F + M
# 4. ZMP 偏移
g = 9.81
delta_x = tau_arm[1] / (m_total * g) # pitch 力矩 -> x 方向偏移
delta_y = -tau_arm[0] / (m_total * g) # roll 力矩 -> y 方向偏移
return np.array([delta_x, delta_y])
# === 示例：臂末端承受 1kg 物体重力 ===
q = pin.neutral(model)
v = np.zeros(model.nv)
f_ee = np.array([0, 0, -1.0 * 9.81, 0, 0, 0]) # 1kg 物体的重力
delta = compute_zmp_shift(model, data, q, v, f_ee)
print(f"ZMP 偏移: dx={delta[0]*100:.1f} cm, dy={delta[1]*100:.1f} cm")
# === 扫描不同臂构型 ===
shoulder_angles = np.linspace(-np.pi/2, np.pi/2, 50)
zmp_shifts = []
for theta in shoulder_angles:
q_test = pin.neutral(model)
q_test[18] = theta # 修改 shoulder pitch
delta = compute_zmp_shift(model, data, q_test, v, f_ee)
zmp_shifts.append(delta[0])
import matplotlib.pyplot as plt
plt.figure(figsize=(8, 5))
plt.plot(np.degrees(shoulder_angles), np.array(zmp_shifts) * 100)
plt.axhline(15, color='r', linestyle='--', label='支撑边界 (15cm)')
plt.axhline(-15, color='r', linestyle='--')
plt.xlabel("Shoulder pitch (deg)")
plt.ylabel("ZMP x-shift (cm)")
plt.title("臂构型 vs ZMP 偏移")
plt.legend()
plt.grid(True)
plt.show()
"Carry while walking" 问题¶
当机器人需要在行走过程中搬运物体时，问题变得更加复杂：
- 步态相位耦合：摆动腿（swing leg）离地时支撑多边形缩小为三角形，ZMP 裕度进一步降低
- 动态负载波动：行走产生的基座加速度叠加臂负载，使 ZMP 在步态周期内剧烈波动
- 摩擦锥动态变化：摆动相中只有三只脚着地，总法向力降低，摩擦裕度减小
qm_control 的解决方案：
# qm_control 风格的前馈补偿伪代码
def compensated_mpc_step(mpc_solver, arm_state, payload_mass):
"""
在 MPC 代价函数中加入臂负载补偿项。
qm_control 的核心思想：将臂负载视为已知扰动，通过修正
质心参考轨迹来前馈补偿。
"""
# 1. 计算臂负载对质心的力矩
tau_load = compute_arm_torque(arm_state, payload_mass)
# 2. 修正 MPC 的 CoM 参考轨迹——向反方向偏移
com_ref_shift = -tau_load / (total_mass * 9.81)
mpc_solver.update_com_reference(com_ref_shift)
# 3. 收紧 MPC 中的摩擦锥约束
friction_margin = compute_friction_margin(arm_state)
mpc_solver.update_friction_cone(friction_margin)
# 4. 求解修正后的 MPC
return mpc_solver.solve()
易错点：静态 vs 动态 ZMP
上面的 ZMP 偏移公式是准静态近似（忽略了 \(\ddot{x}_{\text{CoM}}\) 项）。在实际行走中，\(\ddot{x}_{\text{CoM}}\) 不为零，且与步态相位强相关。完整的动态 ZMP 需要同时考虑惯性力和角动量变化率 \(\dot{\mathbf{L}}_G\)。这正是下一节 CMM 的用武之地——通过 \(\mathbf{A}_G\) 直接追踪角动量，不需要逐关节计算。
练习 72.3¶
[A] ⭐⭐ 如果 Go2 改用更宽的站姿（支撑多边形半长从 0.15 m 增加到 0.20 m），重新计算各臂姿态下的"占支撑半长比"。这说明什么工程设计原则？
[B] ⭐⭐⭐ 推导包含角动量变化率 \(\dot{\mathbf{L}}_G\) 的完整动态 ZMP 公式。提示：从质心处的角动量平衡 \(\dot{\mathbf{L}}_G = \sum (\mathbf{p}_i - \mathbf{p}_G) \times \boldsymbol{\lambda}_i + m(\mathbf{p}_G - \mathbf{p}_{\text{ZMP}}) \times \mathbf{g}\) 出发。
[C] ⭐⭐⭐ 在 Pinocchio 中实现完整的 compute_zmp_shift 函数，扫描臂关节角并绘制 ZMP 偏移曲线。确定对 ZMP 影响最大的臂关节是哪一个？为什么？
72.4 质心动量矩阵 CMM 复合扩展 ⭐⭐⭐¶
动机：从 ZMP 到质心动量¶
上一节用 ZMP 分析了臂负载对稳定性的影响，但 ZMP 是足端视角——它告诉我们"地面反力够不够"，却没有直接回答"系统的动量如何变化"这个更本质的问题。
质心动量矩阵（Centroidal Momentum Matrix, CMM）提供了一个更本质、更统一的视角：直接从质心动量出发描述整个系统的动力学行为。它将"维度爆炸"的关节空间动力学压缩为 6 维的质心空间，同时保留了每个关节对总动量的贡献信息。
CMM 定义回顾与扩展¶
系统在质心处的空间动量（centroidal momentum）定义为：
其中 \(\mathbf{k}_G \in \mathbb{R}^3\) 是绕质心的角动量，\(\mathbf{l}_G = m_{\text{total}} \dot{\mathbf{p}}_{\text{CoM}} \in \mathbb{R}^3\) 是线动量。
CMM \(\mathbf{A}_G(\mathbf{q})\) 是连接广义速度与质心动量的线性映射：
其中 \(\mathbf{A}_G \in \mathbb{R}^{6 \times n_v}\)。对于 Go2+Z1，\(\mathbf{A}_G \in \mathbb{R}^{6 \times 24}\)。
CMM 的分块结构¶
将 \(\mathbf{A}_G\) 按浮基/腿/臂分块：
| 分块 | 维度 | 物理含义 | 数值特征 |
|---|---|---|---|
| \(\mathbf{A}_{G,b}\) | \(6 \times 6\) | 基座速度对质心动量的贡献 | 总质量主导，近似常数 |
| \(\mathbf{A}_{G,\ell}\) | \(6 \times 12\) | 腿关节速度对质心动量的贡献 | 构型依赖，对称步态时部分抵消 |
| \(\mathbf{A}_{G,a}\) | \(6 \times 6\) | 臂关节速度对质心动量的贡献 | 构型依赖极强（本节重点） |
质心动量分解为三部分贡献：
关键洞察：\(\mathbf{A}_{G,a}\dot{\mathbf{q}}_a\) 表示臂运动对系统总角动量和线动量的贡献。特别是角动量部分 \(\mathbf{A}_{G,a}^{\text{ang}}\dot{\mathbf{q}}_a\) 意味着臂可以像"反作用轮"一样用于调节系统角动量——这对 push recovery（抗推恢复）和空中姿态调整至关重要。
CCRBA 算法推导¶
CCRBA（Composite Rigid Body Algorithm for Centroidal Momentum）是计算 CMM 的核心算法。它由 Orin & Goswami (2008) 提出，是 CRBA 的质心空间对偶。
第一步：单个连杆的动量贡献¶
第 \(i\) 个连杆在质心处的动量贡献为：
其中： - \({}^G\mathbf{X}_i^*\) 是从连杆 \(i\) 坐标系到质心坐标系的力变换（force transform，\(6\times6\)） - \(\mathbf{I}_i\) 是连杆 \(i\) 的空间惯量矩阵（\(6 \times 6\)，包含质量、质心偏移、转动惯量） - \({}^i\mathbf{v}_i\) 是连杆 \(i\) 在其 body frame 中的空间速度
回顾 足式/50_空间向量与浮动基座动力学：力变换 \(\mathbf{X}^*\) 是运动变换 \(\mathbf{X}\) 的逆转置：\(\mathbf{X}^* = \mathbf{X}^{-T}\)。它将 body \(i\) frame 中的力/动量变换到 frame \(G\)（质心）中。
第二步：连杆速度与广义速度的关系¶
每个连杆的空间速度可以通过体雅可比（body Jacobian）用广义速度表示：
其中 \({}^i\mathbf{J}_i \in \mathbb{R}^{6 \times n_v}\) 是连杆 \(i\) 在 body frame 中的雅可比矩阵。
第三步：对所有连杆求和¶
因此 CMM 为：
计算复杂度：直接按上式计算需要 \(O(N_{\text{body}} \times n_v)\) = \(O(n^2)\)。Pinocchio 的 CCRBA 实现使用递归方法（与 CRBA 共享复合惯量的递推过程），将复杂度降为 \(O(n)\)。
与质量矩阵的关系¶
CCRBA 和 CRBA 本质上计算的是同一组复合惯量，区别仅在于最终的汇聚参考点： - CRBA 汇聚到关节空间得到 \(\mathbf{M}(\mathbf{q}) \in \mathbb{R}^{n_v \times n_v}\) - CCRBA 汇聚到质心得到 \(\mathbf{A}_G(\mathbf{q}) \in \mathbb{R}^{6 \times n_v}\)
两者的精确关系为：
即 \(\mathbf{A}_G\) 可以从质量矩阵 \(\mathbf{M}\) 的前 6 行（基座行）通过一个从基座原点到质心的坐标变换得到。这个关系在数值验证时非常有用。
角动量守恒与操作¶
对于系统在质心处的动量，牛顿-欧拉方程给出：
当唯一外力是重力时，\(\dot{\mathbf{k}}_G = 0\)（重力通过质心，不产生力矩），因此角动量守恒：
这对复合机器人的三种运动相位有不同意义：
| 运动相位 | 角动量守恒？ | 臂的角色 | 应用 |
|---|---|---|---|
| 多支撑相（四脚站立） | 否（地面反力提供力矩） | 操作工具 | 正常操作 |
| 单支撑相（一只脚站立） | 近似守恒 | "反作用轮" + 操作 | push recovery |
| 空中相（跳跃/飞行） | 严格守恒 | 纯"反作用轮" | 空中翻转、猫式着陆 |
"免费的反作用轮"：与卫星上用电机驱动的反作用轮不同，机械臂的"反作用轮效应"是免费的——臂在执行操作任务的同时，自然地改变了系统角动量。如果控制器能意识到这一点，就可以同时优化操作精度和姿态稳定性。
Pinocchio CMM 计算¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
v = np.random.randn(model.nv) * 0.1 # 微小随机速度
# ========== 方法1：ccrba 计算 CMM ==========
pin.ccrba(model, data, q, v)
A_G = data.Ag.copy() # CMM, (6, nv)
print(f"A_G shape: {A_G.shape}") # (6, 24)
# ========== 方法2：验证与 M 的关系 ==========
pin.crba(model, data, q)
M = data.M.copy()
M = (M + M.T) / 2
pin.centerOfMass(model, data, q)
com = data.com[0]
# 构造从基座原点到质心的力变换
# X_G0 将基座 frame 的力变换到质心 frame
p_base = np.zeros(3) # 基座原点在世界系（neutral config 时为原点）
r_G0 = com - p_base # 从基座到质心的位移
X_G0_star = np.eye(6)
X_G0_star[:3, 3:] = pin.skew(r_G0) # 力变换的叉积项
A_G_from_M = X_G0_star @ M[:6, :]
print(f"A_G (ccrba): \n{A_G[:3, :6]}")
print(f"A_G (from M): \n{A_G_from_M[:3, :6]}")
print(f"差异范数: {np.linalg.norm(A_G - A_G_from_M):.6e}")
# ========== 分块提取与分析 ==========
A_G_base = A_G[:, :6] # 基座列块 (6, 6)
A_G_legs = A_G[:, 6:18] # 腿列块 (6, 12)
A_G_arm = A_G[:, 18:] # 臂列块 (6, 6)
# 各部分对动量的贡献
h_G = A_G @ v
h_base = A_G_base @ v[:6]
h_legs = A_G_legs @ v[6:18]
h_arm = A_G_arm @ v[18:]
print(f"\n=== 质心动量分解 ===")
print(f"总质心动量: {h_G}")
print(f"基座贡献: {h_base}")
print(f"腿贡献: {h_legs}")
print(f"臂贡献: {h_arm}")
print(f"验证（差值范数）: {np.linalg.norm(h_G - h_base - h_legs - h_arm):.2e}")
# 分析臂列块中 线动量 vs 角动量 的贡献
# 注意排列顺序：Pinocchio 的 data.Ag / data.hg 采用 (线动量, 角动量) 布局，
# 即 [:3] 是线动量、[3:] 是角动量——这与本章数学定义 h_G = (k_G, l_G)（角动量在前，
# 沿用 Featherstone/Orin 约定）相反。用 data.Ag 切片时必须按 Pinocchio 的物理顺序取，不能照搬数学下标。
A_G_arm_linear = A_G_arm[:3, :] # 臂对线动量的贡献 (3, 6)
A_G_arm_angular = A_G_arm[3:, :] # 臂对角动量的贡献 (3, 6)
print(f"\n=== 臂各关节对角动量的贡献能力 ===")
for j in range(A_G_arm.shape[1]):
ang_norm = np.linalg.norm(A_G_arm_angular[:, j])
lin_norm = np.linalg.norm(A_G_arm_linear[:, j])
print(f" 臂关节{j}: 角动量贡献 = {ang_norm:.4f}, 线动量贡献 = {lin_norm:.4f}")
OCS2 中的 CentroidalModelInfo¶
OCS2 框架将质心动力学封装在 CentroidalModelInfo 结构体中。理解其数据布局是使用 OCS2 做复合机器人 MPC 的前提：
// OCS2 CentroidalModelInfo 关键字段（简化）
struct CentroidalModelInfo {
size_t generalizedCoordinatesNum; // nq = 25 (Go2+Z1)
size_t actuatedDofNum; // na = 18 (可驱动关节数)
size_t numThreeDofContacts; // 4 (四个足端，各 3D 接触力)
size_t numSixDofContacts; // 0 或 1 (末端 6D wrench)
// 状态向量布局: x = [h_G(6), q(nq)]
// 输入向量布局: u = [f_contact(3*4), f_ee(6), tau(na)]
size_t stateDim; // 6 + 25 = 31
size_t inputDim; // 12 + 6 + 18 = 36
};
| OCS2 变量 | 对应物理量 | Go2+Z1 值 | Go2 值 | 说明 |
|---|---|---|---|---|
stateDim |
\(6 + n_q\) | 31 | 25 | 质心动量(6) + 广义坐标 |
inputDim |
\(3n_c + 6n_{\text{ee}} + n_a\) | 36 | 30 | +6 来自末端 wrench |
numThreeDofContacts |
足端数 | 4 | 4 | 每个足端 3D 力 |
numSixDofContacts |
末端数 | 1 | 0 | 复合机器人新增 |
易错点：OCS2 的状态定义
OCS2 的 centroidal model 使用 \(\mathbf{x} = (\mathbf{h}_G, \mathbf{q})\) 作为状态，而不是经典的 \((\mathbf{q}, \mathbf{v})\)。好处是质心动量 \(\mathbf{h}_G\) 的时间导数直接由外力给出（牛顿-欧拉方程），无需求解 \(n_v\) 维的关节加速度。动力学约束从 \(n_v\) 维降为 6 维，这正是 centroidal dynamics 的计算优势所在。代价是需要在每步通过 \(\mathbf{v} = \mathbf{A}_G^{\dagger} \mathbf{h}_G\) 恢复广义速度（其中 \(\mathbf{A}_G^{\dagger}\) 是 CMM 的伪逆）。
CMM 的时间导数与动力学约束¶
质心动量的时间导数等于所有外力的总和（质心处的牛顿-欧拉方程）：
在 Pinocchio 中，\(\dot{\mathbf{A}}_G\) 通过 dccrba 计算：
# 计算 CMM 的时间导数
pin.dccrba(model, data, q, v)
dAg = data.dAg # d(A_G)/dt, (6, nv)
# 质心动量变化率（两种等价计算方式）：
# 方式1：从 A_G 和加速度
# dh_G = dAg @ v + A_G @ a
#（需要先求出关节加速度 a = aba(...)）
# 方式2：直接从外力（更常用于 MPC）
# dh_G = [sum(p_i x lambda_i); m*g + sum(lambda_i) + f_ee]
# 不需要关节加速度，这正是 centroidal dynamics 的优势
练习 72.4¶
[A] ⭐⭐ 在 Pinocchio 中计算 Go2+Z1 站立构型的 CMM，提取臂列块 \(\mathbf{A}_{G,a}\)。改变臂第一关节（shoulder pitch）从 \(-\pi/2\) 到 \(\pi/2\)，绘制角动量子块的 Frobenius 范数 \(\|\mathbf{A}_{G,a}^{\text{ang}}\|_F\) 随关节角的变化曲线。解释：为什么臂伸展时角动量贡献能力更强？
[B] ⭐⭐⭐ 验证 CMM 与质量矩阵前 6 行的关系：计算 \({}^G\mathbf{X}_0^* \mathbf{M}_{:6,:}\) 并与 data.Ag 对比。提示：需要构造正确的力变换矩阵。
[C] ⭐⭐⭐ 模拟一个场景：Go2+Z1 站立，臂从收拢位快速摆动到全伸展位（1 秒内完成）。使用 Pinocchio 逐帧（dt=0.01s）计算角动量 \(\mathbf{k}_G\)。对比四脚站立和单脚站立两种情况：角动量的变化模式有何本质不同？用角动量守恒解释。
72.5 零空间投影与冗余度利用 ⭐⭐⭐¶
动机：18+ DOF 的冗余度从何而来？¶
Go2+Z1 系统有 24 个广义速度维度（\(n_v = 24\)）。但控制任务通常只占用有限维度：
| 任务 | 维度 | 说明 |
|---|---|---|
| 末端 6D 位姿跟踪 | 6 | \(\mathbf{J}_{\text{ee}} \in \mathbb{R}^{6 \times 24}\) |
| 四足接触约束（4 足 x 3D） | 12 | 保持脚不滑 |
| 浮基"虚约束" | 6 | 浮基不可驱动，通过接触力间接控制 |
总约束维度 = 6 + 12 = 18（浮基通过接触力实现，不额外占维度）。剩余的 \(24 - 18 = 6\) 维冗余度可自由利用。对于人形（\(n_v = 35\)），冗余度更大。
冗余度是复合机器人相比纯四足的核心优势之一。问题是：如何系统性地利用这些冗余度？
零空间投影原理¶
数学定义¶
给定一个主任务雅可比 \(\mathbf{J}_1 \in \mathbb{R}^{m \times n}\)（\(m < n\)），其零空间投影矩阵定义为：
其中 \(\mathbf{J}_1^{\dagger}\) 是 \(\mathbf{J}_1\) 的伪逆。\(\mathbf{N}_1\) 将任意速度投影到 \(\mathbf{J}_1\) 的零空间——即不影响主任务的速度子空间。
物理直觉¶
想象你在抓住一个门把手（6D 约束），同时还想调整肘部位置。由于系统有冗余度，你可以在保持手不动的前提下移动肘部。\(\mathbf{N}_1\) 精确描述了这种"手不动、肘自由"的运动空间。
层级任务优先级¶
对于复合机器人，典型的任务优先级栈为（从高到低）：
| 优先级 | 任务 | 维度 | 原因 |
|---|---|---|---|
| P0（硬约束） | 接触脚不滑 | 12 | 违反则摔倒 |
| P1 | 基座稳定性（ZMP/CoM） | 3-6 | 倾覆则任务失败 |
| P2 | 末端位姿跟踪 | 6 | 操作任务目标 |
| P3 | 关节居中/限位避免 | \(n_v\) | 防止关节到极限 |
| P4 | 姿态舒适度/可操作度 | \(n_v\) | 优化性能指标 |
任务间通过零空间级联保证优先级：
其中 \(\mathbf{N}_2 = \mathbf{I} - (\mathbf{J}_2\mathbf{N}_1)^{\dagger}(\mathbf{J}_2\mathbf{N}_1)\) 是在 \(\mathbf{J}_1\) 零空间内 \(\mathbf{J}_2\) 的再次投影。
阻尼伪逆与数值稳定性¶
当雅可比接近奇异（行列式趋近零）时，标准伪逆 \(\mathbf{J}^{\dagger} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T)^{-1}\) 会发散。阻尼最小二乘（Damped Least Squares, DLS）引入正则化：
其中 \(\lambda\) 是阻尼因子。工程中通常使用自适应阻尼，根据雅可比的条件数动态调整：
其中 \(\sigma_{\min}\) 是雅可比的最小奇异值，\(\epsilon\) 是奇异性阈值（典型值 0.01-0.05）。
HQP vs 加权 QP¶
两种主流实现方式的对比：
| 特性 | HQP（层级 QP） | 加权 QP |
|---|---|---|
| 优先级保证 | 严格：高优先级任务绝不被低优先级影响 | 近似：通过权重差异体现优先级 |
| 数学形式 | 逐级求解 QP + 零空间投影 | 单一 QP，不同任务用不同权重 |
| 求解时间 | 较慢（多次 QP） | 快（单次 QP） |
| 调参难度 | 低（自然优先级顺序） | 高（权重比需要仔细调） |
| 代表框架 | TSID, Hierarchical WBC | OCS2 WBC, wbc_toolbox |
实践建议：如果任务之间的优先级是绝对的（如"宁可末端跟踪偏差也不能摔倒"），用 HQP。如果任务之间需要柔性折中（如"末端精度和能耗的 trade-off"），用加权 QP。qm_control 使用的是加权 QP 方案。
Pinocchio 零空间投影实现¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)
# === 获取末端雅可比 ===
ee_id = model.getFrameId("gripper_link")
J_ee = pin.getFrameJacobian(
model, data, ee_id, pin.LOCAL_WORLD_ALIGNED
) # (6, 24)
# === 阻尼伪逆 ===
def damped_pinv(J, damping=1e-3):
"""
阻尼最小二乘伪逆。
J: (m, n) 雅可比
返回: (n, m) 阻尼伪逆
"""
m = J.shape[0]
return J.T @ np.linalg.inv(J @ J.T + damping**2 * np.eye(m))
# === 自适应阻尼伪逆 ===
def adaptive_damped_pinv(J, eps=0.05, lambda_max=0.1):
"""
自适应阻尼：根据最小奇异值动态调整阻尼系数。
"""
U, S, Vt = np.linalg.svd(J, full_matrices=False)
sigma_min = S[-1]
if sigma_min > eps:
lam2 = 0.0
else:
lam2 = (1 - (sigma_min / eps)**2) * lambda_max**2
m = J.shape[0]
return J.T @ np.linalg.inv(J @ J.T + lam2 * np.eye(m))
# === 零空间投影 ===
J_ee_pinv = adaptive_damped_pinv(J_ee)
N_ee = np.eye(model.nv) - J_ee_pinv @ J_ee # (24, 24)
print(f"零空间维度: {np.linalg.matrix_rank(N_ee, tol=1e-6)}")
# 期望: 24 - 6 = 18（但实际会略小，取决于构型和数值精度）
# === 层级任务：P1=末端跟踪, P2=关节居中 ===
# 期望末端速度
dx_ee_desired = np.array([0.1, 0, 0, 0, 0, 0]) # x 方向 0.1 m/s
# 关节居中速度（向 neutral 位置回归）
q_neutral = pin.neutral(model)
dq_centering = -0.5 * (q[7:] - q_neutral[7:]) # 浮基外的关节
dq_centering_full = np.zeros(model.nv)
dq_centering_full[6:] = dq_centering
# 层级合成
dq = J_ee_pinv @ dx_ee_desired + N_ee @ dq_centering_full
# 验证：P1 任务完美满足
dx_ee_actual = J_ee @ dq
print(f"末端速度误差: {np.linalg.norm(dx_ee_actual - dx_ee_desired):.6e}")
# P2 任务在零空间内尽力满足
dq_centering_actual = N_ee @ dq_centering_full
print(f"关节居中速度范数: {np.linalg.norm(dq_centering_actual):.4f}")
TSID 连接¶
TSID（Task-Space Inverse Dynamics）是 Pinocchio 生态中标准的层级 WBC 框架。对于复合机器人，TSID 的任务配置为：
# TSID 风格的任务配置（概念代码）
import tsid
# 创建 TSID 求解器
solver = tsid.SolverHQP("solver")
robot = tsid.RobotWrapper(model, data)
# === P0: 接触约束（硬约束） ===
for foot_name in ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]:
contact = tsid.Contact6d(foot_name, robot, foot_name)
contact.setFrictionCoefficient(0.7)
contact.setMinNormalForce(10) # 最小法向力 10N
contact.setMaxNormalForce(500) # 最大法向力 500N
solver.addConstraint(contact)
# === P1: CoM 位置跟踪 ===
com_task = tsid.TaskComEquality("com", robot)
com_task.setKp(100 * np.ones(3))
com_task.setKd(20 * np.ones(3))
solver.addTask(com_task, weight=1000, priority=0)
# === P2: 末端位姿跟踪 ===
ee_task = tsid.TaskSE3Equality("ee", robot, "gripper_link")
ee_task.setKp(200 * np.ones(6))
ee_task.setKd(30 * np.ones(6))
solver.addTask(ee_task, weight=100, priority=1)
# === P3: 关节正则化 ===
posture_task = tsid.TaskJointPosture("posture", robot)
posture_task.setKp(10 * np.ones(model.nv - 6))
posture_task.setKd(5 * np.ones(model.nv - 6))
solver.addTask(posture_task, weight=1, priority=2)
易错点：接触约束的维度
对于 3D 点接触（足端），约束维度为 3（速度为零）。对于 6D 面接触，约束维度为 6（包括不转动）。混淆两者会导致 QP 的约束矩阵维度错误，求解器直接报错或给出荒谬结果。Go2 的足端通常建模为 3D 点接触。
练习 72.5¶
[A] ⭐⭐ 计算 Go2+Z1 在站立构型下末端雅可比的奇异值分布。找到最小奇异值并判断是否接近奇异。改变臂构型，找到使最小奇异值最大的构型（最佳可操作度构型）。
[B] ⭐⭐⭐ 实现完整的两级层级逆运动学：P1 = 末端跟踪圆形轨迹，P2 = 关节居中。绘制末端轨迹跟踪误差和关节偏离中位角的时间曲线。验证 P1 严格满足而 P2 尽力满足。
[C] ⭐⭐⭐ 对比阻尼伪逆的三种策略（固定 \(\lambda\), 自适应 \(\lambda\), SVD 截断）在奇异构型附近的行为。哪种在实际 WBC 中最可靠？
72.6 浮基+臂联合状态估计 ⭐⭐¶
动机：统一模型需要统一估计¶
前面几节假设 \(\mathbf{q}\) 和 \(\mathbf{v}\) 是精确已知的。但实际机器人上，浮动基座的状态（位姿和速度）必须通过传感器融合估计。复合机器人的估计问题比纯四足更复杂，因为臂的运动会影响基座状态。
传感器配置¶
| 传感器 | 测量量 | 频率 | 精度 | 安装位置 |
|---|---|---|---|---|
| IMU | 加速度 + 角速度 | 1 kHz | 角度 drift: ~1 deg/min | 基座（躯干） |
| 腿关节编码器 | 关节角 \(\mathbf{q}_\ell\) | 1 kHz | ~0.01 deg | 各腿关节 |
| 臂关节编码器 | 关节角 \(\mathbf{q}_a\) | 1 kHz | ~0.01 deg | 各臂关节 |
| 足端力传感器 | 接触力 \(\boldsymbol{\lambda}_i\) | 500 Hz | ~1 N | 足端（如有） |
| 臂力/力矩传感器 | \(\mathbf{f}_{\text{ee}}\) | 500 Hz | ~0.5 N | 末端法兰（如有） |
EKF 状态估计框架¶
状态向量¶
对于复合机器人的 EKF，状态向量扩展为：
其中 \(\mathbf{b}_a, \mathbf{b}_\omega\) 是 IMU 偏置，\(\mathbf{p}_{c,i}\) 是足端在世界系中的位置（用于腿式里程计）。
注意：臂关节角 \(\mathbf{q}_a\) 直接由编码器测量，不需要估计。但臂的存在影响估计的观测模型——通过臂的前向运动学可以提供额外的约束（如果末端有已知接触）。
预测模型（Process Model）¶
IMU 驱动的预测方程：
其中 \(\mathbf{a}_m, \boldsymbol{\omega}_m\) 是 IMU 原始测量，\(\text{Exp}\) 是 \(SO(3)\) 上的指数映射。
观测模型（Observation Model）¶
观测1：腿式里程计（Leg Kinematics）
当第 \(i\) 只脚与地面接触时，足端位置可由前向运动学计算：
这提供了 3D 位置约束。同时，足端零速度假设给出速度观测：
观测2：臂末端约束（新增）
当臂末端与已知物体接触时，可以提供额外的位置约束：
这在抓取固定物体（如门把手、桌面）时特别有用，相当于给估计器增加了一个"锚点"。
| 观测类型 | 维度 | 可用条件 | 对估计的贡献 |
|---|---|---|---|
| 足端运动学 | 3/足 x 4 | 脚着地 | 位置 + 速度主要来源 |
| 足端零速度 | 3/足 x \(n_c\) | 脚着地且不滑 | 速度约束 |
| 臂末端锚点 | 3 | 末端已知接触 | 位置额外约束（可选） |
| 高度（地形先验） | 1 | 平坦地面假设 | z 方向校正 |
臂运动对估计的影响¶
纯四足的状态估计相对成熟，但加臂后出现新问题：
- IMU 耦合：臂快速运动产生的惯性力通过基座传导到 IMU，增加加速度噪声
- 质心偏移：臂构型变化导致系统质心位置变化，如果估计器假设固定质心，会产生偏差
- 接触模式复杂化：臂末端的接触/非接触状态切换需要额外的接触检测逻辑
# 臂运动对 IMU 测量的影响估算
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
v = np.zeros(model.nv)
a = np.zeros(model.nv)
# 场景1：臂静止
a_arm_static = np.zeros(6)
a[18:] = a_arm_static
tau_static = pin.rnea(model, data, q, v, a)
imu_force_static = tau_static[:6] # 基座行 = IMU 处的等效力
# 场景2：臂快速加速（关节1加速 10 rad/s^2）
a_arm_fast = np.zeros(6)
a_arm_fast[0] = 10.0 # shoulder pitch 加速
a[18:] = a_arm_fast
tau_fast = pin.rnea(model, data, q, v, a)
imu_force_fast = tau_fast[:6]
# IMU 感受到的额外力
delta_imu = imu_force_fast - imu_force_static
print(f"臂加速导致的 IMU 额外力/力矩:")
print(f" 力: {delta_imu[:3]} N")
print(f" 力矩: {delta_imu[3:]} N*m")
print(f" 等效额外加速度: {delta_imu[:3] / data.mass[0]} m/s^2")
InEKF 扩展¶
Invariant Extended Kalman Filter（InEKF）是一种利用 Lie 群结构的 EKF 变体，具有更好的收敛性和一致性。对于浮基机器人：
- 状态 Lie 群：\(SE_2(3)\)（扩展特殊欧几里得群），包含旋转、位置、速度
- 优势：线性化误差与状态无关（右不变误差），保证估计的全局一致性
- 实现：INEKF-based estimator 是 MIT Cheetah 和 Digit 人形机器人的标准方案
对于复合机器人，InEKF 的状态群扩展为：
易错点：接触检测
腿式里程计的前提是正确判断哪只脚着地。错误的接触检测会注入虚假观测，导致估计发散。对于复合机器人，臂末端的接触检测同样重要——特别是在力控操作时，末端的接触/脱离瞬间必须被准确检测。常用方法包括：力阈值检测、GRF 估计、基于概率的接触状态机。
练习 72.6¶
[A] ⭐⭐ 列出 Go2+Z1 系统中所有可用的传感器及其测量方程。比较腿式里程计和臂末端锚点在 EKF 中的数学形式。
[B] ⭐⭐ 定量分析臂运动对 IMU 测量的影响：在不同臂加速度（0, 5, 10, 20 rad/s^2）下，计算 IMU 感受到的等效额外加速度。这个量级与 IMU 的加速度计噪声（典型值 0.01 m/s^2）相比如何？
[C] ⭐⭐⭐ 设计一个简化的 EKF：状态 = [p, v, R]，预测 = IMU 积分，观测 = 腿运动学。在 Python 中实现并用模拟数据测试。然后增加臂末端锚点观测，验证估计精度的提升。
72.7 项目精读：qm_control 与 OCS2 mobile_manipulator ⭐⭐¶
动机：从理论到工程¶
前面六节建立了复合机器人统一动力学的完整理论框架。本节通过精读两个开源项目，理解这些理论如何落地为可运行的代码。
qm_control 框架概览¶
qm_control 是基于 OCS2 框架的四足+臂（quadruped-manipulator）控制器，支持 Go2+Z1 等平台。
代码结构¶
qm_control/
├── qm_description/ # URDF + 机器人描述
│ ├── urdf/
│ │ ├── go2_description/ # Go2 基座 URDF
│ │ ├── z1_description/ # Z1 臂 URDF
│ │ └── go2_z1.urdf # 拼接后的统一 URDF
│ └── config/
│ └── task.info # OCS2 任务配置文件
├── qm_controllers/ # 控制器实现
│ ├── src/
│ │ ├── MPC/ # MPC 求解器
│ │ ├── WBC/ # WBC（加权 QP）
│ │ └── StateEstimate/ # 状态估计
│ └── include/
├── qm_gazebo/ # Gazebo 仿真
└── qm_interface/ # ROS 接口
URDF 拼接关键细节¶
<!-- go2_z1.urdf 中臂的连接方式 -->
<!-- 臂通过 fixed joint 连接到基座的 trunk_link -->
<joint name="arm_base_joint" type="fixed">
<parent link="trunk_link"/>
<child link="arm_link0"/>
<origin xyz="0.285 0.0 0.05" rpy="0 0 0"/>
<!-- xyz: 臂安装在基座前方 28.5cm, 上方 5cm -->
</joint>
<!-- 关键：这个 fixed joint 不增加 DOF -->
<!-- 但它的位姿偏移对 M_ba 耦合项有重大影响 -->
<!-- 安装位置越偏离质心，臂运动对基座的力矩越大 -->
| qm_control 配置项 | 值 | 对应理论 |
|---|---|---|
model.nq |
25 | 7(浮基) + 12(腿) + 6(臂) |
model.nv |
24 | 6 + 12 + 6 |
| MPC 频率 | 100 Hz | OCS2 SLQ solver |
| WBC 频率 | 500 Hz | 加权 QP |
| MPC 预测时域 | 1.0 s | 步态周期覆盖 |
| 摩擦系数 | 0.7 | 摩擦锥约束 |
MPC 与 WBC 的分工¶
┌──────────────────────┐
参考轨迹 ───────> │ MPC (100 Hz) │
(CoM, 末端位姿) │ OCS2 SLQ solver │
│ centroidal model │
└──────────┬───────────┘
│ 最优接触力 λ*
│ 最优状态轨迹 x*
┌──────────▼───────────┐
传感器反馈 ──────> │ WBC (500 Hz) │
(q, v, 接触状态) │ 加权 QP │
│ full dynamics │
└──────────┬───────────┘
│ 关节力矩 τ
▼
关节电机驱动
MPC 在简化的 centroidal model 上规划最优力/状态轨迹（慢循环），WBC 在全维度动力学模型上跟踪 MPC 轨迹并输出关节力矩（快循环）。
OCS2 mobile_manipulator 模板¶
OCS2 框架提供了 ocs2_mobile_manipulator 作为移动操作的通用模板。它与 qm_control 的关键区别：
| 特性 | qm_control | OCS2 mobile_manipulator |
|---|---|---|
| 底盘类型 | 四足（浮基） | 轮式（SE(2)）或浮基 |
| 动力学模型 | centroidal + full | 运动学或 centroidal |
| 接触处理 | 四足步态切换 | 简化（底盘始终接地） |
| 末端任务 | SE(3) 位姿跟踪 | SE(3) 位姿跟踪 |
| 典型 DOF | 24 | 9-12 |
OCS2 模板扩展步骤¶
从 ocs2_mobile_manipulator 扩展到四足+臂：
// 步骤1：定义 CentroidalModelInfo
CentroidalModelInfo info;
info.generalizedCoordinatesNum = 25; // nq
info.actuatedDofNum = 18; // 12 腿 + 6 臂
info.numThreeDofContacts = 4; // 四个足端
info.numSixDofContacts = 0; // 或 1（如果启用末端力控）
// 步骤2：构建 Pinocchio 接口
PinocchioInterface pinocchioInterface(
buildPinocchioModel("go2_z1.urdf", info)
);
// 步骤3：定义代价函数
// 质心跟踪代价 + 末端跟踪代价 + 力正则化
auto costPtr = std::make_unique<CentroidalModelCost>(
pinocchioInterface, info,
comTrackingWeight, // 质心跟踪权重
eeTrackingWeight, // 末端跟踪权重
forceRegWeight // 力正则化权重
);
// 步骤4：定义约束
// 摩擦锥 + 关节限位 + 接触序列
auto constraintPtr = std::make_unique<FrictionConeConstraint>(
info, frictionCoefficient
);
qm_control 的关键创新¶
- 臂负载前馈：将臂末端力估计作为已知扰动注入 MPC，而非作为未知外力由 WBC 被动补偿
- 步态-操作协调：MPC 的步态周期可根据操作任务动态调整（如搬重物时降低步频、增大支撑）
- 紧凑状态定义：使用 \((\mathbf{h}_G, \mathbf{q})\) 状态而非 \((\mathbf{q}, \mathbf{v})\)，将 MPC 动力学约束从 24 维降为 6 维
练习 72.7¶
[A] ⭐⭐ 克隆 qm_control 仓库，阅读 qm_description/urdf/go2_z1.urdf 的关节拓扑。画出关节树图，标注每个关节的类型和自由度。
[B] ⭐⭐ 阅读 qm_control 的 task.info 配置文件，理解 MPC 代价函数中各项权重的物理含义。特别关注质心跟踪权重和末端跟踪权重的数量级差异——这体现了什么优先级？
[C] ⭐⭐⭐ 比较 qm_control 的 WBC（加权 QP）和 足式/60_QP_NLP建模 TSID 的 HQP 在任务优先级保证上的差异。在 Go2+Z1 上，什么情况下加权 QP 会导致低优先级任务干扰高优先级任务？给出具体数值场景。
72.8 从 复合/20_浮动基座臂统一动力学 到下游章节桥接 ⭐¶
本章在课程体系中的位置¶
复合/20_浮动基座臂统一动力学 建立的统一动力学框架是后续所有复合机器人章节的基石。下面梳理每个下游章节如何使用本章内容：
复合/20_浮动基座臂统一动力学 统一动力学
├── 复合/30_多模态MPC 多模态 MPC
│ ├── 使用：centroidal dynamics + CMM
│ ├── 扩展：步态切换的混合动力学
│ └── 关键接口：CentroidalModelInfo → MPC solver
│
├── 复合/40_RL全身控制基础 RL 全身控制
│ ├── 使用：统一方程作为仿真器核心
│ ├── 扩展：观测空间包含臂状态 + 基座状态
│ └── 关键接口：M(q), h(q,v) → reward shaping
│
├── 复合/120_底盘臂联合规划 移动操作
│ ├── 使用：底盘+臂统一运动学（简化版 72.2）
│ ├── 扩展：导航-操作协调
│ └── 关键接口：J_ee, 零空间投影
│
├── 复合/160_四足臂动力学概览 四足臂
│ ├── 使用：完整的 72.2-72.5（核心用户）
│ ├── 扩展：qm_control 实战
│ └── 关键接口：全部
│
└── 复合/220_经典人形全身控制 人形
├── 使用：CMM 扩展到 30+ DOF
├── 扩展：双臂协调、上半身平衡
└── 关键接口：A_G 分块、零空间层级
关键概念桥接表¶
| 复合/20_浮动基座臂统一动力学 概念 | 复合/30_多模态MPC 使用方式 | 复合/40_RL全身控制基础 使用方式 | 复合/160_四足臂动力学概览 使用方式 |
|---|---|---|---|
| \(\mathbf{M}(\mathbf{q})\) 3x3 分块 | centroidal 近似 | 仿真动力学 | 完整模型 WBC |
| 三类力方程 | MPC 约束 | reward shaping | QP 约束 |
| ZMP 偏移 | CoM 参考修正 | 稳定性奖励 | 前馈补偿 |
| CMM \(\mathbf{A}_G\) | MPC 状态方程 | 角动量观测 | 动量跟踪 |
| 零空间 \(\mathbf{N}\) | — | — | 冗余度优化 |
| 状态估计 | EKF 提供 MPC 初始状态 | 仿真中精确 | EKF 实车 |
学习路径建议¶
根据你的研究方向，选择不同的深度：
| 路径 | 重点章节 | 复合/20_浮动基座臂统一动力学 必读部分 | 可略读部分 |
|---|---|---|---|
| A: 四足+臂控制 | 复合/30_多模态MPC, 复合/160_四足臂动力学概览 | 全部 | 无 |
| B: 人形全身控制 | 复合/30_多模态MPC, 复合/220_经典人形全身控制 | 72.2, 72.4, 72.5 | 72.3(数值部分), 72.7 |
| C: 移动操作 | 复合/120_底盘臂联合规划 | 72.1, 72.2, 72.5 | 72.3, 72.4, 72.6 |
| D: RL 端到端 | 复合/40_RL全身控制基础 | 72.1, 72.2 | 72.5, 72.6, 72.7 |
72.9 前沿：GR00T-WBC 与人形全身控制 ⭐⭐⭐¶
NVIDIA GR00T-WBC (2025-2026)¶
NVIDIA 在 GR00T 项目中提出了面向人形机器人的统一全身控制框架 GR00T-WBC，将本章的理论推向 30+ DOF 的极限。
核心挑战：维度的质变¶
从 Go2+Z1 的 \(n_v = 24\) 到人形的 \(n_v = 30-55\)，不仅是量变，更是质变：
| 挑战 | Go2+Z1 (24 DOF) | 人形 (35+ DOF) | 质变原因 |
|---|---|---|---|
| QP 求解时间 | ~0.5 ms | ~5 ms | \(O(n^3)\) 增长 |
| 任务层级数 | 3-4 | 5-7 | 双臂+头+腰 |
| 接触模式 | \(2^4 = 16\) | \(2^2 \times 3^2 = 36+\) | 手掌多点接触 |
| 自碰撞约束 | ~10 对 | ~50 对 | 双臂交叉操作 |
| CMM 列数 | 24 | 35+ | \(\mathbf{A}_{G}\) 更稀疏 |
GR00T-WBC 的技术方案¶
GR00T-WBC 的核心思路是学习+优化混合：
- 离线学习阶段：用 RL 在 IsaacLab 中训练一个"动作先验"（action prior），输出粗糙的全身动作
- 在线优化阶段：WBC 将 RL 输出作为参考，在全维度动力学约束下精修为物理可行的关节力矩
- 关键创新：RL 负责"策略"（做什么），WBC 负责"物理"（怎么做到且不摔倒）
RL Policy (400 Hz)
┌──────────────────┐
观测 ──────> │ 神经网络推理 │──> 关节位置参考 q_ref
(proprioception)│ (action prior) │
└──────────────────┘
│
WBC Layer (1000 Hz) ▼
┌──────────────────────────────────┐
q_ref ──────>│ 统一动力学方程 (本章 72.2) │
接触状态 ───>│ CMM 质心动量约束 (本章 72.4) │──> τ (关节力矩)
力限制 ─────>│ 零空间优先级 (本章 72.5) │
│ 摩擦锥 + ZMP (本章 72.3) │
└──────────────────────────────────┘
与本章的技术连接¶
| GR00T-WBC 组件 | 对应 复合/20_浮动基座臂统一动力学 知识 | 扩展点 |
|---|---|---|
| 统一动力学模型 | 72.2 三类力方程 | 扩展到 30+ DOF，双臂双末端 |
| 质心动量约束 | 72.4 CMM | \(\mathbf{A}_G \in \mathbb{R}^{6 \times 35+}\)，更多臂列块 |
| 稳定性约束 | 72.3 ZMP/摩擦锥 | 双足支撑，支撑多边形更小 |
| 冗余度利用 | 72.5 零空间 | 更深的层级栈，更多冗余 |
| 状态估计 | 72.6 EKF | 双足的估计更难（接触面积小） |
30+ DOF 系统的计算优化¶
面对人形的高维度，几个关键优化技术：
| 技术 | 原理 | 加速比 | 适用场景 |
|---|---|---|---|
| 稀疏 QP 求解器 | 利用 \(\mathbf{M}\) 和 \(\mathbf{J}\) 的稀疏结构 | 2-5x | HQP |
| GPU 并行 | 批量计算多个构型的 CRBA/RNEA | 10-100x | MPC rollout |
| 近似惯量 | 冻结远端连杆惯量为常数 | 1.5-2x | 实时 WBC |
| 模型降阶 | centroidal dynamics 代替 full dynamics | \(O(n) \to O(1)\) | MPC |
# GPU 加速示例：使用 Pinocchio 3.x 的 CasADi 后端
import pinocchio.casadi as pin_casadi
import casadi
# CasADi 符号变量
q_sym = casadi.SX.sym("q", model.nq)
v_sym = casadi.SX.sym("v", model.nv)
# 符号化 CRBA（可自动微分 + GPU 编译）
import pinocchio.casadi as cpin
cmodel = cpin.Model(model)
cdata = cmodel.createData()
cpin.crba(cmodel, cdata, q_sym)
M_sym = cdata.M # CasADi 符号表达式
# 编译为 GPU 可执行函数
M_func = casadi.Function("M", [q_sym], [M_sym])
# 后续可用于 MPC 的 CasADi NLP 求解器
人形全身控制的开放问题¶
| 问题 | 当前状态 | 关键难点 | 相关章节 |
|---|---|---|---|
| 实时性 | 500 Hz 勉强可行 | QP 维度过大 | 复合/30_多模态MPC |
| 双臂协调 | 学术阶段 | 14+ DOF 臂的零空间 | 复合/250_力敏感人形LocoMani |
| 灵巧手操作 | 早期阶段 | 20+ DOF 手指 | 复合/260_VLA_Foundation_Model |
| 全身力控 | 部分实现 | 力传感器+柔顺控制 | 复合/250_力敏感人形LocoMani |
| 仿真到实物 | 主要瓶颈 | 接触模型差距 | 复合/240_ASAP_SimToReal, 复合/270_SimToReal统一方法论 |
练习 72.9¶
[A] ⭐⭐ 计算 H1 人形（\(n_v = 35\)）的 \(\mathbf{A}_G\) 维度，并按双臂分块。与 Go2+Z1 的 \(\mathbf{A}_G\) 分块结构对比，双臂如何增加角动量控制的自由度？
[B] ⭐⭐⭐ 分析 GR00T-WBC 的 RL+WBC 混合架构：为什么不能纯用 RL？为什么不能纯用 WBC？从实时性、安全性、泛化性三个维度论证混合架构的必要性。
[C] ⭐⭐⭐ 阅读 GR00T 相关论文或技术报告，总结其 WBC 层如何处理双足的 ZMP 约束。与本章 72.3 的四足 ZMP 分析对比，双足的关键差异在哪里？
本章小结¶
核心公式速查¶
| 编号 | 公式 | 名称 | 首次出现 |
|---|---|---|---|
| E1 | \(\mathbf{M}\dot{\mathbf{v}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \sum \mathbf{J}_{c,i}^T\boldsymbol{\lambda}_i + \mathbf{J}_{\text{ee}}^T\mathbf{f}_{\text{ee}}\) | 统一动力学方程 | 72.2 |
| E2 | \(\mathbf{M} = \begin{pmatrix} M_{bb} & M_{b\ell} & M_{ba} \\ \cdot & M_{\ell\ell} & M_{\ell a} \\ \cdot & \cdot & M_{aa} \end{pmatrix}\) | 质量矩阵 3x3 分块 | 72.2 |
| E3 | \(\Delta x_{\text{ZMP}} = \frac{\tau_{\text{arm},y}}{m g}\) | 臂引起的 ZMP 偏移 | 72.3 |
| E4 | \(\mathbf{h}_G = \mathbf{A}_G(\mathbf{q})\mathbf{v}\) | CMM 定义 | 72.4 |
| E5 | \(\mathbf{A}_G = \sum_i {}^G\mathbf{X}_i^* \mathbf{I}_i {}^i\mathbf{J}_i\) | CCRBA 公式 | 72.4 |
| E6 | \(\mathbf{N} = \mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J}\) | 零空间投影 | 72.5 |
知识点掌握矩阵¶
| 知识点 | 理解级别 | 代码级别 | 数学级别 | 检验方法 |
|---|---|---|---|---|
| URDF 加载与维度 | 能说出 nq/nv | Pinocchio 加载 | — | 练习 72.1A |
| 统一方程三类力 | 物理直觉 | RNEA + Jacobian | Lagrange 推导 | 练习 72.2B |
| M(q) 分块结构 | 各块物理含义 | 提取并可视化 | 分块展开 | 练习 72.2A |
| ZMP 偏移 | 工程影响 | compute_zmp_shift | 完整推导 | 练习 72.3B |
| CMM 定义与计算 | h_G = A_G v | ccrba + 分块 | CCRBA 推导 | 练习 72.4B |
| 零空间投影 | 优先级直觉 | 层级 IK | 数值稳定性 | 练习 72.5B |
| 状态估计 | 传感器融合概念 | EKF 框架 | 观测方程 | 练习 72.6C |
| 开源项目 | 架构理解 | 配置文件阅读 | — | 练习 72.7A |
Pinocchio API 速查¶
| 功能 | API | 输入 | 输出 | 复杂度 |
|---|---|---|---|---|
| 加载 URDF | buildModelFromUrdf(path, rootJoint) |
URDF 路径 | Model | — |
| 质量矩阵 | crba(model, data, q) |
q | data.M（上三角） |
\(O(n)\) |
| 非线性力 | rnea(model, data, q, v, 0) |
q, v | \(\mathbf{h}\) | \(O(n)\) |
| 正向动力学 | aba(model, data, q, v, tau) |
q, v, tau | \(\dot{\mathbf{v}}\) | \(O(n)\) |
| CMM | ccrba(model, data, q, v) |
q, v | data.Ag |
\(O(n)\) |
| CMM 导数 | dccrba(model, data, q, v) |
q, v | data.dAg |
\(O(n)\) |
| 质心 | centerOfMass(model, data, q) |
q | data.com[0] |
\(O(n)\) |
| 雅可比 | computeJointJacobians(model, data, q) |
q | 内部缓存 | \(O(n)\) |
| Frame 雅可比 | getFrameJacobian(model, data, fid, ref) |
frame id, 参考系 | \(\mathbf{J}\) | \(O(n)\) |
预计学习时间¶
| 内容 | 时间 | 方法 |
|---|---|---|
| 72.0-72.1 维度与加载 | 3h | 阅读 + Pinocchio 实操 |
| 72.2 统一方程推导 | 8h | 手推 + 代码验证 |
| 72.3 ZMP 偏移 | 5h | 数值计算 + 可视化 |
| 72.4 CMM 推导与计算 | 8h | 手推 CCRBA + 代码 |
| 72.5 零空间 | 5h | 代码实现 + TSID |
| 72.6 状态估计 | 4h | 阅读 + 概念理解 |
| 72.7 项目精读 | 4h | 源码阅读 |
| 72.8-72.9 桥接与前沿 | 3h | 阅读 |
| 总计 | ~40h | 约 2 周 |
延伸阅读与参考文献¶
| 编号 | 论文 | 核心贡献 | 与本章关系 |
|---|---|---|---|
| [1] | Orin & Goswami, 2008, "Centroidal momentum matrix" | CCRBA 算法原始论文 | 72.4 |
| [2] | Khatib, 1987, "Operational space formulation" | 操作空间动力学 | 72.5 |
| [3] | Sentis & Khatib, 2005, "Whole-body control" | 层级零空间投影 | 72.5 |
| [4] | Mistry et al., 2010, "Inverse dynamics with contacts" | 浮基 WBC 里程碑 | 72.2 |
| [5] | Righetti et al., 2011, "Unified view" | 投影消去接触力 | 72.2 |
| [6] | Featherstone, 2008, "Rigid Body Dynamics Algorithms" | 空间向量、CRBA、ABA | 全章 |
| [7] | Carpentier et al., 2019, "Pinocchio" | 高效动力学库 | 全章 |
| [8] | qm_control, 2023, GitHub | 四足+臂 MPC+WBC | 72.7 |
| [9] | OCS2, ETH Zurich, GitHub | 最优控制框架 | 72.4, 72.7 |
| [10] | Bloesch et al., 2013, "State estimation for legged robots" | 腿式里程计 + EKF | 72.6 |
下一章预告：复合/30_多模态MPC 多模态 MPC——将本章的统一动力学嵌入 OCS2 的 SLQ/SQP 求解器，实现行走、操作、过渡等多模态的实时最优控制。核心问题：如何在一个 MPC 中同时处理离散的接触模式切换和连续的动力学优化？## 章末回看：从浮动基座到全身控制
本章的核心不是“会调用 Pinocchio 计算一个质量矩阵”，而是理解为什么浮动基座系统必须用统一动力学来描述。固定基座机械臂的问题可以从关节空间开始；四足臂、轮足臂和人形上肢的问题必须从整机开始，因为基座本身既没有直接电机，又会被接触力、末端力和内部动量共同推动。
本质洞察¶
浮动基座臂的难点来自一个事实：机械臂看似只在操作物体，实际上也在操作自己的基座。臂的每一次加速、制动和接触都会通过质量矩阵的耦合项进入基座方程；腿或轮的每一次接触力调整，也会改变末端任务可用的力和运动范围。
因此，全身控制不是把“腿控制器”和“臂控制器”拼起来，而是在同一个动力学方程里决定三类量：驱动关节力矩、环境接触力、任务末端力。只要这三类量分开设计，系统就容易出现局部合理、全局矛盾的行为。
72.10 重新理解统一动力学方程¶
浮动基座系统常写成：
这条公式的教学价值在于它把“谁能直接施力”讲得非常清楚。
第一项 \(\mathbf{S}^T\boldsymbol{\tau}\) 只作用在驱动关节上。浮动基座的前六维没有电机，所以选择矩阵 \(\mathbf{S}\) 的作用不是形式主义，而是在数学上阻止控制器直接给基座施加虚构力矩。
第二项 \(\mathbf{J}_c^T\boldsymbol{\lambda}\) 来自地面接触。腿足机器人能够移动基座，并不是因为基座被电机驱动，而是因为关节力矩通过腿和地面产生接触力，接触力再对整机施加空间力。
第三项 \(\mathbf{J}_{ee}^T\mathbf{f}_{ee}\) 来自末端操作。抓门把手、推箱子、拉抽屉时，末端力不只是任务输出，也会成为整机动力学的一部分。若控制器只在机械臂子系统里考虑末端力，就会低估它对基座平衡的影响。
72.11 分块质量矩阵的物理含义¶
把速度按基座、腿、臂分块后，质量矩阵可以写成：
对角块描述各子系统自身的惯性；非对角块描述“一个子系统加速时，另一个子系统感受到的惯性反作用”。其中 \(\mathbf{M}_{ba}\) 尤其重要，因为它直接告诉我们机械臂运动如何牵动基座。
如果机械臂贴近身体，\(\mathbf{M}_{ba}\) 往往较小；如果机械臂伸得很远，末端负载又较重，\(\mathbf{M}_{ba}\) 会显著变大。这个变化不是控制器调参能消除的，它来自质量分布和杠杆臂长度。真正的工程处理方式是把臂姿态、支撑裕度和接触力一起纳入优化，而不是期待腿控制器事后补救。
72.12 三个容易混淆的量¶
| 符号 | 表示什么 | 常见误解 | 正确理解 |
|---|---|---|---|
| \(\mathbf{q}\) | 配置变量 | 与速度维度相同 | 四元数会让 \(n_q \neq n_v\) |
| \(\mathbf{v}\) | 广义速度 | 普通关节速度拼接 | 浮动基座速度通常是李代数中的 6D twist |
| \(\boldsymbol{\tau}\) | 驱动关节力矩 | 可以直接控制基座 | 只能通过 \(\mathbf{S}^T\) 进入驱动子空间 |
| \(\boldsymbol{\lambda}\) | 接触力 | 只是外部扰动 | 是腿足系统移动基座的主要通道 |
| \(\mathbf{f}_{ee}\) | 末端任务力 | 只影响物体 | 也会通过雅可比转置反作用到整机 |
⚠️ 易错点：把基座六维当成可控关节¶
很多初学者在写 WBC 或逆动力学 QP 时，会把所有广义加速度都当成可以自由指定的变量，然后用伪逆求出一个看似漂亮的解。这个解在仿真里可能短时间不爆炸，但它隐含了一个错误假设：基座六维可以像电机关节一样直接接受力矩命令。
正确做法是保留欠驱动约束：动力学方程的基座行必须由接触力和末端力平衡，而不是由 \(\boldsymbol{\tau}\) 直接平衡。若基座行残差很大，说明接触力、任务力或加速度目标之间存在冲突，不能简单靠增大关节增益解决。
💡 调试顺序：先看物理量，再看优化器¶
当全身控制出现异常时，不要第一时间怀疑求解器。更稳妥的顺序是：
- 检查 \(n_q\)、\(n_v\)、关节索引和 frame id 是否一致。
- 在静止姿态下验证 \(\mathbf{h}\) 是否约等于重力项。
- 用单位接触力测试 \(\mathbf{J}_c^T\boldsymbol{\lambda}\) 的方向是否符合直觉。
- 用单位末端力测试 \(\mathbf{J}_{ee}^T\mathbf{f}_{ee}\) 是否让对应关节产生合理力矩。
- 再检查 QP 权重、约束松弛和摩擦锥。
这种顺序能避免一个常见误区：把坐标系、符号或索引错误包装成“优化器不稳定”。
故障排查¶
| 现象 | 可能原因 | 检查方法 | 修复方向 |
|---|---|---|---|
| 臂一伸出机身就明显倾斜 | \(\mathbf{M}_{ba}\) 耦合被忽略 | 扫描臂姿态并绘制 \(\|M_{ba}\|_F\) | 在 MPC/WBC 中加入质心和角动量约束 |
| WBC 输出力矩很大但基座仍失稳 | 基座行动力学残差大 | 单独打印基座六维残差 | 放松任务目标或增加接触裕度 |
| 接触力方向反常 | 接触雅可比坐标系错 | 用单位法向力做静态测试 | 统一 WORLD/LOCAL 坐标约定 |
| 末端推力导致脚滑 | 摩擦锥没有覆盖操作反力 | 计算每只脚的切向/法向比值 | 限制末端力或重新分配支撑力 |
| 仿真稳定、真机振荡 | 延迟和执行器带宽未建模 | 记录命令与实际力矩相位差 | 降低带宽并加入延迟补偿 |
练习¶
A 型：给定一个四足臂模型，列出 \(\mathbf{M}_{bb}\)、\(\mathbf{M}_{ba}\)、\(\mathbf{M}_{aa}\) 的维度，并说明每个块在物理上代表什么。
B 型：令机器人静止站立，末端在世界系 \(x\) 方向施加 20 N 推力。推导这个力如何通过 \(\mathbf{J}_{ee}^T\mathbf{f}_{ee}\) 影响驱动关节力矩，并分析为什么支撑脚摩擦锥也会被改变。
C 型：设计一个调试实验：机械臂从身体内侧伸到身体外侧，记录质心、ZMP、\(\|M_{ba}\|_F\) 和基座姿态。要求解释这四个曲线之间的因果关系。
章末连接¶
下一章讨论多模态 MPC。它不是从零开始的新主题，而是把本章的统一动力学放进一个带模式序列的优化问题：不同接触模式决定 \(\mathbf{J}_c\) 和可用摩擦锥，不同操作阶段决定 \(\mathbf{J}_{ee}\) 和任务代价，优化器需要在这些结构变化之间保持动力学一致。
72.13 末端外力为什么不能只看机械臂¶
末端外力在固定基座机械臂里常被看成任务层的输出：给末端一个力，机械臂通过雅可比转置把它映射成关节力矩。这个视角在复合机器人里不够，因为末端力的反作用会进入整机动量平衡。
如果四足臂用末端推墙，墙给末端的反作用力会沿着机械臂传回躯干，再由支撑脚通过地面反作用力平衡。此时问题不是“机械臂关节能否输出这个力”这么简单，还要看支撑脚的摩擦锥、基座姿态、质心投影和关节力矩余量。
一个常用的判断方法是把末端力映射到广义力后按基座、腿、臂分块：
其中 \(\boldsymbol{\tau}_a\) 告诉我们机械臂本身需要多大力矩，\(\boldsymbol{\tau}_{\ell}\) 告诉我们腿部关节受到多大耦合影响，\(\mathbf{w}_b\) 则说明整机基座需要由接触力平衡多少空间力。若只看 \(\boldsymbol{\tau}_a\)，就会漏掉最危险的基座和支撑约束。
⚠️ 易错点：把操作力当成已知扰动而不是约束来源¶
在推门、拉抽屉、拖动物体这类任务中，末端力不是一个可以事后补偿的小扰动，而是任务成功的必要条件。若控制器把它放在扰动通道里，常见结果是末端任务看起来完成了，支撑脚却接近滑动边界，或者基座姿态被慢慢推离稳定区。
更合理的建模方式是把末端操作力同时放进任务代价和全身约束：任务层要求它达到目标范围，动力学层要求支撑接触能够平衡它，安全层要求力矩和摩擦裕度仍然足够。这样做会让优化问题更复杂，但能避免把风险藏在控制器外面。
72.14 读懂一次全身控制日志¶
一次有价值的日志不应该只记录末端误差。对于浮动基座臂，至少要同时记录以下量：
| 日志量 | 解释 | 异常时的含义 |
|---|---|---|
| 基座六维动力学残差 | 欠驱动方程是否被满足 | 接触力或任务力不可行 |
| 每只脚摩擦裕度 | 支撑是否接近滑动 | 操作力过大或支撑分配不合理 |
| 末端广义力分块 | 操作力如何影响整机 | 只看臂力矩会漏掉基座风险 |
| 质心和 ZMP | 稳定裕度 | 臂姿态或负载改变了平衡边界 |
| 力矩占限幅比例 | 执行器余量 | 仿真可行但真机可能饱和 |
这些量放在同一条时间轴上，才能看出因果关系。例如末端力上升后，基座残差先变大，随后某只脚摩擦裕度下降，再出现姿态偏差；这说明问题根源在操作力与支撑分配，而不是姿态控制器本身。
72.15 多末端系统的统一雅可比扩展 ⭐⭐⭐¶
动机：人形双臂比四足臂多了什么¶
前面各节以 Go2+Z1（单臂四足）为主要例子。当系统扩展到人形双臂（如 G1 的左臂和右臂）时，统一动力学框架需要做两处扩展：
- 多末端雅可比：每条臂都有独立的末端雅可比 \(\mathbf{J}_{ee,L}\) 和 \(\mathbf{J}_{ee,R}\)，各自 \(6 \times n_v\) 维。末端力项从单项扩展为求和：
- CMM 臂列块分裂：\(\mathbf{A}_{G,a}\) 需要进一步分为左臂和右臂两个子块 \(\mathbf{A}_{G,aL}\) 和 \(\mathbf{A}_{G,aR}\)，分别表示左右臂对质心动量的贡献。
双臂零空间的层级结构¶
双臂系统的零空间层级比单臂更深：
| 优先级 | 任务 | 维度 | Go2+Z1 | G1 双臂 |
|---|---|---|---|---|
| P0 | 接触不滑 | \(3 \times n_c\) | 12 | 6-12 |
| P1 | 基座稳定 | 3-6 | 3-6 | 3-6 |
| P2a | 左臂末端 | 6 | — | 6 |
| P2b | 右臂末端 | 6 | 6 | 6 |
| P3 | 腰部姿态 | 1-3 | — | 1-3 |
| P4 | 关节居中 | \(n_v\) | \(n_v\) | \(n_v\) |
左右臂末端任务可以设为同优先级（P2a = P2b），也可以设一主一从（P2a > P2b）。同优先级意味着两条臂平等竞争零空间资源——如果零空间不够两条臂同时精确跟踪，两条臂会各退让一些。一主一从意味着主臂优先跟踪，从臂在主臂的零空间内尽力跟踪。
Pinocchio 中的双末端雅可比提取¶
import pinocchio as pin
import numpy as np
model = pin.buildModelFromUrdf("g1.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)
pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)
# 中文注释：提取左右臂末端雅可比
left_ee_id = model.getFrameId("left_gripper_link")
right_ee_id = model.getFrameId("right_gripper_link")
J_left = pin.getFrameJacobian(model, data, left_ee_id, pin.LOCAL_WORLD_ALIGNED)
J_right = pin.getFrameJacobian(model, data, right_ee_id, pin.LOCAL_WORLD_ALIGNED)
print(f"左臂雅可比: {J_left.shape}") # (6, nv)
print(f"右臂雅可比: {J_right.shape}") # (6, nv)
# 中文注释：双臂联合雅可比（用于双臂协作任务）
J_bimanual = np.vstack([J_left, J_right]) # (12, nv)
print(f"双臂联合雅可比: {J_bimanual.shape}")
# 中文注释：零空间——在双臂任务满足后的自由空间
J_bimanual_pinv = np.linalg.pinv(J_bimanual)
N_bimanual = np.eye(model.nv) - J_bimanual_pinv @ J_bimanual
print(f"双臂后零空间秩: {np.linalg.matrix_rank(N_bimanual, tol=1e-6)}")
# 中文注释：期望为 nv - 12（如果双臂雅可比满秩）
双臂协作与多机协作的统一视角¶
双臂协作操作（如搬箱子）和多机协作搬运（280_多机协作LocoMani）在数学上是高度同构的：
| 概念 | 双臂 | 多机 |
|---|---|---|
| 末端数量 | 2（左右手） | \(N\)（\(N\) 台机器人） |
| 抓取矩阵 | \(G \in \mathbb{R}^{6 \times 12}\) | \(G \in \mathbb{R}^{6 \times 3N}\) |
| 内力 | 双手之间的挤压力 | 多机之间的拉扯力 |
| 通信延迟 | 0（同一机器人） | 非零（网络） |
| 约束耦合 | 强（共享动力学） | 弱（各自独立动力学） |
双臂可以看成"零延迟、强耦合的二机协作"。因此 280 章的抓取矩阵、内力分解和虚拟耦合理论可以直接应用于双臂操作——只是物理实现从网络通信变成了肌肉骨骼的内部耦合。
练习 72.15¶
[A] ⭐⭐ 计算 G1 人形在双臂抬起箱子时的联合雅可比秩。如果箱子是刚体（12 个约束：左右手各 6D），联合雅可比是否满秩？冗余度是多少？
[B] ⭐⭐⭐ 推导双臂搬运箱子时的内力分解。使用 \(G\) 矩阵将双手接触力分解为物体运动力和内力。解释为什么适度内力对夹持稳定性是必要的。
[C] ⭐⭐⭐ 在 Pinocchio 中实现一个三级层级 IK：P0 = 双脚不滑，P1 = 左手跟踪圆形轨迹，P2 = 右手跟踪直线轨迹。验证 P1 优先级高于 P2。
72.16 Pinocchio 3.x 与 CasADi 后端的符号计算 ⭐⭐⭐¶
为什么需要符号计算¶
前面各节的代码都是数值计算——给定具体的 \(\mathbf{q}\)，计算具体的 \(\mathbf{M}(\mathbf{q})\)。但 MPC 求解器（如 OCS2 使用的 SLQ/SQP）需要动力学方程对状态和输入的雅可比（灵敏度矩阵）。
数值微分（有限差分）可以近似雅可比，但精度低且计算量大（每个变量需要额外一次前向评估）。Pinocchio 3.x 支持 CasADi 后端，可以直接生成动力学方程的符号表达式，进而自动求导。
CasADi 符号化示例¶
import pinocchio as pin
import pinocchio.casadi as cpin
import casadi
# 中文注释：创建 CasADi 符号模型
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
cmodel = cpin.Model(model)
cdata = cmodel.createData()
# 中文注释：定义符号变量
q_sym = casadi.SX.sym("q", model.nq) # 25 维配置
v_sym = casadi.SX.sym("v", model.nv) # 24 维速度
a_sym = casadi.SX.sym("a", model.nv) # 24 维加速度
# 中文注释：符号化 RNEA——得到广义力的符号表达式
cpin.rnea(cmodel, cdata, q_sym, v_sym, a_sym)
tau_sym = cdata.tau # CasADi 符号表达式
# 中文注释：自动求导——对状态求雅可比
dtau_dq = casadi.jacobian(tau_sym, q_sym) # (nv, nq)
dtau_dv = casadi.jacobian(tau_sym, v_sym) # (nv, nv)
# 中文注释：编译为快速可调用函数
rnea_func = casadi.Function("rnea", [q_sym, v_sym, a_sym], [tau_sym])
jac_func = casadi.Function("jac_rnea", [q_sym, v_sym, a_sym], [dtau_dq, dtau_dv])
# 中文注释：数值验证
q_num = pin.neutral(model)
v_num = np.zeros(model.nv)
a_num = np.zeros(model.nv)
tau_val = rnea_func(q_num, v_num, a_num)
jac_q_val, jac_v_val = jac_func(q_num, v_num, a_num)
print(f"RNEA 符号计算结果: tau shape = {tau_val.shape}")
print(f"雅可比 d(tau)/d(q) shape = {jac_q_val.shape}")
符号计算在 MPC 中的应用¶
OCS2 的 SLQ 求解器需要动力学方程的一阶和二阶导数。传统做法是手动推导或数值差分。CasADi 后端让这些导数自动生成：
| 量 | 用途 | 手动推导 | CasADi 自动 |
|---|---|---|---|
| \(\frac{\partial f}{\partial x}\) | 状态转移雅可比 | 困难（\(n_v > 20\)） | 自动 |
| \(\frac{\partial f}{\partial u}\) | 输入雅可比 | 困难 | 自动 |
| \(\frac{\partial^2 \ell}{\partial x^2}\) | 代价 Hessian | 视代价函数而定 | 自动 |
| \(\frac{\partial g}{\partial x}\) | 约束雅可比 | 中等 | 自动 |
实践建议：CasADi 的代码生成（
casadi.Function.generate）可以把符号表达式编译为纯 C 代码，运行速度接近手写代码。这是将 Pinocchio 符号计算部署到嵌入式系统的推荐方式。
练习 72.16¶
[A] ⭐⭐ 使用 Pinocchio CasADi 后端计算 Go2+Z1 在 neutral 构型下的 RNEA 值，与数值 pin.rnea 对比。验证二者差异 < \(10^{-10}\)。
[B] ⭐⭐⭐ 使用 CasADi 自动微分计算质心位置对广义坐标的雅可比 \(\frac{\partial \mathbf{p}_{com}}{\partial \mathbf{q}}\)。与 Pinocchio 的 data.Jcom（通过 jacobianCenterOfMass）对比验证。
72.17 从教学到实战：统一动力学调试的黄金流程 ⭐⭐¶
调试不是猜测，而是物理验证¶
统一动力学相关的 bug 通常不会导致立即崩溃——它们表现为缓慢的漂移、不自然的力矩分配、或在特定构型下的突然失稳。以下是经过实践验证的调试流程。
黄金调试流程¶
Step 1：静态验证。让机器人在 neutral 构型下完全静止。计算 \(\mathbf{h}(\mathbf{q}, \mathbf{0})\)——这应该等于纯重力项。如果不为零（排除数值误差），说明重力方向或质量参数有错。
Step 2：单力测试。在末端施加单位力（如 \([1, 0, 0, 0, 0, 0]^T\)），计算 \(\mathbf{J}_{ee}^T \mathbf{f}\)。检查每个关节的力矩方向是否符合直觉——例如沿 \(x\) 方向的末端力应该主要影响臂的肩关节和肘关节。
Step 3：质量矩阵对称性。验证 \(\|\mathbf{M} - \mathbf{M}^T\|_F < 10^{-10}\)。如果不满足，说明 CRBA 的上三角化处理有误。
Step 4：能量守恒。在无摩擦、无外力仿真中让系统自由运动。计算动能 \(T = \frac{1}{2}\mathbf{v}^T\mathbf{M}\mathbf{v}\) 和势能 \(V\)，验证 \(T + V\) 恒定。如果能量漂移，说明积分器或动力学方程有数值问题。
Step 5：ZMP 一致性。站立时计算 ZMP 位置，验证它在支撑多边形内部。如果 ZMP 在边界外但机器人没有摔倒，说明接触模型或 ZMP 计算有误。
常见 Bug 分类¶
| Bug 类型 | 典型症状 | 根因 | 检测方法 |
|---|---|---|---|
| 坐标系错误 | 末端力方向反 | WORLD/LOCAL 混用 | 单力测试 |
| 索引错误 | 某条腿力矩异常 | 关节索引偏移 | 逐关节禁用测试 |
| 质量参数错 | 静态力矩偏大/偏小 | URDF 质量不匹配 | 称重验证 |
| CRBA 未对称化 | QP 求解器报错 | 只用上三角 | 对称性检查 |
| 雅可比参考系错 | WBC 跟踪方向反 | 参考系不一致 | 可视化力向量 |
调试原则：永远从最简单的验证开始。不要在复杂的 MPC+WBC+RL 混合系统中调试动力学——先用 Pinocchio 的纯数学接口，在最简单的构型和输入下验证每一项。确认基础物理量正确后，再逐步加入控制层。90% 的统一动力学 bug 可以在 Step 1-3 中发现。
练习 72.17¶
[A] ⭐⭐ 对 Go2+Z1 执行黄金调试流程的 Step 1-3。记录每一步的数值结果。如果某一步发现异常，分析可能的原因。
[B] ⭐⭐⭐ 人为引入一个 bug（如将某个关节的质量设为 0），重新执行调试流程。记录 bug 在哪一步被检测到，以及检测到的具体异常。
全章最终回顾：本章从 URDF 加载开始，经过统一方程推导、ZMP 分析、CMM 计算、零空间投影、状态估计和开源项目精读，最终到达 GR00T-WBC 前沿和多末端扩展。这条线索的核心是一个事实：浮动基座系统的控制不能分开设计。臂运动影响基座，基座运动影响末端，接触力平衡所有。只有在统一动力学方程中同时考虑三类力，才能得到物理一致的全身控制。
如果你只记住一个结论，请记住这个：臂看似只在操作物体，实际上也在操作自己的基座。 这个事实通过 \(\mathbf{M}_{ba}\) 耦合项、ZMP 偏移公式和 CMM 臂列块在数学上表达得非常清楚。它不是一个可以靠调参消除的"二阶效应"，而是浮动基座系统的基本物理特征。
从本章到后续章节的核心接口¶
| 后续章节 | 需要的本章输出 | 接口形式 | 频率 |
|---|---|---|---|
| 复合/30 多模态 MPC | \(\mathbf{A}_G, \dot{\mathbf{h}}_G\) | CentroidalModelInfo | 50-200 Hz |
| 复合/40 RL 全身控制 | \(\mathbf{M}, \mathbf{h}, \mathbf{J}_{ee}\) | 仿真器内部 | 仿真频率 |
| 复合/50 操作技能接口 | \(\mathbf{J}_{ee}\), 零空间 | task-frame 转换 | 50-100 Hz |
| 复合/120 底盘臂联合规划 | 正运动学, \(\mathbf{J}_{ee}\) | IK/WBC 接口 | 10-50 Hz |
| 复合/160 四足臂动力学概览 | 全部（72.2-72.7） | qm_control 框架 | MPC+WBC |
| 复合/220 经典人形全身控制 | CMM 双臂扩展 | TSID/HQP | 500-1000 Hz |
| 复合/230 人形全身 RL | 仿真动力学 + 奖励 shaping | IsaacLab 环境 | 训练时 |
| 复合/280 多机协作 | 抓取矩阵, 内力分解 | 物体层协调 | 10-50 Hz |
这张表说明了本章作为"动力学基石"的角色：后续几乎每个复合章节都直接或间接使用本章建立的统一方程、CMM 分块和零空间投影。理解本章是高效学习后续内容的前提。
最终建议：如果你正在开发一个复合机器人控制系统，请确保团队中至少有一个人完整理解了本章的 72.2（统一方程）、72.4（CMM）和 72.5（零空间）。这三节的知识构成了全身控制的数学语言——团队成员之间用这套语言交流，可以避免 80% 的接口误解和集成问题。
术语对照表： - 浮动基座 (Floating Base)：没有固定连接到地面的机器人基座 - 广义坐标 (Generalized Coordinates)：描述系统配置的最小坐标集 - 广义速度 (Generalized Velocity)：配置流形切空间中的速度向量 - 质心动量矩阵 (Centroidal Momentum Matrix, CMM)：将广义速度映射到质心动量的线性映射 - 零空间投影 (Null-Space Projection)：将次要任务投影到主任务零空间的操作 - 阻尼伪逆 (Damped Pseudo-Inverse)：带正则化的伪逆，避免奇异点附近的数值发散 - 选择矩阵 (Selection Matrix)：区分驱动关节和欠驱动基座的矩阵 - 非线性力项 (Nonlinear Force Term)：包含科氏力、离心力和重力的合项 - 接触雅可比 (Contact Jacobian)：将广义速度映射到接触点速度的矩阵 - 力变换 (Force Transform)：空间力在不同坐标系之间的变换 - 复合刚体算法 (Composite Rigid Body Algorithm, CRBA)：递归计算质量矩阵的 \(O(n)\) 算法 - 递归牛顿欧拉算法 (Recursive Newton-Euler Algorithm, RNEA)：递归计算逆动力学的 \(O(n)\) 算法 - 铰接体算法 (Articulated Body Algorithm, ABA)：递归计算正向动力学的 \(O(n)\) 算法 - 层级二次规划 (Hierarchical QP, HQP)：按优先级逐层求解的 QP - 加权二次规划 (Weighted QP)：用权重近似优先级的单一 QP - 惯性反力矩 (Inertial Reaction Torque)：子系统加速度通过耦合项对其他子系统产生的力矩 - 锁关节惯量 (Locked Inertia)：所有关节固定时基座的等效惯量 - ZMP 偏移 (ZMP Shift)：末端力或臂运动导致的零力矩点位置变化 - 臂可操作度 (Arm Manipulability)：衡量臂在当前构型下运动灵活性的标量指标