第75章：操作技能接口——EE Tracking + 抓取流水线

75.0 前置自测

答不出两题以上，建议先回到 复合/20_浮动基座臂统一动力学、复合/30_多模态MPC 与 复合/40_RL全身控制基础 复习。

1. 为什么 SE(3) 末端误差不能用位置差加欧拉角差直接表示？
2. T_ee^world = T_base^world T_ee^base 中，基座抖动会怎样影响 body-frame 末端命令？
3. MPC 或 WBC 层收到末端目标后，为什么还要检查关节限位、自碰撞和支撑裕度？
4. Diffusion Policy 输出一段动作序列时，低层控制器为什么不能盲目执行整段序列？
5. 固定臂操作策略迁移到移动基座时，哪些假设必须成立？

本章目标

本章讨论高层操作技能与底层全身控制之间的接口。

学完后，你应能解释 task-frame 与 body-frame EE tracking 的本质差异，设计“高层操作策略 → 末端 SE(3) 轨迹 → 全身控制器 → 关节命令”的数据流，并能为抓取、推拉、放置和工具使用任务设计可调试的状态机与安全门槛。

本章的重点不是罗列 Diffusion Policy、ACT 或 VLA 的用法，而是回答一个更基础的问题：高层策略输出的“手要去哪”怎样变成移动机器人可以安全执行的全身动作。

知识树

操作技能接口
├── 末端目标表达
│   ├── world-frame
│   ├── body-frame
│   ├── task-frame
│   └── SE(3) 对数误差
├── 高层操作策略
│   ├── 遥操作
│   ├── Diffusion Policy
│   ├── ACT
│   ├── VLA
│   └── 视觉/语言/触觉条件
├── 接口契约
│   ├── 轨迹频率
│   ├── 时间戳
│   ├── 坐标系
│   ├── 速度与加速度限制
│   └── 可行性标志
├── 低层执行器
│   ├── RL WBC
│   ├── MPC-WBC
│   ├── IK+QP
│   └── 阻抗/力控
├── 抓取流水线
│   ├── 接近
│   ├── 预抓取
│   ├── 闭合
│   ├── 提升
│   ├── 搬运
│   └── 放置
└── 部署安全
    ├── 目标过滤
    ├── 延迟补偿
    ├── 自碰撞检查
    ├── 支撑裕度检查
    └── 回退策略

这棵树的根是“接口契约”。

高层策略可以很复杂，也可以只是一个遥操作手柄。

低层控制器可以是 RL，也可以是 MPC 或 WBC。

只要接口契约不清晰，两边都会把错误推给对方。

---

75.1 为什么需要操作技能接口 ⭐⭐

这一节解决的问题是：为什么高层操作策略不能直接输出全身关节动作。

动机：操作策略和全身控制解决的是不同问题

操作策略关心的是任务。

它从图像、语言、物体状态或演示轨迹中决定末端应该怎样移动。

全身控制关心的是物理可行性。

它需要在接触、摩擦、动力学、关节限位和稳定性约束下执行这个末端目标。

把两者混在一个动作向量里，短期看似端到端，长期会让系统难以迁移、难以调试、难以保证安全。

固定机械臂操作策略通常输出末端轨迹或关节轨迹。

移动基座加机械臂后，同一末端轨迹还需要考虑基座移动。

四足或人形更复杂，因为脚下接触不是固定的，身体姿态会被末端动作扰动。

因此高层策略和低层全身控制之间必须有清晰接口。

反面案例：高层直接输出 19 维关节目标

假设一个视觉策略直接输出 Go2+ARX5 的 19 维关节目标。

它在某个仿真环境中训练得很好。

但换成另一台臂长略不同、默认姿态不同、夹爪零位不同的机器人后，关节动作不再对应同样的末端行为。

即使平台不变，基座姿态稍有差异，同一组臂关节角在世界系中的末端位置也会变化。

高层策略被迫学习本应由低层控制器处理的本体差异。

这会降低跨平台迁移能力。

更糟的是，关节动作没有语义。

当任务失败时，你很难判断是视觉识别错了、轨迹不可达、腿没有稳住，还是某个关节超限。

如果高层输出的是末端 SE(3) 目标，错误就更容易定位。

目标是否合理、是否可达、是否被低层跟踪，都可以分别检查。

历史来源：从固定臂到移动操作

传统工业机械臂通常在固定基座上工作。

操作策略只需要考虑机械臂工作空间。

移动操作系统引入了移动底盘。

早期方法常把底盘导航和机械臂规划分成两层。

底盘到达目标附近后，机械臂执行抓取。

四足臂和人形让问题更进一步。

机器人不是停稳后再操作，而是在移动、平衡和接触中操作。

UMI-on-Legs 的关键思想是：固定臂上学到的操作技能可以输出任务帧中的末端轨迹，移动基座上的低层全身控制器负责把这个轨迹执行出来。

这把“操作技能”与“移动平衡”分开。

Diffusion Policy、ACT、VLA 或遥操作系统只要遵守末端轨迹接口，就可以接入低层控制。

接口的三层含义

层次	问题	典型字段
几何接口	目标在哪里	frame、pose、twist
时间接口	什么时候到	timestamp、horizon、dt
物理接口	是否能执行	velocity limit、force mode、valid flag

只传一个 target_pose 是不够的。

低层还需要知道这个 pose 属于哪个坐标系。

需要知道它是当前目标，还是未来轨迹中的第几帧。

需要知道是否允许接触，是否要求高刚度，是否允许低层改变基座。

这些信息共同构成接口契约。

理论：接口是任务变量的选择

全身状态为：

\[ \mathbf{x} = (\mathbf{q}, \mathbf{v}) \]

高层如果直接输出关节动作，相当于选择关节空间任务变量：

\[ \mathbf{y} = \mathbf{q}_{act} \]

高层如果输出末端轨迹，相当于选择操作空间任务变量：

\[ \mathbf{y} = \mathbf{T}_{ee} \in SE(3) \]

二者由正运动学连接：

\[ \mathbf{T}_{ee} = f_{kin}(\mathbf{q}) \]

对复合机器人而言，高层应尽量输出与任务直接相关、与具体本体无关的变量。

抓杯子关心的是夹爪相对杯子的位姿，而不是第 3 个肩关节转多少度。

推门关心的是末端沿门法向施力，而不是某个肘关节速度。

因此操作技能接口通常选择 EE SE(3)、EE twist、gripper command 和接触模式，而不是全身关节向量。

本质洞察：好的操作接口不是把信息传得越多越好，而是把任务相关和本体相关的变量分开。 高层表达任务意图，低层承担本体差异和物理可行性。

类比：乐谱与演奏

高层操作策略像乐谱。

乐谱写的是旋律、节奏和力度。

低层全身控制像演奏者。

不同乐器的手法不同，但可以演奏同一段旋律。

如果乐谱直接写“第 3 根手指弯曲 12 度”，它就只适用于某个人的一只手。

如果高层策略直接输出某台机器人的关节角，它也很难迁移。

末端轨迹接口就是更接近“乐谱”的表达。

小节练习

