第71章：复合机器人全景——三子方向 × 三层级金字塔¶

元信息	值
难度	⭐⭐（全景综述，重理解轻推导）
预计时间	1 周（15-20 小时）
前置依赖	完成腿足足式/30_Pinocchio深度精读-70 或机械臂快速入门 Ch30-46

前置自测¶

在开始本章前，请独立回答以下五个问题。如果有两个以上答不出，建议先回顾对应章节。

**浮动基座（floating base）**是什么？它与固定基座有何本质区别？（足式/30_Pinocchio深度精读）
机械臂动力学与腿足动力学的**最大共性**和**最大差异**分别是什么？（Ch30 vs 足式/50_空间向量与浮动基座动力学）
什么是全身控制（Whole-Body Control, WBC）？它解决的核心矛盾是什么？（足式/90_WBC分层优化与TSID）
MPC 的基本思想是什么？为什么需要"滚动优化"而非一次规划到底？（足式/110_OCS2完整栈与双线程MPC）
说出三种复合机器人的形态（不限于本章将讨论的分类）。

本章目标¶

学完本章，你应能：

画出复合机器人三子方向（D1 轮足 / D2 移动操作 / D3 足式操作）的技术谱系图
写出 Go2+Z1 的**统一广义坐标**向量及其维度
理解"三层级金字塔"（运动学 → 动力学 → 学习）在不同子方向上的具体体现
区分共享技术栈（Pinocchio, OCS2, IsaacLab）与方向专属技术
根据自身背景选定一条学习路径（A/B/C/D），并说出该路径的关键章节

71.1 复合机器人的统一建模¶

动机：为什么需要"统一"？¶

一只四足机器人加上一条机械臂后，系统变成了什么？

反面案例：许多早期实现将腿和臂视为独立子系统——腿控制器只管行走，臂控制器只管末端跟踪，两者通过速度命令松耦合。结果是：当臂抬起重物时，质心前移，但腿控制器对此一无所知，基座开始晃动，严重时甚至摔倒。

这就是**耦合效应被忽视**的代价。复合机器人的核心建模问题是：如何将腿、臂、基座纳入一个统一的动力学模型？

统一广义坐标¶

以 Unitree Go2（四足）+ Z1（6-DOF 臂）为例，构建完整广义坐标：

\[ \mathbf{q} = \begin{pmatrix} \mathbf{q}_{\text{base}} \\ \mathbf{q}_{\text{legs}} \\ \mathbf{q}_{\text{arm}} \end{pmatrix} = \begin{pmatrix} \underbrace{x, y, z, \phi, \theta, \psi}_{6\text{-DOF 浮动基座}} \\ \underbrace{q_{1}, q_{2}, q_{3}, \ldots, q_{12}}_{4 \times 3 = 12\text{-DOF 腿}} \\ \underbrace{q_{13}, q_{14}, \ldots, q_{18}}_{6\text{-DOF 臂}} \end{pmatrix} \]

总维度：$n_v = 6 + 12 + 6 = 24$ DOF（若用 Z1 7-DOF 版则为 25 DOF）。

对于人形 G1（29 关节-DOF），则为：

\[ \mathbf{q}_{\text{G1}} = (\underbrace{\mathbf{q}_{\text{base}}}_{6}, \underbrace{\mathbf{q}_{\text{legs}}}_{12}, \underbrace{\mathbf{q}_{\text{waist}}}_{3}, \underbrace{\mathbf{q}_{\text{arms}}}_{14}) \quad \Rightarrow n_v = 35 \]

注意：浮动基座的方位用四元数表示时，配置空间维度 $n_q = n_v + 1$（四元数 4 维 vs 角速度 3 维），这是 Pinocchio 区分 model.nq 与 model.nv 的根本原因。理解这一点对后续所有操作至关重要。

Pinocchio 3.x 统一建模¶

Pinocchio 的核心优势在于：一个 URDF/MJCF 描述整个系统。不需要分别加载腿模型和臂模型再手动拼接。

import pinocchio as pin

# 加载包含浮动基座+四腿+臂的统一 URDF
model = pin.buildModelFromUrdf(
    "go2_z1.urdf",
    pin.JointModelFreeFlyer()   # 浮动基座
)
data = model.createData()

print(f"nq = {model.nq}")  # 25 (含四元数)
print(f"nv = {model.nv}")  # 24 (切空间)

# 统一前向运动学：一次调用，所有 frame 同时更新
q = pin.neutral(model)
pin.forwardKinematics(model, data, q)

# 统一质量矩阵 M(q) ∈ R^{24×24}
pin.crba(model, data, q)
M = data.M
print(f"M shape = {M.shape}")  # (24, 24)

关键点：M 不是**分块对角的——腿关节行和臂关节列之间存在非零的**交叉耦合项。这些耦合项就是下面要讨论的科里奥利效应的来源。

科里奥利耦合：臂动影响腿¶

考虑整体动力学方程（Euler-Lagrange 形式）：

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^T \boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{f}_c \]

其中 $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$ 是科里奥利/离心力矩阵，$\mathbf{S}$ 是选择矩阵（去除浮动基座的不可驱动维度），$\mathbf{J}_c$ 是接触雅可比，$\mathbf{f}_c$ 是接触力。

当臂快速运动时（$\dot{\mathbf{q}}_{\text{arm}}$ 大），$\mathbf{C}$ 中腿-臂交叉项会产生作用于基座和腿关节的**附加力矩**。其物理本质是：臂运动改变了系统的质心动量（centroidal momentum），腿必须输出补偿力矩来维持平衡。

耦合强度取决于三个因素： - 臂负载质量：5 kg 负载时耦合力矩可达关节额定力矩的 10-20% - 臂运动速度：速度越快，科里奥利力越强（与 $\dot{q}$ 成正比） - 臂伸展长度：力臂越大，对基座的扰动力矩越大

⚠️ 易错点：将臂和腿视为独立子系统（忽略 $\mathbf{M}$ 和 $\mathbf{C}$ 中的耦合项）是最常见的新手错误。后果是基座在操作过程中出现周期性晃动（wobble），严重时导致步态失稳。正确做法是使用统一模型，在同一个 WBC/MPC 优化问题中同时求解腿和臂的力矩。

统一建模的三个层次¶

层次	方法	耦合程度	适用场景
L0 独立控制	腿控制器 + 臂控制器各自独立运行	无耦合	臂轻载（<0.5 kg）、低速
L1 前馈补偿	臂的质心偏移作为前馈补偿项注入腿控制器	单向近似	中等负载（0.5-2 kg）
L2 统一优化	全身统一动力学，单一 QP/NLP 求解	完全耦合	重载、高速操作、精确末端跟踪

复合/20_浮动基座臂统一动力学将深入 L2 层的 Pinocchio 实现细节，复合/30_多模态MPC 将在 OCS2 框架下构建多模态 MPC。

71.2 三子方向分类与金字塔¶

D1 轮足（Wheel-Legged）——效率与灵活性的折中 ⭐⭐¶

轮足机器人在每条腿的末端安装驱动轮：平坦地面用轮式运动（高效、快速，可达 3+ m/s），复杂地形切换为足式运动（越障、爬梯）。

代表平台：Swiss-Mile ANYmal-W、Unitree B2-W

为什么不直接用轮式或足式？ 纯轮式机器人效率极高但越障能力差——5 cm 台阶就可能是不可逾越的障碍。纯足式机器人地形适应性强但效率低——trot 步态下四足机器人的能耗约为同重量轮式机器人的 5-10 倍。轮足方案在两者之间寻找折中：平坦路段用轮子，复杂路段用腿。这个想法在概念上很简单，但工程实现上引入了独特的数学和控制难题。

核心技术挑战： - Pfaffian 非完整约束：轮不能侧滑，产生速度层面的约束 $\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = 0$，这类约束无法积分为位置约束。回顾复合/60_轮式运动学与Pfaffian：Pfaffian 约束的核心性质是它约束了可行速度空间但不约束可达位置空间——轮子虽然不能侧滑，但通过多次前进和转弯，可以到达平面上任意位置。这与位置层约束（如机械臂关节限位）有本质区别，在 MPC 中必须作为等式约束显式加入 - 模式切换：轮 / 足 / 混合三种模态间如何平滑过渡，避免切换瞬间的冲击 - 混合优化：同一个 MPC 中同时处理轮的连续接触和足的间断接触序列

D2 移动操作（Mobile Manipulation）——底盘+臂的松耦合 ⭐⭐¶

移动底盘提供 SE(2) 平面运动能力，机械臂提供 SE(3) 操作能力。两者通过刚性连接形成移动操作系统。

代表平台：Fetch、TIAGo、Mobile ALOHA、Hello Robot Stretch

核心技术挑战： - 运动学冗余：底盘 3-DOF + 臂 6/7-DOF 共同服务于末端 6-DOF 任务，存在冗余，如何利用冗余提高操作灵活性？ - 导航-操作协调：Nav2 负责全局导航到目标区域，MoveIt2 负责臂的局部运动规划，两者的无缝衔接是工程难点 - VLA 端到端：Vision-Language-Action 模型（如 pi0、RT-2）直接从视觉和语言输入映射到全身动作，跳过传统流水线

D3a 四足+臂（Legged Manipulation）——行走中操作 ⭐⭐¶

四足机器人搭载一条机械臂，实现边走边操作（loco-manipulation）。当前**开源最成熟、论文最密集**的复合机器人方向。

代表平台：Go2+Z1、Go2+ARX5、Spot+Arm

核心技术挑战： - 任务优先级冲突：行走稳定性 vs 末端位姿精度，哪个优先？WBC 需要明确的优先级排序 - 接触力分配：四足-地面接触力 + 臂-物体接触力，如何在单一 QP 中联合分配？ - 19+ DOF 实时控制：500 Hz WBC 在 19-DOF 系统上的计算预算约 2 ms，实时性是硬约束

D3b 人形（Humanoid）——最高复杂度的具身智能 ⭐⭐¶

双足行走 + 双臂操作，自由度最高（29-50+ DOF），是复合机器人的终极形态。2024-2025 年因大模型与 RL 的结合而进入爆发期。

代表平台：Unitree G1/H1、Boston Dynamics Atlas（电驱版）、Figure 02

核心技术挑战： - 欠驱动平衡：双足支撑面积远小于四足，ZMP/DCM 约束更紧 - 全身 RL：30+ DOF 的动作空间，RL 探索效率成为关键瓶颈 - Sim-to-Real：高维系统的仿真-实物差距更显著，需要 domain randomization + delta-action 等技术

三层级金字塔¶

                        ┌─────────────┐
                        │   学习层     │  RL / VLA / 模仿学习
                        │  Learn      │  "解决复杂问题的工具"
                       ─┴─────────────┴─
                      ┌───────────────────┐
                      │    动力学层        │  MPC / WBC / DDP
                      │   Dynamics        │  "理解问题的语言"
                     ─┴───────────────────┴─
                    ┌─────────────────────────┐
                    │      运动学层            │  FK / IK / 雅可比
                    │     Kinematics          │  "描述问题的基础"
                   ─┴─────────────────────────┴─

为什么是金字塔？ 因为每一层以下层为基础：

不懂 FK/IK，就无法理解 WBC 约束中的雅可比来源
不懂 WBC 的优先级机制，就不明白 FALCON 为什么要用双智能体分离 locomotion 和 manipulation
不懂 MPC 如何生成参考轨迹，就不知道 RL reward 中的"跟踪误差项"在跟踪什么

学习层并非要"取代"动力学层，而是在动力学方法难以实时求解（30+ DOF）或难以精确建模（接触丰富场景）时提供替代方案。

跨领域类比：这个金字塔结构类似于计算机图形学中的渲染管线——顶点处理（几何变换，对应运动学层）、光栅化（像素计算，对应动力学层）、后处理（AI 去噪/超分辨率，对应学习层）。每一层依赖下层的输出，但可以独立改进。光栅化不理解顶点数据就无法工作，后处理不理解光栅化输出就只能做盲目猜测。复合机器人的学习层也是如此——不理解动力学约束的 RL 策略可以在仿真中"暴力"收敛，但在真机上往往因为违反物理约束而失败。

