第86章：四足+臂动力学与控制概览——ALMA → Sleiman → qm_control

元信息	值
难度	⭐⭐⭐（统一动力学、质心动量、接触切换、任务优先级）
预计时间	1 周，20-25 小时
前置依赖	足式/70_腿足简化模型理论、足式/90_WBC分层优化与TSID、足式/190_腿足RL训练栈、复合/20_浮动基座臂统一动力学、复合/30_多模态MPC
下游章节	复合/170_qm_control精读、复合/180_Deep_WBC精读、复合/190_Visual_WBC精读、复合/200_UMI_on_Legs精读、复合/210_RAMBO混合MPC_RL
本章主线	为什么四足+臂不能拆成"腿控+臂控"，以及怎样把质心、角动量、接触力、末端任务放进同一个控制问题

---

86.0 前置自测

在开始本章前，请先独立回答下面五个问题。

如果有两个以上答不出来，建议先回到对应前置章节重新阅读。

#	问题	对应前置
1	SRBD 模型的平动方程和转动方程分别是什么？接触力为什么常被选为 MPC 输入？	足式/70_腿足简化模型理论
2	WBC 的标准逆动力学 QP 中，决策变量通常包含哪些量？为什么浮动基座不能直接施加关节力矩？	足式/90_WBC分层优化与TSID
3	CMM \(\mathbf{A}_G(q)\) 满足什么关系？臂关节速度为什么会改变全身角动量？	复合/20_浮动基座臂统一动力学
4	SE(3) 末端误差为什么要用 \(\log(T_{ref}^{-1}T)\)，而不是位置误差加欧拉角误差？	复合/30_多模态MPC
5	RL 策略部署时，哪些观测可以来自真机，哪些只能作为训练时的特权信息？	足式/190_腿足RL训练栈

自测标准：

正确题数	建议
5	可以直接阅读本章，并把重点放在跨模型联系上
3-4	可以阅读，但推导处需要对照前置章节
0-2	先补 SRBD、WBC、CMM、SE(3) 误差和 RL 观测设计

---

86.1 本章目标

学完本章后，你应该能完成四件事。

1. 说清楚四足+臂为什么不是"四足控制器 + 机械臂控制器"的简单拼接。
2. 从虚功和浮动基座动力学出发，写出统一方程：

\[ \mathbf{M}(q)\dot{\mathbf{v}}+\mathbf{h}(q,\mathbf{v}) = \mathbf{S}^{T}\boldsymbol{\tau} + \sum_{i\in C_{foot}}\mathbf{J}_{c_i}^{T}\boldsymbol{\lambda}_i + \mathbf{J}_{ee}^{T}\mathbf{f}_{ee} \]

1. 解释质心、角动量、接触力和末端任务如何在 MPC/WBC 中相互耦合。
2. 建立 ALMA、Sleiman Unified MPC、Sleiman Versatile Multi-Contact、qm_control 四条线索之间的技术关系。

本章不是某个单一项目的源码精读。

本章是后续四足臂方向的"坐标系"。

后面的 qm_control、Deep-WBC、Visual-WBC、UMI-on-Legs、RAMBO 都会反复使用本章的变量、约束和接口。

---

86.1B 知识导航

在动手推导之前，先看清楚整章的骨架。本章要解决的根问题只有一句话：当一个浮动基座系统同时长了"会走路的腿"和"会干活的手"，怎样把质心、角动量、接触力、末端任务塞进同一个数学框架里，并据此设计控制器。

整章围绕这个根问题展开为一棵知识树。树根是"为什么不能拆"（86.2），树干是统一动力学方程（86.3）及其两条派生主线——质心动量（86.4）与末端 wrench（86.5），树枝则是把这套理论落到历史脉络（86.6）、架构选型（86.7）、最小实现（86.8）、平台选型（86.9）和评估（86.10）。

小节	知识点	难度	在知识树上的位置
86.2	为什么腿控+臂控不能简单拼接；三层控制栈；四条技术路线	⭐	树根：问题定位
86.3	浮动基座+臂的统一拉格朗日/牛顿-欧拉方程；三类广义力；虚功；质量矩阵三分块	⭐⭐⭐	树干：中心方程
86.3B	接触约束下的约化动力学；基座行消去；约化质量矩阵与零空间投影	⭐⭐⭐⭐	树干的内部结构：方程在接触下如何坍缩
86.4	CMM 复合动量；动量变化率与外力；ZMP 偏移；臂作为动量调节器；任务优先级	⭐⭐⭐	主枝一：动量视角
86.4B	whole-body 雅可比分块；臂运动对基座的反作用；动量耦合的闭式推导	⭐⭐⭐⭐	主枝一的细胞结构：耦合从哪来
86.5	被动扰动 vs 主动接触；接触集合扩展；末端摩擦锥；阻抗模式	⭐⭐⭐	主枝二：外力视角
86.6	Khatib→Sentis→ALMA→Sleiman→qm_control 的历史脉络	⭐⭐	分枝：来龙去脉
86.7	分层 WBC vs 统一 MPC；加权 QP/HQP/混合优先级；选型流程	⭐⭐⭐	分枝：范式取舍
86.8	最小加权 WBC 实现；SE(3) 任务；支撑裕度监控	⭐⭐	分枝：动手
86.9-86.10	平台/仿真选型；评估指标	⭐⭐	叶：工程落地

概念之间的因果链：86.2 提出"不能拆"的论断，但这只是定性结论；86.3 用统一动力学方程把"不能拆"变成可计算的事实（臂加速度通过 \(\mathbf{M}_{ba}\) 进入基座方程，末端力通过 \(\mathbf{J}_{ee}^T\) 前 6 列进入基座方程）；86.3B 进一步说明，在接触约束下这个方程会坍缩成一个更小的"约化动力学"，而坍缩过程恰好揭示了基座为何不可直接驱动；86.4 换一个视角，把同一套耦合压缩成 6 维质心动量，让 MPC 可以实时；86.4B 则反过来打开 86.4 的黑箱，用雅可比分块解释"臂动一下基座为什么会反向动"。后半章（86.6-86.10）都是在这个核心之上做架构、工程和评估的选择。

三条阅读路径：

路径	适合谁	建议顺序
精读（20-25 h）	要做四足臂控制研究	86.2 → 86.3 → 86.3B → 86.4 → 86.4B → 86.5 → 86.6 → 86.7 → 86.8
速读（6-8 h）	已懂 WBC，只想建立四足臂坐标系	86.2 → 86.3 → 86.4 → 86.5 → 86.7
速查（1-2 h）	临时查公式或选型	86.11 速查表 → 对应小节 → 故障排查手册

本质洞察：这一章的所有小节，本质上都在回答同一个问题的不同侧面——"臂和腿共享同一个身体"。86.3 从力的角度回答（同一个动力学方程），86.4 从动量的角度回答（同一个质心动量矩阵），86.5 从接触的角度回答（同一个接触集合），86.7 从优化的角度回答（同一个 QP/OCP）。如果你只能记住一句话，就是：四足臂的所有困难，都来自"一个身体、两类任务"这一个事实。

---

86.2 全景地图：四足臂控制的三层问题 ⭐

这一节解决一个定位问题：四足臂系统到底比纯四足系统多了哪些变量、目标和失败模式。

回顾足式/70：纯四足的规划层通常只关心质心、基座姿态和足端接触力。

回顾足式/90：WBC 把这些参考转换为满足全身动力学、摩擦锥和力矩限幅的关节力矩。

加入机械臂后，系统不只是多了 6 个关节。

机械臂把"操作任务"、"外部接触"、"载荷变化"和"角动量调节"全部带进了浮动基座系统。

86.2.1 从纯四足到四足臂的变量增量

项目	纯四足 Go2	Go2+Z1 / A1+WidowX	增量的含义
浮动基座速度	6	6	仍然不可驱动，只能通过接触力间接控制
腿关节	12	12	仍然承担支撑、摆动和地面反力生成
臂关节	0	6 或 7	新增操作任务、新增惯性和反力矩
末端任务	无	3D/6D pose 或 wrench	目标从"走到哪里"变成"边走边做事"
接触集合	足端	足端 + 末端	手也可能成为接触点，且摩擦/刚度不同
状态估计	基座+腿	基座+腿+臂+物体	臂运动会影响 IMU 和质心估计
安全约束	摩擦锥、力矩限	另加自碰撞、工作空间、末端力	约束冲突更常见

四足臂控制的第一条原则是：

本质洞察：机械臂不是挂在四足机器人上的"外设"。 它是全身动量方程的一部分，也是接触集合的一部分。 只要臂有质量、有速度、能接触外界，它就会改变质心、角动量、足底反力和可行支撑裕度。

86.2.2 为什么不能简单拼接腿控和臂控

设腿控输出期望接触力 \(\lambda_{foot}^{leg}\)，臂控输出期望关节力矩 \(\tau_a^{arm}\)。

朴素拼接会写成：

\[ \boldsymbol{\tau} = \begin{bmatrix} \boldsymbol{\tau}_{leg}^{leg} \\ \boldsymbol{\tau}_{arm}^{arm} \end{bmatrix} \]

这个式子看起来合理，但它隐含了三个错误假设。

错误假设	为什么错	真实后果
腿和臂动力学独立	质量矩阵不是分块对角，臂加速度会进入基座方程	臂快速摆动时基座 roll/pitch 变大
足端接触力不受末端力影响	末端 wrench 通过机身传到足底	推门时某些脚法向力下降，摩擦裕度变小
末端位置误差只由臂关节决定	末端 pose 同时受基座和腿姿态影响	机身晃动会直接变成末端误差

可以用一个反事实实验理解。

让 Go2+Z1 原地站立，腿控制器只维持基座水平，臂控制器让末端在 0.5 秒内向前伸出 0.4 m。

如果两者独立工作，腿控制器不知道臂的惯性变化，臂控制器不知道足底摩擦裕度。

结果通常不是"手更快到达目标"。

结果是基座前倾、前脚法向力增大、后脚法向力下降，末端轨迹还会因为基座俯仰而偏离。

如果末端同时推门，门的反力会通过 \(\mathbf{J}_{ee}^{T}\mathbf{f}_{ee}\) 进入全身动力学。

这时独立腿控甚至不知道自己为什么突然需要更大的后脚摩擦力。

86.2.3 三层控制栈

四足臂常见控制栈可以画成下面的结构。

┌────────────────────────────────────────────────────────────┐
│ Level 3: Task / Perception / Human Command  1-20 Hz        │
│ 输入: 物体位姿、目标命令、视觉地图、操作阶段                 │
│ 输出: base 速度、EE 轨迹、接触模式、任务权重                 │
└──────────────────────────┬─────────────────────────────────┘
                           │
                           ▼
┌────────────────────────────────────────────────────────────┐
│ Level 2: MPC / RL Planner  20-100 Hz                       │
│ 模型: SRBD、centroidal、kino-dynamic、或训练策略             │
│ 输出: CoM / h_G / contact force / EE reference              │
└──────────────────────────┬─────────────────────────────────┘
                           │
                           ▼
┌────────────────────────────────────────────────────────────┐
│ Level 1: WBC / Impedance / Joint Servo  200-1000 Hz         │
│ 模型: 全身动力学 + 接触 Jacobian + 摩擦锥 + 力矩限           │
│ 输出: 关节力矩、关节位置目标、阻抗参数                       │
└────────────────────────────────────────────────────────────┘

这一层级与足式控制相似，但接口明显更复杂。

接口	纯四足	四足臂
高层目标	base velocity、gait	base velocity + EE pose/wrench + object state
MPC 输出	CoM、GRF、足端轨迹	CoM、GRF、EE 轨迹、物体/末端接触力
WBC 任务	base、foot、posture	base、foot、EE、arm posture、collision
低层输出	12 个腿关节命令	18/19 个全身关节命令

86.2.4 四条技术路线

路线	代表	核心思想	优势	主要代价
ZMP + WBC	ALMA 2019	规划 CoM/ZMP 与 EE，再由分层 QP 执行	可解释，适合准静态操作	动态步态和多接触扩展困难
Unified MPC	Sleiman 2021	单一多接触 OCP 规划 locomotion + manipulation	统一、约束清晰	OCP 构造和求解复杂
开源 MPC+WBC	qm_control 2024	OCS2 NMPC + 分层 WBC + 四分支工程实现	可复现、适合教学	依赖重、配置复杂
RL WBC	Deep-WBC/VBC/UMI	单策略或分层策略直接学习全身动作	鲁棒、可处理复杂非线性	安全约束和可解释性弱

这一章重点讲前三条，后两条会在 180、190 和后续章节展开。

⚠️ 常见陷阱

⚠️ 概念误区：把机械臂看成独立子系统

错误做法：腿控制器只看 base，臂控制器只看末端，两者通过 ROS topic 粗略通信。

现象：站立时能用，行走中末端误差变大；推门、搬物时支撑脚突然打滑。

根本原因：臂的惯性、末端外力和基座姿态全部进入同一个浮动基座动力学方程。

正确做法：至少在 WBC 中统一求解动力学；更进一步，在 MPC 中预测末端任务对质心和接触力的影响。

⚠️ 工程陷阱：只增加 EE tracking cost，不调整 locomotion 权重

错误做法：在纯四足 MPC 中直接加 \(\|T_{ee}-T_{ref}\|^2\)，其余权重不变。

现象：手能靠近目标，但机身为了追手而倾斜，支撑裕度下降。

根本原因：末端任务与基座稳定不是同类目标，不能靠随意权重表达安全优先。

正确做法：locomotion 相关约束使用硬约束或高优先级，EE tracking 使用受支撑裕度调度的软任务。

🧠 思维陷阱：认为统一模型一定优于分层模型

实际情况：统一模型理论上更完整，但实时性、建模误差和调试成本更高。

分层模型在工程中常更鲁棒，因为 MPC 失败时 WBC 仍可使用旧参考维持站立。