1. 比较高层输出关节轨迹、末端轨迹和物体相对轨迹三种接口的迁移性。
2. 写出一个操作接口消息应包含的字段，并说明每个字段的作用。
3. 设计一个失败场景，说明只传 target_pose 而不传 frame 会导致什么问题。

⚠️ 常见陷阱

⚠️ 编程陷阱：接口字段没有单位约定

错误做法：位置有的模块用米，有的模块用厘米；角度有的用弧度，有的用角度。

现象：低层动作幅度异常，末端冲向错误位置。

根本原因：接口契约没有写清单位。

正确做法：所有连续物理量使用 SI 单位，姿态误差用弧度，接口层做统一检查。

💡 概念误区：端到端就是没有接口

错误想法：如果使用端到端视觉策略，就不需要中间表示。

实际情况：真实系统仍然需要时间戳、安全检查、限幅和回退策略。

正确理解：端到端可以用于感知到目标生成，但控制执行仍需要明确边界。

🧠 思维陷阱：把接口当成软件格式问题

错误想法：接口只是 ROS message 或 Python dict 的字段命名。

实际情况：接口选择决定了任务变量、控制分工和迁移能力。

正确做法：先从控制理论和任务语义确定变量，再写消息格式。

---

75.2 Task-frame vs Body-frame EE Tracking ⭐⭐⭐

这一节解决的问题是：为什么移动操作中 task-frame 往往比 body-frame 更适合承接高层操作策略。

动机：目标应绑定任务，而不是绑定晃动的身体

固定机械臂的基座不动。

末端相对基座的目标和相对世界的目标几乎等价。

移动机器人不同。

四足或人形行走时，基座会有平移、旋转和高频晃动。

如果高层给的是 body-frame 末端目标，目标会跟着身体晃。

但杯子、门把手、抽屉和桌面不会跟着机器人身体晃。

因此操作目标更自然地定义在 task-frame。

task-frame 可以是物体坐标系、桌面坐标系、门坐标系或世界局部坐标系。

低层控制器负责把 task-frame 目标转换成当前机器人基座下的可执行目标。

反面案例：body-frame 目标导致末端世界系抖动

假设高层要求末端在基座前方 0.5 m：

\[ {}^B\mathbf{p}_{ee}^{cmd} = [0.5, 0, 0.2]^T \]

如果基座 pitch 发生小幅波动，世界系末端目标为：

\[ {}^W\mathbf{p}_{ee}^{cmd} = {}^W\mathbf{p}_{B} + {}^W\mathbf{R}_{B} {}^B\mathbf{p}_{ee}^{cmd} \]

当 \({}^W\mathbf{R}_{B}\) 抖动时，目标在世界系也抖动。

低层控制器会忠实跟踪这个抖动目标。

结果是末端在杯子附近来回晃。

这不是低层控制器不够好，而是目标表达错了。

如果目标定义在杯子坐标系或桌面坐标系，基座抖动不会改变任务目标。

低层控制器会自动调整臂关节补偿基座姿态变化。

坐标变换推导

记：

\[ {}^W\mathbf{T}_B \]

为基座在世界系中的位姿。

记：

\[ {}^B\mathbf{T}_{ee} \]

为末端相对基座的位姿。

末端世界位姿为：

\[ {}^W\mathbf{T}_{ee} = {}^W\mathbf{T}_B {}^B\mathbf{T}_{ee} \]

Body-frame 控制给定：

\[ {}^B\mathbf{T}_{ee}^{cmd} \]

于是世界目标为：

\[ {}^W\mathbf{T}_{ee}^{cmd} = {}^W\mathbf{T}_B {}^B\mathbf{T}_{ee}^{cmd} \]

基座变，世界目标也变。

Task-frame 控制给定：

\[ {}^T\mathbf{T}_{ee}^{cmd} \]

其中 \(T\) 是任务帧。

世界目标为：

\[ {}^W\mathbf{T}_{ee}^{cmd} = {}^W\mathbf{T}_T {}^T\mathbf{T}_{ee}^{cmd} \]

如果任务帧绑定到桌面或物体，基座抖动不会改变目标。

低层需要计算当前基座下的等效目标：

\[ {}^B\mathbf{T}_{ee}^{cmd} = ({}^W\mathbf{T}_B)^{-1} {}^W\mathbf{T}_T {}^T\mathbf{T}_{ee}^{cmd} \]

这个公式是 task-frame tracking 的核心。

误差定义

低层可以在世界系计算误差：

\[ \boldsymbol{\xi}_{err} = \log\left(({}^W\mathbf{T}_{ee})^{-1} {}^W\mathbf{T}_{ee}^{cmd}\right) \in \mathbb{R}^6 \]

也可以把目标转换到基座系后在基座系计算误差。

关键是姿态误差要使用 SE(3) 的对数映射。

不能用欧拉角逐项相减。

在 复合/30_多模态MPC 中，我们已经看到 SE(3) 末端代价：

\[ J_{ee} = \frac{1}{2}\boldsymbol{\xi}_{err}^{T} Q_{ee}\boldsymbol{\xi}_{err} \]

本章把这个代价从 MPC 内部扩展成高层接口约定。

高层发送的不是“某个关节角”，而是“某个任务帧中的 SE(3) 轨迹”。

task-frame 的选择

任务	推荐 task-frame	原因
抓杯子	杯子或桌面局部帧	抓取几何与物体绑定
推门	门铰链或门把手帧	法向和切向清晰
擦桌子	桌面帧	法向力与切向轨迹分离
搬运物体	世界局部帧或物体初始帧	目标不随机器人晃动
遥操作	操作者手柄初始帧	手势相对运动稳定
移动拍摄	相机目标帧	稳定被拍对象

task-frame 不是固定唯一的。

同一个任务可以分阶段切换。

抓取前，task-frame 可以是物体帧。

抓取后，task-frame 可以切换为夹爪或搬运目标帧。

放置时，task-frame 又变为桌面放置区域。

切换时必须保持轨迹连续。

代码：task-frame 目标转换

import numpy as np

def inv_T(T):
    """SE(3) 齐次矩阵求逆。"""
    R = T[:3, :3]
    p = T[:3, 3]
    T_inv = np.eye(4)
    T_inv[:3, :3] = R.T
    T_inv[:3, 3] = -R.T @ p
    return T_inv

def compose_T(A, B):
    """SE(3) 位姿复合。"""
    return A @ B

def task_to_base_target(T_W_B, T_W_task, T_task_ee_cmd):
    """把任务帧末端命令转换为当前基座帧下的末端命令。"""
    T_W_ee_cmd = compose_T(T_W_task, T_task_ee_cmd)
    T_B_ee_cmd = compose_T(inv_T(T_W_B), T_W_ee_cmd)
    return T_B_ee_cmd


# 使用时注意：
# T_W_B 来自状态估计器
# T_W_task 来自视觉/SLAM/物体检测
# T_task_ee_cmd 来自高层操作策略

这段代码展示了 task-frame 的基本变换。

实际系统还要处理时间戳。

如果 T_W_B 是当前时刻，T_W_task 是 100 ms 前的视觉估计，两者不能直接相乘。