各子方向对比总表¶

属性	D1 轮足	D2 移动操作	D3a 四足+臂	D3b 人形
典型 DOF	18-20	9-10	19-25	29-50+
控制频率	200-500 Hz	100-500 Hz	500 Hz	500-1000 Hz
运动基座	浮动（腿式）	SE(2)（轮式）	浮动（腿式）	浮动（腿式）
操作能力	无或有限	强（6/7-DOF 臂）	中-强	强（双臂）
核心难点	模式切换+Pfaffian	导航-操作衔接	稳定-精度冲突	全身高维控制
开源成熟度	中	高	高	中-高（快速增长）
典型落地场景	物流巡检	家庭/仓储服务	工业巡检+操作	通用服务机器人

71.3 共享技术栈与方向专属¶

共享技术栈¶

四个子方向并非割裂的岛屿——它们共享大量基础工具和理论框架。

工具/框架	角色定位	覆盖方向
Pinocchio 3.x	刚体动力学引擎：FK/IK/RNEA/CRBA/ABA + 自动微分	全方向
OCS2	非线性 MPC 框架：switched system + 代码生成	D1, D2, D3a
Crocoddyl / Aligator	DDP/iLQR 轨迹优化	D1, D3a, D3b
IsaacLab	GPU 并行仿真 + RL 训练环境	全方向
MuJoCo	精确接触仿真（用于 sim-to-real 验证）	全方向
ros2_control	硬件抽象层 + 控制器管理器	全方向（部署阶段）

D1 轮足专属技术¶

Pfaffian 约束处理：轮的非完整约束 $\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = 0$ 需要在 MPC 中作为等式约束显式加入，OCS2 中对应 StateInputConstraint
模式切换 FSM / RL 策略：有限状态机管理轮/足/混合模态，或用高层 RL 学习何时切换
轮速-腿力矩协调：同一条腿上轮电机（速度控制）和关节电机（力矩控制）的协调分配

D2 移动操作专属技术¶

Nav2 + MoveIt2 集成：ROS 2 导航栈提供全局路径→局部避障→底盘速度命令，MoveIt2 提供臂的轨迹规划→关节命令，需要自定义 bridge 节点协调两者
VLA 模型：pi0 / RT-2 / Octo 等基础模型从图像+语言指令直接输出末端位姿或关节角序列
Mobile ALOHA 范式：双臂遥操作数据采集 + Action Chunking with Transformers（ACT）策略训练

D3a/D3b 足式操作专属技术¶

Grasp Matrix 与接触力分配：抓取时的力封闭（force closure）条件需要与脚-地面接触力在同一个优化问题中联合求解
任务优先级 WBC：HQP（Hierarchical QP）实现操作任务 vs 平衡任务的优先级仲裁——典型优先级：关节限位 > 摩擦锥 > 平衡 > 末端跟踪
质心动量控制（Centroidal Momentum Control）：通过调节角动量变化率 $\dot{\mathbf{L}}$ 来补偿臂运动引起的基座扰动

共享技术栈的深层统一性 ⭐⭐¶

上面的共享与专属分类是按"用不用某个工具"划分的。更深层的统一在于**数学框架**——所有子方向最终都在求解同一类优化问题：

\[ \min_{x, u} \int_0^T \ell(x, u, t) \, dt + \Phi(x(T)) \]

\[ \text{s.t.} \quad \dot{x} = f_m(x, u), \quad m \in \mathcal{M} \]

\[ g(x, u) \leq 0, \quad h(x, u) = 0 \]

区别仅在于： - **状态 $x$ 的维度和含义**不同：D1 含轮角，D2 含底盘 SE(2)，D3 含浮动基座 SE(3) - **模式集合 $\mathcal{M}$ 的结构**不同：D1 有轮/足/混合，D3 有不同步态相位 - **等式约束 $h$**不同：D1 有 Pfaffian，D3 有接触不滑动 - **不等式约束 $g$**不同：D2 通常只有关节限位和碰撞，D3 还有摩擦锥

理解了这个数学统一性，就能明白为什么 OCS2 可以同时服务 D1（ocs2_switched_model）、D2（ocs2_mobile_manipulator）和 D3a（qm_control 基于 OCS2）——它们共享求解器和问题结构，只是代价函数和约束的具体形式不同。

本质洞察：四个子方向不是四个独立的技术领域，而是同一个最优控制问题在不同机构拓扑和接触模式下的四种实例化。掌握了一种，理解其余三种的门槛会大幅降低。

共享 vs 专属对照表¶

技术模块	共享	D1 专属	D2 专属	D3 专属
Pinocchio 动力学	✓
OCS2 / Crocoddyl MPC	✓
IsaacLab RL 训练	✓
ros2_control 部署	✓
Pfaffian 非完整约束		✓
模式切换 FSM		✓
Nav2 全局导航			✓
MoveIt2 运动规划			✓
VLA 端到端策略			✓
Grasp Matrix 力封闭				✓
质心动量控制				✓
HQP 任务优先级 WBC				✓

71.4 学习路径建议¶

所有路径从本章（复合/10_复合机器人全景）出发，经过公共基础层后进入方向专属章节。

Path A：轮足方向（约 8 周）¶

复合/10_复合机器人全景(全景) → 复合/60_轮式运动学与Pfaffian(轮运动学+Pfaffian) → 复合/70_轮足混合MPC(Bjelonic NMPC)
→ 复合/80_Wheel_Legged_Gym_RL(Wheel-Legged-Gym RL) → 复合/100_模式切换(模式切换) → 复合/110_轮足SimToReal与硬件(sim-to-real)

适合：对高效移动 + 地形适应感兴趣，偏控制优化。工业落地偏物流巡检。

Path B：移动操作方向（约 8 周）¶

复合/10_复合机器人全景(全景) → 复合/20_浮动基座臂统一动力学(统一动力学) → 复合/50_操作技能接口(操作接口)
→ 复合/120_底盘臂联合规划(Nav2+MoveIt2) → 复合/130_OCS2_mobile_manipulator(OCS2 mobile_manipulator)
→ 复合/140_VLA移动操作(VLA/openpi) → 复合/150_Mobile_ALOHA与UMI(Mobile ALOHA)

适合：有 SLAM 背景，希望做"感知→导航→操作"全链路。VLA 是移动操作中研究活跃、工程投入高的方向之一。

Path C：四足+臂方向（约 10 周）¶

复合/10_复合机器人全景(全景) → 复合/20_浮动基座臂统一动力学(统一动力学) → 复合/30_多模态MPC(多模态MPC)
→ 复合/160_四足臂动力学概览(四足+臂概览) → 复合/170_qm_control精读(qm_control精读)
→ 复合/180_Deep_WBC精读(DeepWBC) → 复合/200_UMI_on_Legs精读(UMI-on-Legs)

适合：有腿足基础，想做 loco-manipulation 研究。开源资源最丰富，硬件门槛最低。

Path D：人形方向（约 12 周）¶

复合/10_复合机器人全景(全景) → 复合/20_浮动基座臂统一动力学(统一动力学) → 复合/30_多模态MPC(多模态MPC) → 复合/40_RL全身控制基础(RL全身控制)
→ 复合/220_经典人形全身控制(经典人形WBC) → 复合/230_人形全身RL(人形RL)
→ 复合/240_ASAP_SimToReal(ASAP sim-to-real) → 复合/250_力敏感人形LocoMani(FALCON力敏感)

适合：目标博士方向或产业前沿，愿意投入最长时间。2025 年人形赛道资源爆发式增长。

选择建议：如果犹豫不决，推荐 Path C（四足+臂）入手。理由有三：(1) 开源项目最多（qm_control, DeepWBC, visual_wholebody, UMI-on-Legs），学习材料充足；(2) 硬件门槛最低（Go2+Z1 约 2-3 万元），可实物验证；(3) 技术可迁移——四足+臂的 WBC/RL 经验可直接平移到人形。

71.4b 工程选型实例：仓库物流场景的平台选择 ⭐⭐¶

前面的分类和路径给出了技术视角。下面通过一个具体工程场景，展示如何把技术特性转化为平台选择决策。

场景描述¶

某物流仓库需要一台机器人完成以下任务链：

从充电区出发，经过 80 m 走廊到达货架区（平坦水泥地面）
在货架前停下，从 1.2 m 高的货架上取下一个 2 kg 的包裹
携带包裹返回分拣区，途中需要跨越一道 5 cm 高的地面线槽
到达分拣台后，将包裹放到指定位置

每日运行 8 小时，预算上限 30 万元人民币（含硬件、软件和集成）。

逐项评估¶

需求维度	D1 轮足	D2 移动操作	D3a 四足+臂	D3b 人形
80 m 走廊移动效率	优秀（轮式 3+ m/s）	良好（轮式 1-2 m/s）	一般（步行 0.5-1 m/s）	一般（步行 0.5-1.5 m/s）
1.2 m 货架抓取	差（无臂或臂短）	良好（6/7-DOF 臂）	良好（臂+基座可调高度）	优秀（双臂+身高适配）
2 kg 负载	不适用	良好（大多数臂可胜任）	良好（Z1 支持 3 kg）	良好（多数人形臂 3+ kg）
5 cm 线槽越障	优秀（轮足切腿模式）	差（小轮底盘过不去）	优秀（步行跨越）	优秀（步行跨越）
8 小时续航	良好（轮式省电）	良好（轮式省电）	差（步行耗电大）	差（步行耗电大）
预算 30 万	接近上限	充裕	充裕	严重超标
技术成熟度	中（需要模式切换调试）	高（Nav2+MoveIt2 成熟）	中（WBC 调参密集）	低（全身控制仍在研究阶段）

决策分析¶

首先排除 D3b 人形：预算严重超标（G1 U2 约 36 万元裸机），且全身控制的技术成熟度不足以支撑 8 小时稳定运行。人形的优势在于通用性和人机交互，但本场景不需要。

D3a 四足+臂的问题：续航不足。Go2 步行 8 小时需要至少 3 次换电池（电池容量约 25-45 分钟步行），这在物流场景中不可接受。步行速度也偏慢——80 m 走廊单程需要 80-160 秒，远不如轮式。

D1 轮足的问题：具备移动效率和越障能力的双重优势，但当前商用轮足平台（如 B2-W）大多没有标配操作臂，且价格接近预算上限。加装臂后系统复杂度急剧上升，需要模式切换和 loco-manipulation 的双重调试。

D2 移动操作的优势：Nav2+MoveIt2 技术栈成熟，TIAGo 或 Fetch 类平台价格在 15-25 万元，留有余量。唯一问题是 5 cm 线槽越障——小轮底盘通常只能过 2-3 cm 台阶。

最终建议¶

方案一（推荐）：选择 D2 移动操作平台（如 TIAGo 或定制全向底盘+UR5e），同时在线槽位置安装一个 5 cm 坡道。工程上改造地面比改造机器人便宜得多，坡道成本约 500 元，而选择轮足平台的额外成本可能超过 10 万元。

方案二（技术驱动）：如果地面不可改造，选择 D1 轮足平台并加装轻量臂。此时需要投入额外 2-3 个月用于模式切换和操作系统集成。

本质洞察：工程选型不是"哪个平台最强"，而是"哪个方案的总成本（硬件+软件+运维+风险）最低"。上面的分析表明，一个 500 元的坡道就能消除选择更复杂平台的必要性。最优方案往往不在机器人本身，而在整个系统的设计中。

这个例子揭示了一个普遍原则：复合机器人的选型必须从任务倒推到平台，而非从平台正推到任务。先定义完整的任务链（含环境约束、时间约束、成本约束），再逐个评估每类平台的匹配度，最后综合决策。很多团队犯的错误是先选了一个"酷"的平台（比如人形），然后发现任务其实用不到它 80% 的能力，却要为这些多余能力付出巨大的开发和维护成本。

⚠️ 工程陷阱：上面的分析假设地面条件已知且固定。真实仓库的地面条件会随时间变化——叉车可能划伤地面、货物可能掉落在走廊上、地面线槽可能被临时覆盖或暴露。选型时必须为"意外地面状况"留有安全余量。这也是为什么方案一建议安装坡道而非假设地面完全平坦——坡道是一个确定性的工程措施，而"假设地面平坦"是一个随时可能被违反的前提。