选择架构时要问控制周期、任务接触复杂度、硬件力控能力和可调试性，而不是只问模型是否最完整。

练习 86.2

#	练习	难度
1	画出 Go2+Z1 的三层控制栈，标注每层频率、输入、输出和失败时的降级策略。	⭐
2	设臂末端沿 \(x\) 方向施加 20 N 推力，质心高度 0.35 m，估算对 ZMP 的水平偏移量。	⭐⭐
3	对比 ALMA、Unified MPC、qm_control、Deep-WBC：为"边走边推门"任务选择一种架构，并写出选择理由。	⭐⭐⭐

---

86.3 统一动力学：从虚功到全身方程 ⭐⭐⭐

这一节推导本章最核心的方程。

推导目标不是记住公式，而是理解每一项为什么必须出现。

回顾复合/20：浮动基座系统的广义坐标不是普通欧氏向量。

基座位姿属于 \(SE(3)\)，速度使用 6 维 twist 表示。

因此本章统一使用 \(\mathbf{v}\) 表示广义速度，而不是简单写 \(\dot{\mathbf{q}}\)。

86.3.1 广义坐标和速度

对一个四足+6 DoF 臂系统，定义：

\[ \mathbf{q} = \begin{bmatrix} \mathbf{q}_b \\ \mathbf{q}_{\ell} \\ \mathbf{q}_{a} \end{bmatrix}, \quad \mathbf{v} = \begin{bmatrix} \mathbf{v}_b \\ \dot{\mathbf{q}}_{\ell} \\ \dot{\mathbf{q}}_{a} \end{bmatrix} \]

其中：

符号	维度	物理含义
\(\mathbf{q}_b\)	7	浮动基座位置 + 四元数
\(\mathbf{v}_b\)	6	浮动基座线速度 + 角速度
\(\mathbf{q}_{\ell}\)	12	四条腿各 3 个关节
\(\mathbf{q}_{a}\)	6	机械臂关节
\(\mathbf{v}\)	24	全身速度变量

选择矩阵写为：

\[ \mathbf{S} = \begin{bmatrix} \mathbf{0}_{18\times 6} & \mathbf{I}_{18} \end{bmatrix} \]

所以 \(\mathbf{S}^{T}\tau\) 在前 6 维为零。

这不是实现细节，而是浮动基座机器人的物理事实。

基座没有电机直接施加广义力。

基座只能通过脚和手与外界接触获得力。

86.3.2 动能、势能和 Lagrange 形式

系统总动能为所有刚体动能之和：

\[ T(q,\mathbf{v}) = \frac{1}{2}\mathbf{v}^{T}\mathbf{M}(q)\mathbf{v} \]

势能为：

\[ V(q) = \sum_{i=1}^{N} m_i g z_i(q) \]

Lagrangian 为：

\[ L(q,\mathbf{v}) = T(q,\mathbf{v})-V(q) \]

在流形坐标中直接展开 Euler-Lagrange 会比较繁琐。

工程上更常用 Featherstone 空间向量或 Pinocchio 的递推算法。

但结论形式一致：

\[ \mathbf{M}(q)\dot{\mathbf{v}}+\mathbf{h}(q,\mathbf{v}) = \boldsymbol{\tau}_{gen} \]

其中：

项	含义
\(\mathbf{M}(q)\)	全身质量矩阵
\(\mathbf{h}(q,\mathbf{v})\)	科氏、离心、重力等非线性项
\(\boldsymbol{\tau}_{gen}\)	所有广义力之和

86.3.3 三类广义力

四足臂系统中的广义力分成三类。

第一类是电机力矩：

\[ \boldsymbol{\tau}_{motor} = \mathbf{S}^{T}\boldsymbol{\tau} \]

第二类是足端接触力：

\[ \boldsymbol{\tau}_{foot} = \sum_{i\in C_{foot}}\mathbf{J}_{c_i}^{T}(q)\boldsymbol{\lambda}_{i} \]

第三类是末端扳手：

\[ \boldsymbol{\tau}_{ee} = \mathbf{J}_{ee}^{T}(q)\mathbf{f}_{ee} \]

把三类相加得到：

\[ \boxed{ \mathbf{M}(q)\dot{\mathbf{v}}+\mathbf{h}(q,\mathbf{v}) = \mathbf{S}^{T}\boldsymbol{\tau} + \sum_{i\in C_{foot}}\mathbf{J}_{c_i}^{T}(q)\boldsymbol{\lambda}_{i} + \mathbf{J}_{ee}^{T}(q)\mathbf{f}_{ee} } \]

这个方程是四足臂 WBC 的中心。

它告诉我们：脚、手、电机不是三个独立控制器的输出。

它们是同一个全身动力学方程右端的不同广义力来源。

86.3.4 用虚功解释 \(J^T f\)

为什么末端外力要乘以 \(\mathbf{J}_{ee}^{T}\)？

从虚功出发最直观。

末端发生一个微小位移：

\[ \delta \mathbf{x}_{ee} = \mathbf{J}_{ee}(q)\delta \mathbf{q} \]

末端力做功：

\[ \delta W = \mathbf{f}_{ee}^{T}\delta \mathbf{x}_{ee} = \mathbf{f}_{ee}^{T}\mathbf{J}_{ee}\delta \mathbf{q} = (\mathbf{J}_{ee}^{T}\mathbf{f}_{ee})^{T}\delta \mathbf{q} \]

根据虚功原理，广义力 \(\boldsymbol{\tau}_{ee}\) 满足：

\[ \delta W = \boldsymbol{\tau}_{ee}^{T}\delta \mathbf{q} \]

所以：

\[ \boldsymbol{\tau}_{ee} = \mathbf{J}_{ee}^{T}\mathbf{f}_{ee} \]

这个推导同样适用于足端接触力。

区别只是足端接触力通常是优化变量，而末端扳手可能是任务目标、外部扰动或接触约束的一部分。

86.3.5 质量矩阵的三分块

将速度按基座、腿、臂分块：

\[ \mathbf{v} = \begin{bmatrix} \mathbf{v}_b \\ \mathbf{v}_{\ell} \\ \mathbf{v}_{a} \end{bmatrix} \]

质量矩阵可写成：

\[ \mathbf{M} = \begin{bmatrix} \mathbf{M}_{bb} & \mathbf{M}_{b\ell} & \mathbf{M}_{ba} \\ \mathbf{M}_{\ell b} & \mathbf{M}_{\ell\ell} & \mathbf{M}_{\ell a} \\ \mathbf{M}_{ab} & \mathbf{M}_{a\ell} & \mathbf{M}_{aa} \end{bmatrix} \]

其中最容易被忽略的是 \(\mathbf{M}_{ba}\) 和 \(\mathbf{M}_{ab}\)。

它们表达臂运动与基座动量之间的惯性耦合。

如果臂质量很小、运动很慢，这些块可以近似忽略。

如果臂携带重物、快速摆动或进行动态接触，这些块会直接影响基座稳定。

分块	工程含义	典型影响
\(\mathbf{M}_{bb}\)	基座整体惯性	影响姿态加速度
\(\mathbf{M}_{b\ell}\)	腿摆动对基座的影响	快速摆腿引起姿态扰动
\(\mathbf{M}_{ba}\)	臂运动对基座的影响	搬物、甩臂、推门时明显
\(\mathbf{M}_{\ell a}\)	腿臂之间的关节耦合	自碰撞和力矩分配相关
\(\mathbf{M}_{aa}\)	臂自身惯性	影响末端加速度和扭矩需求

86.3.6 Pinocchio 计算框架

下面代码展示如何在 C++ 中计算统一动力学所需的核心量。

#include <pinocchio/fwd.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <Eigen/Dense>
#include <iostream>

struct QuadArmDynamics {
  pinocchio::Model model;
  pinocchio::Data data;
  pinocchio::FrameIndex ee_frame;
  std::array<pinocchio::FrameIndex, 4> foot_frames;

  explicit QuadArmDynamics(const std::string& urdf)
      : model(), data(model) {
    // 使用 FreeFlyer 表示浮动基座，这是四足和四足臂建模的关键。
    pinocchio::urdf::buildModel(
        urdf, pinocchio::JointModelFreeFlyer(), model);
    data = pinocchio::Data(model);
    ee_frame = model.getFrameId("ee_link");
    foot_frames = {
        model.getFrameId("FL_foot"),
        model.getFrameId("FR_foot"),
        model.getFrameId("RL_foot"),
        model.getFrameId("RR_foot")};
  }

  void compute(const Eigen::VectorXd& q,
               const Eigen::VectorXd& v) {
    // CRBA 计算质量矩阵；Pinocchio 为效率只填上三角。
    pinocchio::crba(model, data, q);
    data.M.triangularView<Eigen::StrictlyLower>() =
        data.M.transpose().triangularView<Eigen::StrictlyLower>();

    // RNEA 在零加速度下返回 h(q,v)=C(q,v)v+g(q)。
    Eigen::VectorXd zero = Eigen::VectorXd::Zero(model.nv);
    Eigen::VectorXd h = pinocchio::rnea(model, data, q, v, zero);

    // 计算所有关节雅可比和 frame 位姿。
    // Pinocchio 3.x 中 framesForwardKinematics 已并入 forwardKinematics；
    // 调用 forwardKinematics 后再调用 updateFramePlacements 即可更新所有 frame。
    pinocchio::computeJointJacobians(model, data, q);
    pinocchio::forwardKinematics(model, data, q);
    pinocchio::updateFramePlacements(model, data);

    Eigen::Matrix<double, 6, Eigen::Dynamic> J_ee(6, model.nv);
    J_ee.setZero();
    pinocchio::getFrameJacobian(
        model, data, ee_frame, pinocchio::LOCAL_WORLD_ALIGNED, J_ee);

    std::cout << "nv = " << model.nv << "\n";
    std::cout << "M size = " << data.M.rows() << " x " << data.M.cols() << "\n";
    std::cout << "h norm = " << h.norm() << "\n";
    std::cout << "J_ee rank estimate = "
              << Eigen::FullPivLU<Eigen::MatrixXd>(J_ee).rank() << "\n";
  }
};

这段代码与纯四足最大的不同不是 model.nv 变大。

真正的差别是 J_ee 的前 6 列通常不为零。

这意味着移动基座会改变末端位姿，末端力也会反作用到浮动基座。

反事实推理：如果 \(\mathbf{J}_{ee}\) 的前 6 列（基座列）全为零会怎样？ 这意味着末端位姿完全不受基座运动影响——就像臂安装在地面上一样。 此时腿控和臂控确实可以独立运行，因为末端力矩不会传到浮动基座。 但在真实四足臂中，这 6 列只有在臂末端恰好位于基座固定点（零力臂）时才近似为零。 只要臂伸出、末端偏离基座正上方，前 6 列就非零，耦合就不可忽略。 这也是为什么"原地站立、臂收拢"时独立控制可行，但"行走、臂伸展"时必须统一优化的物理原因。

回顾足式/30_Pinocchio深度精读中计算 frame Jacobian 的方法：在那里我们用 getFrameJacobian() 计算足端雅可比来构建接触约束。现在同样的函数被用于末端雅可比 \(\mathbf{J}_{ee}\)，但用途不同——在 WBC 中，足端雅可比进入接触不滑约束，而末端雅可比进入操作任务或末端力映射。同一个工具在不同上下文中服务于完全不同的控制目标。

⚠️ 常见陷阱

⚠️ 编程陷阱：忽略 CRBA 的对称化

错误做法：直接使用 data.M 的全部元素。

现象：下三角接近零，求逆或分解结果错误。

根本原因：Pinocchio crba() 为性能只填充上三角。

正确做法：显式把上三角拷贝到下三角，再做矩阵运算。

💡 概念误区：认为末端力只影响臂关节

实际上 \(\mathbf{J}_{ee}\) 包含浮动基座列、腿列和臂列。

所以 \(\mathbf{J}_{ee}^{T}\mathbf{f}_{ee}\) 的前 6 维是末端力对浮动基座的广义力。

如果末端推门，反力会通过机身传到足底接触力分配。

🧠 思维陷阱：把模型维度增长只看成计算问题

维度增长当然会让 QP 更大。

但更重要的是约束结构改变了。

臂让系统新增了末端任务、自碰撞、外部扳手、载荷变化和操作接触。

这些新增项会改变可行域，而不是只增加几列矩阵。

练习 86.3

#	练习	难度
1	用 Pinocchio 加载一个四足+臂 URDF，打印 \(n_q/n_v\)，并解释二者为什么不同。	⭐
2	提取 \(\mathbf{M}_{ba}\)，让臂从收拢到伸展，画出 \(\|\mathbf{M}_{ba}\|_F\) 曲线。	⭐⭐
3	对末端施加一个 6D wrench，计算 \(\mathbf{J}_{ee}^{T}f_{ee}\) 的前 6 维，并解释其物理意义。	⭐⭐⭐

---

86.3B 接触约束下的约化动力学：方程在脚不滑时如何坍缩 ⭐⭐⭐⭐

上一节得到了 24 维的统一动力学方程，它把脚、手、电机的广义力放在了同一个右端。但这个方程有一个让人不安的地方：未知量比方程多。

未知量包括广义加速度 \(\dot{\mathbf{v}}\in\mathbb{R}^{24}\)、足端接触力 \(\boldsymbol{\lambda}\in\mathbb{R}^{3k}\)、关节力矩 \(\boldsymbol{\tau}\in\mathbb{R}^{18}\)，而动力学方程只有 24 行。直接求解是欠定的。这一节解决的问题就是：当脚踩在地上不滑动时，这个看似欠定的系统其实被接触约束"锁"住了一部分自由度，方程会坍缩成一个更小、更干净的形式——约化动力学。 理解这个坍缩过程，是理解"基座为什么不可直接驱动""WBC 为什么要分块"的关键。