需要做延迟补偿或统一到同一时间。

task-frame 与 WBC 的关系

task-frame 不是替代 WBC。

它只是定义目标。

WBC 仍要处理：

1. 末端跟踪。
2. 支撑脚不滑。
3. 基座姿态稳定。
4. 关节限位。
5. 自碰撞。
6. 力矩限制。
7. 接触模式切换。

task-frame 给低层一个更稳定、更任务相关的目标。

低层仍然可以拒绝或修正不可行目标。

例如目标过远时，低层可以 saturate。

目标导致自碰撞时，低层可以降级为保持当前末端。

目标需要基座前移时，低层可以把末端误差转化为底盘或步态命令。

反事实推理：如果不用 task-frame 会怎样

如果用 body-frame 承接固定臂操作策略，策略会看到一个移动基座带来的非平稳输入分布。

固定臂训练时，基座永远不动。

移动基座部署时，目标相对基座不断变化。

高层策略必须在训练外处理这种变化。

这会破坏零样本迁移。

task-frame 的作用是把基座抖动吸收到低层控制器中。

高层仍然在“物体附近怎样移动手”这个相对稳定的问题上工作。

⚠️ 常见陷阱

⚠️ 编程陷阱：左右乘顺序写反

错误做法：把 \({}^W T_B {}^B T_{ee}\) 写成 \({}^B T_{ee} {}^W T_B\)。

现象：目标方向完全错误，旋转后平移也异常。

根本原因：SE(3) 变换不交换，坐标变换的源帧和目标帧必须匹配。

正确做法：在变量名中写清上标和下标，例如 T_W_B、T_B_EE。

💡 概念误区：task-frame 等于 world-frame

错误想法：只要不是 body-frame，就是世界系。

实际情况：task-frame 可以绑定物体、桌面、门、工具或操作者手柄初始帧。

正确理解：task-frame 是任务自然坐标系，world-frame 只是其中一种选择。

🧠 思维陷阱：task-frame 可以解决所有抖动

错误想法：换成 task-frame 后末端一定稳。

实际情况：状态估计延迟、任务帧漂移、低层带宽不足仍会导致抖动。

正确做法：task-frame 只是第一步，还要做滤波、时间同步和阻抗调节。

小节练习

1. 推导 body-frame 目标在基座 yaw 抖动 \(\delta \psi\) 下的世界系位置变化一阶近似。
2. 为“推门”任务定义 task-frame，写出末端位置、姿态和力方向应怎样表达。
3. 编写一个测试：随机生成 T_W_B，验证 T_W_B @ T_B_ee_cmd 等于 T_W_task @ T_task_ee_cmd。

---

75.3 高层操作策略输出什么 ⭐⭐

这一节解决的问题是：Diffusion Policy、ACT、VLA、遥操作和脚本规划器应该向低层发送什么形式的命令。

动机：高层策略输出的不是“动作”，而是“可执行意图”

操作策略常被描述为输出 action。

但在复合机器人中，action 这个词容易混淆。

对高层来说，action 可能是未来 16 步末端位姿。

对低层 RL 来说，action 是当前控制周期的关节目标偏移。

对电机来说，action 是电流、力矩或位置命令。

因此本章把高层输出称为操作命令。

它可以包含一段末端轨迹、夹爪命令、模式标志和置信度。

低层再把操作命令转成全身动作。

常见高层来源

来源	输入	输出	优点	风险
遥操作	手柄/VR/外骨骼	EE pose + gripper	直观，可采数据	延迟和抖动
Diffusion Policy	图像 + 状态	未来动作序列	多模态动作分布	推理延迟
ACT	图像 + 状态	chunked action	长时程平滑	分布偏移
VLA	视觉 + 语言	高层或低层动作	指令泛化	频率低，安全难
脚本规划器	目标几何	分段轨迹	可解释	泛化弱
MPC 高层	物体状态	短时目标	可行性强	建模成本高

它们都可以接入同一个低层，只要输出格式一致。

轨迹窗口而非单点目标

只发送当前末端目标有一个问题。

低层不知道目标接下来会怎样变化。

如果目标突然跳变，低层只能被动追赶。

发送未来轨迹窗口可以提供预瞄。

例如：

\[ \mathcal{Y}_{t:t+H} = \{({}^T T_{ee}^{cmd}(t+k\Delta t), g(t+k\Delta t), m(t+k\Delta t))\}_{k=0}^{H-1} \]

其中 \(g\) 是夹爪命令，\(m\) 是控制模式。

低层可以使用第一个目标作为当前跟踪点，也可以把整个窗口输入策略。

UMI-on-Legs 这类系统常给低层一个未来 EE 目标窗口，让低层提前补偿基座运动。

操作命令结构

from dataclasses import dataclass
import numpy as np

@dataclass
class EECommand:
    """高层操作策略发给低层的末端命令。"""

    frame_id: str                 # 例如 "task:cup"、"world"、"base"
    stamp: float                  # 命令产生时间
    dt: float                     # 轨迹采样间隔
    poses: np.ndarray             # shape = [H, 4, 4]，SE(3) 轨迹
    gripper: np.ndarray           # shape = [H]，夹爪开合命令
    mode: np.ndarray              # shape = [H]，位置/阻抗/力控模式
    stiffness: np.ndarray         # shape = [H, 6]，笛卡尔刚度
    valid: bool                   # 高层是否认为命令有效
    confidence: float             # 视觉或策略置信度

这个结构比单个 pose 冗长。

但它把执行所需的信息说清楚了。

低层看到低置信度命令时可以降低速度或保持当前姿态。

低层看到阻抗模式时可以降低刚度。

低层看到时间戳过旧时可以丢弃命令。

轨迹后处理

高层输出的轨迹通常不能直接执行。

需要经过接口层过滤。

后处理	目的
时间戳检查	防止执行旧命令
坐标系转换	统一到低层需要的 frame
位置限幅	防止目标跳变
速度限幅	防止末端速度过大
姿态插值	避免旋转跳变
可达性检查	防止目标超出工作空间
自碰撞预筛	防止手臂穿腿
支撑裕度检查	防止为追手牺牲平衡

SE(3) 轨迹平滑

位置可以用线性限速：

\[ \Delta \mathbf{p} = \text{clip}(\mathbf{p}_{cmd}-\mathbf{p}_{prev}, v_{max}\Delta t) \]

姿态应在 SO(3) 上插值。

给定当前姿态 \(R_0\) 与目标姿态 \(R_1\)，定义：

\[ \delta = \log(R_0^T R_1) \]

限制旋转增量：

\[ \delta' = \begin{cases} \delta, & \|\delta\| \le \omega_{max}\Delta t \\ \delta \frac{\omega_{max}\Delta t}{\|\delta\|}, & \text{otherwise} \end{cases} \]

新的姿态：

\[ R' = R_0 \exp(\delta') \]

这样不会出现欧拉角跳变。

代码：末端命令限速

import numpy as np
from scipy.spatial.transform import Rotation as Rot