练习 1b：仓库选型扩展（分析 / C 型）

修改上面的场景：

(1) 取消 30 万预算限制，改为"总拥有成本 3 年最低"。

(2) 增加一项新需求——机器人需要在 30 cm 高的台阶上下楼。

重新评估四类平台并给出推荐方案。你的分析中应包含硬件采购、软件开发、运维（电池更换、维修）和人员培训的分项成本估计。

练习¶

练习 1：画出 Go2+Z1 的统一广义坐标向量（笔头 / A 型）¶

(a) 写出 Go2+Z1 的完整广义坐标向量 $\mathbf{q}$，标注每个分量的物理含义。提示：Go2 的四条腿各有 hip/thigh/calf 三个关节，Z1 臂有 6 个旋转关节。

(b) 指出哪些分量属于浮动基座、前左腿（FL）、前右腿（FR）、后左腿（RL）、后右腿（RR）、臂，并标注每部分维度。

(c) 如果基座方位用四元数表示，$n_q$ 和 $n_v$ 分别是多少？用一句话解释差异原因。

自检标准：所写向量与 Pinocchio 加载 go2_z1.urdf 后 [model.names[i] for i in range(model.njoints)] 的输出一致。

练习 2：四子方向对比表（分析 / C 型）¶

制作一张对比表格，行为 D1/D2/D3a/D3b，列至少包含以下 6 个维度：

典型 DOF 数（分 base / legs / arms 分别列出）
基座类型（浮动 vs SE(2)）
操作能力上限（可负载质量 / 末端精度 / 灵巧度）
主流控制架构（MPC+WBC / RL / VLA / 混合）
代表开源项目（至少 2 个）
落地场景（至少 2 个）

在表格下方用 3-5 句话总结：每个方向"不可替代的优势"是什么？

练习 3：Pinocchio 加载四足+臂并计算全身 FK（编码 / A 型）¶

"""
练习 3：用 Pinocchio 加载统一 URDF，验证统一建模的关键特性。

步骤：
  1. 加载 go2_z1.urdf（或用 example_robot_data 中的 solo12 替代四足部分）
  2. 打印 model.nq, model.nv, model.njoints
  3. 遍历所有 joint，打印名称和类型
  4. 在 neutral 配置下计算 FK，打印臂末端 frame 的位姿
  5. 计算质量矩阵 M(q)，检验其稀疏结构
  6. 思考题：M 中哪些块是非零的？为什么？
"""
import pinocchio as pin
import numpy as np

# --- Step 1: 加载模型 ---
# 如果没有 go2_z1.urdf，可用 example_robot_data 中的模型替代
import example_robot_data
robot = example_robot_data.load("solo12")
model, data = robot.model, robot.createData()

# --- Step 2: 基本信息 ---
print(f"nq = {model.nq}, nv = {model.nv}, njoints = {model.njoints}")

# --- Step 3: 关节列表 ---
for i in range(model.njoints):
    print(f"  Joint {i}: {model.names[i]}, type = {model.joints[i].shortname()}")

# --- Step 4: 前向运动学 ---
q = pin.neutral(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
for i in range(model.nframes):
    frame = model.frames[i]
    pos = data.oMf[i].translation
    print(f"  Frame {i}: {frame.name}, pos = [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")

# --- Step 5: 质量矩阵 ---
pin.crba(model, data, q)
M = data.M.copy()
M = np.triu(M) + np.triu(M, 1).T   # crba 只填上三角，补全对称
print(f"\nM shape = {M.shape}")
sparsity = np.count_nonzero(np.abs(M) > 1e-10) / M.size
print(f"M 非零元素比例 = {sparsity:.2%}")

# --- Step 6: 思考 ---
# M 不是分块对角的。基座行/列与所有关节存在耦合，
# 因为运动链上的每个关节都通过刚体连接传递惯性到基座。
# 具体来说：M[0:6, 6:] 不为零，这正是臂-腿耦合的数学来源。

自检标准： - nq - nv 等于 1（四元数多一维） - 质量矩阵非分块对角——打印 M[0:6, 6:] 确认有非零项 - 能用物理直觉解释耦合项的存在：关节运动通过刚体连接传递惯性效应

第一部分小结：全景框架¶

要点	核心内容
统一建模	必须用统一广义坐标建模，忽略腿-臂耦合会导致基座失稳
三子方向	D1 轮足、D2 移动操作、D3a 四足+臂、D3b 人形，各有独特技术挑战
三层金字塔	运动学 → 动力学 → 学习，每层以下层为基础，不可跳过
共享 vs 专属	Pinocchio/OCS2/IsaacLab 全方向共享；Pfaffian/VLA/GraspMatrix 方向专属
学习路径	四条路径（A-D），推荐从 Path C（四足+臂）入手

⚠️ 常见陷阱：全景阶段的认知错误¶

💡 概念误区 1：认为 DOF 越高控制越难

新手常以为 D3b 人形（49 DOF）一定比 D2 移动操作（10 DOF）难控制。实际上，控制难度不仅取决于 DOF 数量，更取决于**约束的复杂度**和**欠驱动程度**。D2 的底盘始终接触地面（无平衡问题），DOF 虽少但约束简单，控制难度中等。D3b 的双足行走存在严格的 ZMP 约束和极小的支撑面积，控制难度极高——即使 DOF 降到 20（如双足无臂），平衡问题仍然远比 D2 困难。正确的排序应该是：D2 < D1 ≈ D3a < D3b，其中 D1 和 D3a 的难度来源不同（D1 来自模式切换，D3a 来自稳定-操作冲突）。

🧠 思维陷阱 2：选择平台时只看论文数量

“D3a 方向论文最多，所以技术最成熟”——这个推理有一半是对的。论文数量多确实说明社区活跃、开源资源丰富。但论文数量多也可能意味着问题还没有被很好地解决，否则就不会有那么多论文试图改进。D2 方向的论文相对少，部分原因是 Nav2+MoveIt2 的工业级实现已经相当成熟，很多工程问题已经被解决，学术新意不如 D3 方向大。从工程落地的角度看，D2 的成熟度反而最高。

下一章预告：复合/20_浮动基座臂统一动力学将深入浮动基座 + 臂的统一动力学，在 Pinocchio 中实现从 URDF 加载到全身 RNEA/ABA 的完整流程，并推导出统一 WBC 的 QP 公式。

接下来把前面的全景框架展开为更具体的数学、平台和学习迁移问题。前四节回答”有哪些方向、为什么需要统一建模”，后面的内容回答”这些方向在方程、案例和工程训练中怎样落地”。

71.5 统一动力学方程的完整推导 ⭐⭐⭐¶

动机：为什么不能各跑各的？¶

前面的统一建模部分已经区分了"独立控制 vs 统一优化"的三个层次。这里追问一个更根本的问题：分别为底盘/腿/臂各写一个控制器，然后并行运行，到底差在哪？

考虑一个具体场景：Go2+Z1 正在行走，臂需要快速将一个 3 kg 的物体从右侧搬到左侧。如果采用独立控制：

腿控制器按照正常步态发送关节力矩，它"看不到"臂在做什么
臂控制器按照轨迹跟踪发送关节力矩，它"看不到"腿在走路

结果：臂横移 3 kg 负载时，系统整体质心在横向上偏移约 8 cm（Z1 工作半径约 0.6 m，3 kg 负载的质心偏移近似为 3 x 0.6 / 24 = 0.075 m）。对于 Go2 的四足支撑多边形（约 35 cm x 25 cm），8 cm 偏移意味着质心已经接近支撑多边形边界。腿控制器在下一个控制周期才通过状态估计"发现"质心偏了，然后被动补偿——但此时已经延迟了 2-10 ms，基座已经产生了可见的晃动。

如果采用统一控制，MPC/WBC 在求解力矩时**同时知道**臂即将横移和腿需要维持平衡，因此可以提前调整支撑脚的地面反力分配，从而在臂运动的同时就保持质心在安全区域内。

这就是"统一"的价值：预见性补偿，而非被动响应。

从 Euler-Lagrange 出发的完整推导¶

复合机器人是一个浮动基座多刚体系统。我们从第一性原理出发推导其运动方程。

Step 1：定义广义坐标

设系统有 $n_b = 6$ 个基座自由度（3 平移 + 3 旋转），$n_l$ 个腿关节自由度，$n_a$ 个臂关节自由度，总的切空间维度为：

\[ n_v = n_b + n_l + n_a \]

对于 Go2+Z1：$n_v = 6 + 12 + 6 = 24$。广义速度向量为：

\[ \dot{\mathbf{q}} = \begin{pmatrix} \boldsymbol{v}_b \\ \boldsymbol{\omega}_b \\ \dot{\mathbf{q}}_l \\ \dot{\mathbf{q}}_a \end{pmatrix} \in \mathbb{R}^{24} \]

其中 $\boldsymbol{v}_b \in \mathbb{R}^3$ 是基座线速度，$\boldsymbol{\omega}_b \in \mathbb{R}^3$ 是基座角速度。

Step 2：系统动能

系统总动能是所有刚体动能之和。设第 $i$ 个刚体在世界坐标系下的速度为 $\boldsymbol{v}_i$，角速度为 $\boldsymbol{\omega}_i$，则：

\[ T = \frac{1}{2} \sum_{i=1}^{N_{\text{body}}} \left( m_i \boldsymbol{v}_i^T \boldsymbol{v}_i + \boldsymbol{\omega}_i^T \mathbf{I}_i \boldsymbol{\omega}_i \right) \]

通过运动学链，每个刚体的速度可以表示为广义速度的线性函数：

\[ \begin{pmatrix} \boldsymbol{v}_i \\ \boldsymbol{\omega}_i \end{pmatrix} = \mathbf{J}_i(\mathbf{q}) \dot{\mathbf{q}} \]

其中 $\mathbf{J}_i(\mathbf{q}) \in \mathbb{R}^{6 \times n_v}$ 是第 $i$ 个刚体的几何雅可比（geometric Jacobian）。

代入动能表达式：

\[ T = \frac{1}{2} \dot{\mathbf{q}}^T \underbrace{\left( \sum_{i=1}^{N_{\text{body}}} \mathbf{J}_i^T \mathbf{M}_i^{\text{spatial}} \mathbf{J}_i \right)}_{\mathbf{M}(\mathbf{q})} \dot{\mathbf{q}} \]

这就是广义质量矩阵 $\mathbf{M}(\mathbf{q}) \in \mathbb{R}^{n_v \times n_v}$ 的来源。它的每一项都是所有刚体惯性贡献的叠加，通过各自的雅可比投影到广义坐标空间。

这揭示了耦合的本质：即使腿关节 $j$ 和臂关节 $k$ 在机构上没有直接连接，只要存在某个刚体 $i$（比如基座本身）的雅可比 $\mathbf{J}_i$ 在第 $j$ 列和第 $k$ 列都非零，$\mathbf{M}_{jk}$ 就不为零。基座就是这个"桥梁"——它连接着所有运动链，因此所有关节都通过基座产生惯性耦合。

Step 3：Euler-Lagrange 方程

对拉格朗日量 $L = T - V$（$V$ 为势能）应用 Euler-Lagrange 方程：

\[ \frac{d}{dt} \frac{\partial L}{\partial \dot{\mathbf{q}}} - \frac{\partial L}{\partial \mathbf{q}} = \boldsymbol{\tau}_{\text{gen}} \]

展开后得到标准形式：

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}_{\text{gen}} \]

其中： - $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \in \mathbb{R}^{n_v \times n_v}$：科里奥利/离心力矩阵 - $\mathbf{g}(\mathbf{q}) \in \mathbb{R}^{n_v}$：重力项 - $\boldsymbol{\tau}_{\text{gen}} \in \mathbb{R}^{n_v}$：广义力

科里奥利矩阵的元素可用 Christoffel 符号表示：

\[ C_{ij} = \sum_{k=1}^{n_v} \frac{1}{2}\left( \frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i} \right) \dot{q}_k \]