86.3B.1 动机：欠定方程与接触约束的配对

回顾 86.3 的统一方程（这里重写一遍以便不翻页）：

\[ \mathbf{M}(q)\dot{\mathbf{v}}+\mathbf{h}(q,\mathbf{v}) = \mathbf{S}^{T}\boldsymbol{\tau} + \mathbf{J}_{c}^{T}(q)\boldsymbol{\lambda} \tag{86.3B-1} \]

为了聚焦接触消去，这里先把末端 wrench 并入广义外力或暂设为零，只保留足端接触力 \(\boldsymbol{\lambda}\)；末端接触会在 86.5 作为接触集合的扩展统一处理。

这个方程的"病"在于：我们既不知道加速度，也不知道接触力。要让它可解，必须补上额外的物理事实。这个事实就是 86.5.2 里写过的**接触不滑约束**——支撑脚相对地面既不平移也不（在点接触下）产生切向滑移，因此支撑脚的空间加速度为零：

\[ \mathbf{J}_{c}\dot{\mathbf{v}}+\dot{\mathbf{J}}_{c}\mathbf{v}=\mathbf{0} \tag{86.3B-2} \]

本质洞察：接触约束不是"附加"在动力学方程上的额外条件，而是补全方程的另一半。动力学（86.3B-1）描述"力怎么产生加速度"，接触约束（86.3B-2）描述"哪些加速度被地面禁止了"。只有把两者配成一对，欠定系统才变成可解系统。这正是为什么 86.3 的方程单独看永远求不出唯一解——它天生就缺一半。

86.3B.2 反面：如果忽略接触约束会怎样

设想我们偷懒，直接对 86.3B-1 求广义逆，把 \(\boldsymbol{\lambda}\) 当成普通未知量和 \(\boldsymbol{\tau}\) 一起解。会发生三件事。

第一，解不唯一。同一个加速度可以由"大接触力 + 小力矩"或"小接触力 + 大力矩"产生，优化器会在这两者之间随意游走，输出抖动。

第二，接触力可能为负。没有 \(\boldsymbol{\lambda}_z\ge 0\) 的约束时，求解器会算出"脚拉着地面"的非物理解——这在真机上意味着脚直接离地。

第三，加速度会违反地面约束。如果不强制 86.3B-2，求出的 \(\dot{\mathbf{v}}\) 会让支撑脚"陷进地里"或"飞起来"，下一时刻的状态估计立刻发散。

偷懒做法	直接后果	物理根因
不加接触约束就求逆	解不唯一、力矩抖动	系统欠定，缺一半方程
不加 \(\lambda_z\ge 0\)	算出"脚拉地面"的负法向力	单侧接触的不等式被丢掉
不强制脚加速度为零	脚陷地/飞起，估计发散	运动学约束未进入动力学

这三条正是约化动力学要系统性消除的病灶。

86.3B.3 历史：从 Aghili 到 OSC 的约束消去思想

把约束动力学投影到一个更小的无约束子空间，这个思想在多体动力学里很早就有。Aghili（2005）系统给出了用约束雅可比零空间投影构造约化动力学的通式；在足式控制语境下，Mistry、Buchli、Schaal（2010）的 "Inverse dynamics control of floating base systems using orthogonal decomposition" 明确把它用于浮动基座机器人——核心是用一个正交分解把动力学拆成"接触一致"和"接触正交"两部分。后来 Sentis 的 WBOSC、Righetti 的 projected inverse dynamics、以及 OCS2/TSID 里的接触约束处理，本质上都是这一思想的不同实现。

理解了这条线，你读 qm_control 的 WbcBase 或 TSID 的 ContactConstraint 时就不会迷失——它们都在做同一件事：用接触约束把大方程投影成小方程。

86.3B.4 理论一：零空间投影消去接触力

第一种消去思路是消掉接触力 \(\boldsymbol{\lambda}\)，得到一个只含 \((\dot{\mathbf{v}},\boldsymbol{\tau})\) 的方程。

构造接触雅可比转置的零空间投影算子。设 \(\mathbf{J}_c\in\mathbb{R}^{3k\times 24}\) 行满秩，定义它的动力学一致零空间投影：

\[ \mathbf{P} = \mathbf{I}-\mathbf{J}_c^{T}\left(\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^{T}\right)^{-1}\mathbf{J}_c\mathbf{M}^{-1} \tag{86.3B-3} \]

这个 \(\mathbf{P}\) 有一个关键性质：它消灭一切接触力的广义力贡献。验证如下——对任意 \(\boldsymbol{\lambda}\)：

\[ \mathbf{P}\,\mathbf{J}_c^{T}\boldsymbol{\lambda} = \mathbf{J}_c^{T}\boldsymbol{\lambda} - \mathbf{J}_c^{T}\left(\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^{T}\right)^{-1}\underbrace{\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^{T}}_{\text{与括号内互逆}}\boldsymbol{\lambda} = \mathbf{J}_c^{T}\boldsymbol{\lambda}-\mathbf{J}_c^{T}\boldsymbol{\lambda} =\mathbf{0} \tag{86.3B-4} \]

阶段小结：到这里我们做了一件事——构造了一个算子 \(\mathbf{P}\)，它左乘任何"接触力的广义力"都得零。接下来要做的是把这个算子作用到整条动力学方程上，让 \(\boldsymbol{\lambda}\) 整体消失。

把 \(\mathbf{P}\) 左乘 86.3B-1，利用 86.3B-4：

\[ \mathbf{P}\mathbf{M}\dot{\mathbf{v}}+\mathbf{P}\mathbf{h} = \mathbf{P}\mathbf{S}^{T}\boldsymbol{\tau} \tag{86.3B-5} \]

接触力没了。这就是 Mistry-Buchli-Schaal 形式的接触一致逆动力学。剩下的只有加速度和力矩，且 86.3B-5 自动保证了解满足接触约束（因为 \(\mathbf{P}\) 的构造里嵌入了 \(\mathbf{J}_c\)）。

每一项的含义如下：

项	含义	为什么这样
\(\mathbf{P}\mathbf{M}\dot{\mathbf{v}}\)	投影后的惯性力	只保留接触约束允许的加速度方向
\(\mathbf{P}\mathbf{h}\)	投影后的非线性力	科氏/重力中违反约束的分量被滤掉
\(\mathbf{P}\mathbf{S}^{T}\boldsymbol{\tau}\)	投影后的电机力矩	决定哪些力矩"用得上"，哪些被约束吃掉

86.3B.5 理论二：基座行消去与"为什么基座不可驱动"

第二种思路更贴近 WBC 工程实现，也更能解释"浮动基座不可直接驱动"这件事的深层含义。

回顾 86.3.1 的选择矩阵 \(\mathbf{S}=[\mathbf{0}_{18\times 6}\;\;\mathbf{I}_{18}]\)，于是 \(\mathbf{S}^T\boldsymbol{\tau}\) 的前 6 行（基座行）恒为零。把统一方程按"基座 6 行 / 关节 18 行"分块：

\[ \begin{bmatrix} \mathbf{M}_{b}\\ \mathbf{M}_{j} \end{bmatrix} \dot{\mathbf{v}} + \begin{bmatrix} \mathbf{h}_{b}\\ \mathbf{h}_{j} \end{bmatrix} = \begin{bmatrix} \mathbf{0}\\ \boldsymbol{\tau} \end{bmatrix} + \begin{bmatrix} \mathbf{J}_{c,b}^{T}\\ \mathbf{J}_{c,j}^{T} \end{bmatrix} \boldsymbol{\lambda} \tag{86.3B-6} \]

其中 \(\mathbf{M}_b\in\mathbb{R}^{6\times 24}\) 是质量矩阵的前 6 行，\(\mathbf{J}_{c,b}^T\in\mathbb{R}^{6\times 3k}\) 是接触雅可比转置的前 6 行。

只看前 6 行（基座行）：

\[ \boxed{ \mathbf{M}_{b}\dot{\mathbf{v}}+\mathbf{h}_{b} = \mathbf{J}_{c,b}^{T}\boldsymbol{\lambda} } \tag{86.3B-7} \]

这 6 个方程里**完全没有 \(\boldsymbol{\tau}\)**。它们说的是一句极其物理的话：

本质洞察：浮动基座的 6 维运动（线动量 + 角动量的变化）只能由接触力 \(\boldsymbol{\lambda}\) 产生，电机力矩对它没有任何直接贡献。这就是"基座不可驱动"（underactuated）的精确数学含义——不是"基座没有电机"这么模糊，而是"基座那 6 行动力学方程的右端不含 \(\boldsymbol{\tau}\)"。机器人想让身体往某个方向加速，唯一的办法是通过脚（和手）蹬地获得反力。86.3B-7 是整个足式与四足臂控制的"宪法"：所有平衡、跳跃、推门，归根结底都是在解这 6 个方程。

这 6 行恰好对应 86.4 要讲的质心动量变化率——把 \(\mathbf{M}_b\dot{\mathbf{v}}+\mathbf{h}_b\) 换成质心坐标表达，就是 \(\dot{\mathbf{h}}_G\)。所以 86.3B-7 和 86.4.2 是同一个物理事实的两种写法：一种用全身质量矩阵的基座行，一种用质心动量矩阵。这也解释了为什么 SRBD/centroidal MPC 能成立——它直接对这 6 行建模，把另外 18 行关节动力学交给 WBC。

86.3B.6 理论三：约化质量矩阵与"可达加速度"

把接触约束 86.3B-2 解出来，可以进一步得到约化形式。接触约束把 24 维加速度限制在一个仿射子空间里。设该约束的解为：

\[ \dot{\mathbf{v}} = \mathbf{N}\dot{\mathbf{v}}_{f} - \mathbf{J}_c^{+}\dot{\mathbf{J}}_c\mathbf{v} \tag{86.3B-8} \]

其中 \(\mathbf{N}\) 的列张成 \(\mathbf{J}_c\) 的零空间（即"不违反接触的加速度方向"），\(\dot{\mathbf{v}}_f\) 是 \(24-3k\) 维的自由加速度，\(\mathbf{J}_c^{+}\) 是 \(\mathbf{J}_c\) 的伪逆。第二项是为抵消 \(\dot{\mathbf{J}}_c\mathbf{v}\) 这个"约束漂移"必须付出的加速度。

代回投影方程并用 \(\mathbf{N}^T\) 左乘，得到约化质量矩阵：

\[ \tilde{\mathbf{M}}=\mathbf{N}^{T}\mathbf{M}\mathbf{N} \tag{86.3B-9} \]

这个 \(\tilde{\mathbf{M}}\) 维度是 \((24-3k)\times(24-3k)\)。直观理解：四脚着地（\(k=4\)，点接触 \(3k=12\)）时，24 维系统被锁掉 12 维，只剩 12 维"可达加速度子空间"。这 12 维正好对应"在不破坏接触的前提下，身体还能怎么动"。

接触状态	锁掉的维度 \(3k\)	剩余可达维度	直观含义
四脚站立	12	12	身体可在支撑内调姿、臂可自由动
三脚支撑（一脚摆动）	9	15	多出摆动腿的 3 维
对角步态双脚支撑	6	18	动态性强但裕度小
单脚 + 单手接触	6	18	操作时常见，约束方向混合

反事实推理：如果不做这个约化，直接在 24 维上跑 WBC 会怎样？数学上仍可解（靠等式约束），但有两个隐患：一是 QP 规模更大、求解更慢；二是数值上接触方向的"硬等式"和任务的"软目标"尺度差异巨大，容易病态。约化到 12 维子空间后，所有自由度都是真正可用的，QP 既小又良态。这就是为什么 TSID、qm_control 这类高性能 WBC 都在内部做某种形式的接触投影，而不是裸跑 24 维。

86.3B.7 理论-工程桥接：约化动力学在 WBC 里长什么样

把上面三种形式连起来，就能看懂一个真实 WBC 的内核。一个接触一致的逆动力学 WBC，本质上在每个控制周期做三步：

1. 用 86.3B-7（基座 6 行）作为**硬等式约束**——保证求出的接触力与基座动量自洽，这是不可松弛的物理定律。
2. 用 86.3B-5 或 86.3B-9 把任务加速度投影到接触允许的子空间——保证任务命令不会"要求脚陷地"。
3. 在剩余自由度上做加权 QP，分配关节力矩与接触力。

下面的伪代码骨架展示了"基座行作为硬约束"这一核心结构，与 86.7.5 的 QuadArmWbcBuilder 衔接：

// 约化动力学 WBC 的核心：基座 6 行是硬等式，关节 18 行是力矩定义式
void addFloatingBaseDynamicsReduced(WbcProblem& qp,
                                    const RobotState& state,
                                    const ContactMode& mode) {
  // M、h、Jc 来自 Pinocchio（注意 M 已对称化，见 86.3.6 陷阱）
  const auto& M  = state.M;          // 24 x 24
  const auto& h  = state.h;          // 24
  const auto& Jc = mode.contactJacobian();  // 3k x 24

  // ---- 基座 6 行：右端不含 tau，是欠驱动约束 ----
  // M_b * vdot + h_b = Jc_b^T * lambda
  // 决策变量 z = [vdot(24), lambda(3k), tau(18)]
  auto M_b   = M.topRows(6);              // 86.3B-7 的 M_b
  auto Jc_bT = Jc.leftCols(6).transpose();// Jc_b^T，6 x 3k
  // 这一块作为 Aeq/beq 的硬等式：基座动量只能由接触力平衡
  appendEquality(qp, /*A=*/concat(M_b, -Jc_bT, zeros(6,18)),
                     /*b=*/ -h.head(6));

  // ---- 关节 18 行：定义 tau，可作为等式或直接代入 ----
  // M_j * vdot + h_j = tau + Jc_j^T * lambda  -> 解出 tau
  auto M_j   = M.bottomRows(18);
  auto Jc_jT = Jc.rightCols(18).transpose();
  appendEquality(qp, /*A=*/concat(M_j, -Jc_jT, -Identity(18)),
                     /*b=*/ -h.tail(18));
}

