第99章：感知-操作-运动统一闭环——从 SLAM 不确定性到安全控制接口¶

元信息	值
难度	⭐⭐⭐⭐（SLAM + 操作策略 + MPC/WBC + 学习系统）
预计时间	1.5 周（25-35 小时）
前置依赖	复合/30_多模态MPC、复合/120_底盘臂联合规划、复合/140_VLA移动操作、复合/260_VLA_Foundation_Model、复合/280_多机协作LocoMani；足式/130_腿足状态估计；SLAM 主线因子图章节
本章主线	把感知、操作、运动三个闭环从松散串联改造成有边界、有不确定性、有安全滤波的统一系统
实践目标	实现 SLAM 协方差调节 MPC 代价、主动观察目标和 VLA/MPC 安全接口

99.0 前置自测¶

#	问题	若答不出应回顾
1	因子图 SLAM 中位姿节点、地标节点、观测边分别代表什么？	SLAM 主线因子图章节
2	OCS2 中 cost、constraint、reference manager 的职责边界是什么？	OCS2 完整栈与复合 MPC
3	VLA 或 Diffusion Policy 输出动作时，为什么仍需要底层安全过滤？	VLA 移动操作与安全章节
4	MPC 与 WBC 的时标为什么不同？哪些信息可以低频更新，哪些必须高频闭环？	腿足 MPC/WBC 章节
5	如果物体在操作过程中移动，经典 SLAM 把它当作外点会造成什么问题？	本章 99.8

本章目标¶

能画出感知、操作、运动三闭环的输入输出、频率和边界。
能把 SLAM 位姿协方差、地图不确定性和物体 pose 噪声传播到 MPC cost 或 chance constraint。
能说明 VLA、TAMP、Diffusion Policy、MPC、WBC、RL WBC 分别应该负责什么，不应该负责什么。
能设计主动感知策略，使机器人为了操作目标主动调整视角、步态或头部朝向。
能实现一个“感知不确定性 → 控制裕度”的最小原型，并通过仿真实验验证。

本章地图¶

统一闭环系统
├── 感知闭环：定位、建图、物体 pose、地形 SDF、协方差
├── 操作闭环：任务规划、抓取、VLA/扩散策略、接触状态、力觉反馈
├── 运动闭环：MPC、WBC、RL WBC、安全滤波、硬件执行
├── 学习边界：学习开放世界语义和残差，物理安全由约束层兜底
├── 时间边界：VLM 1-10 Hz、策略 10-50 Hz、MPC 20-100 Hz、WBC 500-1000 Hz
└── 安全边界：不确定性收紧、控制屏蔽、故障降级、日志回放

本质洞察：统一闭环不是把所有模块合并成一个大网络，而是让每个模块把自己的不确定性、时标和可行性边界显式暴露给下游。

本章讨论的是复合机器人系统中最容易被低估的问题：模块之间到底应该传什么。

传统系统常被画成一条流水线。

摄像头和 LiDAR 做 SLAM。

检测器给出物体位置。

规划器给出抓取目标。

MPC 和 WBC 执行动作。

这条流水线在静态桌面抓取中可以工作。

但在移动操作、四足臂操作和人形操作中，它会很快暴露问题。

机器人走路时相机会晃动。

机械臂会遮挡自己要观察的物体。

物体被推动后不再是静态地图点。

地形不确定性会影响落足安全。

VLA 可能给出语义合理但动力学不可行的动作。

MPC 可以拒绝不可行目标，但如果上层不知道原因，就会反复发同一个坏目标。

因此，统一闭环的目标不是消除模块，而是让模块之间形成明确的反馈关系。

感知层要把不确定性传给操作和运动层。

操作层要把接触意图、目标有效期和失败恢复建议传给运动层。

运动层要把可行性、拒绝原因和安全裕度返回给操作层。

学习模块要输出置信度或候选多样性，而不能把动作直接塞进硬件。

99.1 为什么三闭环分离会失败 ⭐⭐¶

动机：串联系统缺少风险反馈¶

串联系统通常这样工作：

SLAM/感知 → 操作规划 → MPC/WBC → 硬件

这个结构清楚，也容易实现。

问题是它默认每个模块的输出都是“足够正确”的。

SLAM 输出一个 pose。

操作规划默认这个 pose 可用。

MPC 默认目标可靠。

WBC 默认参考不危险。

真实系统中，这些默认都可能不成立。

SLAM pose 有协方差。

物体检测有遮挡和跳变。

VLA 输出有分布外风险。

MPC 目标可能在碰撞约束外。

WBC 可能因为接触状态变化而无法跟踪。

三个典型失败¶

第一类失败是感知不知道任务需求。

机器人要开门，但 SLAM 把算力用在远处墙面特征上。

门把手被手臂遮挡时，感知层没有主动要求调整视角。

第二类失败是操作不知道运动约束。

VLA 或抓取策略给出一个看似合理的末端目标，但目标要求机器人把基座推到支撑边界。

第三类失败是运动不知道感知不确定性。

物体 pose 抖动时，MPC 用高权重强追踪，末端产生不必要的高频运动。

三闭环定义¶

闭环	核心问题	典型输出	典型频率
感知闭环	世界是什么，以及我有多确定	机器人 pose、物体 pose、SDF、协方差	10-100 Hz
操作闭环	为了任务，我要改变世界的哪一部分	技能序列、末端目标、接触意图	1-50 Hz
运动闭环	在动力学和安全约束下如何执行	轨迹、接触力、关节力矩	50-1000 Hz

统一闭环要求这三个闭环互相反馈。

类比：三闭环系统类似于人体的视觉-决策-运动协调。感知闭环对应视觉皮层和前庭系统——它不只"看到"物体，还输出距离判断的不确定性（"这个杯子到底是 30 cm 还是 35 cm 远？"）。操作闭环对应前额叶的决策系统——它决定"先伸手还是先走近"，但必须知道手臂够不够长。运动闭环对应小脑和脊髓反射——它负责高频力矩控制和快速姿态恢复，但不理解"为什么要开门"。三者的频率差异巨大：视觉处理~100 ms，决策~200-500 ms，脊髓反射~10-30 ms。人体的优雅之处在于这三个系统既独立运行又互相反馈——这正是统一闭环要在机器人上实现的目标。

回顾复合/270_SimToReal精读中的讨论：Sim2Real 的核心挑战之一就是感知域差距会传播到下游控制。在那里我们通过域随机化缓解这个问题。统一闭环提供了互补的解决方案——不是让感知输出更准确，而是让控制层知道感知有多不确定，并据此调整行为。

感知层不只输出均值，还输出协方差和有效期。

操作层不只输出目标，还输出任务阶段和接触模式。

运动层不只执行目标，还返回可行性、风险和拒绝原因。

统一参考包¶

统一参考包可以写成：

\[ R=(\mu,\Sigma,frame,t_{\text{stamp}},validity,priority) \]

其中 \(\mu\) 是目标均值，\(\Sigma\) 是不确定性，frame 是坐标系，\(t_{\text{stamp}}\) 是时间戳，validity 是有效期，priority 是优先级。

这比只传一个 pose 更稳。

控制器接收到参考后，可以判断：

坐标系是否匹配。
参考是否过期。
不确定性是否过大。
是否应该降低权重。
是否应该主动观察。

决策表¶

场景	可用架构	不宜架构	判断信号
静态工位抓取	弱耦合串联	复杂统一架构	目标固定且不遮挡
行走中操作	统一闭环	一次性离线规划	姿态和视角持续变化
人旁安全任务	显式不确定性接口	黑箱动作直达硬件	安全边界严格
开放世界整理	VLA + 安全接口	纯解析规则	物体类别和语言目标多
精密装配	力控 + 低不确定目标	高噪声视觉直控	接触误差毫米级

⚠️ 常见陷阱¶

⚠️ 把统一闭环理解为端到端

错误想法：统一就是一个大网络直接输出关节力矩。

典型现象：调试困难，安全边界不清楚。

根本原因：统一的关键是信息闭合，不是结构消失。

正确做法：保留模块边界，显式传递不确定性、有效期和约束。

⚠️ 只传目标不传置信度

错误想法：给 MPC 一个物体 pose 均值即可。

典型现象：目标抖动时末端强追踪。

根本原因：控制刚度应随不确定性调节。

正确做法：把协方差映射为权重和安全裕度。

练习¶

画出一个开门任务中的三闭环信息流。
列举三个串联系统失败但统一闭环可缓解的例子。
定义一个统一参考包应包含哪些字段。

理解失败模式后，需要先确定每个闭环的时间尺度和接口契约。

99.2 时间尺度与接口契约 ⭐⭐⭐¶

动机：低频语义不能阻塞高频控制¶

统一闭环不是同步闭环。

各模块频率差异巨大。

VLM 或大型 VLA 可能只有 1-10 Hz。

扩散策略常在 10-50 Hz 输出动作块。

MPC 常在 20-100 Hz 求解短时轨迹。

WBC 和关节控制常在 500-1000 Hz 运行。

如果高频控制等待低频语义，系统会抖动甚至失稳。

如果低频策略直接写高频力矩，安全边界会消失。

因此，统一闭环需要缓存、插值、有效期和拒绝接口。

接口契约¶

接口契约应至少包含：

输入和输出。
坐标系。
频率。
延迟预算。
有效期。
失效动作。
拒绝原因。