这个公式的物理含义是：当多个关节同时运动时，质量矩阵本身在变化（因为它依赖于构型 $\mathbf{q}$），这种变化产生的虚拟力就是科里奥利力和离心力。具体来说，$\partial M_{ij}/\partial q_k$ 描述了关节 $k$ 运动时，关节 $i$ 和 $j$ 之间的惯性耦合如何改变。

为什么 Christoffel 符号有三项而非一项？三项的对称结构来源于系统动能对配置的二阶偏导数——第一项 $\partial M_{ij}/\partial q_k$ 捕获了关节 $k$ 运动如何改变 $i$-$j$ 耦合，后两项 $\partial M_{ik}/\partial q_j$ 和 $-\partial M_{jk}/\partial q_i$ 保证了 $\mathbf{C}$ 满足一个重要性质：$\dot{\mathbf{M}} - 2\mathbf{C}$ 是反对称矩阵。这个性质在被动控制（passivity-based control）中至关重要——它保证了系统能量的自然耗散结构，使得 PD 控制器在非线性多体系统上仍然全局稳定。如果用其他方式构造 $\mathbf{C}$（比如只取第一项），能量耗散结构会被破坏。

跨领域类比：Christoffel 符号在微分几何中描述的是"曲面上平行移动时方向的偏转"。在多体动力学中，构型空间本身是一个曲面（因为 $\mathbf{M}$ 随 $\mathbf{q}$ 变化），Christoffel 符号描述的就是"在这个弯曲的惯性空间中运动时，产生的额外力"。这与广义相对论中引力效应来自时空弯曲的思想是完全类似的——只不过这里弯曲的是惯性空间而非时空。

Step 4：加入选择矩阵和接触力

复合机器人有两个特殊之处：

(1) 浮动基座不可直接驱动——没有电机直接施加力/力矩给基座。因此需要选择矩阵 $\mathbf{S}$：

\[ \mathbf{S} = \begin{pmatrix} \mathbf{0}_{(n_l+n_a) \times n_b} & \mathbf{I}_{(n_l+n_a)} \end{pmatrix} \in \mathbb{R}^{(n_l+n_a) \times n_v} \]

关节力矩通过 $\mathbf{S}^T$ 映射到广义力空间：$\boldsymbol{\tau}_{\text{actuator}} = \mathbf{S}^T \boldsymbol{\tau}$，其中 $\boldsymbol{\tau} \in \mathbb{R}^{n_l + n_a}$ 是实际的电机力矩。

$\mathbf{S}$ 的物理含义非常直观：它把 $n_l + n_a$ 维的电机力矩向量"嵌入"到 $n_v$ 维的广义力空间中，前 $n_b$ 行填零表示基座不受电机直接驱动。基座的运动完全由接触力和动力学耦合决定——这正是浮动基座系统与固定基座机械臂的根本区别。

(2) 足端和臂末端与环境存在接触。设有 $n_c$ 个接触点，每个接触力 $\mathbf{f}_{c,k} \in \mathbb{R}^3$（或 $\mathbb{R}^6$ 含力矩）通过接触雅可比 $\mathbf{J}_{c,k}$ 映射到广义力空间：

\[ \boldsymbol{\tau}_{\text{contact}} = \sum_{k=1}^{n_c} \mathbf{J}_{c,k}^T \mathbf{f}_{c,k} = \mathbf{J}_c^T \mathbf{f}_c \]

最终的完整运动方程：

\[ \boxed{\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^T \boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{f}_c} \]

这就是复合机器人的统一运动方程。它适用于所有子方向（D1/D2/D3a/D3b），区别仅在于： - $n_v$ 的维度不同（D1: 16-20, D2: 9-10, D3a: 18-25, D3b: 29-50+） - 接触点 $n_c$ 的数量和位置不同 - 对 D1 轮足，还需要额外加入 Pfaffian 非完整约束 $\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = 0$

质量矩阵的分块结构¶

将 $\mathbf{M}$ 按照 base/legs/arm 分块：

\[ \mathbf{M} = \begin{pmatrix} \mathbf{M}_{bb} & \mathbf{M}_{bl} & \mathbf{M}_{ba} \\ \mathbf{M}_{bl}^T & \mathbf{M}_{ll} & \mathbf{M}_{la} \\ \mathbf{M}_{ba}^T & \mathbf{M}_{la}^T & \mathbf{M}_{aa} \end{pmatrix} \]

每个子块的物理含义：

子块	维度（Go2+Z1）	物理含义
$\mathbf{M}_{bb}$	$6 \times 6$	基座的有效惯性（包含所有连接体的反映惯性）
$\mathbf{M}_{bl}$	$6 \times 12$	基座-腿耦合：腿运动对基座的惯性效应
$\mathbf{M}_{ba}$	$6 \times 6$	基座-臂耦合：臂运动对基座的惯性效应
$\mathbf{M}_{ll}$	$12 \times 12$	四条腿自身的惯性（近似分块对角，四腿在运动学上是并联的）
$\mathbf{M}_{la}$	$12 \times 6$	腿-臂耦合：通过基座的间接耦合
$\mathbf{M}_{aa}$	$6 \times 6$	臂自身的惯性

关键观察：

$\mathbf{M}_{bb}$ **不是**常数——它随构型变化，因为连接在基座上的所有肢体的等效惯性会随关节角度改变。当臂完全伸展时，$\mathbf{M}_{bb}$ 中关于俯仰和偏航的分量会显著增大。
$\mathbf{M}_{bl}$ 和 $\mathbf{M}_{ba}$ 是**耦合的元凶**——它们把腿和臂的运动效应传递给基座。从运动方程中提取基座的前 6 行：$\mathbf{M}_{bb}\ddot{\mathbf{q}}_b + \mathbf{M}_{bl}\ddot{\mathbf{q}}_l + \mathbf{M}_{ba}\ddot{\mathbf{q}}_a + \ldots = \mathbf{J}_{c,b}^T \mathbf{f}_c$。可以看到腿加速度 $\ddot{\mathbf{q}}_l$ 和臂加速度 $\ddot{\mathbf{q}}_a$ 通过 $\mathbf{M}_{bl}$ 和 $\mathbf{M}_{ba}$ 直接影响基座加速度。
$\mathbf{M}_{la}$ 是间接耦合——腿和臂本身没有直接机构连接，但通过共同的基座产生二阶耦合。这个子块的元素通常比 $\mathbf{M}_{bl}$ 和 $\mathbf{M}_{ba}$ 小一个数量级，但不为零。
$\mathbf{M}_{ll}$ 近似分块对角——四条腿在运动学上是并联分支，同一条腿内的关节强耦合，不同腿之间仅通过基座弱耦合。

什么时候可以安全忽略耦合项？ 这是一个重要的工程判断。粗略的准则是：如果耦合力矩占主对角力矩的比例低于 5%，忽略耦合对跟踪精度的影响通常在传感器噪声水平以内。对 Go2+Z1 系统，当臂静止或低速运动（$|\dot{\mathbf{q}}_a| < 0.5$ rad/s）且无负载时，$\mathbf{M}_{ba}$ 引起的耦合力矩不到 hip 电机额定力矩的 3%，此时 L0 独立控制足够。但一旦臂开始快速运动或携带负载，耦合力矩会迅速增长到 20-30%，必须切换到 L1 或 L2 策略。实际系统中通常设置一个动态切换逻辑：根据臂速度和负载估计自动选择控制模式——轻载低速用独立控制（省算力），重载高速用统一控制（保稳定）。

科里奥利耦合的物理含义：一个定量估算¶

为了建立直觉，我们做一个粗略的定量估算。

场景：Go2（基座质量约 12 kg）搭载 Z1（臂重约 4.5 kg），臂末端以 2 rad/s 做大幅旋转运动（比如把一个 1 kg 物体从左侧搬到右侧）。

估算科里奥利扰动力矩：

臂末端有效质量约 $m_{\text{eff}} \approx 2$ kg（含负载），工作半径 $r \approx 0.5$ m，角速度 $\omega = 2$ rad/s。

科里奥利力的量级可以从离心力的角度估计。当臂以角速度 $\omega$ 旋转时，负载产生的离心力为：

\[ F_{\text{centrifugal}} = m_{\text{eff}} \omega^2 r = 2 \times 4 \times 0.5 = 4 \text{ N} \]

这个力通过臂传递到基座肩部安装点，力臂约为基座高度 $h \approx 0.3$ m，产生扰动力矩：

\[ \tau_{\text{centrifugal}} \approx F \times h = 4 \times 0.3 = 1.2 \text{ Nm} \]

同时考虑臂运动引起的质心偏移产生的附加重力矩：

\[ \tau_{\text{gravity}} \approx (m_{\text{arm}} + m_{\text{load}}) g \cdot \Delta x_{\text{CoM}} \approx 5.5 \times 9.81 \times 0.08 \approx 4.3 \text{ Nm} \]

再加上角动量变化率引起的陀螺力矩：

\[ \tau_{\text{gyro}} \approx I_{\text{arm}} \dot{\omega} \approx m_{\text{eff}} r^2 \alpha \approx 2 \times 0.25 \times 2 = 1.0 \text{ Nm} \]

总扰动约 $\tau_{\text{total}} \approx 5\text{-}7$ Nm。

Go2 的 hip 关节额定力矩约 23.7 Nm，因此这个扰动约占额定力矩的 21-30%。这绝非可以忽略的"小量"。如果腿控制器不知道这个扰动的存在（独立控制），它只能在下一个控制周期通过状态反馈来被动补偿，导致基座晃动。而统一控制器可以在同一控制周期内前馈补偿这个力矩。

⚠️ 易错点："忽略耦合项不会让系统不工作，只是精度下降一点"——这是最危险的误解。实际上，忽略耦合不仅影响精度，还会影响稳定性。上述 5-7 Nm 的扰动如果不被前馈补偿，而腿控制器的积分增益又设得不够高（或太高导致振荡），系统会出现持续的周期性晃动，臂的末端跟踪精度从统一控制的 2-3 cm 退化到独立控制的 15-20 cm——精度下降约 5-10 倍。

耦合控制 vs 独立控制的定量对比¶

以下表格总结了文献中报告的典型数据（Sleiman 2021, Fu 2022），用于建立直觉：

指标	独立控制	统一控制（MPC+WBC）	差距倍数
末端位姿跟踪精度（3D RMSE）	15-25 cm	2-4 cm	~6x
行走中基座翻滚角波动	±8-12 deg	±2-3 deg	~4x
最大可操作负载	1-2 kg	3-5 kg	~2.5x
步态稳定裕度（ZMP 距边界）	2-3 cm	8-12 cm	~4x
可容忍的臂运动速度	<1 rad/s	>3 rad/s	~3x

核心结论是：耦合控制不是锦上添花，而是重载/高速操作的必要条件。

为什么差距如此之大？ 根本原因是时间常数的差异。独立控制器通过状态反馈来补偿耦合扰动，但反馈环路的延迟至少是一个控制周期（2 ms at 500 Hz）。在这 2 ms 内，扰动已经对基座产生了加速度，基座已经开始偏移。统一控制器则在同一周期内同时知道腿力矩、臂力矩和接触力，可以做到零延迟的前馈补偿。用控制理论的语言说：独立控制只有反馈通道（feedback），统一控制同时有前馈通道（feedforward）和反馈通道。前馈的带宽理论上无限大（不受传感器延迟限制），因此对高频扰动的抑制能力远超纯反馈。

⚠️ 概念误区：有学员认为"只要把反馈增益调高，独立控制器也能补偿耦合扰动"。这是错误的。高增益会放大传感器噪声，导致关节力矩抖动。在实际系统中，反馈增益受噪声水平和执行器带宽的双重限制，不能无限提高。统一控制的前馈路径不受这些限制，因为它使用的是模型预测而非传感器测量。

Pinocchio 计算质量矩阵分块结构的代码示例¶

"""
可视化 Go2+Z1 质量矩阵的分块结构。
演示耦合项的存在与量级。
"""
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt

# 加载统一模型
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()

