第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.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.14 本章总结¶

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

99.15 延伸阅读与代码路径¶

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

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