感知层输出状态估计和不确定性，不输出关节命令。

操作层输出目标序列和接触意图，不绕过安全约束。

运动层可以拒绝不可行目标，并返回可行性原因。

硬件层可以因安全原因覆盖上层命令。

有效期与插值¶

有效参考必须满足：

\[ t_{\text{now}}-t_{\text{stamp}}<T_{\text{valid}} \]

参考轨迹应通过插值进入高频控制：

\[ r(t)=Interp(r_k,r_{k+1},t) \]

如果参考过期，高频控制器不应继续追踪最后一个目标。

它应保持、顺应或进入局部稳定模式。

拒绝接口¶

运动层拒绝目标时，不应只返回失败。

它应返回原因。

常见原因包括：

原因	含义
collision_risk	目标路径会碰撞
support_margin_low	支撑裕度不足
torque_limit	力矩限制不可满足
target_stale	目标过期
target_uncertain	目标不确定性过大
kinematic_unreachable	目标在工作空间外
contact_inconsistent	接触模式与目标冲突

这个拒绝接口是闭环的重要反馈。

操作层收到拒绝后，可以换抓取点、主动观察、移动基座或降低任务要求。

工程分层¶

层级	职责	输入	输出	典型频率
语义/VLM	解析长程意图	语言、图像	子目标	1-10 Hz
操作策略	生成末端目标块	视觉、proprio、语言	EE 序列	10-50 Hz
MPC	动力学可行轨迹	状态、EE 序列、约束	轨迹和反馈增益	20-100 Hz
WBC	力矩与接触力	MPC 参考、本体状态	关节命令	500-1000 Hz
硬件	执行与保护	关节命令、电流、温度	实际运动或保护动作	1 kHz+

决策表¶

问题	优先选择	不宜选择	判断信号
模块频率差大	时间戳缓存 + 插值	同步等待	低频模块阻塞
目标可能过期	有效期和拒绝原因	无限持有目标	物体移动或遮挡
运动不可行	返回可行性诊断	静默失败	MPC 频繁无解
高频安全	本地安全滤波	远程策略直控	延迟不可控
多目标竞争	参考管理层仲裁	多模块直接写控制器	目标跳变

代码示例：统一参考包¶

#include <array>
#include <string>

struct Covariance6d {
  // 中文注释：按 row-major 存 6x6 协方差，便于跨语言消息传输
  std::array<double, 36> value{};
};

struct UnifiedReference {
  std::string frame_id;
  double stamp_sec = 0.0;
  double valid_until_sec = 0.0;
  std::array<double, 7> pose_wxyz_xyz{};
  std::array<double, 6> twist{};
  Covariance6d covariance;
  int priority = 0;
  int source = 0;        // 0=感知，1=操作策略，2=人工遥控
  int contact_mode = 0;  // 0=空中，1=柔顺接触，2=力控
};

bool is_reference_valid(const UnifiedReference& ref, double now_sec) {
  return now_sec >= ref.stamp_sec && now_sec <= ref.valid_until_sec;
}

⚠️ 常见陷阱¶

⚠️ 高频层等待低频层

错误想法：WBC 等新 VLA 动作再输出更一致。

典型现象：控制周期抖动。

根本原因：低频决策不能进入硬实时环。

正确做法：高频层使用最新有效参考和局部反馈。

⚠️ 没有拒绝接口

错误想法：运动层失败时打印错误即可。

典型现象：上层反复发送同一个不可行目标。

根本原因：闭环需要可行性负反馈。

正确做法：定义 goal_rejected 消息和原因码。

练习¶

为 G1 开门任务列出每个模块的频率和延迟预算。
设计一个 goal_rejected 消息，至少包含 5 个字段。
解释为什么 WBC 不应直接订阅 VLM 输出。

有了时标契约，下面先处理感知闭环：它不仅提供 pose，还提供可计算的不确定性。

99.3 感知闭环：SLAM、物体 pose 与地形 SDF ⭐⭐⭐¶

动机：感知输出必须能被控制使用¶

感知闭环的输出不只是地图。

它应输出控制可用的状态、几何和风险。

对行走操作，关键感知包括：

基座 pose 和 twist。
物体 6D pose。
可落足区域。
地形高度和法向。
障碍物 SDF 或 ESDF。
接触对象和接触状态。
每类估计的不确定性。

如果感知层只输出彩色点云，控制层还要重新解释环境。

如果感知层只输出 pose 均值，控制层不知道应该信任多少。

状态估计¶

基座状态估计通常融合 IMU、关节编码器、足端接触、视觉或 LiDAR。

对腿足和人形平台，状态估计不只是 SLAM。

它还包含本体运动学和接触约束。

例如，足端着地时可以提供零速度约束。

机械臂末端接触稳定物体时，也可以提供额外约束。

但机械臂快速运动会扰动基座 IMU。

这要求估计器理解全身运动。

地形与障碍¶

地形建图服务于落足和基座规划。

障碍 SDF 服务于碰撞约束。

ESDF 约束可写成：

\[ h(p)=\Phi(p)-r_{\text{safe}}\ge0 \]

其中 \(\Phi(p)\) 是点 \(p\) 到障碍的距离场。

如果要把它放进 MPC，需要梯度：

\[ \nabla_q h=\nabla_p\Phi(p)^\top J_p(q) \]

这就是感知到控制的数学接口。

物体 pose¶

物体 pose 可以来自检测、点云匹配、关键点跟踪、AprilTag、神经网络或多模态融合。

但操作控制需要的不只是瞬时 pose。

它需要：

物体 pose 均值。
pose 协方差。
观测时间戳。
物体速度估计。
物体运动模型。
与机器人目标 frame 的关系。

如果物体 pose 抖动，操作层应先判断是否需要主动观察，而不是让运动层高刚度追踪。

工程分层¶

层级	职责	输入	输出	典型频率
状态估计	基座 pose/twist	IMU、关节、SLAM	初始状态	100-400 Hz
几何建图	地形和障碍	LiDAR/RGB-D	SDF/栅格	5-30 Hz
物体估计	目标 6D pose	检测、匹配、跟踪	物体状态	10-60 Hz
不确定性	输出协方差和可信区	因子图、滤波器	风险参数	同估计频率

决策表¶

任务	推荐感知输出	不宜输出	原因
落足规划	地形高度 + SDF + 协方差	RGB 语义图	需要几何安全
抓取任务	物体 pose + grasp 候选	仅 bbox	需要 6D 对位
开门任务	把手 pose + 铰链估计	门板平面 alone	接触受铰链约束
长时任务	因子图和回环	纯里程计	漂移会累积
人旁操作	人体区域 SDF + 速度	静态障碍框	人会移动

⚠️ 常见陷阱¶

⚠️ 感知输出过抽象

错误想法：只给上层一个可行或不可行标签即可。

典型现象：控制层无法调整裕度。

根本原因：控制需要连续风险量。

正确做法：输出距离、梯度、协方差和时间戳。

⚠️ 动态物体全剔除

错误想法：移动物体都是 SLAM 异常点。

典型现象：门、抽屉、被搬物体无法建模。

根本原因：操作对象本身就是动态地标。

正确做法：把任务相关物体作为显式地标跟踪。

练习¶

列出 G1+双臂开门任务需要的感知输出。
说明 ESDF 梯度如何进入避障约束。
给物体 pose 估计设计一个有效期策略。

感知输出进入控制层时，最关键的桥梁是不确定性传播。

99.4 SLAM 不确定性到 MPC 代价与约束 ⭐⭐⭐⭐¶

动机：协方差是控制风险输入¶

SLAM 的协方差不是报告中的附属量。

它是控制系统的风险输入。

同一个落足点，如果地图高度确定，可以积极落脚。

如果高度不确定，应提高代价或收紧约束。

同一个物体目标，如果 pose 不确定，应降低末端刚度或先主动观察。

固定权重 MPC 在不确定区域过于自信。

过度保守又会让机器人在可行区域不敢动作。

因此要把不确定性映射为连续的控制参数。

权重调度¶

末端 pose 协方差较大时，可以降低末端追踪权重：

\[ Q(\Sigma)=\frac{Q_{\max}}{1+\alpha\operatorname{tr}(\Sigma)} \]

同时设置下限：

\[ Q_{\min}\le Q(\Sigma)\le Q_{\max} \]

如果不设置下限，目标会突然失去作用。

如果不设置上限，高置信目标会让 MPC 过于刚性。

约束收紧¶

障碍物位置不确定时，安全距离应变大：

\[ d(x,obs)\ge d_0+k\sqrt{\lambda_{\max}(\Sigma_{obs})} \]

这里 \(\lambda_{\max}\) 是协方差最大特征值。

这表示沿最不确定方向增加安全边界。

chance constraint 近似 ⭐⭐⭐⭐¶

如果约束 \(h(x)\ge0\) 受高斯噪声影响，可用一阶近似：

\[ \mu_h-k_\delta\sigma_h\ge0 \]

其中 \(k_\delta\) 由希望的置信度决定。

例如 95% 置信度时，\(k_\delta\) 常取约 1.64（单侧正态分位数）。99% 置信度时 \(k_\delta \approx 2.33\)。

推导过程：设真实约束值为 \(\tilde{h} = h(x) + \epsilon\)，其中 \(\epsilon \sim \mathcal{N}(0, \sigma_h^2)\)。我们要求 \(P(\tilde{h} \geq 0) \geq 1 - \delta\)，即：