def limit_position_step(p_prev, p_cmd, max_speed, dt):
    """限制末端位置单步变化。"""
    dp = p_cmd - p_prev
    max_step = max_speed * dt
    norm = np.linalg.norm(dp)
    if norm > max_step:
        dp = dp / (norm + 1e-9) * max_step
    return p_prev + dp

def limit_rotation_step(R_prev, R_cmd, max_omega, dt):
    """用 SO(3) 对数限制姿态单步变化。"""
    R_delta = R_prev.T @ R_cmd
    rotvec = Rot.from_matrix(R_delta).as_rotvec()
    max_angle = max_omega * dt
    angle = np.linalg.norm(rotvec)
    if angle > max_angle:
        rotvec = rotvec / (angle + 1e-9) * max_angle
    R_step = Rot.from_rotvec(rotvec).as_matrix()
    return R_prev @ R_step

这个限速器应放在高层和低层之间。

它不替代低层控制器。

它保证传给低层的目标不会出现不合理跳变。

高层输出频率

Diffusion Policy 或 VLA 的推理频率通常低于低层控制频率。

例如高层 10 Hz，低层 100 Hz。

这意味着低层每个周期不能等待新高层命令。

常见做法是维护命令缓冲。

高层每次写入一段未来轨迹。

低层按当前时间从轨迹中插值目标。

如果高层短暂超时，低层可以继续执行缓冲中的安全轨迹。

如果缓冲耗尽，低层进入保持或回退模式。

⚠️ 常见陷阱

⚠️ 编程陷阱：高层轨迹没有时间戳

错误做法：低层收到什么就执行什么。

现象：网络卡顿后机器人执行过期目标。

根本原因：没有判断命令产生时间与当前控制时间的差。

正确做法：每条轨迹带 stamp 和 dt，低层丢弃过旧命令。

💡 概念误区：高层越智能，低层越简单

错误想法：VLA 能理解任务，所以低层只要照做。

实际情况：高层通常不懂实时接触、摩擦、力矩和稳定裕度。

正确理解：高层负责意图，低层负责物理约束，接口层负责过滤。

🧠 思维陷阱：把轨迹平滑交给神经网络自己学

错误想法：训练数据够多，策略会学会不输出跳变轨迹。

实际情况：分布外视觉输入、遮挡和延迟会产生突变。

正确做法：接口层必须有确定性的限速、限幅和可行性检查。

小节练习

1. 设计一个 EECommand 结构，加入力控目标和接触允许标志。
2. 推导姿态限速公式，并说明为什么不能对四元数逐元素 clip。
3. 设计一个高层 10 Hz、低层 100 Hz 的轨迹缓冲插值方案。

---

75.4 低层全身控制如何消费 EE 命令 ⭐⭐⭐

这一节解决的问题是：低层 RL WBC、MPC-WBC 或 IK-QP 如何把末端命令变成全身动作。

动机：低层不是轨迹播放器

如果低层只是播放关节轨迹，移动基座上的操作很容易失败。

因为脚下接触会变化。

基座姿态会变化。

末端目标可能被高层延迟污染。

物体接触会产生外力。

低层必须在每个控制周期重新解释 EE 命令。

它要决定如何分配腿、腰、臂和基座的运动。

它还要决定任务不可行时如何降级。

三种低层实现

低层类型	输入	输出	优点	风险
RL WBC	本体 + EE 轨迹窗口	关节位置目标	运行快，能学耦合	可解释性弱
MPC-WBC	状态 + EE 目标 + 约束	接触力/速度/力矩参考	约束清晰	建模和计算复杂
IK-QP	当前状态 + EE 目标	关节速度/位置	简单可解释	动力学稳定性弱

实际系统可以混合。

例如高层轨迹先经过 IK-QP 检查可达性，再由 RL WBC 执行。

或者 MPC 给出基座和接触计划，RL 输出残差。

RL WBC 消费方式

RL 低层通常把 EE 命令加入观测。

可以加入当前误差：

\[ \boldsymbol{\xi}_{0} = \log((T_{ee})^{-1}T_{ee}^{cmd}(t)) \]

也可以加入未来窗口：

\[ [\boldsymbol{\xi}_{0}, \boldsymbol{\xi}_{1}, \ldots, \boldsymbol{\xi}_{H-1}] \]

未来窗口让策略知道末端即将向哪里走。

策略输出全身关节位置目标。

奖励中包含 EE 跟踪、基座稳定、动作平滑和安全项。

这种方式适合高频部署。

缺点是约束靠训练学习和终止惩罚表达，不如 QP 显式。

MPC-WBC 消费方式

MPC 可以把 EE 命令写成 cost：

\[ \ell_{ee} = \frac{1}{2} \|\log((T_{ee})^{-1}T_{ee}^{cmd})\|_{Q_{ee}}^2 \]

同时保留质心动量、摩擦锥、接触模式和自碰撞约束。

MPC 输出较低频参考，例如接触力、基座轨迹和臂速度。

WBC 在高频把这些参考转为关节力矩。

这条路线更可解释。

但在四足臂和人形中，实时求解成本很高。

所以工程上常把 MPC 作为慢速规划，WBC 或 RL 作为快速执行。

IK-QP 消费方式

IK-QP 在速度层求解：

\[ \min_{\dot q} \|J_{ee}\dot q - \dot x_{ee}^{cmd}\|^2 + \lambda \|\dot q\|^2 \]

约束：

\[ \dot q_{min} \le \dot q \le \dot q_{max} \]

可以加入关节限位、自碰撞近似和基座保持项。

IK-QP 对固定臂很常用。

对于四足臂，单独 IK 不足以保证足端接触和基座稳定。

它适合做高层命令的可达性检查，或站立慢速操作的基线。

可行性过滤

低层执行前应检查命令是否可行。

检查	方法	失败处理
工作空间	末端距离和 IK 残差	限幅或拒绝
关节限位	预测 IK 解	投影到安全范围
自碰撞	几何距离	停止或绕行
支撑裕度	CoM 到支撑边界距离	降低 EE 权重
速度限制	\(\|\Delta x\|/\Delta t\)	轨迹限速
目标年龄	当前时间减 stamp	丢弃旧命令
置信度	高层 confidence	保持或减速

代码：低层命令消费流程

class LowLevelExecutor:
    """低层全身控制器消费高层 EE 命令的框架。"""

    def __init__(self, policy, safety_filter, command_buffer):
        self.policy = policy
        self.safety_filter = safety_filter
        self.command_buffer = command_buffer

    def step(self, state, now):
        # 1. 从缓冲区按当前时间取出末端目标窗口
        ee_window = self.command_buffer.query(now)

        # 2. 检查时间戳、速度、工作空间和安全边界
        filtered_window, status = self.safety_filter.apply(state, ee_window)

        # 3. 根据过滤结果决定控制模式
        if not status.valid:
            filtered_window = self.command_buffer.hold_current_target(state)

        # 4. 构造低层策略观测
        obs = build_policy_observation(state, filtered_window)

        # 5. 策略输出关节目标
        action = self.policy(obs)
        q_des = map_action_to_joint_targets(action, state)

        # 6. 低层 PD 或 WBC 执行
        return q_des, status