# 随机非奇异配置
q = pin.randomConfiguration(model)
pin.crba(model, data, q)
M = data.M.copy()
M = np.triu(M) + np.triu(M, 1).T  # crba 只填上三角

# 按 base(6) / legs(12) / arm(6) 分块
n_b, n_l, n_a = 6, 12, 6
M_bb = M[:n_b, :n_b]
M_bl = M[:n_b, n_b:n_b+n_l]
M_ba = M[:n_b, n_b+n_l:]
M_ll = M[n_b:n_b+n_l, n_b:n_b+n_l]
M_la = M[n_b:n_b+n_l, n_b+n_l:]
M_aa = M[n_b+n_l:, n_b+n_l:]

# 打印各块的 Frobenius 范数——量化耦合强度
print(f"||M_bb|| = {np.linalg.norm(M_bb):.3f}")
print(f"||M_bl|| = {np.linalg.norm(M_bl):.3f}  (base-leg coupling)")
print(f"||M_ba|| = {np.linalg.norm(M_ba):.3f}  (base-arm coupling)")
print(f"||M_ll|| = {np.linalg.norm(M_ll):.3f}")
print(f"||M_la|| = {np.linalg.norm(M_la):.3f}  (leg-arm coupling, indirect)")
print(f"||M_aa|| = {np.linalg.norm(M_aa):.3f}")

# 可视化稀疏结构
fig, ax = plt.subplots(figsize=(8, 8))
im = ax.imshow(np.log10(np.abs(M) + 1e-15), cmap='viridis')
ax.set_title("log10(|M(q)|) -- Go2+Z1 Mass Matrix")
ax.axhline(y=5.5, color='r', linestyle='--', label='base/legs boundary')
ax.axhline(y=17.5, color='orange', linestyle='--', label='legs/arm boundary')
ax.axvline(x=5.5, color='r', linestyle='--')
ax.axvline(x=17.5, color='orange', linestyle='--')
ax.legend()
plt.colorbar(im, label='log10(magnitude)')
plt.tight_layout()
plt.savefig("mass_matrix_structure.png", dpi=150)

预期结果：热力图中 base-legs 和 base-arm 区域存在显著非零值（偏黄/亮色），legs-arm 区域有较弱但非零的耦合（偏蓝/绿色），而 legs 子块近似分块对角（四条腿之间的跨腿耦合很弱）。

71.6 每个子方向的深度案例分析 ⭐⭐¶

前面的方向分类给出了四类系统的基本定位。下面对每个方向进行深度案例分析，覆盖代表性平台、典型任务、控制架构、开源项目和主要瓶颈五个维度。

D1 轮足深度分析：Swiss-Mile ANYmal-W¶

代表性平台硬件规格

Swiss-Mile 基于 ETH Zurich RSL 实验室的 ANYmal 平台改造，在每条腿末端增加了驱动轮。

参数	值
名称	ANYmal-W (Swiss-Mile variant)
腿结构	4 条腿 x 3 关节（HAA, HFE, KFE）
轮	4 个独立驱动轮（每腿末端 1 个）
总主动 DOF	16（12 腿关节 + 4 轮电机）
基座质量	~50 kg（含电池和计算单元）
最大轮式速度	>6 m/s（平坦地面）
最大足式越障高度	~40 cm
传感器	LiDAR + IMU + 深度相机 + 关节编码器
机载计算	NVIDIA Jetson Orin
估计价格	~$150k（研究级）

Unitree B2-W 是商业化替代方案，结构类似但关节数、负载能力和采购价格会随配置变化。学习时更应关注它与 ANYmal-W 共享的轮足控制问题：轮式高效移动、足式越障、以及两种接触模式之间的平滑切换。

典型任务：城市环境末端配送

Swiss-Mile 在 2023 年 Science Robotics 上发表的工作演示了一个极具代表性的任务：在苏黎世城市环境中，机器人需要：

在人行道上以 3-4 m/s 轮式行驶（高效移动）
遇到路缘石时切换为足式模式跨越（高 15-20 cm）
到达目标建筑后在室内窄走廊中以轮式低速行驶
上下 3-5 级台阶时切换为足式爬梯

这个任务链的核心挑战在于**模式切换的时机和平滑性**。何时从轮式切到足式？切换瞬间如何避免速度突变导致的冲击？

控制架构

感知模块: LiDAR + Camera -> 地形分类（平坦/台阶/斜坡/碎石）
                                    | 10 Hz
高层决策: 地形类型 -> 模式选择 (wheel / leg / hybrid)
                                    |
轨迹层:  OCS2 Nonlinear MPC (mode-switching) -> 参考轨迹
         - 代价函数: 速度跟踪 + 能耗最小化 + 平滑约束
         - 约束: Pfaffian（轮模式）+ 接触序列（足模式）
         - 求解频率: 50-100 Hz
                                    |
执行层:  WBC (QP) -> 关节力矩 + 轮速度命令
         - 优先级: 关节限位 > 摩擦锥 > 基座姿态 > 速度跟踪
         - 频率: 400 Hz
                                    |
硬件层:  EtherCAT -> 关节电机（力矩模式）+ 轮电机（速度模式）

核心开源项目

OCS2 (leggedrobotics/ocs2)：提供了 ocs2_switched_model_interface 用于多模态 MPC，是 Swiss-Mile 方法的基础框架
Wheel-Legged-Gym：基于 Isaac Gym 的轮足 RL 训练环境，提供端到端替代方案

当前瓶颈：任意地形上的平滑模式切换

这是轮足方向最难的未解问题。当前方法通常预定义离散的模态集合（pure wheel / pure leg / hybrid），通过有限状态机或高层 RL 选择模态。但真实地形是连续变化的（比如从平坦逐渐过渡到碎石），离散模态切换会在边界处产生振荡（反复在两个模态间跳动）。理想的方案是学习一个连续的"模态混合系数"，使轮和腿的贡献比例随地形连续变化——这是一个活跃的研究方向。

D2 移动操作深度分析：Mobile ALOHA¶

代表性平台硬件规格

参数	值
名称	Mobile ALOHA
底盘	AgileX Tracer（差速轮式底盘）
机械臂	2x ViperX 300s（6-DOF 各臂，12 总）
总主动 DOF	14（2 轮电机 + 12 臂关节）
基座自由度	SE(2)：x, y, yaw
底盘质量	~30 kg（含电池）
臂工作半径	~0.6 m
最大负载	~0.75 kg / 臂
传感器	2x 腕部摄像头 + 1x 顶部摄像头
机载计算	笔记本电脑（用于策略推理）
价格	~$32k (BOM cost)

典型任务：家庭服务操作

Mobile ALOHA (Zhao et al., 2024) 的标志性演示包括：

厨房清洗：走到水槽前，打开水龙头，清洗盘子，关水龙头，放入碗柜
衣物折叠：从洗衣机取出衣物，展开，折叠，放入收纳篮
烹饪协助：从冰箱取出食材，放到砧板上，使用厨具

这些任务的共同特点是**长时域、多步骤、需要双臂协调**。与工业操作不同，家庭环境高度非结构化——物体形状、位置、材质都不确定。

控制架构

Mobile ALOHA 的架构极简，代表了 VLA 端到端路线：

遥操作采集: 人类示教 50-100 个 episode
                    |
策略训练:   ACT (Action Chunking with Transformers)
            - 输入: 3 个摄像头图像 + 当前关节角
            - 输出: 未来 100 步的全身关节角序列（action chunk）
            - 训练: 监督学习，约 4 小时 GPU 训练
                    |
部署推理:   策略网络 @ 4 Hz -> 关节位置命令 @ 50 Hz（插值）
            - 无 MPC，无 WBC，无力控
            - 纯位置控制，末端精度靠策略学习

这个架构的颠覆性在于：完全不需要动力学模型。没有 Pinocchio，没有 OCS2，甚至没有 FK/IK。策略网络直接从像素到关节角。代价是：(1) 需要大量遥操作数据；(2) 泛化能力受限于训练分布；(3) 不能做力敏感任务。

核心开源项目

aloha (tonyzhaozh/aloha)：硬件搭建指南 + 数据采集代码 + ACT 策略训练
lerobot (huggingface/lerobot)：HuggingFace 的统一机器人学习框架，内置 ACT/Diffusion Policy 等多种策略
openpi (Physical-Intelligence/openpi)：pi0 VLA 模型的开源推理框架

当前瓶颈：长时域多步骤任务规划

Mobile ALOHA 在单步操作上表现出色，但面对需要 10+ 步的长时域任务（如"整理整个厨房"），ACT 策略的效果急剧下降。根本原因是行为克隆（BC）无法处理复合误差累积——每一步的微小偏差在后续步骤中被放大。当前的解决方向包括：(1) 层级策略（high-level task planner + low-level motor skill）；(2) 在线纠错（closed-loop replanning）；(3) 扩大训练数据规模（pi0 路线）。这仍是一个开放的研究问题。

D3a 四足+臂深度分析：Go2+Z1 / qm_control¶

代表性平台硬件规格

参数	值
名称	Unitree Go2 + Z1
腿结构	4 腿 x 3 关节 = 12 DOF
臂	Z1 6-DOF
总主动 DOF	18（12 + 6）
基座质量	~15 kg（Go2）+ 4.5 kg（Z1）= 19.5 kg
臂负载能力	3 kg（静态）/ 1 kg（动态行走中）
臂工作半径	~0.65 m
传感器	LiDAR + 深度相机 + IMU + 关节编码器
通信	CycloneDDS (ROS 2) + Unitree SDK
价格	Go2 EDU ~$8.5k + Z1 ~$6.5k = $15k

这是常见的四足+臂研究平台组合。替代选择包括 Go2+ARX5 等成本更低的臂配置；具体预算受采购渠道、传感器、算力和末端执行器影响，文档中不把价格当作技术判断依据。

典型任务：行走中地面物体抓取

核心演示场景（来自 qm_control, Deep WBC, UMI-on-Legs 等工作）：

机器人以 0.5 m/s trot 步态行走
感知系统检测到地面上的目标物体（如一个杯子）
在不停步的情况下，臂末端下降到物体位置并抓取
抬起物体，继续行走到放置点释放

这个任务的难度在于**行走和抓取必须同时进行**。抓取需要臂末端精确到达目标位置（精度要求 2-3 cm），但行走导致基座持续晃动（振幅 3-5 cm），臂必须实时补偿基座运动才能保持末端稳定。

控制架构（qm_control）

感知层:   深度相机 -> 目标物体 6D 位姿
                               | 10-30 Hz
任务层:   末端目标位姿 -> MPC 参考轨迹
                               |
MPC 层:   OCS2 Nonlinear MPC (SQP solver)
          - 状态: [base_pose, joint_angles, joint_velocities] in R^{36+}
          - 决策变量: joint_accelerations + contact_forces
          - 代价: 末端跟踪 + 基座姿态 + 能耗
          - 约束: 动力学 + 摩擦锥 + 关节限位 + 运动学限制
          - 频率: 100 Hz, 预测时域 0.5-1.0 s
                               |
WBC 层:   QP-based Whole-Body Controller
          - 将 MPC 的参考轨迹分解为关节力矩
          - 优先级: 关节限位 > 摩擦锥 > 基座平衡 > EE 跟踪
          - 频率: 500 Hz
                               |
底层:     Unitree SDK -> 关节力矩命令 -> EtherCAT -> 电机

qm_control 的独特之处在于其**四分支 WBC 设计**（对应四种接触模式：四足站立、trot 行走、单臂操作、双任务 loco-manipulation），每种模式有不同的任务优先级配置。复合/170_qm_control精读将对此进行源码级精读。

核心开源项目

qm_control (skywoodsz/qm_control)：基于 OCS2 的四足+臂 MPC+WBC 框架，独立开源项目（非 ETH RSL 出品）
Deep-WBC (MarkFzp/Deep-Whole-Body-Control)：端到端 RL 替代方案，18-DOF 的 teacher-student 训练流程
visual_wholebody (Alescontrela/visual_wholebody)：分层 RL 架构——高层视觉策略输出末端目标，低层运动策略执行
umi-on-legs (real-stanford/umi-on-legs)：将桌面操作策略跨平台迁移到四足+臂上