\[ P\left(\frac{\tilde{h} - \mu_h}{\sigma_h} \geq \frac{-\mu_h}{\sigma_h}\right) \geq 1 - \delta \]

由正态分布的性质，这等价于 \(\frac{\mu_h}{\sigma_h} \geq \Phi^{-1}(1 - \delta) = k_\delta\)，即 \(\mu_h \geq k_\delta \sigma_h\)。

本质洞察：chance constraint 把"安全边界应该留多大"这个工程问题转化为"你愿意接受多大的违约概率"这个可量化的风险问题。它比"统一加大 10 cm 安全距离"更有理论依据——在不确定性小的方向留小裕度，不确定性大的方向留大裕度。

这类约束比简单加大安全距离更有概率解释。回顾复合/270_SimToReal精读中对域随机化的讨论：域随机化通过扩大训练分布来获得鲁棒性，chance constraint 则通过显式建模不确定性来收紧约束。两者是互补的安全策略——域随机化在训练时使用，chance constraint 在部署时使用。

代码示例：协方差到权重和安全边界¶

import numpy as np


def bounded_weight_from_covariance(cov6, q_min, q_max, alpha):
    """把 6x6 协方差映射为有限范围内的追踪权重。"""
    cov6 = np.asarray(cov6).reshape(6, 6)
    # 中文注释：位置和姿态可以分开调度；这里给出简化版本
    risk = float(np.trace(cov6))
    q = q_max / (1.0 + alpha * risk)
    return float(np.clip(q, q_min, q_max))


def obstacle_margin_from_covariance(cov3, base_margin, k_sigma):
    """根据障碍物位置协方差收紧安全距离。"""
    eig_max = float(np.max(np.linalg.eigvalsh(cov3)))
    sigma = np.sqrt(max(eig_max, 0.0))
    return base_margin + k_sigma * sigma


Q_ee = bounded_weight_from_covariance(
    np.eye(6) * 0.002,
    q_min=10.0,
    q_max=300.0,
    alpha=80.0,
)

工程分层¶

层级	职责	输入	输出	典型频率
协方差传播	从 SLAM 到任务 frame	pose 图协方差	任务协方差	10-100 Hz
权重调度	调整追踪刚度	目标协方差	MPC cost	20-100 Hz
约束收紧	给未知区域留裕度	地图协方差	安全边界	20-100 Hz
模式切换	不确定时先观察	风险指标	主动感知目标	1-10 Hz

决策表¶

场景	推荐动作	不宜动作	判断信号
目标 pose 噪声大	降低末端权重并主动观察	继续高刚度追踪	目标抖动
地形不确定	落足代价增加	忽略协方差	脚底高度估计不稳
地图协方差过大	切换到保守运动	强行执行操作	chance constraint 不可行
协方差方向性强	沿最大不确定方向收紧	统一膨胀所有方向	过度保守
估计短时失效	保持目标但降权	删除目标	遮挡很短暂

⚠️ 常见陷阱¶

⚠️ 协方差直接相加

错误想法：不做坐标系变换就合并噪声。

典型现象：某些方向过保守或过激进。

根本原因：协方差依附坐标系。

正确做法：用雅可比进行协方差传播。

⚠️ 权重调度无界

错误想法：协方差一大就把权重降为零。

典型现象：任务目标突然消失。

根本原因：控制器需要连续过渡。

正确做法：设置权重上下限和低通滤波。

练习¶

把 FAST-LIO 输出的位姿协方差映射为落足代价权重。
推导障碍物位置协方差如何收紧距离约束。
设计目标 pose 协方差过大时的主动观察触发条件。

不确定性传播让控制更稳，但有些不确定性应通过主动改变视角来降低。

99.5 主动感知：为了操作目标而移动传感器 ⭐⭐⭐¶

动机：感知动作也是任务动作¶

主动感知的核心是：机器人可以为了更好完成操作而移动传感器。

这个传感器可能是头部相机。

也可能是腕部相机。

也可能是整个基座。

行走操作中，主动观察必须同时满足动力学稳定和任务进度。

如果只优化视角，机器人可能走到不可落足区域。

如果只优化末端操作，关键接触点可能被手臂遮挡。

信息增益¶

主动感知可用信息增益度量：

\[ IG(a)=H(b)-\mathbb E_{z\mid a}[H(b^+)] \]

其中 \(b\) 是 belief，\(a\) 是观察动作，\(z\) 是未来观测。

在工程中，可以使用更简单的指标：

物体 pose 协方差下降。
未知体素数量减少。
抓取点可见面积增加。
关键边缘或铰链观测数量增加。
SDF 空洞减少。

联合目标¶

主动感知不能只最大化信息。

它必须和运动稳定、任务时间、碰撞风险共同考虑：

\[ J=J_{\text{task}}+\lambda_u J_{\text{uncertainty}}+\lambda_s J_{\text{stability}} \]

当接触已经建立时，主动观察动作应更小。

例如，开门过程中可以转头观察门缝，而不应突然移动整个基座。

代码示例：主动观察候选评分¶

from dataclasses import dataclass


@dataclass
class ViewCandidate:
    name: str
    expected_entropy_drop: float
    motion_cost: float
    stability_cost: float
    collision_free: bool
    reachable: bool


def score_view(candidate: ViewCandidate,
               w_info: float,
               w_motion: float,
               w_stability: float) -> float:
    if not candidate.collision_free or not candidate.reachable:
        return -1e9
    return (w_info * candidate.expected_entropy_drop
            - w_motion * candidate.motion_cost
            - w_stability * candidate.stability_cost)


def choose_next_view(candidates):
    # 中文注释：主动观察必须同时考虑信息收益和运动可行性
    return max(candidates,
               key=lambda c: score_view(c, 1.0, 0.2, 0.5))

工程分层¶

层级	职责	输入	输出	典型频率
候选生成	生成可观察姿态	目标、地图、可见性	候选视角	1-10 Hz
可行性筛选	检查落足和碰撞	候选视角、SDF	可行视角	10-50 Hz
控制执行	移动头、腰、基座或腕部	可行视角	轨迹	50-500 Hz
观测更新	更新协方差	新图像、点云	belief	10-60 Hz

决策表¶

场景	主动感知动作	不宜动作	判断信号
物体被遮挡	头部或基座主动观察	等待策略	遮挡稳定存在
落足未知	先扫地形再走	盲走到目标	地形 SDF 空洞大
接触已建立	小幅头部或腕部动作	大幅移动基座	接触力敏感
任务时间紧	小幅视角动作	大范围重定位	信息收益有限
目标低置信	主动观察或降级	高刚度追踪	pose 协方差大

⚠️ 常见陷阱¶

⚠️ 信息增益脱离可行性

错误想法：选择最优视角后再让机器人过去。

典型现象：规划失败或耗时过长。

根本原因：视角是控制动作的一部分。

正确做法：把可行性和信息增益联合优化。

⚠️ 主动观察打断接触

错误想法：只要能看清，移动基座也可以。

典型现象：接触力突变。

根本原因：接触任务需要连续力控。

正确做法：接触阶段优先头部、腕部或小幅腰部动作。

练习¶

为开门任务设计三个主动观察动作。
给落足未知区域定义一个信息增益指标。
讨论主动感知动作与操作动作冲突时如何仲裁。

主动感知解决“看不清”，但操作闭环还要决定“做什么”和“如何接触”。

99.6 操作闭环边界：TAMP、VLA、扩散策略与力反馈 ⭐⭐⭐⭐¶

动机：策略负责意图，安全由控制层兜底¶

操作闭环把任务语义转成末端目标、抓取、接触模式和恢复动作。

TAMP 擅长长时程离散结构。

VLA 擅长开放世界语义。

Diffusion Policy 擅长生成多模态动作分布。

这些策略很有用，但不应直接越过运动可行性和安全过滤。

各类方法的边界¶

方法	擅长	不擅长
TAMP	长时程任务结构、符号约束、可解释恢复	复杂接触细节和开放语义
VLA	语言理解、开放物体、跨任务泛化	形式化安全和硬实时控制
Diffusion Policy	多模态动作块、示教模仿	分布外安全保证
MPC	动力学、约束、短时预测	开放世界语义
WBC	高频力矩、接触任务优先级	长时任务规划
RL WBC	复杂运动技能、恢复和残差	可解释硬约束

技能接口¶

操作层输出应是技能，而不是裸关节动作。

一个技能可以写成：

\[ skill=(frame,target\_sequence,contact\_mode,force\_range,timeout) \]

字段含义如下：

字段	含义
frame	目标所在坐标系
target_sequence	短时 EE 目标序列
contact_mode	空中、柔顺接触、力控或释放
force_range	允许力范围
timeout	技能有效期

运动层可以接受技能，也可以拒绝技能。

拒绝并不代表失败。

拒绝是一种反馈。

力反馈¶

力反馈不只属于 WBC。

它也应该回到操作层。

接触力超过阈值可以表示接触建立。

切向力异常可以表示滑移。

法向力不足可以表示抓取不牢。

力矩方向异常可以表示工具卡住。

操作层收到这些事件后，应能切换技能或请求重新观察。

代码示例：策略动作安全投影¶

import numpy as np