这段流程强调一点：低层每步都要先处理命令，再调用策略。

策略不应该直接面对未过滤的高层输出。

阻抗与力控模式

很多操作任务不是纯位置跟踪。

推门需要沿法向施力。

擦桌子需要保持法向压力。

插孔需要低刚度避免卡死。

接口中应包含 mode 和 stiffness。

笛卡尔阻抗目标：

\[ \mathbf{F}_{cmd} = K(\mathbf{x}_{cmd}-\mathbf{x}) + D(\dot{\mathbf{x}}_{cmd}-\dot{\mathbf{x}}) + \mathbf{F}_{ff} \]

低层再通过：

\[ \tau_{ee} = J_{ee}^{T}\mathbf{F}_{cmd} \]

将末端 wrench 转为关节力矩或 WBC 任务。

对于 RL 低层，stiffness 可以作为观测输入。

策略学会在低刚度模式下更柔顺，在高刚度模式下更精确。

⚠️ 常见陷阱

⚠️ 编程陷阱：低层执行未过滤的高层目标

错误做法：高层输出目标后直接拼进观测。

现象：遮挡或识别跳变时，末端突然冲向远处。

根本原因：缺少限速、可达性和置信度检查。

正确做法：接口层必须先过滤，再给低层。

💡 概念误区：IK 成功就代表全身可行

错误想法：只要臂能通过 IK 到目标，全身任务就可执行。

实际情况：IK 不考虑足端摩擦、基座稳定和接触力分配。

正确理解：IK 是几何可行性，全身控制还需要动力学可行性。

🧠 思维陷阱：不可行目标必须强行追踪

错误想法：高层目标就是任务，低层失败说明低层不够强。

实际情况：真实系统必须允许低层拒绝危险目标。

正确做法：接口中显式返回 status，让高层知道目标被限幅、拒绝或降级。

小节练习

1. 设计一个低层 status 结构，包含 valid、reason、tracking_error、safety_margin。
2. 写出 IK-QP 的目标函数，加入关节居中和基座姿态保持两项。
3. 设计一个阻抗模式切换条件：空中高刚度，接触后低刚度并限制法向力。

---

75.5 抓取流水线：从接近到放置 ⭐⭐

这一节解决的问题是：如何把连续末端跟踪组织成一个完整抓取任务。

动机：抓取不是一个目标点

抓取通常被简化为“把夹爪移动到物体位置然后闭合”。

真实任务要复杂得多。

机器人要先接近物体。

要让夹爪姿态对齐抓取轴。

要控制接近速度，避免撞飞物体。

要闭合夹爪并判断是否抓住。

要提升物体并保持平衡。

要搬运到目标位置。

最后要放置并松开。

每个阶段的控制模式、奖励、容错和安全检查都不同。

因此抓取流水线应由状态机组织。

抓取阶段

阶段	目标	控制模式	主要风险
Search	找到物体和任务帧	感知更新	目标漂移
Approach	末端到预抓取位	位置控制	速度过快
Align	夹爪姿态对齐	姿态控制	关节限位
Pregrasp	接近接触前位	低速位置	撞到物体
Close	夹爪闭合	夹爪力/位置	夹空或夹碎
Lift	提升物体	位置+稳定	负载扰动
Transport	搬运	task-frame 跟踪	基座晃动
Place	放置	低速位置/阻抗	撞桌面
Release	松开	夹爪控制	物体滑落
Retreat	后撤	位置控制	二次碰撞

状态机不是退回传统规划。

它是把任务阶段的安全约束显式化。

高层策略可以决定每个阶段的目标。

状态机负责阶段切换和安全守护。

预抓取位

直接把末端目标设为物体中心通常不对。

抓取需要预抓取位：

\[ T_{pre} = T_{obj} T_{grasp} T_{offset} \]

其中 \(T_{grasp}\) 是物体相对夹爪的理想抓取变换。

\(T_{offset}\) 是沿接近方向后退的一小段距离。

例如沿夹爪 x 轴后退 10 cm。

低层先到预抓取位，再沿接近方向低速靠近。

这能减少撞击。

夹爪闭合判断

夹爪闭合不能只看时间。

应结合以下信号：

信号	含义
gripper position	是否达到闭合目标
gripper velocity	是否被物体阻挡
motor current/force	是否产生夹持力
object motion	物体是否被带动
EE force	是否接触异常

如果没有力传感器，可以用夹爪位置误差和电机电流估计是否夹住。

如果夹爪完全闭合但没有阻力，可能夹空。

如果夹爪闭合很少但电流很高，可能夹到硬物边缘或卡住。

状态机伪代码

from enum import Enum

class GraspStage(Enum):
    SEARCH = 0
    APPROACH = 1
    ALIGN = 2
    PREGRASP = 3
    CLOSE = 4
    LIFT = 5
    TRANSPORT = 6
    PLACE = 7
    RELEASE = 8
    RETREAT = 9
    FAILSAFE = 10

class GraspPipeline:
    """抓取任务状态机，输出 EECommand。"""

    def __init__(self):
        self.stage = GraspStage.SEARCH

    def update(self, perception, robot_state, now):
        if self.stage == GraspStage.SEARCH:
            if perception.object_found:
                self.stage = GraspStage.APPROACH

        elif self.stage == GraspStage.APPROACH:
            if robot_state.ee_error_norm < 0.08:
                self.stage = GraspStage.ALIGN

        elif self.stage == GraspStage.ALIGN:
            if robot_state.ee_rot_error_norm < 0.15:
                self.stage = GraspStage.PREGRASP

        elif self.stage == GraspStage.PREGRASP:
            if robot_state.ee_error_norm < 0.02:
                self.stage = GraspStage.CLOSE

        elif self.stage == GraspStage.CLOSE:
            if robot_state.grasp_confirmed:
                self.stage = GraspStage.LIFT
            elif robot_state.close_timeout:
                self.stage = GraspStage.FAILSAFE

        elif self.stage == GraspStage.LIFT:
            if robot_state.lift_height > 0.10:
                self.stage = GraspStage.TRANSPORT

        # 每个阶段都可以生成不同模式的末端命令
        return self.make_command(perception, robot_state, now)

这里的状态机只负责阶段逻辑。

具体的轨迹生成仍可以来自高层策略。

状态机可以把高层策略的输出裁剪到当前阶段允许范围。

失败恢复

抓取失败不应只有“重新开始”。

应按失败类型处理。

失败类型	检测	恢复
未找到物体	视觉置信度低	重新扫描或靠近
目标跳变	目标速度过大	冻结上一稳定目标
预抓取不可达	IK 或低层拒绝	改变基座位置
夹空	夹爪完全闭合且无力	回到 pregrasp
夹持不稳	提升时物体滑动	降低速度并重新夹
基座不稳	roll/pitch 或 CoM margin 异常	停止操作，收臂
接触力过大	EE force 超限	降低刚度或退回

反事实推理：如果没有状态机会怎样

如果把抓取看成一段连续末端轨迹，系统很难处理离散事件。

物体没检测到时，轨迹从哪里开始？