当前瓶颈：接触丰富场景中的稳定操作

当前大多数工作处理的是"抓取-搬运"类任务——臂与物体的接触是瞬时的、简单的（夹爪闭合）。但真正有价值的工业任务（如拧螺丝、插拔连接器、擦拭表面）涉及**持续的、力敏感的接触**。在这些任务中，臂末端必须同时控制位置和力，而行走引起的基座振动会直接传递到接触力中。当前的 WBC 框架通常只做位置跟踪，不做力控——这是从"能抓住东西"到"能精细操作"的关键技术鸿沟。FALCON (Zhang 2025) 和 SoFTA (Gupta 2025) 正在尝试解决这个问题。

为什么力控如此困难？ 位置控制依赖的信号——关节编码器——精度高、噪声低、延迟小。力控依赖的信号——力/力矩传感器或电流反馈——噪声大、带宽低、且受机械耦合影响。更关键的是，接触刚度会放大位置误差：如果末端与刚性表面接触，0.1 mm 的位置误差可能产生 10 N 的接触力变化。这意味着行走中基座 3-5 mm 的振动会被放大为 30-50 N 的力波动——远超精细操作的容忍范围。解决这个问题需要同时做到三件事：(1) 通过阻抗控制降低等效接触刚度；(2) 通过 WBC 前馈补偿基座振动；(3) 通过传感器融合提高力估计精度。这三者的协同设计是 D3a 方向当前最活跃的研究前沿。

D3b 人形深度分析：Unitree G1 / 全身 RL¶

代表性平台硬件规格

参数	值
名称	Unitree G1 EDU
版本	U2 (29 DOF) / U3 (29 DOF + Dex3 灵巧手)
腿 DOF	2x 6 = 12（hip roll/yaw/pitch + knee + ankle pitch/roll）
腰 DOF	3（roll/pitch/yaw）
臂 DOF	2x 7 = 14（shoulder pitch/roll/yaw + elbow + wrist roll/pitch/yaw）
总主动 DOF	29（不含灵巧手）/ 49（含 Dex3）
身高	1.32 m
体重	~35 kg
最大行走速度	>2 m/s
臂负载能力	3 kg / 臂
传感器	深度相机 + IMU + 关节编码器 + (可选) F/T 传感器
计算	NVIDIA Jetson Orin
价格	U2 ~$52k / U3 ~$64k

典型任务：行走中的上肢操作

人形的典型演示场景：

双足行走到工作台前
识别目标物体（如一瓶水）
伸手抓取并倒入杯中
放下水瓶，端起杯子递给人类

与四足+臂相比，人形的额外挑战是**双足平衡远比四足困难**。四足的支撑多边形面积约 35 x 25 = 875 cm^2，而双足行走中单腿支撑期的有效支撑面积仅约 25 x 10 = 250 cm^2。质心必须时刻保持在这个极小的区域内，否则就会摔倒。

控制架构（当前两大并行路线）

路线一：经典 MPC+WBC（以 NVIDIA GR00T 为代表）

感知:    视觉 -> 目标位姿
               |
运动规划: DCM/LIPM 步态生成 -> 质心轨迹 + 落脚点
               |
MPC:     全身 NMPC（OCS2 或 Crocoddyl）
         - 决策变量: 全身加速度 + 接触力
         - 频率: 50-100 Hz
               |
WBC:     动量-based WBC（Centroidal Dynamics）
         - 优先级: 角动量调节 > CoM 跟踪 > 末端跟踪
         - 频率: 500-1000 Hz
               |
底层:    关节力矩命令

路线二：端到端 RL（以 ASAP 为代表）

仿真训练: IsaacLab + domain randomization
          - 观测: proprioception + (可选) 视觉
          - 动作: 全身关节目标角度（PD 控制）
          - 奖励: 速度跟踪 + 基座稳定 + 能耗 + 末端跟踪
          - Teacher-Student 架构
                    |
Sim-to-Real: delta-action residual（ASAP 的关键创新）
             - 不直接输出绝对关节角，而是输出相对于参考动作的残差
             - 参考动作来自人类动捕或优化轨迹
             - 大幅缩小 sim-to-real gap
                    |
部署:     Student 策略 @ 50 Hz -> PD 控制器 @ 1000 Hz

核心开源项目

unitree_rl_gym (unitreerobotics/unitree_rl_gym)：G1/H1 的 RL 训练基线
ASAP (LeCAR-Lab/ASAP)：代表性人形 sim-to-real 方法
human2humanoid (LeCAR-Lab/human2humanoid)：H2O/OmniH2O 人类遥操作重定向
GR00T-WBC (NVlabs/GR00T-WholeBodyControl)：NVIDIA 的人形全身控制框架

当前瓶颈：40+ DOF 全身优化的实时求解

当 DOF 超过 40（加入灵巧手后），MPC+WBC 路线面临严峻的计算挑战。以 G1 U3 (49 DOF) 为例：

质量矩阵 $\mathbf{M} \in \mathbb{R}^{49 \times 49}$，计算 CRBA 一次约 50 us
WBC QP 有 49 个决策变量 + 数十个约束，求解一次约 200-500 us
要在 1000 Hz 下运行，总预算只有 1000 us

这意味着 MPC+WBC 必须在 500-800 us 内完成一次完整的 sense-plan-act 循环。当前的 QP 求解器在 49 DOF 下勉强可以做到，但几乎没有余量加入更复杂的约束。

RL 路线不受实时性限制（策略推理约 1 ms），但面临 49 维动作空间的探索效率极低的挑战。当前解决方案包括：(1) 分层 RL（上下体分离训练）；(2) 动作空间降维（synergy / PCA）；(3) 从人类动捕初始化策略。

71.7 三层金字塔的数学框架 ⭐⭐⭐¶

运动学层：复合系统的正/逆运动学¶

复合系统的雅可比拼接

对于 Go2+Z1 这样的系统，我们关心多个末端同时的运动学。定义全身雅可比（whole-body Jacobian）：

\[ \mathbf{J}_{\text{whole}} = \begin{pmatrix} \mathbf{J}_{\text{base}} \\ \mathbf{J}_{\text{foot},1} \\ \vdots \\ \mathbf{J}_{\text{foot},4} \\ \mathbf{J}_{\text{EE}} \end{pmatrix} \in \mathbb{R}^{(6+4 \times 3+6) \times 24} \]

其中 $\mathbf{J}_{\text{base}} \in \mathbb{R}^{6 \times 24}$ 是基座雅可比（对于浮动基座，前 6 列为单位矩阵），$\mathbf{J}_{\text{foot},k} \in \mathbb{R}^{3 \times 24}$ 是第 $k$ 个足端的位置雅可比，$\mathbf{J}_{\text{EE}} \in \mathbb{R}^{6 \times 24}$ 是臂末端的 6D（位置+姿态）雅可比。

逆运动学的冗余利用

末端 6D 任务只需要 6 个自由度，但系统有 24 DOF（减去 6 DOF 浮动基座约束后有效 18 DOF），存在巨大的冗余。冗余可以用来：

优化辅助目标（如远离关节限位、最小化能耗）
满足额外约束（如保持基座水平、保持脚在地面上）
避免奇异配置

标准的冗余IK公式利用加权伪逆和零空间投影：

\[ \dot{\mathbf{q}} = \mathbf{J}^{\dagger}_W \mathbf{v}_{\text{task}} + (\mathbf{I} - \mathbf{J}^{\dagger}_W \mathbf{J}) \dot{\mathbf{q}}_0 \]

其中 $\mathbf{J}^{\dagger}_W = \mathbf{W}^{-1}\mathbf{J}^T(\mathbf{J}\mathbf{W}^{-1}\mathbf{J}^T)^{-1}$ 是加权伪逆，$\dot{\mathbf{q}}_0$ 是零空间中的辅助任务速度。权重矩阵 $\mathbf{W}$ 可以用来偏好某些关节（比如让底盘承担大范围移动，让臂做精细调整）。

动力学层：MPC 的统一公式¶

复合机器人的 MPC 在三层金字塔中处于核心位置。其标准离散时间公式如下：

\[ \min_{\mathbf{x}_{0:N}, \mathbf{u}_{0:N-1}} \sum_{k=0}^{N-1} \left[ (\mathbf{x}_k - \mathbf{x}_k^{\text{ref}})^T \mathbf{Q} (\mathbf{x}_k - \mathbf{x}_k^{\text{ref}}) + \mathbf{u}_k^T \mathbf{R} \mathbf{u}_k \right] + (\mathbf{x}_N - \mathbf{x}_N^{\text{ref}})^T \mathbf{Q}_f (\mathbf{x}_N - \mathbf{x}_N^{\text{ref}}) \]

\[ \text{s.t.} \quad \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) \quad \text{(动力学)} $$ $$ \mathbf{x}_0 = \hat{\mathbf{x}} \quad \text{(当前状态初始化)} $$ $$ \mathbf{u}_{\min} \leq \mathbf{u}_k \leq \mathbf{u}_{\max} \quad \text{(力矩限制)} $$ $$ \mathbf{f}_{c,k} \in \mathcal{FC} \quad \text{(摩擦锥约束)} $$ $$ p_{\text{foot},k} = p_{\text{foot}}^{\text{fixed}} \quad \text{if contact} \quad \text{(接触不滑动)} $$ $$ \mathbf{q}_{\min} \leq \mathbf{q}_k \leq \mathbf{q}_{\max} \quad \text{(关节限位)} \]

其中： - $\mathbf{x}_k = (\mathbf{q}_k, \dot{\mathbf{q}}_k)$ 是全身状态 - $\mathbf{u}_k = (\boldsymbol{\tau}_k, \mathbf{f}_{c,k})$ 是关节力矩和接触力 - $\mathcal{FC}$ 是摩擦锥（通常线性化为四面体或八面体近似）：$|f_{c,x}| + |f_{c,y}| \leq \mu f_{c,z}$ - $\mathbf{Q}, \mathbf{R}$ 是状态和控制代价权重矩阵 - $N$ 是预测时域步数，典型值 20-50 步

代价函数中的 $\mathbf{Q}$ 矩阵不是均匀的——复合机器人需要针对不同状态分量设置不同权重。以 Go2+Z1 为例，一个典型的权重分配思路是：

状态分量	$\mathbf{Q}$ 中的权重量级	理由
基座位置 $(x,y,z)$	中（10）	全局定位精度要求适中
基座姿态 (roll, pitch)	高（100）	平衡稳定性的直接指标
基座偏航 (yaw)	中（10）	方向跟踪
腿关节角	低（1）	允许腿灵活调整以补偿
臂末端位姿	高（100-500）	操作任务的精度要求
各关节速度	低-中（1-10）	平滑性约束

不同子方向的 MPC 差异

MPC 组件	D1 轮足	D2 移动操作	D3a 四足+臂	D3b 人形
状态维度	~32	~18	~36	~58
接触模式数	3（轮/足/混合）	1（底盘始终接触）	4-8（步态相位）	4+（步态相位）
特殊约束	Pfaffian 非完整	无	EE 力约束	ZMP/CoP
预测时域	0.5-1.0 s	1.0-2.0 s	0.5-1.0 s	0.5-1.5 s
求解频率	50-100 Hz	10-50 Hz	50-100 Hz	50-100 Hz
典型求解器	SQP (OCS2)	SQP / IPOPT	SQP (OCS2)	SQP / DDP

学习层：RL 观测/动作空间设计¶

当 MPC 因为维度太高或接触模型太复杂而无法实时求解时，RL 提供了替代路线。RL 的关键设计决策在于观测空间和动作空间如何定义。

典型 RL 观测空间（以 Go2+Z1 为例，参考 Deep-WBC 设计）：