def project_delta_ee(delta, max_translation, max_rotation):
    """对策略输出的末端增量做最小安全投影。"""
    delta = np.asarray(delta, dtype=float).copy()
    trans = delta[:3]
    rot = delta[3:]

    trans_norm = np.linalg.norm(trans)
    if trans_norm > max_translation:
        trans *= max_translation / (trans_norm + 1e-9)

    rot_norm = np.linalg.norm(rot)
    if rot_norm > max_rotation:
        rot *= max_rotation / (rot_norm + 1e-9)

    delta[:3] = trans
    delta[3:] = rot
    return delta


def reject_if_stale(reference_age_ms, confidence, max_age_ms, min_confidence):
    # 中文注释：过期目标和低置信动作都不应进入运动层
    if reference_age_ms > max_age_ms:
        return True, "reference_expired"
    if confidence < min_confidence:
        return True, "low_confidence"
    return False, "accepted"

这个投影只是最小示例。

真实系统还需要碰撞约束、支撑裕度约束、力矩约束和接触模式约束。

工程分层¶

层级	职责	输入	输出	典型频率
任务规划	选择技能序列	语言、场景图	技能图	0.1-2 Hz
策略生成	输出短时动作块	图像、proprio、目标	EE 目标序列	10-50 Hz
接触监督	判断接触与滑移	力、触觉、视觉	模式事件	50-500 Hz
安全接口	投影或拒绝动作	策略动作、约束	安全目标	50-200 Hz

⚠️ 常见陷阱¶

⚠️ 策略输出过低层

错误想法：让 VLA 直接给 1 kHz 力矩更端到端。

典型现象：延迟大且安全边界不清。

根本原因：开放世界策略不适合硬实时安全。

正确做法：让策略输出 EE 目标或技能参数。

⚠️ 忽略运动层反馈

错误想法：上层只管生成目标，失败由控制器处理。

典型现象：MPC 反复无解。

根本原因：闭环需要可行性负反馈。

正确做法：接收拒绝原因并改目标或主动观察。

练习¶

为“从冰箱取饮料”设计 TAMP、VLA、MPC 的职责划分。
写出一个技能接口字段表。
讨论扩散策略输出动作块时，如何做 receding horizon 安全投影。

操作层给出目标，运动层必须在动力学和接触约束中执行或拒绝。

99.7 运动闭环边界：MPC、WBC、RL WBC 与安全滤波 ⭐⭐⭐⭐¶

动机：运动闭环是物理底座¶

运动闭环是统一系统的物理底座。

它是三闭环中唯一直接和牛顿力学打交道的部分——力矩命令产生加速度，加速度改变速度和位置，接触力决定机器人会不会摔倒。感知层和操作层可以犯错后重试，但运动层的错误往往不可逆——一个关节力矩尖峰可能在 2 ms 内让电机过流保护，一次支撑丧失可能在 200 ms 内导致跌倒。

运动闭环由四个互补的子模块组成，每个负责不同的时间尺度和安全层次：

┌────────────────────────────────────────────────────┐
│ MPC（20-100 Hz）                                    │
│ 职责：未来 0.5-2 秒的动力学可行轨迹                   │
│ 优势：预见性——可以提前发现"3 步后会失稳"               │
│ 局限：模型简化、频率有限、不擅长接触切换               │
├────────────────────────────────────────────────────┤
│ WBC（500-1000 Hz）                                  │
│ 职责：当前时刻的任务优先级和物理约束                    │
│ 优势：精确——全身动力学、摩擦锥、关节限位都满足          │
│ 局限：无预见性——只看当前时步                           │
├────────────────────────────────────────────────────┤
│ RL WBC（50-200 Hz）                                 │
│ 职责：复杂运动技能和未建模动力学补偿                    │
│ 优势：灵活——可以学习传统控制难以实现的技能              │
│ 局限：无形式化安全保证                                │
├────────────────────────────────────────────────────┤
│ 安全滤波（100-1000 Hz）                              │
│ 职责：动作投影和硬约束兜底                             │
│ 优势：可验证——CBF 或 QP 约束有数学保证                 │
│ 局限：保守——可能限制性能                               │
└────────────────────────────────────────────────────┘

MPC 负责短时预测和约束。

WBC 负责高频力矩和接触。

RL WBC 可以提供复杂运动技能。

安全滤波负责在学习策略和硬件之间保持可验证边界。

只靠 MPC 可能无法覆盖复杂接触和开放环境。

只靠 RL 策略会缺少形式化约束。

只靠 WBC 跟踪目标会缺少未来预见和全局可行性。

不是 X 而是 Y：运动闭环的四个子模块不是"选一个用"，而是"各守其位"。MPC 不负责高频力矩（太慢），WBC 不负责预测未来（太短视），RL 不负责安全保证（太黑箱），安全滤波不负责任务性能（太保守）。它们的组合覆盖了从长时预见到瞬时安全的完整时间尺度。

MPC 的职责¶

MPC 适合处理可建模的短时约束：

摩擦锥。
落足位置。
障碍距离。
力矩裕度。
物体短时轨迹。
末端可达性。
基座姿态和支撑多边形。

MPC 不适合直接理解开放语言指令。

它也不适合在 1 kHz 直接输出完整关节力矩。

WBC 的职责¶

WBC 适合处理高频任务优先级：

接触脚不滑。
基座姿态稳定。
末端阻抗。
关节限位。
力矩限制。
自碰撞约束。

典型 WBC QP：

\[ \min_{\dot v,\tau,f_c} \sum_k w_k\|J_k\dot v+\dot J_k v-a_k^{ref}\|^2 \]

约束包括：

\[ M\dot v+h=S^\top\tau+J_c^\top f_c \]

\[ f_c\in\mathcal K_{\text{fric}} \]

\[ \tau^- \le \tau \le \tau^+ \]

RL WBC 与残差¶

RL WBC 适合复杂技能。

例如粗糙地形恢复、快速转身、动态推拉、接触丰富操作。

一种安全组合是：

\[ u=u_{\text{model}}+u_{\text{residual}} \]

其中 \(u_{\text{model}}\) 来自解析控制或 MPC，\(u_{\text{residual}}\) 来自学习策略。

残差必须有幅值、频率和能量限制。

它还应经过安全滤波。

安全滤波¶

安全滤波可以用 QP 投影：

\[ u^{safe}=\arg\min_u\|u-u^{cand}\|_R^2 \]

约束：

\[ h_j(x,u)\ge0 \]

也可以使用控制障壁函数（Control Barrier Function, CBF）：

\[ \dot h(x,u)+\alpha h(x)\ge0 \]

CBF 的完整推导与直觉：

CBF 的核心思想来自一个简单的观察：如果我们能定义一个函数 \(h(x)\)，使得 \(h(x) \geq 0\) 等价于"系统处于安全集内"，那么只要保证 \(h\) 永远不变负，系统就永远安全。

Step 1：定义安全集。设安全集为 \(\mathcal{C} = \{x \mid h(x) \geq 0\}\)。例如，末端到人体的最小距离约束 \(h(x) = d(x_{\text{ee}}, x_{\text{human}}) - d_{\min}\)，则 \(h(x) \geq 0\) 表示"末端离人足够远"。

Step 2：推导不变条件。要让 \(\mathcal{C}\) 正向不变（一旦在里面就出不去），需要在 \(\mathcal{C}\) 的边界上（\(h = 0\)）保证 \(\dot{h} \geq 0\)——即系统不能"向外穿越"边界。但仅在边界处要求 \(\dot{h} \geq 0\) 在数值上不稳定——因为离散控制可能在一个时步内跨过边界。

Step 3：引入安全裕度。CBF 用 \(\alpha h(x)\) 提供安全裕度——当 \(h\) 还有余量时（\(h > 0\)），允许 \(\dot{h}\) 为负（可以朝边界靠近），但靠近速度不能超过 \(\alpha h\)：

\[ \dot{h}(x, u) \geq -\alpha h(x) \]

\(\alpha > 0\) 是一个设计参数。它的物理含义是"允许的最快接近边界的指数衰减率"。\(\alpha\) 越大，系统可以越快地靠近边界（但不会穿过）；\(\alpha\) 越小，系统离边界越远就开始减速。

Step 4：展开为控制约束。对仿射系统 \(\dot{x} = f(x) + g(x)u\)：

\[ \underbrace{L_f h(x)}_{f(x) \text{ 对 } h \text{ 的李导数}} + \underbrace{L_g h(x)}_{g(x) \text{ 对 } h \text{ 的李导数}} u + \alpha h(x) \geq 0 \]

这是关于 \(u\) 的线性约束——可以直接加入 QP 或 WBC 的约束集。

类比：CBF 类似于汽车的自适应巡航系统。当车距足够时（\(h\) 大），你可以随意加速减速（\(u\) 自由）。当车距缩短时（\(h\) 小），系统开始限制最大加速度（\(u\) 受约束）。当车距接近最小值时（\(h \to 0\)），系统强制刹车（\(\dot{h} \geq 0\)）。CBF 把这个直觉形式化为数学约束。

反事实推理：如果不用 CBF 而是用硬距离约束 \(d > d_{\min}\) 会怎样？硬约束不考虑速度方向——即使系统正在远离障碍物，只要距离小于 \(d_{\min}\)，QP 就可能判为不可行。CBF 的优势是考虑了速度方向：向安全方向运动时放宽约束，向危险方向运动时收紧约束。这让控制器在安全和性能之间取得更好的平衡。