注意基座行（前 6 行）的等式里 tau 的系数恒为零——代码里直接体现为 zeros(6,18)。这不是实现技巧，而是 86.3B-7 的物理事实在数据结构上的投影。任何把基座行也写上 tau 系数的实现都是错的，会让求解器误以为"可以用电机直接推基座"。

⚠️ 常见陷阱

⚠️ 编程陷阱：在基座 6 行的动力学等式里给 \(\boldsymbol{\tau}\) 配了非零系数

错误做法：复制关节行的等式模板时，没把基座行的 \(\boldsymbol{\tau}\) 系数清零。

现象：WBC 求出的力矩偏小但仍能"站住"，因为求解器误以为电机能直接推基座；一旦上真机或加扰动立刻发散。

根本原因：违反了 86.3B-7——基座动力学右端本就不含 \(\boldsymbol{\tau}\)。

正确做法：基座 6 行的 \(\boldsymbol{\tau}\) 系数恒为 \(\mathbf{0}_{6\times 18}\)，只让接触力 \(\boldsymbol{\lambda}\) 出现在右端。自检：打印等式矩阵中 \(\boldsymbol{\tau}\) 对应的前 6 行块，必须全零。

💡 概念误区：以为约化动力学"丢掉了"信息

新手想法："消去接触力或投影到 12 维，是不是把接触力算不出来了？"

实际上：接触力没有丢，只是从"和加速度耦合的未知量"变成了"由基座 6 行方程 86.3B-7 反解出的量"。约化只是改变了求解顺序——先在子空间求加速度/力矩，再回代求接触力。信息守恒，只是分解方式不同。

为什么重要：理解这点才能在调试时知道去哪里读接触力——它在 86.3B-7 的反解里，不在约化后的 QP 主变量里。

🧠 思维陷阱：把零空间投影当成"可有可无的数值技巧"

新手想法："不投影也能用等式约束求解，投影只是为了快一点。"

实际上：投影不只是提速。它保证了任务命令永远落在接触允许的子空间内，从源头杜绝"脚陷地"这类不可行命令。等式约束是"事后惩罚不可行"，投影是"事前排除不可行"，两者在病态构型下行为差异很大。

正确思维：把投影理解为"约束的几何先验"，而非"求解器的加速开关"。

练习 86.3B

#	练习	难度
1	对四脚站立的 Go2+Z1，写出 \(\mathbf{J}_c\) 的维度，并计算约化加速度子空间的维数 \(24-3k\)。	⭐⭐
2	用 Pinocchio 构造投影算子 \(\mathbf{P}\)（公式 86.3B-3），数值验证 \(\mathbf{P}\mathbf{J}_c^T=\mathbf{0}\)。	⭐⭐⭐
3	把基座 6 行方程 86.3B-7 与 86.4.2 的 \(\dot{\mathbf{h}}_G\) 表达式对照，说明二者为什么是同一物理事实的两种写法。	⭐⭐⭐⭐

---

86.4 质心动量：把腿、臂、接触力放到同一张图里 ⭐⭐⭐

这一节解决"全身动力学太大，MPC 如何还能实时"的问题。

回顾足式/70：SRBD 把整个机器人近似为一个刚体，MPC 只优化接触力和质心运动。

回顾复合/20：CMM 把高维关节速度映射到 6 维质心动量。

四足臂中，CMM 是连接臂运动、角动量和接触力的核心工具。

86.4.1 质心动量定义

系统在质心处的动量为：

\[ \mathbf{h}_G = \begin{bmatrix} \mathbf{k}_G \\ \mathbf{l}_G \end{bmatrix} \]

其中：

符号	含义
\(\mathbf{k}_G\)	绕质心的角动量
\(\mathbf{l}_G=m\dot{\mathbf{p}}_G\)	线动量

CMM 满足：

\[ \boxed{ \mathbf{h}_G = \mathbf{A}_G(q)\mathbf{v} } \]

对四足臂进行分块：

\[ \mathbf{A}_G = \begin{bmatrix} \mathbf{A}_{G,b} & \mathbf{A}_{G,\ell} & \mathbf{A}_{G,a} \end{bmatrix} \]

于是：

\[ \mathbf{h}_G = \mathbf{A}_{G,b}\mathbf{v}_{b} + \mathbf{A}_{G,\ell}\mathbf{v}_{\ell} + \mathbf{A}_{G,a}\mathbf{v}_{a} \]

这就是臂能改变全身角动量的数学表达。

不是因为臂有某种特殊控制器。

而是因为臂关节速度的列块 \(\mathbf{A}_{G,a}\) 本来就在质心动量矩阵中。

86.4.2 动量变化率与外力

质心动量的变化率由外部扳手决定。

在平地点接触和一个末端接触的情况下：

\[ \dot{\mathbf{h}}_G = \begin{bmatrix} \sum_i (\mathbf{p}_{c_i}-\mathbf{p}_G)\times \boldsymbol{\lambda}_i + (\mathbf{p}_{ee}-\mathbf{p}_G)\times \mathbf{f}_{ee,lin} + \boldsymbol{\tau}_{ee} \\ m\mathbf{g} + \sum_i \boldsymbol{\lambda}_i + \mathbf{f}_{ee,lin} \end{bmatrix} \]

这个公式把四类量放到一张图里：

量	在公式中的位置	控制含义
质心位置 \(\mathbf{p}_G\)	力臂项	决定同一接触力产生多大力矩
足端接触力 \(\lambda_i\)	线动量和角动量方程	主要平衡来源
末端线力 \(\mathbf{f}_{ee,lin}\)	额外外力	推、拉、托举都会改变足底负载
末端力矩 \(\tau_{ee}\)	角动量方程	拧阀、旋转接触会强烈影响姿态

86.4.3 支撑裕度与 ZMP 偏移

考虑末端施加水平推力 \(F_x\)，臂高度为 \(z_{ee}\)。

ZMP 在 \(x\) 方向近似满足：

\[ x_{zmp} = x_{com} - \frac{z_{com}}{g}\ddot{x}_{com} + \frac{\tau_{arm,y}}{mg} - \frac{z_{ee}F_x}{mg} \]

最后一项说明：高处水平推力会产生倾覆力矩。

这就是为什么推门时不能只看手的力。

你必须同时看支撑多边形里还有多少 ZMP 裕度。

操作	主要风险	控制器应关注
向前推门	后脚法向力下降	后脚摩擦裕度、ZMP 后边界
向后拉抽屉	前脚法向力下降	前脚摩擦裕度、基座 pitch
举高重物	CoM 上升、惯量变大	支撑多边形收缩、角动量
拧阀门	末端扭矩反作用	yaw/roll 角动量和足端切向力

86.4.4 臂是操作器，也是动量调节器

在常规机械臂中，臂的目标是让末端到达目标。

在四足臂中，臂还可以调节全身角动量。

类比卫星的反作用轮：

卫星通过内部飞轮加减速改变姿态。

四足臂可以通过臂段摆动改变 \(\mathbf{k}_G\)。

不同点在于，臂还要执行操作任务，不能只服务于姿态控制。

因此四足臂的难点不是"能不能用臂调姿态"。

难点是当末端 tracking 和角动量调节冲突时，如何确定优先级。

另一个有帮助的类比来自**花样滑冰**：

滑冰运动员旋转时收紧手臂，角速度加快——这是角动量守恒的经典演示。四足臂中，机械臂收拢或伸展改变的不是旋转速度，而是全身角动量在基座和臂之间的分配。当臂快速伸展时，\(\mathbf{A}_{G,a}\) 的列块变大，同样的臂关节速度会在角动量中占更大比重。但滑冰运动员没有外部接触力矩（冰面几乎无摩擦），所以角动量守恒；而四足臂有四个脚与地面接触，接触力可以注入或吸收角动量——这正是腿式系统比卫星或冰上运动员更灵活的原因，也是 WBC 需要统一优化接触力和臂运动的根本理由。

还有一个来自**杂技平衡术**的类比：马戏团杂技演员在独轮车上托盘子。演员的双手是"操作器"，负责保持盘子水平；双腿和身体是"支撑器"，负责保持独轮车平衡。如果演员只关注盘子而忽略平衡，盘子和人一起倒地。如果只关注平衡而不管盘子，盘子掉了任务失败。四足臂面临完全相同的问题：末端任务（盘子）和支撑稳定（独轮车）必须同时满足，且通过同一个身体耦合。与杂技演员不同的是，四足臂有四个"腿"提供更大的支撑多边形，但它的"手"（机械臂）有更大的质量和惯量，补偿起来更难。

86.4.5 任务优先级图

一个保守的四足臂 WBC 任务优先级如下：

P0  硬约束
    ├─ 全身动力学方程
    ├─ 支撑脚不滑动
    ├─ 摩擦锥
    ├─ 关节限位
    └─ 力矩限幅

P1  生存任务
    ├─ 基座 roll/pitch 稳定
    ├─ CoM 保持在支撑裕度内
    └─ 角动量变化率限制

P2  运动任务
    ├─ 期望 base 速度
    ├─ 摆动腿轨迹
    └─ 步态相位一致性

P3  操作任务
    ├─ EE 位置跟踪
    ├─ EE 姿态跟踪
    └─ EE 力/阻抗跟踪

P4  舒适与正则
    ├─ 关节居中
    ├─ 可操作度最大化
    ├─ 自碰撞距离增大
    └─ 扭矩变化率平滑

这张图体现一个原则：

本质洞察：四足臂的操作精度必须服从接触可行性。 手够不到目标时，可以移动身体或换步态。 脚失去摩擦裕度时，机器人会直接失去执行任何操作的能力。

86.4.6 CMM 与支撑裕度的计算框架

#include <pinocchio/fwd.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <Eigen/Dense>
#include <string>

struct MomentumMonitor {
  pinocchio::Model model;
  pinocchio::Data data;

  explicit MomentumMonitor(const std::string& urdf)
      : model(), data(model) {
    // CMM 必须基于完整四足臂模型；这里同样使用 FreeFlyer 浮动基座。
    pinocchio::urdf::buildModel(
        urdf, pinocchio::JointModelFreeFlyer(), model);
    data = pinocchio::Data(model);
  }

  Eigen::MatrixXd computeCmm(const Eigen::VectorXd& q,
                             const Eigen::VectorXd& v) {
    // ccrba 同时计算质心动量矩阵、质心位置和动量。
    pinocchio::ccrba(model, data, q, v);
    return data.Ag;
  }

  double armAngularMomentumAbility(const Eigen::MatrixXd& Ag,
                                   int arm_start,
                                   int arm_dof) const {
    // Pinocchio 的 data.Ag 采用 (线动量, 角动量) 布局：前三行(0..2)是线动量、后三行(3..5)是角动量。
    // 要衡量臂速度影响角动量的能力，应取角动量块（行 3..5），而非前三行。
    Eigen::MatrixXd Ag_arm_ang = Ag.block(3, arm_start, 3, arm_dof);
    return Ag_arm_ang.norm();
  }

  double zmpShiftFromEndEffectorForce(double z_ee,
                                      double fx,
                                      double mass,
                                      double gravity) const {
    // 简化估算：高处水平推力导致的 ZMP 偏移。
    return -z_ee * fx / (mass * gravity);
  }
};

⚠️ 常见陷阱

⚠️ 概念误区：只用 CoM 位置判断稳定

错误做法：只要 CoM 投影在支撑多边形内，就认为可安全操作。

现象：站立搬物时看起来可行，快速推拉时仍然翻倒。

根本原因：动态操作中角动量变化和末端外力会移动 ZMP。

正确做法：同时监控 CoM、ZMP、足底法向力、摩擦裕度和角动量变化率。

⚠️ 编程陷阱：把 CMM 的线动量和角动量顺序搞反

不同库对空间向量排列可能使用 angular-first 或 linear-first。

Pinocchio 的 centroidal momentum（data.hg / data.Ag）按 linear-first 组织：前三行是线动量、后三行是角动量（与足式/50 的"约定提示"一致；注意这与 Featherstone/Orin 数学公式常用的 angular-first 相反）。

使用前必须打印单位量纲并做一个简单测试：只让基座线速度非零，观察哪三行变化。

🧠 思维陷阱：认为角动量任务越强越好

角动量约束太强会压制末端操作和摆腿。

角动量任务应限制危险变化率，而不是让 \(\mathbf{k}_G\) 永远为零。

动态操作有时需要有计划地引入角动量，再通过接触力回收。

练习 86.4

#	练习	难度
1	计算站立构型下 \(\mathbf{A}_{G,a}\) 的 Frobenius 范数，并比较臂收拢和伸展两种构型。	⭐⭐
2	设末端在 0.6 m 高处推门，推力 30 N，机器人总质量 19 kg，估算 ZMP 偏移。	⭐⭐
3	为"单脚支撑时插电"任务设计 P0-P4 任务优先级，并说明哪些任务应允许松弛。	⭐⭐⭐

---

86.4B whole-body 雅可比分块与臂对基座的反作用 ⭐⭐⭐⭐

86.4 给出了一个结论：臂关节速度通过 CMM 的列块 \(\mathbf{A}_{G,a}\) 改变全身角动量。这个结论很有用，但它是个"黑箱"——它告诉你臂会影响基座，却没说**影响有多大、怎么算、能不能反过来用**。这一节打开这个黑箱。

核心问题是：臂动一下，基座到底会反向动多少？ 答案藏在 whole-body 雅可比的分块结构里。理解了它，你才能回答三个工程上反复出现的问题：为什么"臂伸出去基座会下沉"？为什么"甩臂能帮机器人转身"？为什么"末端 tracking 误差里有一部分根本不是臂的错"？

86.4B.1 动机：末端速度由谁决定