观测分量	维度	来源	为什么需要
基座角速度 $\boldsymbol{\omega}_b$	3	IMU	平衡反馈核心信号
重力方向（基座坐标系）	3	IMU 积分	倾斜感知
速度命令 $v_x^{\text{cmd}}, v_y^{\text{cmd}}, \omega_z^{\text{cmd}}$	3	高层策略	运动目标
末端目标位姿 $\mathbf{p}_{\text{EE}}^{\text{cmd}}$	6	任务层	操作目标
关节角 $\mathbf{q}_j$	18	编码器	本体感受
关节角速度 $\dot{\mathbf{q}}_j$	18	编码器微分	动态状态
上一步动作 $\mathbf{a}_{t-1}$	18	策略输出	动作平滑性
总计	69

典型 RL 动作空间选择：

动作定义	维度	策略频率	优点	缺点
关节目标角度（PD）	18	50 Hz	稳定，易训练	不能直接控制力
关节力矩	18	500 Hz	精确力控	难训练，需高频
目标角度残差	18	50 Hz	结合参考动作	依赖参考质量

关节目标角度 + 底层 PD 控制器是全身 RL 中很常用的选择（ASAP、Deep-WBC、FALCON 等工作都采用类似接口），因为 PD 控制器自身提供了阻尼和刚度，使得策略不需要学习低层稳定化。

为什么不直接输出力矩？ 这个问题值得深入理解。力矩输出意味着策略必须在每个控制步都输出物理上合理的力矩——如果策略输出的力矩方向稍有偏差，没有底层 PD 来纠正，关节会立刻偏离安全范围。这就像开车时没有方向盘自回正功能——稍一松手方向就跑偏。PD 控制器就是这个"自回正"机制：策略输出目标角度，PD 自动计算到达目标所需的力矩。即使策略输出有噪声或延迟，PD 也会把关节拉回目标附近。这一层保护在 sim-to-real 中至关重要——仿真中力矩输出可能因为参数差异在真机上产生完全不同的运动，但目标角度 + PD 的组合对参数变化更鲁棒。

反事实推理：如果所有复合机器人都用力矩输出而非 PD 目标，RL 训练的样本效率会下降约 5-10 倍（经验数据），因为策略需要同时学习稳定化和任务完成。更危险的是，力矩策略在 sim-to-real 时更容易因为延迟和摩擦差异而崩溃——一个在仿真中稳定的力矩策略可能在真机上第一步就摔倒。

三层交互：信息如何在金字塔中流动¶

以一个 Go2+Z1 的"行走中抓取"任务为例，三层的信息流如下：

感知 -> 目标物体 6D 位姿
          |
运动学层: IK 检查可达性 -> 判断是否需要移动基座
          |                                    ^
动力学层: MPC 生成全身参考轨迹（含接触时序）    | 反馈: 实际状态
          |                                    ^
学习层:   RL 策略在 MPC 参考基础上输出残差动作  |
          |
执行:     PD 控制器 -> 关节力矩 -> 电机

各层的输入/输出/频率/典型工具对照表：

层	输入	输出	频率	典型工具
运动学层	目标位姿、当前关节角	关节角参考（IK）、雅可比	100-500 Hz	Pinocchio, KDL, TRAC-IK
动力学层	当前状态、目标轨迹、接触时序	参考轨迹、接触力分配	50-100 Hz	OCS2, Crocoddyl, Aligator
学习层	观测向量（proprioception + 视觉）	关节目标角度 / 力矩	20-100 Hz	IsaacLab + rsl_rl, SB3

关键洞察：三层的频率形成了一个层级——运动学最快（可以在控制循环内多次调用），动力学中等（MPC 需要求解优化问题），学习最慢（神经网络推理有延迟）。这自然地形成了"学习提供高层指导 -> MPC 细化轨迹 -> 运动学保证实时执行"的分工。在混合架构中（如 RAMBO），MPC 以 100 Hz 运行提供参考轨迹，RL 以 50 Hz 输出残差修正，PD 以 1000 Hz 执行最终关节命令——三层频率的差异并不矛盾，而是各司其职。

71.8 从 SLAM 到复合机器人的认知跨越 ⭐⭐¶

作为 SLAM 背景的学员进入复合机器人领域，需要完成三个重要的认知跨越。这些跨越不是技术细节的变化，而是**思维模式的根本转换**。

跨越一：从"估计世界"到"改变世界"¶

SLAM 的核心动作是观察：传感器获取数据，算法估计状态（位姿、地图）。整个过程中，机器人不需要与环境产生物理接触——它是一个被动的观察者。SLAM 的输出是一张地图和一条轨迹，但世界本身没有被改变。

复合机器人的核心动作是交互：末端执行器必须与物体产生接触，施加力，改变物体状态。这意味着：

环境不再是静态的——机器人的动作会改变环境状态
感知需要理解物理属性（质量、摩擦、刚度），而非仅仅几何属性
规划必须考虑动作的因果效应——推一个杯子会导致什么结果？

思维转换：从"如何准确知道世界是什么样"转变为"如何让世界变成我想要的样子"。前者是估计问题，后者是控制问题。

跨领域类比：这个跨越类似于从天文学到航天工程的转变。天文学家观测星体运动、估计轨道参数——世界不会因为你的观测而改变。航天工程师必须点火推进、调整姿态——每一个动作都改变了飞行器的状态，而且动作的后果不可撤销。SLAM 工程师在进入复合机器人领域时，最需要建立的心智模型是：你不再是观察者，你是参与者，你的每一个控制命令都会产生物理后果。

跨越二：从"单体感知"到"接触物理"¶

SLAM 不建模接触：在 SLAM 中，机器人在自由空间中运动，障碍物被视为"不可穿透的几何体"。SLAM 算法从不需要知道墙壁的摩擦系数或桌子的刚度。

复合机器人的一切都关于接触：

脚与地面的接触决定了能否行走（摩擦锥约束）
手与物体的接触决定了能否抓取（力封闭条件）
接触力的大小和方向必须被精确控制——太小抓不住，太大压碎物体

数学上的差异：SLAM 的优化问题是无约束或简单约束的（位姿图优化、Bundle Adjustment），而接触问题引入了**互补约束**（complementarity constraints）：

\[ 0 \leq f_n \perp d \geq 0 \]

即法向接触力 $f_n$ 和穿透深度 $d$ 不能同时非零——要么不接触（$d > 0, f_n = 0$），要么接触（$d = 0, f_n > 0$）。这种非光滑约束使得优化问题从 SLAM 中熟悉的光滑非线性最小二乘变成了含互补约束的数学规划（MPCC），求解难度上了一个台阶。

跨越三：从"开环轨迹"到"力闭环"¶

SLAM 的轨迹是开环的：一旦 SLAM 输出了定位结果，导航模块规划一条路径，执行模块跟踪这条路径。即使有闭环（如路径重规划），反馈量仍然是位置/速度——几何量。

复合机器人需要力闭环：在操作任务中，仅仅到达正确的位置是不够的。例如，将一个插头插入插座：

末端必须到达插座的位置（位置控制）
然后施加适当的力将插头推入（力控制）
如果遇到阻力，必须调整方向而非蛮力推入（阻抗控制）

这需要力/力矩传感器的反馈，以及将力信息融入控制回路的能力。SLAM 中的传感器（LiDAR、相机、IMU）全部是非接触式的；复合机器人还需要接触式传感器（F/T 传感器、触觉传感器、关节电流反馈）。

SLAM 技能迁移表¶

好消息是，SLAM 中积累的数学工具和工程经验在复合机器人中有直接的对应物：

SLAM 技能	复合机器人对应	迁移方式
因子图优化（GTSAM）	接触约束优化（QP/NLP）	同为稀疏结构化优化
Bundle Adjustment（Ceres）	轨迹优化（Crocoddyl, IPOPT）	同为非线性最小二乘 / NLP
EKF/UKF 状态估计	浮动基座状态估计	直接复用，增加接触检测
点云处理（PCL/Open3D）	物体 6D 位姿估计	增加分割和匹配
视觉里程计（VO/VIO）	视觉伺服（PBVS/IBVS）	从定位转向控制
占据栅格地图	可操作性地图	从"能不能走"到"能不能够到"
ROS 2 / Nav2	ROS 2 / MoveIt2 / ros2_control	框架共享，替换具体模块
李群李代数（SO(3)/SE(3)）	同群用于刚体运动学	完全相同的数学
协方差传播	不确定性感知操作	同样的概率框架

⚠️ 关键认知：你在 SLAM 中学到的优化方法（Ceres Solver 中的 Levenberg-Marquardt、GTSAM 中的 iSAM2）与复合机器人中的轨迹优化（Crocoddyl 中的 DDP、OCS2 中的 SQP）是**同一个数学框架的不同实例化**。区别仅在于代价函数和约束的具体形式：SLAM 最小化重投影误差和里程计残差，复合机器人最小化跟踪误差和能耗，同时满足接触约束和摩擦锥。如果你能理解 GTSAM 的因子图如何编码优化问题结构，你就能理解 OCS2 的 MPC 如何编码动力学约束。数学能力是完全可迁移的。

迁移路径示例¶

假设你是一个精通 LIO-SAM 的 SLAM 工程师，你的技能如何具体迁移到四足+臂方向（Path C）：

周次	学习内容	复用的 SLAM 技能	新增内容
1-2	复合/10_复合机器人全景统一建模	Pinocchio 类似运动链计算（类比 TF tree）	浮动基座 + 臂
3-4	复合/30_多模态MPC 多模态 MPC	非线性优化（Ceres 类比 OCS2）	接触约束 + 模式切换
5-6	复合/160_四足臂动力学概览 qm_control	ROS 2 系统集成	WBC + 优先级仲裁
7-8	复合/180_Deep_WBC精读 Deep WBC	RL 训练（如果有 IsaacLab 经验）	全身 RL 设计
9-10	复合/200_UMI_on_Legs精读 UMI-on-Legs	视觉处理（点云/深度图）	操作策略迁移

综合练习：从全景到可执行设计¶

练习 4：计算 Go2+Z1 的 24x24 质量矩阵并可视化分块结构（编码 / A 型）¶

"""
练习 4：深入分析质量矩阵的分块结构。

目标：
  1. 加载 Go2+Z1 URDF，在多个配置下计算质量矩阵
  2. 可视化 M 的分块结构，标注 base/legs/arm 边界
  3. 定量分析耦合项随臂配置变化的趋势
  4. 验证：臂越伸展，base-arm 耦合越强

步骤：
  (a) 加载模型，生成 50 个随机配置
  (b) 对每个配置计算 M(q)，提取 ||M_ba|| (base-arm coupling norm)
  (c) 同时记录臂末端到基座的距离
  (d) 绘制散点图：x = 臂伸展距离, y = ||M_ba||
  (e) 预期结果：正相关——臂越伸展，惯性耦合越强
"""
import pinocchio as pin
import numpy as np

# --- 加载模型 ---
# 替换为你的 URDF 路径
model = pin.buildModelFromUrdf("go2_z1.urdf", pin.JointModelFreeFlyer())
data = model.createData()

n_b, n_l, n_a = 6, 12, 6
N_samples = 50
coupling_norms = []
ee_distances = []

# 获取臂末端 frame ID（根据你的 URDF 调整名称）
ee_frame_id = model.getFrameId("z1_link06")

for _ in range(N_samples):
    q = pin.randomConfiguration(model)

    # 质量矩阵
    pin.crba(model, data, q)
    M = data.M.copy()
    M = np.triu(M) + np.triu(M, 1).T

    M_ba = M[:n_b, n_b+n_l:]
    coupling_norms.append(np.linalg.norm(M_ba, 'fro'))

    # 末端距离
    pin.forwardKinematics(model, data, q)
    pin.updateFramePlacements(model, data)
    ee_pos = data.oMf[ee_frame_id].translation
    base_pos = q[:3]
    ee_distances.append(np.linalg.norm(ee_pos - base_pos))

# --- 你的任务：绘制散点图并拟合趋势线 ---
# 预期：coupling_norm 与 ee_distance 正相关
# 物理解释：惯性矩正比于 质量 x 距离^2，
#           臂越伸展，等效惯性越大，耦合越强

自检标准： - 散点图呈现正相关趋势（相关系数 > 0.5） - 能用一句话解释物理原因：惯性矩正比于质量乘以距离的平方，臂越伸展，等效惯性越大 - bonus：找到耦合最强和最弱的配置，画出对应的机器人姿态