工程注意：CBF 假设 \(h(x)\) 可微且 \(L_g h \neq 0\)（即控制输入能影响安全函数）。对某些安全约束（如"地面摩擦力足够"），\(h\) 的解析形式可能不容易写出，此时 QP 投影方法更实用。

QP 投影的优点是工程实现直接，容易和 WBC 结合——把安全约束直接作为 QP 的不等式约束加入即可，不需要推导李导数。

工程分层¶

层级	职责	输入	输出	典型频率
MPC	预测和约束	状态、目标、地图	轨迹/反馈	20-100 Hz
WBC	高频物理执行	轨迹、接触状态	力矩	500-1000 Hz
RL WBC	复杂技能生成	观测、命令	动作或残差	50-200 Hz
安全滤波	动作投影与限幅	候选动作、约束	安全动作	100-1000 Hz

决策表¶

场景	推荐组合	不宜组合	判断信号
精确可解释任务	MPC + WBC	纯 RL	约束可建模
敏捷复杂动作	RL WBC + 安全滤波	全解析规划	模型误差大
人旁操作	约束优先	无过滤策略	安全责任高
接触模式频繁变化	WBC + 阻抗 + 监督	高刚度位置控制	力突变明显
目标不确定	降权、收紧、主动观察	高刚度追踪	协方差大

⚠️ 常见陷阱¶

⚠️ 把学习残差无限放大

错误想法：RL residual 可以覆盖全部误差。

典型现象：策略把系统推到约束边界。

根本原因：残差应补偿模型误差，而不是替代安全。

正确做法：限制残差幅值并经安全滤波。

⚠️ MPC 权重代替硬约束

错误想法：把自碰撞写成低权重代价即可。

典型现象：紧急场景仍可能碰撞。

根本原因：安全边界不能只靠软代价。

正确做法：关键安全写成约束或屏障。

练习¶

列出一个 G1 开门任务中 MPC、WBC、RL WBC 各自的输入输出。
设计一个 CBF 安全滤波用于限制末端靠近人体。
解释为什么关节限位应在 WBC 或安全层再次检查。

运动层定义了物理边界，学习模块则应被放在能最大增益、最小风险的位置。

99.8 物体级 SLAM：把被操作物体从外点变成地标 ⭐⭐⭐⭐¶

动机：操作对象不是异常点¶

经典 SLAM 常把运动物体视为异常观测。

但移动操作中，门、抽屉、箱子、杯子正是机器人要操作的对象。

如果把这些物体全部剔除，操作目标会从地图中消失。

如果把门当静态墙，开门后地图会错误。

如果只做检测不做时序估计，目标 pose 会抖动。

物体级 SLAM 的目标是同时估计机器人、环境和可操作物体状态。

物体运动模型¶

物体可以按运动自由度分类：

类型	状态	示例
刚体	\(SE(3)\) pose + twist	箱子、杯子、工具
铰接体	铰链位姿 + 角度	门、柜门
滑动体	滑轨位姿 + 位移	抽屉
柔性体	低维形状参数	软管、布料

门可以建模为：

\[ x_d=(T_{w h},\theta,\dot\theta) \]

其中 \(T_{w h}\) 是铰链坐标系，\(\theta\) 是开门角度。

抽屉可以建模为：

\[ x_s=(T_{w r},s,\dot s) \]

其中 \(T_{w r}\) 是滑轨坐标系，\(s\) 是滑动位移。

操作动作也是观测¶

操作动作会改变物体。

这不是 SLAM 的麻烦，而是信息来源。

如果机器人推门，门的转动轨迹可以帮助估计铰链轴。

如果机器人拉抽屉，抽屉运动方向可以帮助估计滑轨方向。

观测模型可以写成：

\[ p(z\mid x,u) \]

它显式依赖机器人动作 \(u\)。

这就是 SLAM 与操作耦合的核心。

因子图表示¶

物体级因子图可以包含：

机器人位姿节点 \(T_{w b,t}\)。
物体状态节点 \(x_{o,t}\)。
视觉观测因子。
运动模型因子。
接触动作因子。
几何结构先验因子。

观测因子可写为：

\[ e=\log(T_{\text{obs}}^{-1}T_{\text{model}}(x_o)) \]

接触动作因子可约束物体状态变化与末端动作一致。

工程分层¶

层级	职责	输入	输出	典型频率
检测层	找到物体和关键点	图像/点云	候选观测	5-30 Hz
跟踪层	滤波物体状态	观测、模型	物体 pose	10-60 Hz
结构层	估计关节或运动模型	时序轨迹	铰链/滑轨参数	1-10 Hz
控制层	使用物体状态	物体 pose/协方差	目标与约束	20-100 Hz

决策表¶

场景	推荐模型	不宜模型	判断信号
刚体搬运	SE(3) 物体地标	静态地图点	物体被机器人移动
开门	铰链模型	自由 SE(3) 目标	运动被关节约束
抽屉	滑轨一维模型	通用刚体模型	自由度低且强约束
杯子抓取	刚体 + 抓取候选	仅 2D bbox	需要 6D 对齐
软管操作	低维形状模型	单刚体	明显柔性

⚠️ 常见陷阱¶

⚠️ 操作对象被滤掉

错误想法：动态剔除把门把手和箱子删除可以提高 SLAM 稳定性。

典型现象：控制目标间歇消失。

根本原因：任务相关动态物体应保留。

正确做法：在感知前端标注任务对象并进入地标图。

⚠️ 物体模型过强

错误想法：一开始就假设完美铰链。

典型现象：模型错时估计发散。

根本原因：真实环境有松动、遮挡和非理想结构。

正确做法：从弱模型开始，逐步增加结构约束。

练习¶

为开门任务写出门状态向量。
说明操作动作如何提高铰链估计精度。
比较刚体地标和铰接地标在控制接口上的差异。

物体状态进入统一闭环后，需要一个整体架构把感知、操作和运动连接起来。

99.9 统一架构：参考管理、风险总线与安全过滤 ⭐⭐⭐⭐¶

动机：多个模块不能直接抢控制器¶

统一系统需要一个参考管理层。

参考管理层把来自感知、操作、人工遥控和恢复模块的目标变成运动层可消费的参考。

风险总线汇总地图、目标、网络、硬件和策略置信度。

安全过滤在所有候选动作进入硬件前做最后约束检查。

没有参考管理，多个模块会同时写目标。

没有风险总线，安全判断分散在各处且相互矛盾。

没有安全过滤，学习输出或过期目标可能直接进入控制器。

参考管理¶

参考管理层维护：

目标队列。
优先级。
有效期。
坐标系。
来源。
风险等级。
接触模式。

参考选择可以写成：

\[ r^*=\arg\max_r priority(r)-risk(r)-staleness(r) \]

这里的 risk 来自风险总线。

staleness 来自时间戳和有效期。

风险总线¶

风险总线应包含连续量和离散事件。

连续量包括：

目标协方差。
最小障碍距离。
支撑裕度。
力矩裕度。
网络 p99 延迟。
策略置信度。

离散事件包括：

目标过期。
接触丢失。
MPC 无解。
WBC 约束违反。
硬件温度过高。
人工接管。

安全过滤¶

安全过滤可以在多个层次出现。

操作层可以拒绝低置信技能。

MPC 可以拒绝不可行目标。

WBC 可以加入硬约束。

硬件层可以限幅或急停。

动作过滤可写成：

\[ u^{safe}= \arg\min_u\|u-u^{cand}\|_R^2 \quad \text{s.t.}\quad h_j(x,u)\ge0 \]

工程分层¶

层级	职责	输入	输出	典型频率
参考管理	选择有效目标	技能、pose、协方差	当前参考	10-100 Hz
风险总线	汇总风险指标	感知、控制、网络、硬件	风险状态	10-100 Hz
安全过滤	投影或拒绝动作	候选动作、约束	安全动作	100-1000 Hz
执行回馈	报告成功和失败原因	控制结果	事件	10-100 Hz

决策表¶

问题	优先选择	不宜选择	判断信号
多目标竞争	参考管理优先级	各模块直接写控制器	目标冲突
风险来源多	风险总线	局部布尔变量	诊断困难
候选动作复杂	QP/CBF 投影	简单限幅	多约束耦合
人工遥控介入	高优先级但仍受安全约束	完全绕过保护	人旁风险
策略低置信	拒绝或请求观察	强制执行	分布外输入

⚠️ 常见陷阱¶

⚠️ 多个模块抢目标

错误想法：VLA、TAMP、人工遥控都能直接发布目标。

典型现象：控制器目标跳变。

根本原因：目标写入需要仲裁。

正确做法：建立参考管理层和优先级规则。

⚠️ 安全过滤只做限幅

错误想法：把关节速度 clamp 当作安全。

典型现象：仍可能自碰撞或失稳。

根本原因：安全约束是状态相关的。

正确做法：使用约束投影或 WBC 任务约束。

练习¶

设计一个风险总线字段表。
写出参考管理层如何处理人工遥控与自主策略冲突。
给一个候选 EE 目标设计安全过滤步骤。

架构定义好之后，还需要用最小原型验证：不确定性是否真的改变控制行为。

99.10 最小原型：SLAM 协方差调节 MPC 落足代价 ⭐⭐⭐¶

动机：先验证一条信息通道¶