回顾 86.3.6 提到的一个事实：四足臂的末端雅可比 \(\mathbf{J}_{ee}\) 前 6 列（基座列）通常不为零。把它写成分块形式，按"基座 / 腿 / 臂"切：

\[ \dot{\mathbf{x}}_{ee} = \mathbf{J}_{ee}\mathbf{v} = \begin{bmatrix} \mathbf{J}_{ee,b} & \mathbf{J}_{ee,\ell} & \mathbf{J}_{ee,a} \end{bmatrix} \begin{bmatrix} \mathbf{v}_b\\ \dot{\mathbf{q}}_\ell\\ \dot{\mathbf{q}}_a \end{bmatrix} = \mathbf{J}_{ee,b}\mathbf{v}_b + \mathbf{J}_{ee,\ell}\dot{\mathbf{q}}_\ell + \mathbf{J}_{ee,a}\dot{\mathbf{q}}_a \tag{86.4B-1} \]

这个分块直接回答了 86.2.2 里的第三个"错误假设"——"末端位置误差只由臂关节决定"。86.4B-1 说得很清楚：末端速度有三个来源。臂只贡献了 \(\mathbf{J}_{ee,a}\dot{\mathbf{q}}_a\) 这一项。基座一晃（\(\mathbf{v}_b\ne 0\)），末端立刻通过 \(\mathbf{J}_{ee,b}\mathbf{v}_b\) 跟着晃。

本质洞察：在四足臂上，"末端误差"是一个被三方共同决定的量。如果你只用臂关节去修正末端误差（即只调 \(\dot{\mathbf{q}}_a\)），你其实是在用一个执行器去补偿另外两个执行器（基座、腿）引入的扰动。这就是为什么固定基座机械臂的控制经验不能直接搬到四足臂——固定基座时 \(\mathbf{J}_{ee,b}=\mathbf{0}\) 且 \(\mathbf{v}_b\equiv\mathbf{0}\)，末端误差确实只由臂决定；浮动基座一旦成立，这个前提就塌了。

86.4B.2 反面：把基座当固定会怎样

设想一个常见的错误实现：工程师把固定基座机械臂的逆运动学（IK）直接拿来用，求解时假设基座不动，只解臂关节增量 \(\Delta\mathbf{q}_a=\mathbf{J}_{ee,a}^{+}\,\mathbf{e}_{ee}\)。

这在机器人**完全静止**时勉强能用。但只要机器人在走、或者臂自身的运动引起了基座反作用（下一小节会量化），基座就会动，而这个 IK 完全看不见 \(\mathbf{J}_{ee,b}\mathbf{v}_b\) 这一项。结果是末端在世界系里画出的轨迹和命令对不上，且误差随步态周期性振荡。

假设	隐含数学	真机后果
基座固定，只解臂 IK	强行令 \(\mathbf{J}_{ee,b}\mathbf{v}_b=\mathbf{0}\)	行走时末端轨迹随步态周期抖动
用臂修一切末端误差	把 \(\mathbf{J}_{ee,b}\mathbf{v}_b\) 的扰动也压给臂	臂关节速度饱和，仍追不上
忽略臂对基座的反作用	漏掉 \(\mathbf{M}_{ba}\dot{\mathbf{v}}_a\)	臂快速运动时基座"莫名"俯仰下沉

第三行正是这一节真正要量化的东西。

86.4B.3 理论：臂运动引起的基座反作用——动量守恒视角

现在来正面回答"臂动一下基座反向动多少"。最干净的推导用质心动量守恒。

考虑机器人**腾空或瞬时无外部接触力矩**的极端情形（落脚切换的瞬间、跳跃滞空、或接触力对某方向力臂为零的瞬间）。此时全身角动量守恒，\(\mathbf{k}_G=\text{const}\)。把 86.4.1 的 CMM 分块代入，并暂时只看基座和臂（腿设为锁定）：

\[ \mathbf{A}_{G,b}\,\mathbf{v}_b+\mathbf{A}_{G,a}\,\dot{\mathbf{q}}_a=\mathbf{k}_G=\text{const} \tag{86.4B-2} \]

对它求关于"臂动作"的响应。如果初始静止（\(\mathbf{k}_G=0\)），那么任何臂运动都必须被基座的反向运动抵消：

\[ \boxed{ \mathbf{v}_b = -\,\mathbf{A}_{G,b}^{-1}\,\mathbf{A}_{G,a}\,\dot{\mathbf{q}}_a } \tag{86.4B-3} \]

这就是定量答案。\(-\mathbf{A}_{G,b}^{-1}\mathbf{A}_{G,a}\) 是一个 \(6\times n_a\) 的矩阵，它把"臂关节速度"直接映射成"基座反向 twist"。这个矩阵正是机器人学里的**反作用零空间映射**（reaction null-space mapping，Nenchev/Yoshida 1999 在自由漂浮空间机器人上首先系统化）。

本质洞察：四足臂在滞空或弱接触瞬间，本质上是一个"自由漂浮空间机器人"。卫星机械臂动一下，卫星本体就反向转一下，靠的是同一个公式 86.4B-3。地面四足臂的不同之处仅在于：脚一旦着地，接触力就接管了 \(\dot{\mathbf{h}}_G\)（见 86.4.2），守恒被打破，基座反作用被地面"吸收"一部分。所以同样幅度的甩臂，滞空时基座反应剧烈，四脚撑地时被接触力压住——这解释了 86.4.4 里"花样滑冰无接触则守恒、四足臂有接触可注入/吸收角动量"的差别的精确来源。

86.4B-3 也给了一个建设性结论：既然臂能可预测地驱动基座反向运动，就可以**反过来用**——故意设计臂的轨迹去产生想要的基座角动量，这正是 86.4.4"臂作为动量调节器"的数学实现。甩臂帮机器人转身（yaw 调节）、伸臂配合落脚调 pitch，都是在主动利用 86.4B-3。

86.4B.4 理论：接触存在时的完整耦合——回到质量矩阵

86.4B-3 是"无外部力矩"的理想情形。一般情形下脚在地上，接触力参与平衡，必须回到 86.3.5 的质量矩阵分块。看基座行（这正是 86.3B-7）展开臂的贡献：

\[ \underbrace{\mathbf{M}_{bb}\dot{\mathbf{v}}_b}_{\text{基座自身惯性}} + \underbrace{\mathbf{M}_{b\ell}\ddot{\mathbf{q}}_\ell}_{\text{腿反作用}} + \underbrace{\mathbf{M}_{ba}\ddot{\mathbf{q}}_a}_{\text{臂反作用}} + \mathbf{h}_b = \mathbf{J}_{c,b}^{T}\boldsymbol{\lambda} \tag{86.4B-4} \]

\(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\) 这一项就是"臂加速度对基座的惯性反作用力"。它是 86.2.1 表格里"臂快速摆动时基座 roll/pitch 变大"的精确来源，也是 86.3.5 强调 \(\mathbf{M}_{ba}\) 不可忽略的原因。

把它解读成控制语言：臂要做一个加速度 \(\ddot{\mathbf{q}}_a\)，就会在基座 6 行里注入一个广义力 \(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\)。这个力要么被接触力 \(\mathbf{J}_{c,b}^T\boldsymbol{\lambda}\) 吸收（脚多扛一点力），要么转化成基座加速度 \(\mathbf{M}_{bb}\dot{\mathbf{v}}_b\)（基座动起来）。WBC 的任务之一，就是让接触力主动吸收这个反作用，把基座扰动压到最小。

情形	谁吸收臂反作用 \(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\)	现象
四脚撑地 + WBC 协调	接触力 \(\boldsymbol{\lambda}\) 主动补偿	基座几乎不动，足底力小幅变化
四脚撑地 + 腿臂独立控制	无人补偿，转成 \(\dot{\mathbf{v}}_b\)	基座 roll/pitch 跳变
滞空 / 弱接触	接触力无力补偿，回到 86.4B-3	基座按守恒反向运动

86.4B.5 理论-工程桥接：把基座列"还"给末端任务

知道了末端速度的三方来源（86.4B-1），WBC 里就有一个对应的正确做法：末端任务的雅可比必须用完整的 \(\mathbf{J}_{ee}\)（含基座列），而不是只用臂列 \(\mathbf{J}_{ee,a}\)。

具体地，末端加速度级任务（对照 86.8.5 的 SE(3) 任务）应写成：

\[ \mathbf{J}_{ee}\dot{\mathbf{v}}+\dot{\mathbf{J}}_{ee}\mathbf{v}=\ddot{\mathbf{x}}_{ee}^{des} \tag{86.4B-5} \]

这里 \(\mathbf{J}_{ee}\) 作用在**完整的** \(\dot{\mathbf{v}}\)（含基座加速度 \(\dot{\mathbf{v}}_b\)）上。这样 WBC 在分配关节力矩时，会自动协调"让基座配合末端"还是"让臂多动"，因为 QP 同时握着基座和臂的自由度。下面对照展示正确与错误的雅可比用法：

// ✅ 正确：末端任务用完整 whole-body Jacobian（含基座列）
// J_ee: 6 x nv（nv=24），作用在完整 vdot 上
void addEndEffectorTaskFull(WbcProblem& qp,
                            const RobotState& state,
                            const EndEffectorReference& ref) {
  Eigen::Matrix<double,6,Eigen::Dynamic> J_ee(6, state.nv);
  getFrameJacobian(state.model, state.data, state.ee_frame,
                   pinocchio::LOCAL_WORLD_ALIGNED, J_ee);  // 前 6 列非零
  Eigen::Matrix<double,6,1> dJv = frameClassicAcceleration(/*...*/);
  // 任务: J_ee * vdot = xddot_des - dJ_ee * v
  // vdot 是完整 24 维，WBC 自行协调基座与臂的分配
  appendTask(qp, /*A=*/blockOnVdot(J_ee), /*b=*/ref.acceleration - dJv,
             /*weight=*/state.w_ee);
}

// ❌ 错误：只取臂列，假装基座不动
void addEndEffectorTaskArmOnly(WbcProblem& qp, /*...*/) {
  // 只用 J_ee.rightCols(arm_dof) 作用在 qddot_a 上
  // 隐含强行令 J_ee_b * vdot_b = 0 —— 行走时末端会周期性偏离
  // 这是把固定基座 IK 错误搬到浮动基座的典型 bug
}

错误版本的隐患在 86.4B.2 已经分析：它假装 \(\mathbf{J}_{ee,b}\mathbf{v}_b=0\)，于是基座一动末端就漂。正确版本把基座列"还"给了任务，让 86.4B-1 的三方来源都被纳入优化。

本质洞察：whole-body 控制里"whole-body"三个字的精确含义，就体现在 \(\mathbf{J}_{ee}\) 用不用基座列这一个细节上。用了基座列，控制器就在"用整个身体追末端"；不用，就退化成"只用臂追末端，听天由命地承受基座扰动"。一字之差，是 whole-body 控制和"固定基座 IK + 凑合"的分水岭。

86.4B.6 与 CMM 的统一：两个雅可比是一家人

最后把这一节和 86.4 缝起来。本节用了两个分块雅可比：\(\mathbf{J}_{ee}\)（末端速度）和 \(\mathbf{A}_G\)（质心动量）。它们看似无关，其实是同一个全身运动学的两个投影。

• \(\mathbf{A}_G\) 把全身速度投影到 6 维质心动量——回答"身体整体的动量怎么变"，这是 MPC 关心的（86.4）。
• \(\mathbf{J}_{ee}\) 把全身速度投影到 6 维末端 twist——回答"手尖怎么动"，这是操作任务关心的（86.4B）。

两者的基座列都非零，意味着基座运动同时进入"全身动量"和"末端运动"两本账。WBC 的本质，就是在这两本账（外加足端接触账）之间找一组关节力矩与接触力，让它们同时尽量满足。这也呼应了知识导航里那句话——所有困难都来自"一个身体、两类任务"，而这两个雅可比就是"一个身体"被"两类任务"分别观测的结果。

雅可比	投影目标	服务的层	基座列的意义
\(\mathbf{A}_G(q)\)	6 维质心动量 \(\mathbf{h}_G\)	MPC / 平衡	基座运动直接贡献动量
\(\mathbf{J}_{ee}(q)\)	6 维末端 twist	操作任务	基座运动直接搬动末端
\(\mathbf{J}_{c,i}(q)\)	3/6 维接触点速度	接触约束	基座运动会"踢"接触点

⚠️ 常见陷阱

⚠️ 编程陷阱：末端任务雅可比只取臂列

错误做法：从 \(\mathbf{J}_{ee}\) 里切出臂对应的列，单独对臂关节求解，等价于把固定基座 IK 搬过来。

现象：静止时末端跟踪正常，机器人一开始走，末端就随步态周期性偏离目标，误差频率恰好等于步频。

根本原因：丢掉了 86.4B-1 中的 \(\mathbf{J}_{ee,b}\mathbf{v}_b\) 项——基座运动对末端的贡献被无视。

正确做法：末端任务必须用完整 whole-body 雅可比（含基座列），让 WBC 协调基座与臂。自检：把机器人原地踏步，看末端误差是否出现步频谐波；若有，多半是这个 bug。

💡 概念误区：以为臂对基座的反作用只在滞空时才有

新手想法："反作用零空间映射 86.4B-3 是空间机器人的东西，地面机器人脚撑着就没有了。"

实际上：反作用一直存在，只是被接触力吸收了一部分（见 86.4B-4 的 \(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\) 项）。四脚撑地时如果 WBC 不主动用接触力补偿，臂的反作用照样会让基座跳变；滞空只是"无人补偿"的极端版本。

为什么重要：这决定了你要不要在 WBC 里显式建模 \(\mathbf{M}_{ba}\)——答案是要，无论脚在不在地上。

🧠 思维陷阱：把"臂调动量"和"臂做操作"看成可以分别优化的两件事

新手想法："让臂先服务操作任务，剩下的自由度再去调角动量。"