夹爪没夹住时，是否继续提升？

放置时接触桌面，是否继续向下压？

这些问题不能只靠轨迹网络隐式学习。

状态机把离散事件显式化，让连续控制器只处理当前阶段的物理执行。

⚠️ 常见陷阱

⚠️ 编程陷阱：阶段切换没有滞回

错误做法：误差小于阈值进入下一阶段，大于阈值马上退回。

现象：状态在两个阶段之间来回抖动。

根本原因：传感器噪声和控制误差导致阈值附近反复穿越。

正确做法：使用滞回、最短停留时间和置信度累计。

💡 概念误区：抓取成功只看夹爪是否闭合

错误想法：夹爪闭合就等于抓住。

实际情况：夹爪可能夹空、夹偏或夹住后滑落。

正确做法：结合夹爪位置、电流、物体运动和提升后的状态判断。

🧠 思维陷阱：状态机降低学习方法的泛化能力

错误想法：状态机是手工规则，会限制策略。

实际情况：状态机只表达安全和阶段结构，高层仍可学习每阶段目标。

正确理解：学习负责连续决策，状态机负责离散安全边界。

小节练习

1. 为“拿起杯子并放到桌面右侧”写出完整状态机和每个阶段的 EECommand。
2. 设计夹爪抓取成功判据，不使用外部视觉，只使用夹爪位置、电流和末端运动。
3. 在状态切换中加入滞回，说明如何避免 PREGRASP 与 CLOSE 之间抖动。

---

75.6 数据采集接口：遥操作、UMI 与跨平台演示 ⭐⭐

这一节解决的问题是：高层操作策略的数据从哪里来，以及数据坐标系如何与低层接口对齐。

动机：数据采集方式决定策略能学到什么

操作策略通常依赖演示数据。

演示数据可以来自 VR 遥操作、手持夹爪、同构外骨骼、固定臂示教或人体视频。

不同数据源记录的量不同。

VR 遥操作通常给出手柄位姿。

UMI 手持夹爪给出夹爪相机、手部轨迹和夹爪开合。

同构外骨骼给出关节级动作。

人体视频给出人体关键点或 SMPL 轨迹。

要接入移动基座低层，最终都要变成一致的 task-frame 末端命令。

数据源对比

数据源	记录内容	优点	难点
VR 遥操作	手柄 SE(3)、按钮	在线控制直观	延迟、操作者疲劳
手持夹爪	夹爪轨迹、相机、开合	无需机器人采集大量数据	轨迹对齐和 SLAM 漂移
同构外骨骼	关节角、手指动作	映射精确	硬件定制
固定臂示教	关节/末端轨迹	数据干净	移动基座差异
人体视频	关键点、SMPL	数据规模大	重定向和物理可行性

UMI 思路的关键

UMI 类方法的核心不是某个具体硬件，而是数据表示。

它把操作演示记录为与任务对象相关的末端轨迹。

如果轨迹表达在任务帧中，固定臂、手持夹爪和移动基座都可以共享。

移动机器人执行时，只要低层能在 task-frame 中跟踪这段末端轨迹，高层策略就不必重新学习“腿怎么走”。

这就是跨平台复用的基础。

数据对齐

演示数据必须对齐以下量。

对齐项	为什么重要
时间戳	图像、位姿和夹爪动作必须同步
坐标系	末端轨迹必须知道相对哪个 frame
相机外参	图像到任务帧需要标定
夹爪零位	开合命令需要一致含义
轨迹频率	高层训练和低层执行要插值
任务阶段	抓取、提升、放置的标签有助训练

如果时间戳错 100 ms，快速抓取时末端目标会明显滞后。

如果坐标系错一个旋转，策略可能学到镜像动作。

如果夹爪零位不一致，策略会在接近前提前闭合或放置前提前松开。

轨迹重采样

演示轨迹频率可能是 30 Hz、60 Hz 或不稳定。

低层通常需要固定频率命令。

重采样应在 SE(3) 上完成。

位置用线性插值。

姿态用球面插值或指数映射插值。

夹爪命令可以低通或保持。

不要把四元数逐元素线性插值后不归一化。

代码：演示轨迹转换为 EECommand

def demo_to_command(demo, task_frame_id, target_dt=0.05):
    """把演示数据转换为统一 EECommand。"""

    # 1. 按固定 dt 重采样时间轴
    times = make_uniform_time_grid(demo.timestamps[0], demo.timestamps[-1], target_dt)

    # 2. 插值位置
    positions = interpolate_positions(demo.ee_positions, demo.timestamps, times)

    # 3. 插值姿态，实际实现应使用 SO(3) 插值
    rotations = interpolate_rotations_so3(demo.ee_rotations, demo.timestamps, times)

    # 4. 组合成 SE(3)
    poses = []
    for p, R in zip(positions, rotations):
        T = np.eye(4)
        T[:3, :3] = R
        T[:3, 3] = p
        poses.append(T)

    # 5. 夹爪命令重采样
    gripper = interpolate_gripper(demo.gripper, demo.timestamps, times)

    return EECommand(
        frame_id=task_frame_id,
        stamp=float(times[0]),
        dt=target_dt,
        poses=np.stack(poses, axis=0),
        gripper=gripper,
        mode=np.zeros(len(times), dtype=np.int64),
        stiffness=np.ones((len(times), 6)) * 50.0,
        valid=True,
        confidence=1.0,
    )

这段伪代码把不同来源的演示统一为同一接口。

只要后续低层消费 EECommand，数据来源可以替换。

跨平台迁移条件

固定臂策略迁移到移动基座不是无条件成立。

至少需要满足五个条件。

条件	含义
任务帧一致	高层轨迹相对任务对象表达
夹爪几何相近	抓取宽度和接触点相似
工作空间覆盖	移动平台能把末端送到目标范围
低层跟踪能力足够	EE 误差在策略容忍范围内
感知分布相近	相机视角、光照和遮挡不过度偏移

如果夹爪形状差异很大，固定臂策略的抓取姿态可能不适用。

如果移动基座低层只能提供 10 cm 精度，而高层策略需要 1 cm 精度，迁移也会失败。

如果任务帧来自不稳定 SLAM，轨迹会漂移。

⚠️ 常见陷阱

⚠️ 编程陷阱：重采样时姿态插值不归一化

错误做法：四元数逐元素线性插值后直接使用。

现象：旋转矩阵不正交，末端姿态慢慢漂移。

根本原因：单位四元数位于球面上，普通线性插值会离开球面。

正确做法：使用 slerp 或 log/exp 插值，并归一化。

💡 概念误区：演示数据越多越能迁移

错误想法：只要采集大量固定臂数据，就能直接迁移到移动基座。

实际情况：如果数据表达在固定机器人本体坐标中，数量再多也难迁移。

正确理解：迁移性来自任务帧表达和低层跟踪能力，而不只是数据规模。

🧠 思维陷阱：忽视低层误差对高层策略的影响

错误想法：高层只输出目标，不需要关心低层误差。

实际情况：高层训练时如果假设末端完美执行，部署时低层误差会造成分布偏移。