最小原型应足够小，但必须闭环完整。

建议从“地图不确定性影响落足代价”开始。

这个原型连接 SLAM、MPC 和运动安全。

它不依赖完整 VLA，也能验证统一闭环的信息接口。

直接做全栈开门会同时引入太多变量。

只离线画协方差热力图不能证明控制行为改变。

只调权重不记录地图不确定性无法解释结果。

系统结构¶

LiDAR/RGB-D → elevation map + variance
                     ↓
             foothold cost map
                     ↓
              locomotion MPC
                     ↓
           WBC / RL locomotion policy

SLAM 或建图模块输出每个栅格高度均值和方差。

MPC 的 foothold cost 根据方差增加。

机器人在可选路径中偏向低不确定区域。

主动感知版本可让机器人先扫描高价值未知区域。

落足代价¶

落足代价可以写成：

\[ J_{\text{foot}}= J_{\text{terrain}}+ \lambda_\sigma\sigma_z^2(p_{\text{foot}}) \]

其中 \(\sigma_z^2\) 是落足点高度方差。

如果地形高度不确定，落足代价增加。

如果没有可行低不确定路径，系统应降低速度或主动观察。

A/B 实验¶

对照组使用固定落足代价。

实验组使用协方差调节代价。

扰动包括：

地图点云稀疏。
局部遮挡。
传感器噪声。
低矮障碍。
斜坡边缘。

记录：

任务成功率。
最小支撑裕度。
落足高度误差。
路径长度。
主动观察次数。
控制器无解次数。

工程分层¶

层级	职责	输入	输出	典型频率
建图	输出高度均值/方差	点云、位姿	不确定性栅格	5-30 Hz
代价映射	把方差变成 cost	栅格、足端候选	落足代价	20-100 Hz
MPC	选择轨迹和落足	状态、代价图	轨迹	20-100 Hz
评测	统计路径和稳定性	日志	对比结果	离线

⚠️ 常见陷阱¶

⚠️ 原型过大

错误想法：一开始集成 VLA、手臂、开门、SLAM。

典型现象：失败原因不可分。

根本原因：统一闭环也需要分阶段验证。

正确做法：先验证单一信息通道闭环。

⚠️ 代价映射未归一化

错误想法：直接把协方差数值乘到 cost。

典型现象：MPC 权重尺度异常。

根本原因：SLAM 协方差单位和控制代价不同。

正确做法：用分位数、截断和低通滤波。

练习¶

实现一个 2D 栅格方差到落足代价的映射函数。
设计 A/B 实验：固定权重 vs 协方差权重。
记录机器人是否主动避开高不确定区域。

最小原型完成后，统一闭环的价值应通过故障排查和综合项目继续验证。

99.10B 从最小原型到完整系统的工程路线 ⭐⭐⭐¶

99.10B.1 扩展路线图¶

99.10 的最小原型验证了一条信息通道：SLAM 地图不确定性 → MPC 落足代价。从这个原型扩展到完整的统一闭环系统，需要分阶段增加信息通道，每个阶段都是一个可独立验证的子系统。

阶段	新增信息通道	系统产物	验证指标
Stage 0	地图方差 → 落足代价	最小原型	落足错误率
Stage 1	物体 pose 协方差 → EE 权重	物体跟踪模块	末端抖动幅度
Stage 2	遮挡检测 → 主动观察	主动感知模块	协方差恢复时间
Stage 3	运动可行性 → 目标拒绝	拒绝接口	重试次数
Stage 4	策略置信度 → 安全投影	安全过滤器	约束违反率
Stage 5	完整闭环	开门/取物系统	安全成功率

每个阶段的扩展应遵循一个关键原则：先证明新通道的因果效应，再集成到系统中。具体地，对于每个新增的信息通道，必须完成以下实验：

A/B 实验：有通道 vs 无通道，对比关键指标
消融：去掉新通道，验证指标确实下降
扰动：在新通道的输入端注入噪声，验证系统退化是否平滑

99.10B.2 物体 pose 协方差到末端权重的扩展（Stage 1）¶

从最小原型扩展到 Stage 1 需要增加物体 pose 估计模块和末端权重调度。

物体 pose 估计可以使用三种方案，按复杂度排列：

方案	输入	输出	适用场景
AprilTag	单目/双目相机	6D pose + 协方差	已知物体、结构化环境
模型匹配	RGB-D + CAD 模型	6D pose + 不确定性	已知物体、非结构化环境
基于学习	RGB + 检测器	6D pose + 置信度	未知物体

末端权重调度的数学形式与 99.4 中的地图协方差调度相同，但应用在不同的控制通道上：

\[ Q_{ee}(\Sigma_{obj}) = \frac{Q_{ee,max}}{1 + \alpha_{obj} \cdot \text{tr}(\Sigma_{obj,pos})} \]

工程要点：物体 pose 协方差和地图协方差的量纲不同。地图高度方差通常在 \(10^{-4}\) 到 \(10^{-2}\) m\(^2\) 范围，而物体 pose 位置方差通常在 \(10^{-6}\) 到 \(10^{-3}\) m\(^2\) 范围。两者的 \(\alpha\) 参数需要分别标定，不能共用。

反事实推理：如果不做末端权重调度，物体 pose 抖动时会怎样？MPC 会用高刚度追踪一个不稳定的目标，末端跟着目标高频振荡。这种振荡不仅浪费能量，还可能导致接触力突变——如果机器人正在抓取物体，振荡可能导致脱手。

99.10B.3 主动感知模块的扩展（Stage 2）¶

主动感知是统一闭环最具差异化的组件——它让感知层从"被动观测"变成"主动探索"。

实现主动感知需要三个子模块：

遮挡检测器：判断当前视角下目标物体是否被遮挡
视角候选生成器：生成可以消除遮挡的观察姿态
可行性筛选器：从候选中筛选运动学可行、动力学安全的视角

视角候选的评分公式扩展自 99.5：

\[ s(c) = w_{info} \cdot \Delta H(c) - w_{motion} \cdot J_{motion}(c) - w_{stability} \cdot J_{stability}(c) - w_{contact} \cdot J_{contact}(c) \]

这里增加了接触代价 \(J_{contact}\)——当机器人正在执行接触任务（如推门）时，主动观察不应打断接触。接触代价在接触已建立时取高值，迫使主动观察选择小幅度的头部或腕部动作而非大幅基座移动。

99.10B.4 拒绝接口的扩展（Stage 3）¶

拒绝接口是运动层向操作层的反馈通道。没有这个通道，操作层会反复发送同一个不可行目标。

拒绝消息的完整结构应包含：

struct GoalRejection {
    uint64_t goal_id;                     // 被拒绝的目标 ID
    std::string rejection_reason;          // 拒绝原因编码
    double min_feasible_distance;          // 到最近可行目标的距离
    std::array<double, 7> suggested_goal;  // 建议的替代目标（如果有）
    double confidence;                     // 替代建议的置信度
    double timestamp;                      // 拒绝时间戳
};

关键设计决策：拒绝消息应包含建议的替代目标。这让操作层不需要从零开始搜索新目标——它可以直接使用运动层建议的"最近可行目标"作为起点。

类比：拒绝接口类似于 GPS 导航中的"重新规划路线"。当导航发现前方路不通时，它不只说"此路不通"（简单拒绝），而是说"此路不通，建议走另一条路，多走 200 米"（拒绝+替代+代价）。操作层收到这样的反馈后，可以做出更好的决策。这个类比的边界是：GPS 有完整的路网地图可以规划替代路线，而运动层的"替代目标"只是局部最优的近似。

99.10B.5 系统级集成的时序约束¶

当多个信息通道同时工作时，时序约束变得至关重要。每个模块的输出都有时间戳和有效期，下游模块必须检查上游数据的新鲜度。

时序约束图：
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
SLAM pose (100 Hz)
  → valid for 20 ms
  → 用于 MPC 初始状态

物体 pose (30 Hz)
  → valid for 100 ms
  → 用于 EE 目标生成

Diffusion chunk (5 Hz)
  → valid for 300 ms
  → 用于 EE 目标序列

MPC 轨迹 (50 Hz)
  → valid for 40 ms
  → 用于 WBC 参考

WBC 力矩 (500 Hz)
  → 即时执行
  → 无缓存

全局时钟同步精度要求：< 1 ms
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

如果时钟同步精度不够，最常见的症状是"末端轨迹锯齿"——因为不同模块的数据时间戳不一致，插值出来的目标会在时间轴上来回跳变。

⚠️ 常见陷阱¶

⚠️ 一次性集成所有通道

错误想法：把所有模块同时接入系统，一次性调试。

典型现象：系统不工作时无法定位问题来源。

根本原因：多个信息通道互相影响，组合失败模式比单通道多指数倍。

正确做法：每次只增加一个通道，验证后再加下一个。

⚠️ 忽略时钟同步

错误想法：所有模块在同一台电脑上运行，时间自然一致。

典型现象：末端轨迹有 5-10 ms 级别的抖动。

根本原因：不同线程/进程的系统调用时间戳有微秒到毫秒级偏差。

正确做法：使用单一时间源（如 clock_gettime(CLOCK_MONOTONIC)），所有模块引用同一时钟。

💡 概念误区：认为扩展就是"加模块"

每个新模块不只增加功能，还增加了新的失败模式和时序约束。