实际上：同一个臂关节速度 \(\dot{\mathbf{q}}_a\) 同时出现在末端任务 86.4B-1 和动量映射 86.4B-3 里，二者共享自由度、必然耦合。当 \(n_a=6\) 且末端是 6D 任务时，臂自由度被操作任务占满，几乎没有余量调动量——这时只能靠基座/腿或牺牲部分末端精度。

正确思维：把"操作"和"动量调节"当成竞争同一组自由度的两个任务，用 86.4.5 的优先级图去裁决，而不是幻想它们互不干扰。

练习 86.4B

#	练习	难度
1	用 Pinocchio 取出 \(\mathbf{J}_{ee,b}\)（末端雅可比前 6 列），让臂保持不动只晃基座，验证末端速度确实由 \(\mathbf{J}_{ee,b}\mathbf{v}_b\) 决定。	⭐⭐
2	在滞空假设下，用公式 86.4B-3 计算给定臂关节速度时的基座反向 twist，并与"甩臂转身"的直觉对照。	⭐⭐⭐
3	设计一个实验：四脚撑地时让臂做同一个快速加速度，分别开启/关闭 WBC 对 \(\mathbf{M}_{ba}\ddot{\mathbf{q}}_a\) 的接触力补偿，比较基座 pitch 峰值。	⭐⭐⭐⭐

---

86.5 末端 wrench：被动扰动与主动接触 ⭐⭐⭐

这一节解决末端力如何进入控制器的问题。

末端 wrench 有两种完全不同的建模方式。

第一种是被动扰动。

机器人不知道外界什么时候推它，控制器只估计或鲁棒抑制。

第二种是主动接触。

机器人有计划地用手推门、拉抽屉、撑墙或搬物，控制器必须把末端接触纳入接触集合。

86.5.1 被动扰动模型

被动扰动可以写成：

\[ \mathbf{M}\dot{\mathbf{v}}+\mathbf{h} = \mathbf{S}^{T}\tau + \mathbf{J}_c^{T}\lambda + \mathbf{J}_{ee}^{T}\hat{\mathbf{f}}_{dist} \]

其中 \(\hat{\mathbf{f}}_{dist}\) 可以来自：

来源	优势	局限
腕部 F/T 传感器	直接测量接触力	成本高，安装和标定复杂
动量观测器	不需要额外传感器	对模型误差敏感
关节电流估计	硬件已有	减速器摩擦和温漂影响大
RL 隐变量	可吸收复杂误差	可解释性差

被动扰动下，WBC 常做两件事。

第一，放松低优先级的末端 tracking，避免为了追踪而抵抗不可预测外力。

第二，提高姿态、CoM、摩擦裕度相关任务的权重或优先级。

86.5.2 主动接触模型

主动接触时，末端成为接触集合的一部分：

\[ C(t) = C_{foot}(t)\cup C_{ee}(t) \]

对应接触雅可比：

\[ \mathbf{J}_{c}^{(\sigma)} = \begin{bmatrix} \mathbf{J}_{foot,1} \\ \cdots \\ \mathbf{J}_{foot,k} \\ \mathbf{J}_{ee} \end{bmatrix} \]

接触不滑约束：

\[ \mathbf{J}_{c}^{(\sigma)}\dot{\mathbf{v}} + \dot{\mathbf{J}}_{c}^{(\sigma)}\mathbf{v} = \mathbf{0} \]

模式变量从纯四足的：

\[ \sigma_{foot}(t)\in\{0,1\}^{4} \]

扩展为：

\[ \sigma(t)\in\{0,1\}^{4}\times\{0,1\}^{n_{ee}} \]

一个推门任务可以表示为：

阶段	足端模式	末端模式	主要任务
接近	trot/walk	EE=0	移动到门前，末端预对准
触碰	多足支撑	EE=1	软接触，限制冲击
推动	稳定支撑	EE=1	末端力/位姿混合控制
脱离	稳定支撑	EE=0	释放接触，恢复行走

86.5.3 末端摩擦锥和脚底摩擦锥不同

脚底通常是橡胶接触地面。

末端可能接触金属门把手、木门、墙面、把手、软物体。

所以脚底摩擦系数和末端摩擦系数不应共用。

接触	典型摩擦	建模方式	注意点
橡胶脚垫-地面	0.5-0.9	点接触摩擦锥	法向力非负
夹爪-门把手	0.2-0.5	约束方向依赖	可能有滚动或滑动
末端-墙面	0.3-0.7	单侧接触	只能推不能拉
抓取物体	依夹爪而定	物体动力学增广	需处理物体惯量

如果把末端摩擦系数设得过高，优化器会认为手可以提供大量切向力。

实际接触一旦滑动，足底补偿也会失效。

86.5.4 末端阻抗模式

实际操作中，末端不总是纯位置控制。

更常见的是阻抗控制：

\[ \mathbf{f}_{ee}^{ref} = \mathbf{K}_{imp}(\mathbf{x}_{ref}-\mathbf{x}_{ee}) + \mathbf{D}_{imp}(\dot{\mathbf{x}}_{ref}-\dot{\mathbf{x}}_{ee}) \]

这个力再通过全身 WBC 分配到关节力矩和足底反力。

模式	适用任务	风险
位置控制	空中到达、抓取预对准	接触瞬间冲击大
力控制	推门、按按钮、擦拭	需要力传感或估计
阻抗控制	接触过渡、柔顺操作	增益难调
混合控制	插入、开门、托举	坐标系和约束方向要清晰

⚠️ 常见陷阱

⚠️ 工程陷阱：末端接触进入 WBC，却没有进入 mode schedule

错误做法：手推门时只在代价里加力跟踪，不把 EE 接触加入约束集合。

现象：WBC 仍把末端当作自由空间任务，接触力估计和加速度约束冲突。

正确做法：触碰阶段切换 mode，加入 EE 接触约束或阻抗任务，并同步调整足端支撑任务。

💡 概念误区：末端力越大，操作越稳

末端力越大，足底需要承担的反作用力越大。

如果支撑多边形和摩擦裕度不足，增大末端力会让系统更不稳。

操作力必须与足底可行力共同优化。

🧠 思维陷阱：把主动接触和外部扰动混为一谈

主动接触有目标、方向和阶段。

外部扰动不一定可预测。

前者应进入规划和约束，后者更多依赖观测、鲁棒控制和降级策略。

练习 86.5

#	练习	难度
1	为"推门"任务写出四阶段 mode schedule，明确每阶段 EE 是否为接触点。	⭐⭐
2	比较脚底摩擦锥和末端墙面接触约束，写出二者在线性 QP 中的不同不等式。	⭐⭐⭐
3	设计一个阻抗增益调度策略：接近时低刚度，稳定接触后提高法向刚度，脱离时降低阻尼。	⭐⭐⭐

---

86.6 从 ALMA 到 Unified MPC：历史线索与技术动机 ⭐⭐

这一节把几个代表工作放在同一条线里理解。

历史不是为了记名字。

历史能帮助你理解为什么现在的四足臂架构长成这个样子。

86.6.1 Khatib 到 Sentis：操作空间和任务优先级

Khatib 1987 的操作空间公式把机械臂控制从关节空间推向任务空间。

核心思想是：末端任务应该用末端动力学描述，而不是只在关节空间做 PD。

操作空间惯量为：

\[ \Lambda(q) = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{T})^{-1} \]

动力学一致伪逆为：

\[ \bar{\mathbf{J}} = \mathbf{M}^{-1}\mathbf{J}^{T}\Lambda \]

Sentis 和 Khatib 将这个思想推广到全身任务优先级。

对四足臂来说，这条线提供了两个基础概念。

第一，末端任务应该在任务空间表达。

第二，低优先级任务必须在高优先级任务的零空间中执行。

86.6.2 ALMA：动态行走中操作的早期样板

ALMA 使用 ANYmal + Kinova 臂，展示了动态移动操作。

它的典型结构是：

ZMP / motion planner
    ├─ CoM reference
    ├─ base reference
    └─ EE reference
        ↓
Hierarchical QP WBC
    ├─ dynamics
    ├─ contact
    ├─ base stabilization
    ├─ EE tracking
    └─ posture

ALMA 的贡献在于证明四足臂不是只能静态站立操作。

它也暴露了 ZMP 规划的局限。

当任务涉及快速步态、多接触切换和物体动力学时，ZMP+参考轨迹不够统一。

86.6.3 Sleiman 2021：统一 MPC

Sleiman 2021 将 locomotion 和 manipulation 放进同一个 switched OCP。

简化表达为：

\[ \min_{\mathbf{x},\mathbf{u}} \int_{0}^{T} \ell^{(\sigma(t))}(\mathbf{x},\mathbf{u},t)\,dt + \Phi(\mathbf{x}(T)) \]

约束为：

\[ \dot{\mathbf{x}} = \mathbf{f}^{(\sigma(t))}(\mathbf{x},\mathbf{u}) \]

其中 \(\sigma(t)\) 表示接触模式。

这种写法把足端接触、末端接触和物体动力学统一到模式切换系统中。

它比 ALMA 更适合多阶段任务。

代价是 OCP 构造更复杂，求解器和参考管理器要求更高。

86.6.4 Sleiman 2023：接触模式也要规划

Unified MPC 假设接触模式序列大体已知。

但真实操作中，机器人可能需要选择：

场景	可能接触
开洗碗机	手拉把手、脚抵门、身体保持支撑
通过弹簧门	腿卡门、手推门、身体侧向移动
搬动物体	手抓、脚调整支撑、物体接触环境

Sleiman 2023 使用外层搜索生成接触模式，再由内层轨迹优化求连续运动。

这说明四足臂已经不只是控制问题。

它逐渐变成任务与运动规划、接触选择、全身控制共同构成的问题。

86.6.5 qm_control：开源工程主轴

qm_control 的价值在于把理论路线落到可读代码上。

它不是最通用的框架。

它的优势是将 OCS2、Pinocchio、WBC、ros_control、状态估计和四足臂 URDF 串成完整链路。

模块	作用
qm_description	A1 + 6DoF 臂统一 URDF
qm_interface	OCS2 OCP 构造，包含状态、输入、代价和约束
qm_wbc	分层 QP WBC，把 MPC 参考转为力矩
qm_estimation	线性 KF 基座估计
qm_controllers	ros_control 控制器入口
四分支	运动、力扰动、柔顺、实机链路的不同侧面

86.6.6 架构对照表

维度	ALMA	Sleiman 2021	Sleiman 2023	qm_control
主模型	ZMP + WBC	Centroidal switched OCP	接触搜索 + 多接触 OCP	OCS2 NMPC + WBC
是否规划接触模式	主要人工设计	给定模式序列	外层搜索生成	配置和参考管理
末端任务	参考轨迹跟踪	OCP 内统一代价	多接触任务段	EE cost + WBC task
物体动力学	弱	可增广	强	以工程扩展为主
可复现性	论文为主	OCS2 框架可参考	无独立完整代码	开源工程最直接

⚠️ 常见陷阱

⚠️ 概念误区：认为 ALMA 已经过时所以不用学

ALMA 的 ZMP+WBC 思路仍然是理解四足臂任务分层的好入口。

它展示了准静态到动态操作的工程过渡。

Unified MPC 解决的是更强的统一性和多接触问题，而不是否定 WBC 分层思想。

⚠️ 工程陷阱：把 OCS2 legged_robot 和 mobile_manipulator 直接拼目录

两个示例的模型、状态、输入和参考管理不同。

四足臂需要重新定义状态布局、接触模式、EE 代价和 WBC 接口。

正确做法是先比较两个示例的 Interface 和 PreComputation，再设计统一数据结构。

🧠 思维陷阱：把开源项目当作唯一真理

qm_control 是很好的工程入口，但不是唯一正确架构。

它的依赖、分支和平台选择都有历史原因。

学习时要抽象出状态布局、代价、约束、参考管理和 WBC 任务，而不是只记文件名。

练习 86.6

#	练习	难度
1	画出 ALMA、Sleiman 2021、qm_control 的数据流图，标注 MPC 和 WBC 的分界。	⭐⭐
2	阅读 OCS2 legged_robot 与 mobile_manipulator 的顶层目录，列出各自最核心的 5 个类。	⭐⭐
3	设计一个"开抽屉"任务：说明哪些阶段可用 ALMA 式分层，哪些阶段更需要 Unified MPC。	⭐⭐⭐

---

86.7 分层 WBC 与统一 MPC：两种范式的取舍 ⭐⭐⭐

这一节给出架构选型框架。

你以后读 qm_control、Deep-WBC、RAMBO 时，都要不断判断它们在这个坐标系中的位置。

86.7.1 分层 WBC 的思路

分层 WBC 把问题拆成两层。

上层用简化模型规划较长时域的参考。

下层用全身动力学在当前时刻求满足约束的力矩。

MPC:
  min   long-horizon cost
  s.t.  simplified dynamics
  out   h_G_ref, lambda_ref, q_ref, ee_ref

WBC:
  min   task tracking + torque smoothness
  s.t.  full dynamics, contact, friction, limits
  out   tau

分层的好处是实时性和调试性。

MPC 不必直接处理所有关节动力学。

WBC 不必规划未来 1 秒的复杂接触序列。

86.7.2 统一 MPC 的思路

统一 MPC 希望在一个 OCP 中同时处理更多事情。

它的表达能力更强。

例如可以让优化器自己权衡：

冲突	统一 MPC 的处理方式
手要前伸，基座要稳定	同一 OCP 里同时看 EE cost 和 centroidal dynamics
推门需要力，脚底摩擦有限	接触力和末端力在同一约束中优化
物体移动会改变任务	物体动力学作为状态增广
未来要换接触	mode schedule 在 horizon 内体现

统一 MPC 的代价是求解更难。

如果 OCP 失败，底层可能没有足够强的独立恢复能力。

因此实际工程常采用混合方案。