正确做法：训练高层时注入低层跟踪噪声，或用真实低层 rollout 收集数据。

小节练习

1. 比较 VR 遥操作、UMI 手持夹爪和同构外骨骼在四足臂任务中的数据对齐难点。
2. 设计一个演示数据格式，包含图像、末端轨迹、夹爪、任务阶段和时间戳。
3. 分析固定臂开抽屉策略迁移到 Go2+Arm 时可能失败的三个条件。

---

75.7 安全门槛与回退策略 ⭐⭐⭐

这一节解决的问题是：当高层命令不可信或低层无法执行时，系统怎样安全退回。

动机：操作系统必须能拒绝命令

真实机器人不能把所有高层输出都当成真理。

视觉可能误检。

语言指令可能模糊。

Diffusion 采样可能产生短时异常轨迹。

遥操作可能有网络延迟。

物体可能被人拿走。

如果低层无条件执行，危险会直接传到硬件。

因此接口层必须有安全门槛和回退策略。

安全门槛分类

门槛	检查量	典型动作
目标年龄	now - stamp	过旧则保持
目标速度	\(\|\Delta p\|/\Delta t\)	限速
工作空间	末端目标距离	投影到可达域
自碰撞	几何距离	拒绝或绕开
关节限位	IK 预测解	限幅
支撑裕度	CoM margin	降低 EE 权重
力限制	EE force 或估计力	切阻抗或后退
置信度	perception confidence	等待或减速

回退策略

回退不是简单急停。

急停在某些接触任务中可能反而危险。

例如搬运物体时突然松开会掉落。

推门时突然撤力可能反弹。

应该根据任务阶段选择回退。

场景	回退策略
高层短暂超时	保持上一安全轨迹并减速
目标置信度低	冻结末端，等待重新检测
EE 目标不可达	请求高层移动基座或重采样
基座不稳	收臂到安全位，优先站稳
接触力过大	降低刚度，沿反方向退让
夹持失败	回到 pregrasp 并重试
自碰撞风险	停止臂运动，保持腿稳定

安全状态机

操作状态机应嵌套安全状态。

正常模式下执行任务。

警告模式下降速和提高稳定权重。

危险模式下停止操作并收臂。

紧急模式下切断或进入硬件安全流程。

NORMAL
  ├── confidence low → CAUTION
  ├── margin low → CAUTION
  └── force high → CAUTION

CAUTION
  ├── recovered → NORMAL
  ├── still unsafe → SAFE_HOLD
  └── severe violation → EMERGENCY

SAFE_HOLD
  ├── high-level reset → NORMAL
  └── severe violation → EMERGENCY

代码：命令安全过滤器

class CommandSafetyStatus:
    def __init__(self, valid=True, reason="ok", scale=1.0):
        self.valid = valid
        self.reason = reason
        self.scale = scale

class CommandSafetyFilter:
    """对高层 EE 命令做确定性安全过滤。"""

    def __init__(self, max_age, max_speed, min_confidence):
        self.max_age = max_age
        self.max_speed = max_speed
        self.min_confidence = min_confidence

    def apply(self, state, cmd, now):
        if cmd is None:
            return None, CommandSafetyStatus(False, "missing_command", 0.0)

        if now - cmd.stamp > self.max_age:
            return None, CommandSafetyStatus(False, "stale_command", 0.0)

        if cmd.confidence < self.min_confidence:
            return cmd, CommandSafetyStatus(False, "low_confidence", 0.2)

        if estimate_max_ee_speed(cmd) > self.max_speed:
            cmd = limit_command_speed(cmd, self.max_speed)
            return cmd, CommandSafetyStatus(True, "speed_limited", 0.7)

        if state.com_margin < 0.03:
            return cmd, CommandSafetyStatus(True, "low_com_margin", 0.3)

        if state.self_collision_risk:
            return None, CommandSafetyStatus(False, "self_collision_risk", 0.0)

        return cmd, CommandSafetyStatus(True, "ok", 1.0)

scale 可以传给低层，用来降低末端权重或速度。

这样低层不是只有执行和拒绝两个选项。

它可以在不安全时温和降级。

安全与学习的关系

安全过滤器会改变高层命令分布。

如果高层训练时没有见过这种过滤，部署时可能不适应。

因此建议把过滤器也放进仿真训练闭环。

高层输出异常目标时，仿真低层也会限速或拒绝。

高层逐渐学会输出更可执行的命令。

对于低层 RL，也应把 status 或 scale 作为观测。

否则低层不知道当前目标为什么突然变慢或被保持。

⚠️ 常见陷阱

⚠️ 编程陷阱：安全过滤只在真机部署启用

错误做法：训练时低层看到原始命令，真机时加入过滤器。

现象：部署时目标分布改变，策略行为变差。

根本原因：训练和部署接口不一致。

正确做法：仿真训练、回放和真机部署使用同一套过滤逻辑。

💡 概念误区：回退就是急停

错误想法：发现异常立即停止所有运动。

实际情况：接触和搬运任务中，突然停止可能造成更大风险。

正确理解：回退应根据任务阶段选择保持、减速、收臂、放置或急停。

🧠 思维陷阱：安全门槛越严越好

错误想法：阈值设得很保守，系统就更安全。

现象：机器人频繁拒绝任务，无法完成操作。

正确做法：把安全阈值分为警告、降级和硬停止三级，而不是单一硬阈值。

小节练习

1. 为抓取流水线设计每个阶段的回退策略。
2. 写一个测试用例：高层命令突然跳变 30 cm，验证过滤器将其限速。
3. 分析安全过滤器放在高层前、接口层和低层内部三种位置的优缺点。

---

75.8 综合项目：Diffusion Policy 到 RL WBC 的最小流水线 ⭐⭐

本章综合项目实现一个最小操作流水线。

高层可以是真 Diffusion Policy，也可以先用脚本轨迹模拟。

重点是接口、过滤、缓冲和低层执行。

项目目标

1. 定义 EECommand 数据结构。
2. 实现 task-frame 到 base-frame 的目标转换。
3. 实现轨迹缓冲和时间插值。
4. 实现位置与姿态限速。
5. 将 EE 轨迹窗口加入低层 RL 观测。
6. 实现抓取状态机。
7. 实现安全过滤器和 status 返回。
8. 在仿真中完成“接近方块、闭合夹爪、提升、放置”的任务。

目录建议

operation_interface_demo/
├── commands/
│   ├── ee_command.py
│   ├── buffer.py
│   └── filters.py
├── frames/
│   ├── se3.py
│   └── task_frame.py
├── pipeline/
│   ├── grasp_state_machine.py
│   ├── high_level_stub.py
│   └── executor.py
├── low_level/
│   ├── obs_adapter.py
│   └── policy_runner.py
├── tests/
│   ├── test_frames.py
│   ├── test_buffer.py
│   └── test_filter.py
└── run_demo.py

这个项目不是训练一个新高层。

它验证高层和低层之间的数据契约。

里程碑 1：坐标系正确

构造一个静态任务帧。

让基座在仿真中左右晃动。

目标保持在任务帧中不动。

检查转换后的 base-frame 目标是否随基座变化。