正确理解：扩展是"加模块 + 加验证 + 加容错"。

练习¶

为 Stage 1 设计 A/B 实验：有/无物体协方差调度的末端抖动对比。
为 Stage 3 实现一个最小的 GoalRejection 消息，并设计操作层收到拒绝后的行为策略。
画出完整系统中所有模块的时序依赖图，标注每条数据通道的频率和有效期。

99.10C 不确定性感知控制的理论根基 ⭐⭐⭐⭐¶

99.10C.1 从贝叶斯决策到控制¶

统一闭环中的不确定性感知控制可以从贝叶斯决策理论的角度严格推导。

考虑一个简单的决策问题：机器人要决定是否落脚到位置 \(p\)。位置 \(p\) 的地形高度 \(h(p)\) 是不确定的，服从高斯分布 \(h(p) \sim \mathcal{N}(\mu_h, \sigma_h^2)\)。落脚安全要求 \(h(p) > h_{min}\)。

贝叶斯最优决策是最小化期望损失：

\[ a^* = \arg\min_a \mathbb{E}_{h \sim p(h|z)}[L(a, h)] \]

其中损失函数 \(L\) 在安全落脚时为 0，在不安全落脚时为正（表示跌倒的代价）。

对于高斯分布的 \(h\)，这个期望损失可以解析计算。当 \(\mu_h - k_\delta \sigma_h > h_{min}\) 时，落脚的期望损失足够低——这就是 99.4 中 chance constraint 的来源。

99.10C.2 信息价值与主动感知¶

主动感知的理论基础是信息价值（Value of Information, VoI）。

一个观察动作 \(a_{obs}\) 的信息价值定义为：

\[ VoI(a_{obs}) = V^*(b^+) - V^*(b) \]

其中 \(b\) 是当前 belief，\(b^+\) 是观察后的预期 belief，\(V^*\) 是最优策略的期望回报。

直觉上，如果观察能显著改善后续决策的质量，这个观察就有高价值。具体到落足决策：如果当前地形高度方差 \(\sigma_h^2\) 很大，导致 chance constraint 不可行，那么一次扫描（降低 \(\sigma_h^2\)）可能让原本不可行的路径变成可行——这就是高信息价值。

本质洞察：主动感知的本质是用运动成本换取信息收益。机器人为了看清地形需要走到更好的视角（运动成本），但看清地形后可以走更短的路径或避免跌倒（信息收益）。当信息收益大于运动成本时，主动感知是值得的。

99.10C.3 不确定性传播的数学工具¶

从 SLAM 的位姿协方差传播到控制代价需要一系列数学工具：

协方差传播（一阶近似）：如果观测量 \(y = f(x)\)，且 \(x \sim \mathcal{N}(\mu_x, \Sigma_x)\)，则：

\[ \Sigma_y \approx J_f \Sigma_x J_f^T \]

其中 \(J_f = \frac{\partial f}{\partial x}\bigg|_{\mu_x}\) 是 Jacobian。

在统一闭环中，这个传播链条通常是：

\[ \Sigma_{SLAM} \xrightarrow{J_{frame}} \Sigma_{task} \xrightarrow{J_{cost}} \Delta Q_{MPC} \]

从 SLAM 坐标系到任务坐标系的变换需要坐标变换的 Jacobian，从任务协方差到 MPC 代价变化需要代价函数的梯度。

⚠️ 常见陷阱¶

⚠️ 协方差传播忽略非线性

一阶近似只在不确定性较小时准确。当协方差很大时（如长时间无回环的 SLAM），一阶近似可能严重低估不确定性。

正确做法：在协方差特别大的情况下使用 Unscented Transform 或 Monte Carlo 采样。

💡 概念误区：认为贝叶斯决策需要精确的概率模型

工程实践中概率模型总是近似的。关键是模型的相对顺序正确——高不确定时确实更保守，低不确定时确实更积极。绝对值不需要很准。

练习¶

推导从 SLAM 位姿协方差 \(\Sigma_{SE(3)}\) 到末端位置协方差 \(\Sigma_{ee}\) 的完整 Jacobian 链。
计算一个具体的 VoI：假设当前地形方差 \(\sigma_h^2 = 0.01\) m\(^2\)，扫描后 \(\sigma_h^2 = 0.001\) m\(^2\)，chance constraint 置信度 95%，安全距离 \(h_{min} = 5\) cm。扫描前后可行路径长度变化多少？
讨论 Monte Carlo 采样和 Unscented Transform 在不确定性传播中的适用场景和计算代价。

99.11 故障排查手册¶

症状	可能原因	排查步骤	修复方向
末端追踪目标持续抖动	物体 pose 协方差大但权重未调低	查看 pose 时间序列和 \(Q_{ee}\) 调度	降低权重、低通目标、主动观察
机器人反复拒绝操作目标	目标在动力学或碰撞上不可行	查看 goal_rejected 原因码	让操作层换抓取点或调整基座
落足进入未知区域	地图不确定性没有进入 foothold cost	检查栅格方差和落足代价	加入协方差代价和约束收紧
VLA 输出动作造成危险姿态	动作未经过安全投影	对比投影前后动作和约束值	加入 CBF/QP 过滤
主动观察导致任务变慢	信息增益权重过高	记录信息收益和路径成本	按任务阶段调低观察权重
操作对象从地图中消失	动态物体被前端剔除	检查动态点过滤规则	把任务对象注册为物体地标
回环后目标突然跳变	SLAM 坐标修正未平滑传给控制	检查 frame tree 和参考时间戳	对任务 frame 做缓变更新
WBC 周期抖动	低频模块进入硬实时路径	记录控制循环耗时分布	用缓存和插值隔离低频模块

99.12 综合项目：感知不确定性感知的开门系统¶

项目目标¶

在 G1 或 Go2+Z1 仿真中实现一个开门任务。

感知层估计门把手 pose、门平面、铰链轴和协方差。

操作层生成接近、抓取、转动、拉开、通过五个技能。

运动层用 MPC/WBC 执行，并在目标不确定时降低末端刚度或主动观察。

安全层过滤不可行末端目标，并返回拒绝原因。

子模块验收¶

模块	核心输出	验收标准
door_perception	把手 pose + 协方差	静态 RMS < 2 cm，遮挡时协方差升高
active_view	下一观察姿态	能降低把手 pose 方差
skill_manager	技能序列和接触意图	每个技能有有效期和失败恢复
mpc_interface	EE 目标和权重	权重随协方差连续变化
wbc_controller	力矩和接触力	接触力无尖峰，关节限位满足
safety_filter	接受/拒绝及原因	不可行目标不进入执行层

实验设计¶

实验	变量	对照	指标
E1 pose 噪声	把手 pose 噪声 0-5 cm	固定权重/协方差权重	成功率、力峰值
E2 遮挡	遮挡比例 0-70%	主动观察开/关	观察次数、最终误差
E3 地形未知	门前未知区域大小	落足代价开/关	落足风险、路径长度
E4 策略动作	VLA 或扩散目标块	安全过滤开/关	拒绝次数、约束违反
E5 回环跳变	SLAM pose 修正幅度	平滑/不平滑	目标跳变和控制冲击

最小验收命令示例¶

# 固定权重基线
uv run scripts/run_door_task.py --config configs/door_fixed_weight.yaml

# 协方差调度版本
uv run scripts/run_door_task.py --config configs/door_uncertainty_aware.yaml

# 生成对比指标
uv run scripts/evaluate_door_task.py --a runs/fixed/latest --b runs/uncertainty/latest

99.13 练习合集¶

用一个二维例子推导协方差椭圆如何收紧障碍物距离约束。
把一个 VLA 输出的 8 步 EE 目标块改写为 MPC 可消费的参考轨迹。
设计一个风险总线，包含感知风险、运动风险、接触风险、网络风险、硬件风险。
为“拿起透明杯子”设计主动感知动作，说明为什么透明物体需要不同策略。
写出门铰链状态估计的因子图节点和边。
比较 CBF 投影和 WBC 硬约束在安全过滤中的优缺点。
讨论学习残差在接触丰富任务中可能失效的三个原因。
把 SLAM 回环修正传给控制层时，设计一种平滑更新方法。
用日志回放定位一次“目标过期仍执行”的问题。
画出完整开门系统中每条消息的频率、坐标系和有效期。

99.13B 统一闭环的评测方法论 ⭐⭐⭐¶

99.13B.1 为什么统一闭环难以评测¶

统一闭环系统的评测比单模块评测困难得多。单模块（如纯 SLAM、纯 MPC）有明确的性能指标和标准 benchmark。但统一闭环的性能取决于模块间的协作质量——一个模块的"最优"配置可能在系统层面是次优的。

例如，SLAM 的最优配置可能是最大化定位精度，但这需要密集的回环检测，会消耗大量计算资源，挤压 MPC 的求解时间。系统层面最优的 SLAM 配置可能是"精度稍低但计算资源占用少"的折中方案。

99.13B.2 三层评测框架¶

统一闭环系统应从三个层次评测：

模块层评测：每个模块独立评测其核心功能。

模块	独立指标	评测方法
SLAM	位姿 ATE/RPE	标准数据集对比
物体估计	pose RMS、协方差校准度	已知目标对比
MPC	求解时间、可行性	约束违反统计
WBC	力矩跟踪误差	单关节阶跃响应
安全滤波	约束违反率	随机输入测试