例如上层统一程度较高的 MPC，底层仍保留强 WBC 和安全降级。

86.7.2B 分层架构的"安全降级"优势

反事实推理：如果统一 MPC 求解失败（比如约束不可行或超时），整个控制栈会怎样？ 在纯统一架构中，MPC 失败意味着没有新的控制命令——机器人在该周期可能执行上一步的过时命令或直接停止。 在分层架构中，MPC 失败时 WBC 仍然可以使用上一次的参考轨迹继续运行。WBC 本身频率更高（500-1000 Hz），即使 MPC 在 50 Hz 漏掉一两次，WBC 仍然在维持站立和姿态稳定。 这就是分层架构的核心安全优势：下层控制器提供了对上层失败的天然缓冲。 实际工程中，这个"缓冲"常常是选择分层而非统一架构的最重要理由——不是因为统一架构理论更差，而是因为失败时的降级行为更可控。

86.7.3 加权 QP、HQP 与混合优先级

对四足臂 WBC，常见三种实现方式如下。

方法	优先级保证	求解次数	调参难度	适用
单层加权 QP	无严格保证	1	权重敏感	入门、实时强约束
严格 HQP	强	多次	排优先级为主	安全关键、论文验证
混合优先级	中到强	1-2	中等	工程常用

单层加权 QP 可以写成：

\[ \min_{\dot{v},\lambda,\tau} \sum_i w_i \|\mathbf{A}_i\dot{\mathbf{v}}-\mathbf{b}_i\|^2 + \rho\|\tau-\tau_{ref}\|^2 \]

约束包括：

\[ \mathbf{M}\dot{\mathbf{v}}+\mathbf{h} = \mathbf{S}^{T}\tau+\mathbf{J}_{c}^{T}\lambda+\mathbf{J}_{ee}^{T}f_{ee} \]

\[ \mathbf{D}\lambda\le \mathbf{d} \]

\[ \tau_{min}\le \tau\le \tau_{max} \]

HQP 则逐层求解：

\[ \min_{\mathbf{z},s_k}\|s_k\|^2 \quad \text{s.t.} \quad \mathbf{A}_k\mathbf{z}=\mathbf{b}_k+s_k \]

并保持上层最优残差不被破坏。

86.7.4 架构选择流程

任务是否安全关键？
  ├─ 是 → 是否需要严格接触/力矩保证？
  │       ├─ 是 → HQP 或混合优先级 WBC
  │       └─ 否 → 加权 QP + 强监控
  └─ 否 → 是否需要复杂接触规划？
          ├─ 是 → Unified MPC / TAMP + WBC
          └─ 否 → 分层 MPC + WBC

是否有大量示教或仿真数据？
  ├─ 是 → 可考虑 RL WBC 或 RL residual
  └─ 否 → 优先 model-based baseline

86.7.5 一个最小 WBC QP 框架

struct WbcProblem {
  Eigen::MatrixXd H;
  Eigen::VectorXd g;
  Eigen::MatrixXd Aeq;
  Eigen::VectorXd beq;
  Eigen::MatrixXd Aineq;
  Eigen::VectorXd bineq;
};

class QuadArmWbcBuilder {
 public:
  WbcProblem build(const RobotState& state,
                   const MpcReference& ref,
                   const ContactMode& mode) {
    WbcProblem qp;
    // 决策变量 z = [vdot, lambda, tau]。
    allocate(qp, state.nv, mode.forceDim(), state.actuatedDof());
    addFloatingBaseDynamics(qp, state, mode);
    addContactAcceleration(qp, state, mode);
    addFrictionCone(qp, mode);
    addTorqueLimits(qp, state);
    addBaseTask(qp, state, ref);
    addMomentumTask(qp, state, ref);
    addEndEffectorTask(qp, state, ref);
    addPostureRegularizer(qp, state, ref);
    return qp;
  }

 private:
  void addEndEffectorTask(WbcProblem& qp,
                          const RobotState& state,
                          const MpcReference& ref) {
    // 末端任务使用加速度级 PD：
    // J_ee * vdot + dJ_ee * v = xddot_des。
    // xddot_des 由 SE(3) 误差和末端速度误差生成。
  }
};

这个框架不绑定具体求解器。

教学时可用 OSQP 或 ProxQP。

实机高频 WBC 更推荐小规模密集 QP 求解器，并严格避免控制热路径上的堆分配。

⚠️ 常见陷阱

⚠️ 工程陷阱：在 WBC 中使用慢速非线性求解器

WBC 运行在 200-1000 Hz。

它需要小规模 QP 快速求解，而不是每次启动 IPOPT 或 SNOPT。

如果单次控制周期 1 ms，求解器和动力学计算总和必须稳定低于预算。

💡 概念误区：权重差 100 倍就等于严格优先级

加权 QP 的权重只能表达偏好，不能表达绝对优先。

当低优先级任务误差很大或雅可比尺度很大时，仍可能污染高优先级任务。

安全任务应进入硬约束或更高层级。

🧠 思维陷阱：只追求理论最优

控制器在机器人上必须能降级。

一个可解释、可监控、可退回站立模式的分层方案，常常比一个难以调试的统一大优化更适合早期平台。

练习 86.7

#	练习	难度
1	用矩阵维度估算 Go2+Z1 的 WBC QP 规模：变量、等式、不等式各多少。	⭐⭐
2	用同一任务实现加权 QP 和两层 HQP，比较 EE tracking 与 base pitch 稳定之间的冲突。	⭐⭐⭐
3	设计一个 WBC 降级策略：QP 不可行时怎样放松任务、保留哪些硬约束、输出什么安全命令。	⭐⭐⭐

---

86.8 代码实验：纯加权 WBC 的最小实现路线 ⭐⭐

这一节给出本章的实践骨架。

目标不是替代 qm_control。

目标是让你在进入 qm_control 之前，先亲手组装一个最小问题。

86.8.1 实验目标

平台：Go2+Z1 或 A1+6DoF 臂。

仿真：MuJoCo、Gazebo 或 Isaac Sim 均可。

任务：机器人四脚站立，末端跟踪一个小幅正弦轨迹。

必须记录：

指标	目标
末端位置 RMS	小于 3 cm
基座 roll/pitch 最大值	小于 3 deg
足底法向力	全程非负且不低于安全阈值
摩擦锥裕度	不出现明显越界
WBC 求解时间	低于控制周期 50%

86.8.2 模块分解

quad_arm_wbc_demo/
  include/
    RobotModel.hpp
    WbcProblem.hpp
    QpSolver.hpp
    ReferenceGenerator.hpp
  src/
    RobotModel.cpp
    WbcProblem.cpp
    QpSolver.cpp
    main.cpp
  config/
    robot.yaml
    wbc_weights.yaml
    contact_schedule.yaml

86.8.3 状态接口

struct RobotState {
  Eigen::VectorXd q;       // Pinocchio 构型，含浮动基座四元数
  Eigen::VectorXd v;       // 广义速度
  Eigen::VectorXd tau_meas;// 实测或仿真反馈力矩
  Eigen::Vector3d base_rpy;
  Eigen::Vector3d com;
  double time = 0.0;
};

struct EndEffectorReference {
  pinocchio::SE3 pose;
  Eigen::Matrix<double, 6, 1> velocity;
  Eigen::Matrix<double, 6, 1> acceleration;
  Eigen::Matrix<double, 6, 1> wrench;
};

86.8.4 参考轨迹

EndEffectorReference makeSineReference(double t,
                                       const pinocchio::SE3& nominal) {
  EndEffectorReference ref;
  ref.pose = nominal;
  // 小幅正弦，避免一开始就把系统推到工作空间边界。
  ref.pose.translation().x() += 0.05 * std::sin(2.0 * M_PI * 0.25 * t);
  ref.velocity.setZero();
  ref.velocity(0) = 0.05 * 2.0 * M_PI * 0.25 *
                    std::cos(2.0 * M_PI * 0.25 * t);
  ref.acceleration.setZero();
  ref.wrench.setZero();
  return ref;
}

86.8.5 末端 SE(3) 加速度任务

Eigen::Matrix<double, 6, 1> se3TaskAcceleration(
    const pinocchio::SE3& current,
    const Eigen::Matrix<double, 6, 1>& current_vel,
    const EndEffectorReference& ref,
    const Eigen::Matrix<double, 6, 6>& Kp,
    const Eigen::Matrix<double, 6, 6>& Kd) {
  // current.actInv(ref.pose) = T_current^{-1} T_ref。
  // 这是当前末端局部坐标中的左不变误差。
  pinocchio::SE3 error_se3 = current.actInv(ref.pose);
  Eigen::Matrix<double, 6, 1> error = pinocchio::log6(error_se3).toVector();
  Eigen::Matrix<double, 6, 1> vel_error = ref.velocity - current_vel;
  return ref.acceleration + Kp * error + Kd * vel_error;
}

这里固定采用 LOCAL/body-frame 约定：error、current_vel、ref.velocity 以及上游用于生成速度的末端 Jacobian 都必须在同一个局部坐标系中。

如果控制器选择 LOCAL_WORLD_ALIGNED，就不能直接复用上面的 current.actInv(ref.pose) 注释和速度残差，而应把位姿误差、速度和 Jacobian 一起转换到世界对齐坐标。

坐标系不一致时，单独看位置误差可能还正常，但姿态误差和阻尼项会沿错误方向施力。

86.8.6 支撑裕度监控

struct SupportMargin {
  double min_normal_force = 0.0;
  double max_friction_ratio = 0.0;
  double zmp_margin = 0.0;
};

SupportMargin evaluateSupport(const Eigen::VectorXd& lambda,
                              double mu,
                              const SupportPolygon& polygon,
                              const Eigen::Vector2d& zmp) {
  SupportMargin out;
  out.min_normal_force = std::numeric_limits<double>::infinity();
  out.max_friction_ratio = 0.0;
  for (int i = 0; i < 4; ++i) {
    Eigen::Vector3d f = lambda.segment<3>(3 * i);
    out.min_normal_force = std::min(out.min_normal_force, f.z());
    double tangential = std::hypot(f.x(), f.y());
    double ratio = tangential / std::max(1e-3, mu * f.z());
    out.max_friction_ratio = std::max(out.max_friction_ratio, ratio);
  }
  out.zmp_margin = polygon.signedDistanceToBoundary(zmp);
  return out;
}

86.8.7 实验对照

实验至少跑三组。

组别	配置	预期现象
A	只追 EE，不约束基座姿态	末端误差小，基座晃动大
B	EE + base 姿态加权 QP	两者折中，权重敏感
C	base 姿态高优先，EE 低优先	稳定性好，末端误差增大

这三组实验比阅读结论更重要。

你会直观看到"操作精度与行走稳定"不是文字上的冲突，而是同一个 QP 中的数值冲突。

⚠️ 常见陷阱

⚠️ 编程陷阱：正弦幅度太大

新手常把末端轨迹幅度设为 0.3 m 以上。

这会直接触发工作空间边界、自碰撞或支撑裕度不足。

最小实验应从 0.03-0.05 m 开始。

⚠️ 工程陷阱：只看末端误差，不记录足底力

末端误差小不代表控制器安全。

如果足底力接近摩擦锥边界或法向力接近零，真机上风险很高。

每次实验都要同时记录末端误差、基座姿态、接触力和求解时间。

🧠 思维陷阱：仿真站住就认为控制器正确

仿真器的接触、阻尼和积分器可能掩盖控制问题。

应通过扰动、摩擦系数变化、载荷变化和延迟测试控制器鲁棒性。

练习 86.8

#	练习	难度
1	完成 A/B/C 三组实验，绘制末端误差和基座 pitch 的时间曲线。	⭐⭐
2	将摩擦系数从 0.8 降到 0.35，观察哪组最先失败，并解释原因。	⭐⭐⭐
3	在末端轨迹中加入 100 ms 延迟，比较 WBC 是否出现相位滞后和扭矩尖峰。	⭐⭐⭐

---

86.9 硬件与仿真平台选型 ⭐⭐

这一节帮助你理解为什么很多论文选不同平台。

四足臂平台不是越贵越好。

教学、算法研究、力控操作、野外任务对平台的要求不同。

86.9.1 常见平台对比

平台	优势	局限	适合任务
Go2+Z1	成本低、社区活跃、URDF 多	载荷裕度有限，臂重影响明显	教学、快速复现、轻量操作
A1/Aliengo+Z1	qm_control 默认生态接近	硬件较旧，维护成本更高	qm_control 复现
B1/B2+Z1	载荷大、稳定性更强	成本和体积增加	重载、户外、推拉任务
ANYmal+DynaArm	论文生态强、力控能力好	获取门槛高	高质量研究验证
Spot+Spot Arm	商业成熟、感知强	底层控制封闭	应用集成，不适合 WBC 研究
Go2+ARX5	UMI-on-Legs 生态，轻量	payload 小，精度有限	操作策略迁移、视觉模仿

86.9.2 仿真器选择

仿真器	优势	局限	适合
MuJoCo	接触稳定、调试轻量	大规模 RL 并行不如 GPU 原生	WBC/MPC 小规模验证
Gazebo Classic	ROS1 生态兼容	接触和实时性一般	qm_control 原生链路
IsaacGym	大规模并行训练快	已逐步被新平台替代	Deep-WBC 原始复现
IsaacLab / Isaac Sim	当前主流，传感器丰富	版本锁定和资源要求高	RL、视觉、Sim2Real
RaiSim	腿足传统生态	商业许可和视觉弱	部分四足 RL

86.9.3 选型决策

目标是学习统一动力学？
  → MuJoCo + Pinocchio 最轻量

目标是复现 qm_control？
  → Ubuntu 20.04 + ROS Noetic + Gazebo + OCS2

目标是训练 Deep-WBC/VBC？
  → IsaacGym 或迁移到 IsaacLab

目标是真机轻量操作？
  → Go2+ARX5 或 Go2+Z1