同时检查世界系目标是否稳定。

通过标准：

指标	阈值
世界系目标漂移	接近 0
base-frame 目标变化	与基座运动一致
SE(3) 乘法测试	数值误差 < 1e-6

里程碑 2：缓冲与限速

让高层以 10 Hz 发送轨迹。

低层以 100 Hz 查询。

人为制造目标跳变和通信暂停。

验证低层不会执行过期或跳变命令。

通过标准：

情况	预期
正常轨迹	平滑插值
目标跳变	限速后执行
100 ms 暂停	缓冲继续
缓冲耗尽	进入保持

里程碑 3：抓取状态机

使用脚本生成物体 task-frame。

状态机依次输出 pregrasp、close、lift、place。

低层执行 EE tracking。

通过标准：

阶段	通过标准
Approach	EE 到达预抓取位
Close	夹爪命令闭合
Lift	物体高度增加
Transport	基座稳定
Place	物体接近目标区域

里程碑 4：替换高层策略

把脚本高层替换为 Diffusion Policy 或 ACT。

要求高层输出仍然转换为 EECommand。

低层和安全过滤器不改。

这一步验证接口隔离是否成功。

如果替换高层需要改低层观测顺序或控制器代码，说明接口还不够干净。

综合练习

1. 实现 test_frames.py，随机生成 100 个 SE(3) 变换，验证 task-frame 转换公式。
2. 实现 CommandBuffer.query(now)，支持线性位置插值和 SO(3) 姿态插值。
3. 在仿真中把高层命令延迟固定为 150 ms，测试低层是否进入减速模式。
4. 把低层从 RL WBC 替换为 IK-QP，比较同一 EECommand 下的表现。
5. 为失败抓取加入一次重试逻辑，并记录成功率变化。

---

75.9 延伸阅读

主题	推荐材料	难度	阅读目的
SE(3) 跟踪代价	复合/30_多模态MPC	⭐⭐⭐	理解末端误差和自碰撞约束
RL 全身低层	复合/40_RL全身控制基础	⭐⭐	理解低层观测、动作和奖励
UMI 数据采集	Chi et al. UMI	⭐⭐	理解手持夹爪演示
UMI-on-Legs	Ha et al. CoRL 2024	⭐⭐⭐	理解 task-frame 接口
Diffusion Policy	Chi et al. RSS 2023	⭐⭐⭐	理解动作序列去噪
ACT	Action Chunking with Transformers	⭐⭐⭐	理解 chunked action 输出
OpenTeleVision	VR 遥操作系统	⭐⭐	理解在线遥操作数据源
FALCON/SoFTA	人形力敏感操作	⭐⭐⭐	理解上下体和频率解耦

阅读这些材料时，不要只问“模型结构是什么”。

更应该问：它输出的动作变量是什么，坐标系是什么，频率是多少，低层如何保证安全。

这四个问题决定了方法能否进入真实复合机器人系统。

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
末端目标方向反了	SE(3) 乘法顺序或 frame 错	1. 打印 T_W_B 2. 做单位变换测试 3. 可视化坐标轴	75.2
世界系末端抖动	使用 body-frame 目标或任务帧漂移	1. 固定 task-frame 2. 对比 world/body 目标 3. 检查状态估计延迟	75.2
低层突然冲向远处	高层目标跳变未限速	1. 打印目标速度 2. 开启限速器 3. 检查视觉置信度	75.3/75.7
高层卡顿后继续执行旧动作	轨迹没有时间戳或过期检查	1. 打印 now-stamp 2. 设置 max_age 3. 缓冲耗尽进入保持	75.3
IK 可达但机器人摔倒	几何可行但动力学不可行	1. 检查 CoM margin 2. 降低 EE 权重 3. 加支撑裕度门槛	75.4
抓取阶段来回跳	状态机无滞回	1. 加最短停留时间 2. 加双阈值 3. 平滑检测信号	75.5
夹爪闭合但没抓住	缺少抓取确认	1. 检查夹爪位置 2. 检查电流或力 3. 提升后观察物体	75.5
固定臂策略迁移失败	数据不在 task-frame 或低层精度不足	1. 检查演示坐标系 2. 注入低层误差训练 3. 验证工作空间	75.6
真机比仿真滞后	高层/状态估计延迟未建模	1. 测量端到端延迟 2. 训练中随机延迟 3. 使用目标预瞄	75.3/75.7
安全过滤频繁拒绝任务	阈值过严或高层输出不可达	1. 统计拒绝原因 2. 分级降级 3. 请求高层重采样	75.7

---

本章小结

知识点	核心结论	工程落点
操作接口	高层表达任务意图，低层处理物理可行性	用 EECommand 契约连接两层
task-frame	目标应绑定任务对象，而不是晃动的身体	使用 T_B_cmd = T_W_B^{-1} T_W_task T_task_cmd
高层输出	轨迹窗口比单点目标更适合低层预瞄	带时间戳、模式、刚度和置信度
低层消费	RL、MPC、IK-QP 都可消费同一接口	先过滤命令，再构造观测
抓取流水线	抓取是多阶段任务，不是一个点	使用状态机和阶段安全门槛
数据采集	迁移性来自 task-frame 表达和低层跟踪能力	统一演示数据到 SE(3) 轨迹
安全回退	真实系统必须能拒绝和降级命令	设置时间、速度、可达性和支撑门槛

本章把第 74 章的低层全身 RL 放进了操作系统中。

下一份动作模仿理论将进一步讨论：如果高层命令和低层控制都来自人类演示或动捕数据，如何从参考运动、对抗先验、技能潜空间和重定向流程中训练出可部署的全身行为。

章末统一练习与故障排查

⚠️ 易错点一：只看单个指标。 50_操作技能接口 中的任何结论都应同时检查任务指标、物理约束和软件接口。只看总误差或总奖励，容易把模型错误误判为参数问题。

💡 易错点二：忽略坐标系和时间戳。 复合机器人控制链很长，坐标系、采样频率和延迟一旦没有显式记录，后续所有优化和学习结果都会失去解释力。

🧠 易错点三：把演示成功当成系统可靠。 教学实验应至少包含一次扰动、一次异常输入和一次日志复盘，才能说明方法的边界。

练习

1. 选择本章一个核心公式，写出每一项的单位、坐标系和数据来源。
2. 选择本章一个代码片段，说明它依赖哪些配置项；如果配置错一个符号，会出现什么日志现象？
3. 设计一个只改变单个因素的实验，用来验证本章最关键的工程判断。

本质洞察：复合机器人文档中的公式、代码和项目不是三块孤立内容。公式定义可行边界，代码实现边界，项目用日志证明边界是否真实存在。

故障排查

症状	优先怀疑	验证动作
仿真正常但部署异常	观测、坐标系或时间戳不一致	用同一段日志离线回放训练端和部署端
指标突然变差	模式切换、限幅或安全壳触发	画出模式、保护标志和控制命令
调参没有效果	根因不是权重而是模型假设错误	回到最小实验，关闭无关模块
结果难以复现	配置没有版本化	保存模型哈希、配置哈希和随机种子