练习 5：仿真耦合控制 vs 独立控制的差异（编码 / A 型）¶

"""
练习 5：用 MuJoCo 仿真对比耦合控制与独立控制。

场景：Go2+Z1 站立，臂做正弦运动 q_arm(t) = A * sin(2*pi*f*t)
观察：基座 roll/pitch 角的波动

实验设计：
  (a) 独立控制：腿 PD 控制器只看基座状态，不知道臂在动
  (b) 耦合控制：腿 PD 控制器加入臂运动的前馈补偿项
  (c) 对比两种情况下基座 roll/pitch 的最大偏差

前馈补偿的物理思路：
  臂运动引起的质心偏移 delta_CoM 可以通过 Pinocchio 的
  pin.centerOfMass(model, data, q) 提前计算，
  然后调整腿的目标关节角来补偿这个偏移。

参数设置：
  A = 0.8 rad (臂运动幅度)
  f = 0.5 Hz  (臂运动频率)
  T_sim = 10.0 s (仿真时长)
  dt = 0.002 s (仿真步长)
"""
import mujoco
import numpy as np

# --- 加载 MuJoCo 模型 ---
# model = mujoco.MjModel.from_xml_path("go2_z1.xml")
# data = mujoco.MjData(model)

# --- 你的任务 ---
# 1. 实现独立控制器：腿用固定目标的 PD，臂用正弦轨迹的 PD
# 2. 实现耦合控制器：在独立控制器基础上，加入 CoM 补偿
# 3. 记录基座 roll/pitch 角度时间序列
# 4. 绘制对比图，标注最大偏差
# 
# 预期结果：
#   独立控制：roll 波动 +/- 5-10 deg
#   耦合控制：roll 波动 +/- 1-3 deg
#   提升约 3-5 倍

自检标准： - 能成功运行两种控制器的仿真 - 耦合控制的基座稳定性明显优于独立控制（至少 2 倍改善） - 能定量报告改善倍数，并用物理直觉解释为什么会有改善

练习 6：为 Go2+Z1 操作任务设计 RL 观测空间（设计 / C 型）¶

这是一道开放式设计题。假设你要训练一个 RL 策略，让 Go2+Z1 在 trot 行走中抓取地面上的物体。

(a) 设计观测空间。回答以下问题并论证你的选择：

是否需要包含基座线速度？（提示：IMU 可以直接测量角速度，但线速度需要积分或外部定位。在仿真中容易获取，但 sim-to-real 时可能不可靠）
是否需要包含臂末端位姿？（提示：可以从关节角通过 FK 计算，但增加了计算开销。或者让策略自己隐式学习运动学关系）
是否需要包含目标物体的位置？（提示：如果包含，传感器延迟和噪声如何处理？用特权信息训练 teacher 再蒸馏到 student 是常见做法）
是否需要历史信息？（提示：部分可观测性问题——当前帧可能无法区分"正在加速"和"正在减速"，需要多帧信息或 RNN/Transformer）
观测应该在世界坐标系还是基座坐标系下表示？（提示：基座坐标系下的观测对策略更友好——策略不需要学习坐标变换，泛化性更好）

(b) 设计动作空间。回答以下问题：

输出关节目标角度还是关节力矩？各有什么利弊？
腿和臂的动作频率是否应该相同？（提示：腿需要 50+ Hz 来维持平衡，臂操作 10-20 Hz 可能就够了）
是否需要动作裁剪（action clipping）或动作平滑？为什么？

(c) 设计奖励函数。列出至少 5 个奖励项，并给出权重设计的直觉：

奖励项	物理含义	建议权重量级	理由
速度跟踪	基座速度接近目标	高	行走是基本功能
基座稳定	roll/pitch 接近零	高	不摔倒是前提
末端跟踪	EE 接近目标物体	中-高	任务核心目标
能耗惩罚	关节力矩平方和	低	鼓励平滑但不应主导
存活奖励	每步常数奖励	中	鼓励策略学会不摔倒
接触裕度	足端切向力远离摩擦锥边界	中	抓取时仍要保留支撑安全余量

自检标准： - 观测空间维度在 50-100 之间（太少信息不足，太多难训练） - 能为每个设计决策给出物理层面的理由（不是"别人这么做的"，而是"因为物理上..."） - 奖励函数没有相互矛盾的项（如同时以最高权重奖励"高速移动"和"低能耗"而没有平衡机制） - bonus：阅读 Deep-WBC 或 visual_wholebody 的论文，对比你的设计与他们实际使用的设计，分析异同

本章小结：复合机器人为什么值得单独成章¶

复合机器人不是把“移动底盘、腿足系统、机械臂、视觉模型”并排放在一起。它真正带来的难点，是这些子系统会在同一个闭环里互相改变问题本身：底盘的位置决定机械臂是否够得到，机械臂的姿态改变整机质心，接触状态改变可用力，视觉系统的延迟又改变控制器看到的任务边界。

本质洞察¶

如果只从部件看，复合机器人像是多个成熟模块的组合；如果从控制闭环看，它其实是一个不断改变约束条件的系统。最关键的教学线索不是“有哪些平台”，而是“当一个模块动作时，另一个模块的数学问题如何变化”。

举一个最朴素的例子：固定基座机械臂伸手抓杯子时，基座位置可以当成常量；轮式移动操作平台伸手抓杯子时，底盘位置是可控变量；四足臂伸手抓杯子时，底盘位姿既是可控变量，又受接触力和支撑多边形限制；人形机器人伸手抓杯子时，双足支撑、上肢动量、视觉延迟和全身平衡同时进入同一个决策问题。平台越复杂，越不能把“规划、控制、感知、执行”割裂讲解。

⚠️ 易错点：把复合系统拆成互不通信的模块¶

工程上最常见的失败不是某个模块完全不能用，而是每个模块单独看都能工作，放到一起后边界条件互相冲突：

现象	表面原因	更深层原因
机械臂轨迹可行，但整机后仰	臂太快或负载太大	操作规划没有把质心和支撑裕度纳入约束
底盘到达目标点，但末端够不到	导航目标选错	导航没有以操作可达性为代价
视觉识别正确，但抓取失败	延迟或标定误差	控制器把感知输出当成瞬时真实状态
强化学习策略仿真稳定，实物抖动	域随机化不足	执行器带宽、地面接触和时延没有进入训练分布

💡 学习路径建议¶

阅读本方向时，建议始终把每章放回四个问题里检查：

当前章新增了什么状态变量？
当前章新增了什么约束？
当前章把哪个子系统从“背景条件”提升成了“优化变量”？
当前章最容易在真机上暴露的失败模式是什么？

这样读下去，后续的浮动基座动力学、多模态 MPC、轮足系统、人形全身控制、VLA 移动操作就不会是孤立主题，而是一条逐步扩展的主线。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
系统集成后动作不稳定	坐标系或时间戳不同步	1.打印各模块时间戳差 2.检查 TF tree 是否一致 3.验证 IMU 与关节数据同步性	复合/20_浮动基座臂统一动力学 72.6
规划结果看似可行但执行失败	可达域或支撑裕度不足	1.计算末端目标的 IK 残差 2.检查 CoM 投影是否在支撑多边形内 3.打印摩擦锥裕度	复合/30_多模态MPC 73.3
RL 策略无法迁移到真机	执行器模型 sim-to-real gap	1.比较命令频率和真机 PD 频率 2.检查力矩限幅是否一致 3.对比仿真和真机的延迟	复合/40_RL全身控制基础 74.6
视觉闭环频繁误抓	感知置信度被滥用	1.打印检测置信度分布 2.检查低置信度目标是否仍被当作硬约束 3.加入目标过滤和延迟补偿	复合/50_操作技能接口 75.7
臂伸展后机器人倾倒	质心偏移超出支撑范围	1.用 Pinocchio 计算 CoM 偏移 2.扫描臂姿态绘制 ZMP 轨迹 3.检查负载质量	复合/20_浮动基座臂统一动力学 72.3

跨章综合练习¶

综合练习 7：统一建模 + MPC 权重 + RL 观测的端到端设计（需综合复合/10_复合机器人全景、复合/20_浮动基座臂统一动力学、复合/30_多模态MPC、复合/40_RL全身控制基础知识）

你要为 Go2+Z1 设计一个"行走中伸手触碰指定位置"的任务。要求：

(a) 写出统一广义坐标向量、质量矩阵维度和三类力的显式形式（复合/10_复合机器人全景 71.5 + 复合/20_浮动基座臂统一动力学 72.2）。

(b) 为 MPC 设计代价函数权重分配表，要求 locomotion 安全权重至少比 manipulation 跟踪权重高一个数量级（复合/30_多模态MPC 73.1）。解释为什么不能让两者权重相同。

(c) 为 RL 低层策略设计观测空间（不超过 90 维），明确每个观测项的坐标系、部署来源和是否属于特权信息（复合/40_RL全身控制基础 74.3）。

(d) 预测三种可能的失败模式，并为每种给出排查步骤。

自检标准：四个子问题的答案之间应保持一致——MPC 的状态维度应与统一建模匹配，RL 的观测维度应与 MPC 的状态/输入对应，失败模式应来自前面三个设计选择中的具体弱点。完成后将你的设计与 qm_control 或 Deep-WBC 的实际实现对比，记录设计差异并分析原因。

练习¶

A 型：任选一个移动操作任务（例如从书架上取书、将垃圾放入垃圾桶、为桌面擦拭消毒等），用”状态、输入、约束、代价”四类变量重新描述它，不允许只写模块名称。每类变量至少列出 3 个具体项，并标注维度和单位。

B 型：解释为什么“先导航到物体附近，再运行固定基座抓取”在狭窄空间中经常失败。要求从可达域、碰撞、视角和底盘姿态四个角度分析。

C 型：设计一个最小复合机器人实验：移动底盘携带 2-DoF 小臂抓取桌面物体。写出你会记录的 10 个信号，并说明每个信号用于诊断哪类问题。提示：信号应覆盖运动学（关节角、末端位姿）、动力学（力矩、接触力）和系统（延迟、通信状态）三个维度。

子块	维度（Go2+Z1）	物理含义
\(\mathbf{M}_{bb}\)	\(6 \times 6\)	基座的有效惯性（包含所有连接体的反映惯性）
\(\mathbf{M}_{bl}\)	\(6 \times 12\)	基座-腿耦合：腿运动对基座的惯性效应
\(\mathbf{M}_{ba}\)	\(6 \times 6\)	基座-臂耦合：臂运动对基座的惯性效应
\(\mathbf{M}_{ll}\)	\(12 \times 12\)	四条腿自身的惯性（近似分块对角，四腿在运动学上是并联的）
\(\mathbf{M}_{la}\)	\(12 \times 6\)	腿-臂耦合：通过基座的间接耦合
\(\mathbf{M}_{aa}\)	\(6 \times 6\)	臂自身的惯性

状态分量	\(\mathbf{Q}\) 中的权重量级	理由
基座位置 \((x,y,z)\)	中（10）	全局定位精度要求适中
基座姿态 (roll, pitch)	高（100）	平衡稳定性的直接指标
基座偏航 (yaw)	中（10）	方向跟踪
腿关节角	低（1）	允许腿灵活调整以补偿
臂末端位姿	高（100-500）	操作任务的精度要求
各关节速度	低-中（1-10）	平滑性约束

观测分量	维度	来源	为什么需要
基座角速度 \(\boldsymbol{\omega}_b\)	3	IMU	平衡反馈核心信号
重力方向（基座坐标系）	3	IMU 积分	倾斜感知
速度命令 \(v_x^{\text{cmd}}, v_y^{\text{cmd}}, \omega_z^{\text{cmd}}\)	3	高层策略	运动目标
末端目标位姿 \(\mathbf{p}_{\text{EE}}^{\text{cmd}}\)	6	任务层	操作目标
关节角 \(\mathbf{q}_j\)	18	编码器	本体感受
关节角速度 \(\dot{\mathbf{q}}_j\)	18	编码器微分	动态状态
上一步动作 \(\mathbf{a}_{t-1}\)	18	策略输出	动作平滑性
总计	69