目标是力敏感推拉？
  → B1/B2 或 ANYmal 级平台更合适

86.9.4 控制频率建议

层级	建议频率	说明
视觉/任务层	5-20 Hz	物体位姿和阶段切换不需要 1 kHz
MPC	20-100 Hz	horizon 优化，不必每个电机周期求解
WBC	200-1000 Hz	受力和姿态需要快速响应
电机伺服	1-10 kHz	由硬件驱动层实现
RL 低层策略	50-200 Hz	常通过 decimation 与仿真步长解耦

⚠️ 常见陷阱

⚠️ 工程陷阱：在轻量平台上做重载力控

Go2+Z1 适合教学和轻量操作。

如果任务是强推门、托重物或长时间接触，载荷裕度可能不足。

平台选型必须先看总质量、臂质量、payload、足底摩擦和电机热限制。

💡 概念误区：仿真器越复杂越适合入门

入门阶段需要可快速重启、可打印矩阵、可复现实验。

复杂视觉和大规模并行可以后移。

先用 Pinocchio + MuJoCo 证明控制链，再迁移到 IsaacLab 更稳。

🧠 思维陷阱：把商业平台当作控制研究平台

如果底层力矩和 WBC 接口封闭，很多控制算法无法验证。

商业平台适合应用任务和感知集成，不一定适合研究全身动力学控制。

练习 86.9

#	练习	难度
1	为"教学演示、推门、视觉抓取、真机力控"四类任务分别选平台和仿真器。	⭐
2	估算 Go2+Z1 和 B1+Z1 的末端推力裕度差异，说明质量和支撑多边形的影响。	⭐⭐
3	设计一个从 MuJoCo WBC 到 IsaacLab RL 的迁移路线，列出每一步的验证指标。	⭐⭐⭐

---

86.10 评估指标：不要只看手有没有到 ⭐⭐

四足臂任务的评价必须覆盖运动、操作、接触、安全和实时性。

只看末端位置误差会掩盖很多问题。

86.10.1 指标总表

类别	指标	典型阈值	说明
末端跟踪	位置 RMS	1-3 cm	教学平台可放宽
末端跟踪	姿态 RMS	3-8 deg	取决于任务
基座稳定	roll/pitch 最大值	2-5 deg	行走中可略大
支撑安全	最小法向力	> 0 且有裕度	避免脚离地
摩擦安全	\(\|f_t\|/(\mu f_n)\)	< 0.8	留出建模误差
动量	角动量峰值	任务相关	关注突变
约束	自碰撞最小距离	> 2-5 cm	取决于模型精度
实时性	WBC p99 时间	< 周期 50%-70%	留出通信和驱动
鲁棒性	外推恢复时间	< 1-2 s	push recovery 指标

86.10.2 指标之间的冲突

追求	可能牺牲	原因
更小 EE 误差	更大基座晃动	机身姿态参与末端运动
更大末端力	更小摩擦裕度	反作用力由足底承担
更高阻抗	更大冲击	接触瞬间力尖峰
更平滑扭矩	更慢末端响应	控制能量受限
更高步速	更差操作精度	基座扰动和视觉延迟增加

86.10.3 实验报告模板

每个四足臂实验至少记录下面内容。

项目	内容
机器人	平台、质量、臂型号、payload
仿真/真机	仿真器版本或硬件版本
控制频率	MPC/WBC/策略/电机频率
任务	EE 目标、步态、接触阶段
约束	摩擦系数、力矩限、关节限、自碰撞距离
指标	末端、基座、接触力、求解时间
失败模式	滑动、翻倒、碰撞、求解失败、热保护

⚠️ 常见陷阱

⚠️ 工程陷阱：只报告平均求解时间

实时控制更关心 p99 和最大值。

一次 10 ms 的偶发尖峰可能比平均 0.5 ms 更危险。

记录求解时间分布，而不是只写均值。

💡 概念误区：末端精度高就代表操作成功

推门、擦拭、插入等任务还需要力、方向、接触稳定和阶段完成条件。

位置误差只是其中一个指标。

🧠 思维陷阱：忽视失败样本

四足臂任务的价值往往体现在失败模式分析。

每次失败都应归类为接触、估计、规划、WBC、执行器或感知问题。

练习 86.10

#	练习	难度
1	为"边走边抓杯子"设计 8 个指标，并说明每个指标的传感来源。	⭐⭐
2	设计一个推恢复实验，对比冻结臂和允许臂摆动两种策略的恢复率。	⭐⭐⭐
3	用一组实验数据写出失败归因表：哪些是模型问题，哪些是约束问题，哪些是执行延迟问题。	⭐⭐⭐

---

86.11 本章小结

86.11.1 核心公式速查

公式	含义
\(\mathbf{M}\dot{\mathbf{v}}+\mathbf{h}=\mathbf{S}^{T}\tau+\sum\mathbf{J}_{c}^{T}\lambda+\mathbf{J}_{ee}^{T}f_{ee}\)	四足臂统一动力学
\(\mathbf{h}_G=\mathbf{A}_G(q)\mathbf{v}\)	质心动量矩阵定义
\(\dot{\mathbf{h}}_G=[\sum r_i\times f_i;\,mg+\sum f_i]\)	质心动量变化率
\(x_{zmp}=x_{com}-z_{com}\ddot{x}/g+\tau_y/(mg)-z_{ee}F_x/(mg)\)	含末端力的 ZMP 偏移近似
\(J_c\dot{v}+\dot{J}_c v=0\)	接触不滑加速度约束
\(f_{ee}^{ref}=K(x_{ref}-x)+D(\dot{x}_{ref}-\dot{x})\)	末端阻抗控制

86.11.2 知识点掌握矩阵

知识点	入门	工程	研究
为什么不能拼接腿控和臂控	能举反例	能用数据证明耦合	能设计统一优化问题
统一动力学	能写公式	能用 Pinocchio 计算	能推导广义力来源
约化动力学与接触消去	能说出基座行不含 \(\tau\)	能写基座 6 行硬约束	能用零空间投影构造约化方程
CMM 与角动量	能解释臂列块	能监控角动量	能做动量中心 WBC
whole-body 雅可比分块	能说出末端速度三方来源	能用完整 \(\mathbf{J}_{ee}\) 建任务	能用反作用零空间映射调基座
末端接触	能区分扰动和主动接触	能写 mode schedule	能做多接触规划
WBC 优先级	能画任务图	能组装 QP	能比较 HQP/加权/混合
架构选型	能说出优缺点	能按任务选型	能提出混合方案

86.11.3 与后续章节的连接

本章建立了四足臂控制的"坐标系"。后续每章都在这个坐标系中的不同位置工作：

后续章节	本章提供的基础	具体依赖关系
复合/170_qm_control精读	统一动力学、OCS2+WBC 分层、任务优先级	qm_control 的 OCP 直接使用本章 86.3 的统一方程；WBC 任务矩阵对应本章 86.4.5 的优先级图；摩擦锥约束对应 86.5.3 的末端接触建模
复合/180_Deep_WBC精读	为什么 RL 要把腿臂放进统一动作空间	Deep-WBC 的统一策略思想来源于本章 86.2.2 中"不能简单拼接"的论证；其观测设计需要理解 86.4 中 CMM 与角动量的关系
复合/190_Visual_WBC精读	视觉目标如何变成低层 EE goal-reaching	视觉系统提供的末端目标如何通过本章 86.5 的接口进入 WBC 或 RL 策略
复合/200_UMI_on_Legs精读	task-frame EE tracking 如何吸收基座扰动	UMI 的操作轨迹在 body frame 中表达，正是利用了本章 86.3.6 中 \(\mathbf{J}_{ee}\) 前 6 列非零的物理事实
复合/210_RAMBO混合MPC_RL	前馈 model-based 与反馈 RL residual 的接口	RAMBO 的 model-based 前馈部分复用了本章的统一动力学和分层 WBC

---

累积项目：四足臂全身控制器的最小闭环

本章新增项目模块是"四足臂统一动力学与最小 WBC"。

目标是在仿真中搭建一个可观测、可调试、可扩展的 baseline。

阶段 1：统一模型加载

1. 准备 Go2+Z1、A1+6DoF 或任意四足臂 URDF。
2. 使用 Pinocchio JointModelFreeFlyer 加载。
3. 打印 \(n_q/n_v\)、关节名、frame 名。
4. 计算 \(M(q)\)、\(h(q,v)\)、\(J_{foot}\)、\(J_{ee}\)。
5. 可视化质量矩阵三分块。

阶段 2：站立 WBC

1. 决策变量设为 \(\dot{v},\lambda,\tau\)。
2. 加入全身动力学等式。
3. 加入四足接触不滑约束。
4. 加入摩擦锥和力矩限幅。
5. 加入 base 姿态、CoM、EE tracking 和 posture 正则。
6. 使用 QP 求解器输出力矩。

阶段 3：末端正弦任务

1. 先让机器人静态站立。
2. 再让末端执行 0.05 m 幅度正弦轨迹。
3. 记录末端误差、基座姿态、足底力和求解时间。
4. 调整 EE 权重，观察稳定性变化。
5. 加入外部推力，测试扰动恢复。

阶段 4：主动接触准备

1. 为末端接触添加 mode flag。
2. 设计接近、触碰、推动、脱离四阶段。
3. 在触碰阶段降低末端刚度。
4. 在推动阶段加入末端 wrench reference。
5. 在脱离阶段释放 EE 接触并恢复行走参考。

---

延伸阅读

材料	类型	难度	为什么读
Khatib 1987 Operational Space Formulation	论文	⭐⭐⭐	操作空间和动力学一致伪逆的源头
Sentis & Khatib 2005 Whole-Body Behaviors	论文	⭐⭐⭐	任务优先级和全身行为
Bellicoso et al. 2019 ALMA	论文	⭐⭐	四足动态操作的早期完整样板
Sleiman et al. 2021 Unified MPC	论文	⭐⭐⭐⭐	统一多接触 MPC 的核心
Sleiman et al. 2023 Versatile Multi-Contact	论文	⭐⭐⭐⭐	接触模式搜索与多阶段任务
Zhang et al. 2024 qm_control	论文/代码	⭐⭐⭐	开源四足臂 MPC+WBC 主轴
Pinocchio centroidal API	文档	⭐⭐	CMM、CCRBA、动量计算
OCS2 legged_robot 与 mobile_manipulator	代码	⭐⭐⭐	理解框架如何表达 locomotion 与 manipulation

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
末端跟踪准确但基座大幅俯仰	EE 权重过高，base 姿态优先级不足	1. 降低 EE 权重 2. 增加 base pitch 任务 3. 检查 ZMP 裕度	86.2, 86.7
QP 输出足底水平力过大	摩擦锥约束缺失或摩擦系数过高	1. 打印 \(\|f_t\|/(\mu f_n)\) 2. 检查不等式矩阵 3. 降低 \(\mu\) 做压力测试	86.5, 86.10
Pinocchio 质量矩阵求逆异常	CRBA 下三角未对称化或构型无效	1. 对称化 data.M 2. 检查四元数归一化 3. 检查 URDF 惯量	86.3
末端推门时后脚离地	末端反力没有进入支撑裕度评估	1. 估算 ZMP 偏移 2. 增大支撑脚间距 3. 降低推力或改用多足支撑	86.4, 86.5
WBC 求解时间偶发超时	动态分配、求解器 warm start 失效、约束切换过猛	1. 记录 p99/max 2. 固定矩阵尺寸 3. 平滑 mode 切换 4. 使用更适合小 QP 的求解器	86.7, 86.10
EE 接触阶段振荡	位置刚度过高，阻抗和接触模式不同步	1. 触碰阶段降低刚度 2. 加速度参考限幅 3. 检查接触 flag 与力任务是否同步	86.5
臂快速摆动导致 IMU 估计漂移	状态估计未建模臂引起的基座加速度	1. 降低臂加速度 2. 在估计器中加入全身模型 3. 用足端接触更新校正	86.4
统一 MPC 参考进入 WBC 后扭矩尖峰	MPC/WBC 时间戳不同步或插值不连续	1. 检查参考时间基准 2. 对 q/force/EE 参考做样条插值 3. 限制参考变化率	86.7
WBC 算出力矩偏小但真机一动就发散	基座 6 行动力学等式里给 \(\tau\) 配了非零系数	1. 打印等式矩阵中 \(\tau\) 对应的前 6 行块 2. 确认其恒为 \(\mathbf{0}_{6\times18}\) 3. 核对选择矩阵 \(\mathbf{S}\)	86.3B
行走时末端误差出现步频谐波	末端任务雅可比只取臂列，丢了基座列	1. 原地踏步看误差是否含步频分量 2. 改用完整 \(\mathbf{J}_{ee}\) 3. 确认基座列非零	86.4B

---

综合项目：边走边推门的架构设计

目标：设计一个不要求一次实现完整代码、但要求变量和接口自洽的四足臂推门系统。

项目要求

1. 选择一个平台：Go2+Z1、B1+Z1 或 ANYmal+DynaArm。
2. 写出统一动力学方程中的所有维度。
3. 设计四阶段 mode schedule。
4. 设计 MPC 状态、输入、代价和约束。
5. 设计 WBC 的 P0-P4 任务优先级。
6. 设计末端阻抗策略。
7. 设计 8 个评估指标。
8. 写出 5 个可能失败模式和对应降级策略。

交付格式

部分	交付物
动力学	变量维度表 + 方程
接触	mode schedule + 摩擦约束
控制	MPC/WBC 数据流图
实验	指标表 + 日志字段
安全	故障排查和降级策略

推荐完成顺序

1. 先画控制栈。
2. 再写变量维度。
3. 再写接触模式。
4. 再写代价和约束。
5. 最后写实验指标。

这个项目完成后，你再读 qm_control 的 QMInterface.cpp 和 WbcBase.cpp 会容易很多。