通道层评测：每条信息通道独立评测其因果效应。

信息通道	A/B 实验设计	核心指标
地图协方差→落足代价	固定代价 vs 协方差代价	落足错误率
物体协方差→EE 权重	固定权重 vs 协方差权重	末端抖动幅度
遮挡→主动观察	被动等待 vs 主动观察	协方差恢复时间
不可行→拒绝反馈	无反馈 vs 有反馈	重试次数和恢复时间

系统层评测：端到端任务评测。

任务	评测指标组
开门	安全成功率、任务时间、接触力峰值、落足错误数
取物	安全成功率、末端精度、主动观察次数、拒绝次数
整理	安全成功率、任务完成度、人工介入次数、总能耗

本质洞察：统一闭环的评测必须区分"系统改进"和"机制改进"。系统改进（"加了模块 X 后总体成功率提高了"）不足以说明 X 的价值——可能是其他模块的间接效应。机制改进必须通过消融验证——"去掉 X 后成功率下降，且下降发生在 X 预期影响的维度上"。

99.13B.3 安全成功率的定义与计算¶

安全成功率是统一闭环最重要的单一指标：

\[ SR_{safe} = \frac{N_{success} \cap N_{no\_violation}}{N_{total}} \]

其中 \(N_{no\_violation}\) 是没有触发任何安全约束违反的试验次数。安全约束包括：

约束	阈值	判定方式
关节限位	超过 soft limit 的 95%	瞬时违反
自碰撞距离	小于 2 cm	瞬时违反
接触力峰值	超过 200 N	瞬时违反
支撑裕度	支撑多边形内切圆半径小于 3 cm	持续 50 ms
基座倾斜	roll/pitch 超过 25°	瞬时违反

一个试验即使最终成功完成任务，但中间触发了安全违反，也不算安全成功。

这个定义比单纯的成功率更有工程价值——一个成功率 90% 但安全成功率只有 60% 的系统，意味着 30% 的试验在危险条件下完成，不适合真实部署。

⚠️ 常见陷阱¶

⚠️ 只报告成功率不报告安全成功率

错误做法：论文只写 success rate = 85%。

根本原因：如果其中 20% 触发了安全约束但恰好没导致失败，这个系统不可部署。

正确做法：同时报告成功率、安全成功率和安全违反类型分布。

💡 概念误区：认为消融实验只需要去掉模块

消融的粒度应该和假设的粒度匹配。

如果假设是"协方差调节落足代价有效"，消融应该是关闭协方差调度（但保留其他模块），而不是去掉整个感知模块。

练习¶

为一个开门任务设计完整的三层评测方案（模块/通道/系统）。
计算一组实验数据的安全成功率：10 次试验中 8 次完成任务、其中 2 次触发了关节限位违反但未摔倒。
讨论"安全违反但任务成功"的试验是否应该在论文中详细分析。

99.14 本章总结¶

主题	核心结论	工程落点
三闭环	统一是信息闭合与边界清晰	统一参考包
时间尺度	低频决策不能阻塞高频控制	缓存、插值、有效期
感知输出	pose 必须带协方差和坐标系	SLAM/物体/SDF 接口
不确定性传播	协方差应改变权重和安全裕度	cost 调度与约束收紧
主动感知	视角动作服务任务目标	信息增益 + 可行性筛选
操作边界	策略输出目标和意图，不绕过安全层	技能接口与拒绝原因
运动边界	MPC/WBC/RL 各有职责	安全滤波和 WBC 约束
物体级 SLAM	操作对象是动态地标	铰接体和滑动体模型

99.14B 统一闭环与 2024-2025 前沿工作的关系 ⭐⭐⭐¶

99.14B.1 统一闭环在前沿研究中的定位¶

2024-2025 年移动操作领域的一个明显趋势是从单模块优化转向系统级集成。早期工作专注于提升单个模块的性能——更准确的 SLAM、更快的 MPC、更强的 RL 策略。但最近的工作开始关注模块之间的交互质量——感知如何影响控制、控制如何反馈感知。

本章讨论的统一闭环正是这个趋势的核心。它提供了一种结构化的方式来思考"模块之间应该传什么"。

99.14B.2 与遮挡感知操作的关系¶

2025 年的一项代表性工作研究了移动操作器在物体遮挡下的非抓取操作。它提出了一个显式建模遮挡不确定性的框架——预测碰撞可能性的分布，并通过不确定性提取来引导机器人的主动感知行为。

这与本章 99.5 主动感知的思想高度一致：不确定性不只是被动接受的，而是可以通过主动改变视角来降低的。区别在于该工作关注物体级遮挡，本章更关注地图级和任务级不确定性。

99.14B.3 与主动 SLAM 的关系¶

Active SLAM 社区长期研究如何通过主动探索来同时优化定位精度和地图完整性。本章的主动感知模块可以看作 Active SLAM 在移动操作场景下的特化——不只为了"地图更完整"而探索，而是为了"操作任务更安全"而观察。

两个领域的交叉点在于：信息增益的定义。Active SLAM 的信息增益通常是关于全局地图不确定性的（如地图熵），而移动操作的信息增益应该是关于任务相关目标的不确定性（如门把手 pose 协方差）。

本质洞察：统一闭环的核心贡献不是某个特定算法，而是一种系统设计范式——让每个模块把自己的不确定性和边界显式暴露给下游。这个范式可以容纳不同的具体实现——SLAM 可以是 GTSAM 或 ORB-SLAM3，MPC 可以是 OCS2 或 acados，策略可以是 Diffusion Policy 或 VLA。范式不变，实现可以随技术演进而更新。

⚠️ 常见陷阱¶

⚠️ 把统一闭环等同于某个具体系统

错误想法：统一闭环就是"GTSAM + OCS2 + Diffusion Policy + CBF"的组合。

实际上：统一闭环是一种架构原则——显式传递不确定性和边界。

具体使用哪些算法取决于任务和资源。

练习¶

比较 Active SLAM 和移动操作中"信息增益"的定义差异。
如果把本章的 MPC 从 OCS2 换成 acados，哪些接口需要修改？哪些不需要？
讨论统一闭环范式在人形操作和四足臂操作中的实例化差异。

99.15 延伸阅读与代码路径¶

材料	难度	阅读重点
Grandia 2023 Perceptive NMPC	⭐⭐⭐	感知地图如何进入 NMPC 约束
Pankert 2020 Perceptive MPC	⭐⭐⭐	ESDF、避障与连续移动操作
OCS2 mobile_manipulator	⭐⭐⭐	参考管理、SE(3) 目标和约束接口
openpi / OpenVLA / Diffusion Policy	⭐⭐⭐	策略输出形式与安全接口
GTSAM / Kimera 系列	⭐⭐⭐	因子图、不确定性和物体级估计

下一章连接：本章把感知、操作、运动三个闭环的边界讲清楚。下一章将把这些技术能力转化为博士阶段的研究路线，重点不再是”读哪些论文”，而是”搭建什么系统、验证什么假设、用哪些指标证明贡献”。

99.16 研究实践建议¶

给新手的建议¶

从最小信息通道开始。不要试图一次性搭建完整的统一闭环。先选一条信息通道（如地图方差→落足代价），完成闭环验证后再扩展。
画清楚坐标系。统一闭环中涉及世界坐标系、机体坐标系、任务坐标系、物体坐标系——每个数据的坐标系必须显式标注。坐标系错误是最常见的集成 bug。
时间戳比数值更重要。在调试多模块系统时，检查时间戳一致性通常比检查数值正确性更优先。很多”数值错误”的根因是”用了过时的数据”。

给有经验者的建议¶

不确定性不需要很准确。协方差调度的关键是相对顺序正确（高不确定时更保守），而不是绝对数值精确。如果校准协方差太困难，可以用经验风险分位数替代。
安全层应独立于学习模块。安全过滤器的正确性不应依赖于策略的训练质量。即使策略完全失效（输出随机动作），安全层也应保证系统不会进入危险状态。
物体级 SLAM 是高回报但高风险的研究方向。如果时间有限，可以先用简化方案（如 AprilTag + EKF）验证物体状态进入控制的价值，再决定是否投入完整的物体级 SLAM。

99.17 版本信息速查¶

工具/框架	推荐版本	用途
OCS2	最新 main	MPC 求解和参考管理
Pinocchio	3.x	动力学和 Jacobian 计算
GTSAM	4.2+	因子图 SLAM 和协方差
IsaacLab	最新	GPU 并行 RL 训练
ROS 2 Humble/Iron	LTS	通信和消息传递
openpi / Diffusion Policy	最新	操作策略生成
MuJoCo	3.x	Sim2Sim 验证

99.18 常见误解汇总¶

误解	正确理解
统一闭环就是端到端大模型	统一的是信息反馈，不是模块合并
协方差越准系统越好	关键是相对顺序正确，绝对值可以近似
主动感知总是值得的	信息收益必须大于运动成本和任务延迟
安全过滤只是简单限幅	安全约束是状态相关的，需要 CBF 或 QP 投影
物体级 SLAM 必须完美	可以从简化方案（AprilTag + EKF）开始验证价值
拒绝接口只是打印错误	应包含原因码、替代建议和时间戳
所有模块应同步运行	异步是必然的，需要缓存、插值和有效期管理
MPC 权重可以代替硬约束	安全边界必须用约束或屏障函数保证