第95章：力敏感人形 Loco-Manipulation——FALCON / SoFTA / HOMIE

元信息	值
难度	⭐⭐⭐⭐（接触力控 + 双策略 RL + 遥操作接口 + 频率解耦）
预计时间	2 周（55-70 小时）
核心平台	Unitree G1、Booster T1、Unitree H1、HOMIE 同构外骨骼
主线	力敏感任务 → 阻抗控制理论 → 双策略训练 → 频率解耦 → 遥操作和技能库

本章定位 本章面向已经学过腿足简化模型、WBC、复合机器人动力学的读者。 写作重点不是罗列论文名，而是把数学假设、控制接口和真实系统故障连成一条可推理的链。 读完后应能解释每个控制量从哪里来、为什么这样定义、用错以后会怎样。

核心参考文献 - Zhang et al., "FALCON: Learning Force-Adaptive Humanoid Loco-Manipulation," L4DC 2026, arXiv:2505.06776 - Li et al., "Hold My Beer (SoFTA): Learning Gentle Humanoid Locomotion and End-Effector Stabilization Control," CoRL 2025, arXiv:2505.24198 - Bao et al., "HOMIE: Humanoid Loco-Manipulation with Isomorphic Exoskeleton Cockpit," RSS 2025, arXiv:2502.13013 - Li et al., "BFM-Zero: A Promptable Behavioral Foundation Model for Humanoid Control Using Unsupervised RL," 2025, arXiv:2511.04131 - Hogan, "Impedance Control: An Approach to Manipulation," ASME J. Dynamic Systems, 1985

---

95.0 前置自测

#	问题	前置知识	合格答案关键词
1	WBC 中末端 wrench 如何进入全身动力学？	复合/20、92章	\(J_{ee}^T f_{ee}\)
2	RL reward hacking 在腿足中常见形式有哪些？	足式/190	抖动、趴地、滑行
3	Teacher-Student 和 DAgger 解决什么问题？	93章	稀疏部署观测和分布偏移
4	ASAP 对接触触发为何困难？	94章	离散模式切换导致转移不连续
5	力控与位置控的核心差异是什么？	复合/30	接触任务需调节 wrench 和阻抗

本章目标

1. 理解力敏感 loco-manipulation 与普通全身模仿的根本差异：外力是任务变量而非扰动。
2. 掌握阻抗控制的完整理论：从弹簧-阻尼模型到笛卡尔阻抗，包含无源性与稳定性证明。
3. 掌握 FALCON 双策略训练：共享本体观测、上体 EE tracking、下体 gait stability。
4. 理解 SoFTA 的上体高频与下体低频解耦，以及异步控制的训练和部署影响。
5. 掌握 HOMIE 同构外骨骼、SkillBlender、BFM-Zero 等路线的接口设计差异。
6. 掌握力控调参实战：Kp/Kd 选择、力范围标定、安全限制的工程方法。

---

95.1 力敏感任务的独特性：外力不再是噪声 ⭐⭐

从行走到走动操作

普通行走任务中，外力通常被当作扰动——策略的目标是"尽管有扰动仍然保持行走"。力敏感 loco-manipulation 中，外力是任务本身的一部分。

考虑以下对比：

	普通行走	力敏感 Loco-Manipulation
外力角色	扰动（越小越好）	任务变量（需要精确控制）
策略目标	抵抗外力保持稳定	在施加/承受外力的同时保持稳定
失败模式	摔倒	摔倒 + 力控失败（杯子洒了/门没推开/物体掉了）
信息需求	本体感知即可	本体感知 + 力/力矩感知或隐式力估计

具体任务分析

任务	外力特征	主要风险	策略需要学会的能力
负载运输	持续重力和惯性力 (5-20 N)	CoM 偏移、力矩饱和	姿态补偿和步态放缓
开门	法向接触 + 转动约束 (10-40 N)	手滑、脚滑、肩部过载	力位混合和身体借力
端杯子	末端加速度约束 (<0.5 m/s²)	液体晃动	上体稳定和柔顺步态
拉车	大水平力 (40-100 N)	ZMP/CMP 贴边	身体后倾和步幅调节
双手搬运	双臂内力与物体约束	手臂互相打架	双臂协调和下体补偿
工具使用	周期性力 (5-30 N)	节奏失配、共振	力节奏同步和共振避免
人-机协作搬运	人施加的不可预测力	突然松手导致加速	力突变检测和快速响应

阶段小结：以上任务列表展示了力敏感 loco-manipulation 的多样性——从几牛顿的精细操作到上百牛顿的重载搬运，从稳态持续力到冲击瞬态力。策略设计必须覆盖这个完整的力范围谱，而不仅仅针对某一种任务优化。这也是 FALCON 力课程设计需要从 0 N 逐步推进到 100 N 的根本原因——策略必须在整个力范围内都保持稳定。

从 Hybrid RL 视角理解力敏感 loco-manipulation

Wang et al. (2025) 提出了一种混合 RL 架构用于人形 loco-manipulation——将视觉运动控制（visuomotor control）和力控制解耦为两个层级。视觉层负责目标识别和高层运动规划，力控层负责接触安全和精细操作。这种分层架构与 FALCON 的上下体解耦是正交的——可以在 FALCON 的双策略框架中进一步引入视觉-力控的层级解耦，形成"上体视觉策略 + 上体力控策略 + 下体步态策略"的三层架构。

为什么只做 IK 不够

上体 IK 可以让手到达目标位姿。但一旦末端接触物体，物体反作用力会通过手臂传到躯干，再传到脚底。如果下体策略没有把这股力纳入观测或训练，机器人会表现为脚底打滑、躯干倾倒或关节饱和。

全身动力学方程明确地展示了这种耦合：

\[ M(q)\dot{v}+h(q,\dot{q})=S^T\tau+J_c^T\lambda+J_{ee}^T f_{ee} \]

其中 \(M\) 是广义质量矩阵，\(h\) 包含科氏力、离心力和重力，\(S\) 是选择矩阵（排除浮动基座），\(\tau\) 是关节力矩，\(J_c^T\lambda\) 是地面接触力的贡献，\(J_{ee}^T f_{ee}\) 是末端外力的贡献。

这条方程说明末端力 \(f_{ee}\) 不是"上体局部问题"。通过全身动力学，\(J_{ee}^T f_{ee}\) 影响每一个关节的加速度，包括腿部关节。

本质洞察：力敏感任务的控制目标不是"让手更硬"，而是"让全身知道手正在受力"。如果下体不知道上体外力，行走稳定性会被当作未知扰动处理，策略只能被动挽救。FALCON 与 SoFTA 的设计都在解决"上体精度"和"下体稳定"如何共享信息的问题。

反事实推理：如果我们坚持只用 IK 处理上体，不让下体策略感知外力会怎样？假设机器人在推门，需要持续施加 30 N 的水平力。IK 计算出了正确的手臂关节角度，但这 30 N 的反作用力通过肩关节传到躯干，等效于在躯干质心处施加一个约 30 N × 0.5 m = 15 Nm 的倾倒力矩。如果下体策略按照"无外力行走"训练，它不知道这个额外力矩的存在，只会在机器人开始倾倒时才被动反应——此时可能已经来不及了。

---

95.2 阻抗控制理论：从弹簧-阻尼模型到笛卡尔阻抗 ⭐⭐⭐

动机：为什么需要阻抗控制

回顾 Rule 4 的要求——先展示问题再给理论。

问题：机器人末端接触物体时，应该用多大的力？

如果用位置控制——把末端锁定在目标位置——任何位置误差都会产生巨大的接触力（刚度无穷大）。这在开门、插拔等任务中会导致物体或机器人损坏。

如果用纯力控——直接控制接触力——那么在空中运动时没有力反馈，无法控制位置（阻抗为零）。

阻抗控制的核心思想是：不控制位置，也不控制力，而是控制位置和力之间的"关系"——即阻抗。

弹簧-阻尼模型：最简单的阻抗

最简单的阻抗就是弹簧-阻尼系统：

\[ f = K(x^d - x) + D(\dot{x}^d - \dot{x}) \]

其中 \(K \in \mathbb{R}^{6\times6}\) 是刚度矩阵，\(D \in \mathbb{R}^{6\times6}\) 是阻尼矩阵，\(x^d\) 是期望位姿，\(x\) 是实际位姿。

物理直觉：把末端想象成连接在期望位置和实际位置之间的弹簧和阻尼器。弹簧提供恢复力（把末端拉向期望位置），阻尼器提供阻尼力（防止振荡）。

参数	大值效果	小值效果	典型任务
刚度 \(K\)	位置精准但碰撞硬	柔顺但位置误差大	空中运动用大值，接触操作用中小值
阻尼 \(D\)	抑制振荡但反应慢	灵活但容易振荡	按任务速度和稳定性需求调节

跨领域类比：阻抗控制类似于开车时的"油门-刹车"协调。位置控制是把方向盘锁死——精准但无法适应路况。力控制是只看速度表——能控制速度但不知道在哪。阻抗控制是"在目标车道保持适当弹性"——偏离车道时自动纠正，但遇到障碍时柔顺避让。刚度 \(K\) 相当于"纠偏的力度"，阻尼 \(D\) 相当于"纠偏的平滑度"。

从关节空间到笛卡尔空间的完整推导

Step 1：关节空间阻抗

在关节空间中，阻抗控制的力矩为：

\[ \tau = K_q(q^d - q) + D_q(\dot{q}^d - \dot{q}) + g(q) \]

其中 \(g(q)\) 是重力补偿项。这是最简单的实现，但 \(K_q\) 和 \(D_q\) 在关节空间中定义，与末端空间的物理直觉不直接对应。

Step 2：笛卡尔阻抗的推导

我们希望在笛卡尔空间中定义阻抗行为 \(f = K(x^d - x) + D(\dot{x}^d - \dot{x})\)，但电机只能输出关节力矩 \(\tau\)。需要把笛卡尔力映射到关节力矩。

末端速度和关节速度的关系（运动学）：

\[ \dot{x} = J(q)\dot{q} \]

末端力和关节力矩的关系（静力学对偶）：

\[ \tau_{task} = J(q)^T f \]

因此笛卡尔阻抗在关节空间的等价力矩为：

\[ \tau = J^T \left[ K(x^d - x) + D(\dot{x}^d - \dot{x}) \right] + g(q) \]

Step 3：加入惯量整形（完整动力学阻抗）

上面的公式只考虑了位置和速度，没有考虑加速度。完整的阻抗控制还需要整形末端的惯量特性。期望的末端动力学为：

\[ M_d(\ddot{x}^d - \ddot{x}) + D(\dot{x}^d - \dot{x}) + K(x^d - x) = f_{ext} \]

其中 \(M_d\) 是期望惯量矩阵，\(f_{ext}\) 是外部接触力。要实现这个行为，控制力矩为：

\[ \tau = M(q)J^{-1}\left[M_d^{-1}\left(D(\dot{x}^d-\dot{x})+K(x^d-x)-f_{ext}\right)-\dot{J}\dot{q}\right]+h(q,\dot{q}) \]

这个公式需要动力学模型 \(M(q)\) 和 \(h(q,\dot{q})\)，以及外力测量或估计 \(f_{ext}\)。

陷阱警告 ⚠️

概念误区：认为阻抗控制和力控制是同一件事

新手想法："阻抗控制不就是控制力吗？"

实际上：阻抗控制和力控制是互补的概念。Hogan (1985) 明确区分了两者——力控制直接调节接触力的大小，阻抗控制调节位移和力之间的动态关系。力控制需要力传感器闭环，阻抗控制可以开环实现（只需位置/速度反馈）。关键区别在于：阻抗控制在自由空间中表现为位置控制（弹簧把末端拉向目标），在接触时表现为力控制（弹簧被压缩产生可控的接触力）——一个框架统一了两种场景

正确理解：阻抗控制定义的是"因果关系"——从外部位移到接触力的映射。力控制定义的是力本身。

阻抗控制在操作空间中的分解

实际力敏感任务中，不同方向可能需要不同的阻抗行为。例如推门时：

• 法线方向（推力方向）：低刚度 + 前馈力，允许柔顺接触
• 切线方向（沿门面）：高刚度，保持手在门把手上
• 旋转方向：中等刚度，允许手随门旋转

这通过在任务坐标系（task frame）中定义对角刚度矩阵实现：

\[ K_{task} = \text{diag}(k_x, k_y, k_z, k_{rx}, k_{ry}, k_{rz}) \]

方向	推门（N/m 或 Nm/rad）	端杯子	擦桌子
\(k_x\)（前进）	200	1000	500
\(k_y\)（侧向）	1000	1000	1000
\(k_z\)（垂直）	800	500	300
\(k_{rx}\)	50	100	50
\(k_{ry}\)	50	100	50
\(k_{rz}\)	20	100	100

任务坐标系中的刚度需要变换到基座坐标系：

\[ K_{base} = R_{task}^T K_{task} R_{task} \]

其中 \(R_{task}\) 是任务坐标系到基座坐标系的旋转矩阵。

本质洞察：阻抗控制的精髓不在于"设定一组参数"，而在于"根据任务语义分配不同方向的力学行为"。一个好的阻抗设计反映了工程师对任务物理的理解——哪个方向需要精确定位（高刚度），哪个方向需要柔顺适应（低刚度），哪个方向需要主动施力（前馈）。RL 策略学习可变阻抗参数的本质，就是让策略自动学会这种"方向-刚度"的对应关系。

加入前馈力

许多任务需要主动施力（推门、拉车），不能只依赖弹簧式的被动力。加入前馈力项：

\[ f_{cmd} = K(x^d - x) + D(\dot{x}^d - \dot{x}) + f_{ff} \]

前馈力场景	\(f_{ff}\) 设置	效果
推门	法向 10-40 N	持续施加推力
拉车	水平 40-100 N	持续施加拉力
按压	垂直 5-20 N	保持接触压力
端杯子	垂直补偿重力	抵消杯子重量

---

95.3 无源性与稳定性：被动性证明的完整推导链 ⭐⭐⭐⭐

为什么需要无源性证明

阻抗控制器在参数选择不当时可能不稳定——输出的能量超过了输入的能量，导致系统发散。无源性（passivity）提供了一种与模型无关的稳定性保证：如果系统是无源的，它不会自己产生能量，因此在与任何无源环境交互时都是稳定的。

跨领域类比：无源性类似于热力学第二定律——"你不能造出永动机"。无源系统不会凭空产生能量，就像真实物理系统不会凭空产生热量。如果控制器违反了无源性，它就像一个"能量源"，可能向环境注入越来越多的能量，导致振荡甚至发散。

无源性的数学定义

一个系统从端口 \((f, \dot{x})\) 的角度是无源的，当且仅当存在一个非负的存储函数 \(V(x) \geq 0\)，使得对所有时间 \(t\)：

\[ V(x(t)) - V(x(0)) \leq \int_0^t f(\tau)^T \dot{x}(\tau) d\tau \]

左边是系统内部存储能量的变化，右边是外部通过端口注入的能量。无源性说的是：系统内部能量的增加不能超过外部注入的能量——系统只能耗散能量，不能产生能量。

弹簧-阻尼阻抗的无源性证明

Step 1：定义存储函数

对于弹簧-阻尼阻抗 \(f = K(x^d - x) + D(\dot{x}^d - \dot{x})\)，选择弹性势能作为存储函数：

\[ V(x) = \frac{1}{2}(x^d - x)^T K (x^d - x) \]

Step 2：计算能量变化率

\[ \dot{V} = -(x^d - x)^T K \dot{x} = -(x^d - x)^T K \dot{x} \]

这里假设 \(\dot{x}^d = 0\)（期望位置固定），简化推导。

Step 3：计算外部功率

外部对系统注入的功率为：

\[ P_{ext} = f^T \dot{x} = [K(x^d - x) + D(0 - \dot{x})]^T \dot{x} \]

\[ = (x^d - x)^T K \dot{x} - \dot{x}^T D \dot{x} \]

Step 4：验证无源性条件

\[ \dot{V} = -(x^d - x)^T K \dot{x} = -P_{ext} - \dot{x}^T D \dot{x} + P_{ext} - (x^d - x)^T K \dot{x} \]

整理得：

\[ \dot{V} - P_{ext} = -\dot{x}^T D \dot{x} \leq 0 \quad (\text{当 } D \succeq 0) \]

即：

\[ \dot{V} \leq P_{ext} \]

存储能量的变化率不超过外部注入功率。积分得到无源性条件：

\[ V(t) - V(0) \leq \int_0^t f(\tau)^T \dot{x}(\tau) d\tau \]

结论：当 \(K \succeq 0\)（正半定刚度）且 \(D \succeq 0\)（正半定阻尼）时，弹簧-阻尼阻抗控制器是无源的。阻尼项 \(D\) 耗散能量（\(\dot{x}^T D \dot{x} \geq 0\)），保证系统严格无源。

反事实推理：如果 \(D\) 有负特征值会怎样？负阻尼意味着速度越大，系统注入的能量越多——正反馈。小扰动会被放大成大振荡。这在实际工程中表现为末端"自激振荡"——即使没有外部激励，机器人也会开始抖动。

从无源性到 \(\mathcal{L}_2\) 增益：量化鲁棒性

无源性是一种定性保证——"系统不产生能量"。但工程中我们还需要定量保证——"系统对扰动的放大倍数是多少"。这就引出了 \(\mathcal{L}_2\) 增益的概念。

一个系统从输入 \(u\) 到输出 \(y\) 的 \(\mathcal{L}_2\) 增益定义为：

\[ \gamma = \sup_{u \neq 0} \frac{\|y\|_{\mathcal{L}_2}}{\|u\|_{\mathcal{L}_2}} = \sup_{u \neq 0} \frac{\sqrt{\int_0^\infty \|y(t)\|^2 dt}}{\sqrt{\int_0^\infty \|u(t)\|^2 dt}} \]

对于阻抗控制器，输入是外部扰动力 \(f_{dist}\)，输出是末端位移误差 \(e = x^d - x\)。\(\mathcal{L}_2\) 增益越小，系统对扰动的抑制能力越强。

对于弹簧-阻尼阻抗，\(\mathcal{L}_2\) 增益可以解析计算：

\[ \gamma = \frac{1}{\min_\omega |j\omega D + K|} = \frac{1}{\sqrt{K^2 + \omega_{worst}^2 D^2}} \]

其中 \(\omega_{worst}\) 是增益最大的频率。增大 \(K\) 或 \(D\) 都能减小 \(\gamma\)（提高鲁棒性），但代价是柔顺性下降。

跨领域类比：\(\mathcal{L}_2\) 增益类似于电路中的阻抗匹配。电路设计中，信号源和负载的阻抗匹配可以最大化功率传输。阻抗控制中，\(\mathcal{L}_2\) 增益告诉我们"最坏情况下扰动力能引起多大的位置偏差"。两者的相似之处在于都是频域分析；不同之处在于电路追求最大传输，阻抗控制追求最小传输（最大抑制）。

变阻抗控制的无源性问题

在力敏感任务中，刚度和阻尼可能随任务状态变化（接近物体时降低刚度以柔顺接触）。如果 \(K(t)\) 和 \(D(t)\) 是时变的，上面的无源性证明不再直接成立——因为 \(V(t)\) 的变化还包含 \(\dot{K}\) 项。

能量储罐（Energy Tank）方法

现代做法是引入一个虚拟的"能量储罐"\(T\)，初始化为一定量的能量 \(T_0\)。阻尼耗散的能量"充入"储罐，变阻抗需要的额外能量从储罐"取出"：

\[ \dot{T} = \dot{x}^T D \dot{x} - P_{param} \]

其中 \(P_{param}\) 是参数变化引起的额外功率。只要 \(T \geq 0\)（储罐不为空），系统保持无源。当储罐为空时，冻结参数变化，防止能量注入。

本质洞察：能量储罐方法不是一个数学技巧，而是一种工程设计原则——它把"稳定性预算"显式化了。每次改变刚度/阻尼都会"花费"一些稳定性预算，阻尼运动会"赚回"预算。当预算用完时，系统自动变得保守。这与 94 章 ASAP 中的安全裕度思想是一致的。

---

95.4 RL 动作空间与力控接口选择 ⭐⭐⭐

动机：动作空间决定了策略能学会什么

RL 策略的动作空间是它能表达的控制范围。选错动作空间，即使奖励设计完美、训练充分，策略也可能无法实现任务。

对于力敏感 loco-manipulation，有五种常见的动作空间选择。它们在力控能力、训练难度和部署安全性之间有不同的权衡：

动作接口	输出 \(a_t\)	力控能力	训练难度	部署安全性	典型使用者
关节目标位置	\(q^d\)	间接（通过低层 PD）	低	高	FALCON, SoFTA
关节位置残差	\(q^{ref}+\delta q\)	间接	低	高	ExBody, HOVER
末端位姿残差	\(\delta x_{ee}\)	中等	中	中	Mobile-TeleVision
可变阻抗参数	\(K, D\)	高（显式）	高	低（需约束）	学术研究
关节力矩	\(\tau\)	最高	最高	最低	很少用于真机

为什么 FALCON 和 SoFTA 都选择关节目标位置？

关节目标位置 \(q^d\) 是最稳定的选择。低层 PD 控制器 \(\tau = K_p(q^d - q) + K_d(0 - \dot{q})\) 天然提供阻尼（\(K_d\) 项），即使策略输出不合理的 \(q^d\)，PD 控制器也不会产生无穷大的力矩。

\[ a_t = q^{default} + s_{action} \cdot \tanh(\text{network\_output}_t) \]

其中 \(q^{default}\) 是默认站立姿态，\(s_{action}\) 是动作缩放因子。\(\tanh\) 确保策略输出有界。

反事实推理：如果用关节力矩作为动作空间会怎样？策略直接输出每个关节的力矩值。这在理论上表达力最强——可以直接控制任意力矩分布。但在实践中：(1) 训练初期策略输出随机力矩，机器人会立即失控摔倒，样本效率极低；(2) 没有低层 PD 的阻尼保护，高频抖动直接传到电机；(3) 真机的力矩量程和仿真不一致，sim-to-real gap 更大。FALCON 论文实验证实，力矩空间策略的训练收敛速度比关节位置空间慢 3-5 倍。

可变阻抗参数作为动作空间

学术上有一条研究路线是让策略输出阻抗参数 \(K_t, D_t\)，使机器人在不同接触阶段自动调节刚度和阻尼：

\[ a_t = [q_t^d, K_t, D_t] \]

优点是策略可以学会"接近物体时降低刚度"。缺点是 (1) \(K, D\) 的值域需要严格约束以保证无源性（见95.3节），(2) 动作空间维度翻倍以上导致训练困难，(3) 初学者不好设计奖励来引导合理的阻抗变化。

import torch


def joint_position_action(default_pose: torch.Tensor,
                          policy_output: torch.Tensor,
                          action_scale: float = 0.25) -> torch.Tensor:
    """中文注释：关节目标位置接口，tanh 限幅确保输出有界。"""
    return default_pose + action_scale * torch.tanh(policy_output)


def impedance_wrench(x: torch.Tensor, dx: torch.Tensor,
                     x_des: torch.Tensor, dx_des: torch.Tensor,
                     stiffness: torch.Tensor, damping: torch.Tensor,
                     force_ff: torch.Tensor) -> torch.Tensor:
    """中文注释：末端阻抗控制的 6D wrench 计算。"""
    return stiffness * (x_des - x) + damping * (dx_des - dx) + force_ff

Mobile-TeleVision 路线：IK + RL 的过渡方案

Mobile-TeleVision 一类系统把问题拆开：上体用 IK 精确跟随遥操作命令，下体用 RL 保持行走。这种架构比端到端全身策略更容易调试。

CVAE 运动先验

\[ z \sim q_\psi(z|motion), \qquad motion \sim p_\theta(motion|z, command) \]

CVAE 把上体动作压缩成 latent \(z\)，作为下体策略的条件。下体不需要看到完整的未来动作，只需要知道"上体接下来大致要怎样动"。

模块	输入	输出	优点	限制
上体 IK	手部目标、躯干目标	上体关节目标	可解释、精确	不懂动力学
运动先验	上体动作历史	latent 条件	压缩扰动信息	表示容量是否足够需验证
下体 RL	本体 + latent + 速度命令	腿部动作	鲁棒行走	上体外力仍可能未知

这种架构的局限在于上体 IK 不考虑动力学——它不知道伸手动作会对躯干产生多大的反作用力矩。FALCON 的端到端双策略方案通过让下体也看到末端目标，隐式学会了对上体动作的预补偿，克服了这个局限。

---

95.5 FALCON 双策略架构深度分析 ⭐⭐⭐⭐

为什么拆成上体和下体

单一网络同时优化末端精度和步态稳定，容易出现梯度冲突。上体想快速追手，可能要求大幅肩腰运动。下体想保持稳定，可能希望躯干缓慢变化。这两个目标在同一个梯度更新中互相竞争——改善上体跟踪的参数变化可能恶化下体稳定性，反之亦然。

FALCON（Force-Adaptive Loco-Manipulation, Zhang et al. 2025）的解决方案是双策略设计：

\[ a_t^u=\pi_u(o_t),\qquad a_t^l=\pi_l(o_t), \qquad a_t=[a_t^u, a_t^l] \]

上体策略 \(\pi_u\) 和下体策略 \(\pi_l\) 有独立的网络参数和独立的奖励，但共享同一个观测向量 \(o_t\)。

跨领域类比：双策略设计类似于公司的"产品部门"和"运维部门"。产品部门（上体）追求新功能和用户体验（末端精度），运维部门（下体）保证系统稳定运行（步态稳定）。两个部门共享公司的基础设施信息（观测），但有独立的 KPI（奖励），由各自的团队独立优化。如果只有一个部门同时负责功能和稳定性，功能开发和稳定性保障会互相制约。

奖励设计的完整分解

\[ r^u = r_{ee\text{-}track} + r_{upper\text{-}posture} + r_{smooth}^u \]

\[ r^l = r_{vel\text{-}track} + r_{balance} + r_{gait} + r_{smooth}^l \]

上体奖励项详解

奖励项	数学表达	权重范围	物理意义
末端位置跟踪	\(\exp(-\|p_{ee} - p_{ee}^d\|^2 / \sigma_p^2)\)	0.4-0.6	手到目标位置的距离
末端姿态跟踪	\(\exp(-\|e_{rot}\|^2 / \sigma_r^2)\)	0.1-0.2	手的朝向误差
上体姿态正则	\(-\|q_{upper} - q_{upper}^{default}\|^2\)	0.05-0.1	防止上体畸形姿态
上体动作平滑	\(-\|a_t^u - a_{t-1}^u\|^2\)	0.05-0.1	减少高频抖动

下体奖励项详解

奖励项	数学表达	权重范围	物理意义
速度跟踪	\(\exp(-\|v_{base} - v_{cmd}\|^2 / \sigma_v^2)\)	0.3-0.5	行走速度跟踪
平衡正则	\(-\|roll\|^2 - \|pitch\|^2\)	0.1-0.2	躯干姿态稳定
步态奖励	接触时序匹配	0.1-0.2	期望的左右交替着地
能耗正则	\(-\|\tau \cdot \dot{q}\|\)	0.02-0.05	减少关节功耗
下体平滑	\(-\|a_t^l - a_{t-1}^l\|^2\)	0.05-0.1	减少腿部抖动

共享观测的接口设计

观测	上体需要	下体需要	原因
IMU 和基座角速度	需要	需要	外力会影响全身姿态
所有关节角和角速度	需要	需要	全身状态耦合
末端目标位姿	需要	需要	下体要预补偿上体动作带来的扰动
速度命令	可选	需要	步态目标
外力课程标签	需要	需要	学习力补偿
上一步动作 \(a_{t-1}\)	需要	需要	动作平滑

陷阱警告 ⚠️

概念误区：认为双策略就是让上下体互相不知道

新手想法："既然拆开了，上体和下体就各管各的吧"

实际上：完全隔离会让上体外力成为下体的未知扰动。下体策略如果不知道上体要把手伸到哪里、要承受多大的力，就无法提前调整步态

正确做法：上下体共享全部本体观测和命令信息，只在动作输出头和奖励上解耦。FALCON 让下体也看到末端目标，这样下体可以在上体伸手前就调整重心

协调损失

\[ L = L_{PPO}(\pi_u, r^u) + L_{PPO}(\pi_l, r^l) + \lambda L_{coord} \]

\(L_{coord}\) 可以约束：

1. 腰部一致性：上下体策略在腰部关节的输出应该一致
2. 全身能耗：上下体动作的总能耗不超过硬件限制
3. 动作平滑：上下体动作的组合在时间上是平滑的

FALCON 的跨平台泛化

FALCON 的一个重要实验结果是：相同的训练配置（不修改奖励或力课程）可以直接部署到不同的人形机器人上。论文在 Unitree G1 和 Booster T1 上都成功部署了策略，说明双策略架构对机器人构型不敏感。

机器人	自由度	部署任务	力范围
Unitree G1	23	负载运输、拉车、开门	0-100 N
Booster T1	29	负载运输、推门	0-60 N

FALCON 报告相比单策略基线，上体关节跟踪精度提高 2 倍，同时保持鲁棒行走。

梯度冲突的定量分析

为什么双策略能解决梯度冲突？这里给出定量分析。

在单策略架构中，假设策略参数为 \(\theta\)，总损失为 \(L = L^u(\theta) + L^l(\theta)\)。梯度为：

\[ \nabla_\theta L = \nabla_\theta L^u + \nabla_\theta L^l \]

当两个梯度方向接近相反（余弦相似度为负）时，合梯度的模长远小于各分量——参数更新变得微弱且方向不确定。

\[ \cos(\nabla_\theta L^u, \nabla_\theta L^l) = \frac{\nabla_\theta L^u \cdot \nabla_\theta L^l}{\|\nabla_\theta L^u\| \|\nabla_\theta L^l\|} \]

FALCON 论文的消融实验测量了单策略训练中上体梯度和下体梯度的余弦相似度，发现在训练中期（50-150M 步）平均值约为 \(-0.3\)，在力课程进入高级阶段后进一步恶化到 \(-0.5\)。

双策略通过使用独立参数 \(\theta_u\) 和 \(\theta_l\) 彻底消除了参数空间中的梯度冲突：

\[ \nabla_{\theta_u} L^u \quad \text{和} \quad \nabla_{\theta_l} L^l \quad \text{作用在不同参数上，互不干扰} \]

反事实推理：如果我们不拆策略，而是用多任务学习中的梯度手术（Gradient Surgery, Yu et al. 2020）来处理冲突会怎样？梯度手术通过投影去除冲突分量：\(g_u' = g_u - \frac{g_u \cdot g_l}{g_l \cdot g_l} g_l\)。这在理论上可行，但实践中有两个问题：(1) PPO 的梯度估计本身有方差，投影会进一步放大方差；(2) 投影后的梯度不再是任何一个目标的无偏估计，可能导致策略在两个目标之间振荡。FALCON 的双策略方案更简单也更稳定。

双策略的网络架构选择

架构选项	优点	缺点	FALCON 选择
完全独立 MLP	实现简单、无耦合	无法共享特征表示	否
共享 backbone + 独立 head	可共享低级特征	backbone 仍有梯度冲突	否
独立 encoder + 独立 head	各自学习特征、动作解耦	参数量翻倍	是
交叉注意力	可以互相 attend	实现复杂	否（未来方向）

FALCON 选择"独立 encoder + 独立 head"——上体和下体各有自己的 MLP encoder（3 层，256 units），输出各自的动作。两个 encoder 接收相同的观测向量，但学习不同的特征表示。

import torch
import torch.nn as nn


class DualPolicyNetwork(nn.Module):
    """中文注释：FALCON 风格的双策略网络，上下体独立 encoder。"""

    def __init__(self, obs_dim: int, upper_act_dim: int,
                 lower_act_dim: int, hidden: int = 256):
        super().__init__()
        # 中文注释：上体独立 encoder + 动作头
        self.upper_encoder = nn.Sequential(
            nn.Linear(obs_dim, hidden), nn.ELU(),
            nn.Linear(hidden, hidden), nn.ELU(),
        )
        self.upper_head = nn.Linear(hidden, upper_act_dim)

        # 中文注释：下体独立 encoder + 动作头
        self.lower_encoder = nn.Sequential(
            nn.Linear(obs_dim, hidden), nn.ELU(),
            nn.Linear(hidden, hidden), nn.ELU(),
        )
        self.lower_head = nn.Linear(hidden, lower_act_dim)

    def forward(self, obs: torch.Tensor):
        # 中文注释：上下体共享观测但使用独立的特征提取
        upper_feat = self.upper_encoder(obs)
        lower_feat = self.lower_encoder(obs)
        upper_action = self.upper_head(upper_feat)
        lower_action = self.lower_head(lower_feat)
        return upper_action, lower_action

FALCON 的训练工程细节

训练阶段划分

FALCON 的训练不是一步到位，而是分阶段推进：

训练阶段	内容	目标	步数
Phase 0	仅下体训练，无外力	下体学会稳定行走	50M
Phase 1	加入上体，小范围 EE 目标	上下体初步协调	50M
Phase 2	加入力课程（0-40N）	学会在外力下保持稳定	100M
Phase 3	力课程增大（40-100N）+ 多样化任务	力自适应泛化	100M

为什么不从头同时训练上下体？ 如果上下体从头开始同时训练，初期两个策略都输出随机动作，机器人立即摔倒，获得的有效样本极少。先让下体学会走路（Phase 0），再加入上体（Phase 1），让上体在"下体已经能走"的基础上学习末端控制，收敛更快。

反事实推理：如果跳过 Phase 0 直接同时训练会怎样？FALCON 论文的消融实验表明，从头同时训练的策略在 300M 步后仍然无法达到分阶段训练 200M 步的性能。原因是下体策略在早期缺乏稳定性基础，上体的随机动作不断把机器人推倒，有效的末端跟踪样本极少。

---

95.6 SoFTA 软化接触：力课程学习的数学形式化 ⭐⭐⭐⭐

3D 力课程设计

直接给随机策略施加 100 N 外力，训练早期会大量摔倒，样本效率极低。力课程从小力开始，逐渐增大方向、幅值和持续时间。

课程的数学形式化

设训练步数为 \(n\)，最大训练步数为 \(N\)，课程进度 \(\rho(n) = \min(1, n/N)\)。外力幅值的课程为：

\[ F_{max}(\rho) = F_{init} + (F_{target} - F_{init}) \cdot \rho^\alpha \]

其中 \(\alpha \geq 1\) 控制课程的非线性度。\(\alpha = 1\) 是线性课程，\(\alpha > 1\) 让早期增长更慢、后期更快。

阶段	力幅值	方向	任务	\(\rho\) 范围
初级	0-10 N	固定水平	站立和慢走	0-0.2
中级	10-40 N	随机水平	端物和推门	0.2-0.5
高级	40-100 N	3D 随机	拉车和强扰动	0.5-0.8
动态	正弦或脉冲	时变	冲击和滑移恢复	0.8-1.0

FALCON 的力矩限感知课程

FALCON 的一个关键创新是力课程不仅考虑外力大小，还考虑关节力矩是否可行。在增加外力之前，先用 RNEA 检查当前姿态下所需的关节力矩是否在硬件限制内：

\[ \tau_{required} = S\left(M\dot{v}+h-J_c^T\lambda-J_{ee}^T f_{ext}\right) \]

\[ \text{Feasible} \Leftrightarrow \|\tau_{required}\|_\infty \leq \tau_{limit} \cdot \text{margin} \]

其中 margin 通常设为 0.85（留 15% 安全裕度）。

import numpy as np


def feasible_torque(tau: np.ndarray, tau_limit: np.ndarray, margin: float = 0.85) -> bool:
    """中文注释：检查力矩是否留有安全裕度。"""
    return bool(np.all(np.abs(tau) <= margin * tau_limit))


def force_curriculum(step: int, max_step: int, max_force: float,
                     init_force: float = 0.0, alpha: float = 1.5) -> float:
    """中文注释：非线性力课程，alpha>1时早期增长更慢。"""
    rho = min(1.0, step / max_step)
    return init_force + (max_force - init_force) * (rho ** alpha)


def sample_3d_force(max_force: float) -> np.ndarray:
    """中文注释：在球体内均匀采样 3D 力向量。"""
    direction = np.random.randn(3)
    direction /= np.linalg.norm(direction) + 1e-8
    magnitude = np.random.uniform(0, max_force)
    return direction * magnitude

课程速度与训练效率的定量关系

力课程的速度参数 \(\alpha\) 直接影响训练效率。这里给出一个分析框架来指导参数选择。

定义"有效样本比率" \(\eta(\rho)\) 为当前力级别下机器人没有摔倒的 episode 比例。力课程的设计目标是在整个训练过程中保持 \(\eta(\rho) > \eta_{min}\)（推荐 \(\eta_{min} = 0.6\)）。

\[ \eta(\rho) \approx \eta_0 \cdot \exp\left(-\frac{F_{max}(\rho) - F_{comfortable}}{F_{scale}}\right) \]

其中 \(F_{comfortable}\) 是策略当前能稳定承受的力，\(F_{scale}\) 是衰减常数。如果课程推进太快（\(F_{max}(\rho)\) 远超 \(F_{comfortable}\)），\(\eta\) 急剧下降，训练进入"大量摔倒→几乎无有效梯度"的死循环。

自适应课程的实现

FALCON 的一个改进方案是自适应课程——根据当前训练成功率动态调整推进速度：

\[ \rho_{t+1} = \rho_t + \beta \cdot (\eta_t - \eta_{target}) \]

当 \(\eta_t > \eta_{target}\) 时课程加速推进，当 \(\eta_t < \eta_{target}\) 时课程减速甚至回退。这种闭环课程设计不需要手动选择 \(\alpha\)，但引入了新的超参数 \(\beta\) 和 \(\eta_{target}\)。

import numpy as np


def adaptive_curriculum(current_rho: float, success_rate: float,
                        target_rate: float = 0.7,
                        step_size: float = 0.01) -> float:
    """中文注释：根据当前成功率自适应调整力课程进度。"""
    delta = step_size * (success_rate - target_rate)
    new_rho = np.clip(current_rho + delta, 0.0, 1.0)
    return new_rho

力课程的观测问题

如果真机没有腕部力传感器，策略不能依赖精确外力值。训练中可以给 critic（或 teacher 策略）使用外力真值，但部署的 actor 应主要依赖本体历史推断。

这与 Teacher-Student 的信息不对称问题类似。回顾93章：teacher 使用 privileged observation（包括外力真值、地形参数等），student 只使用部署可用的观测（关节角、IMU 等），通过行为克隆将 teacher 的知识蒸馏到 student。

力估计的隐式学习

在 FALCON 的训练中，actor 网络从历史本体状态中隐式学习外力估计。这种隐式估计的原理是：外力会改变关节角速度和基座姿态的变化模式。如果策略网络接收最近 \(H\) 步的本体历史 \(o_{t-H:t}\)，它可以通过状态变化的"异常模式"推断外力的大致方向和幅值。

\[ \hat{f}_{ext} \approx g_{implicit}(o_{t-H:t}) \quad \text{（网络隐式学习，无显式监督）} \]

这种隐式方法的优点是不需要力传感器，缺点是力估计精度低于显式传感器，尤其对静态力（如持续端重物）的估计不如动态力（如推门冲击）准确。

陷阱警告 ⚠️

编程陷阱：力课程中只随机化力的大小不随机化力的方向

错误做法：force = [random_magnitude, 0, 0]（只有水平 x 方向）

现象：策略只学会了在 x 方向受力时的补偿，从侧面或上方受力时立即失稳

根本原因：力的方向和作用点都应该在训练中充分随机化，否则策略只在训练分布内鲁棒

正确做法：在单位球面上均匀采样力的方向，在合理范围内随机化力的作用点

---

变阻抗控制在 RL 中的实现

将无源性约束集成到 RL 训练中，需要在策略输出上施加约束。一种实用方法是：

1. 策略输出的 \(K_t\) 通过 softplus 激活保证正半定
2. 策略输出的 \(D_t\) 通过 \(D_t = 2\zeta_t\sqrt{K_t \cdot m_{eff}}\) 参数化，\(\zeta_t\) 通过 sigmoid 限制在 \([0.5, 1.5]\)
3. 在奖励中加入能量储罐监控项——当储罐接近空时惩罚参数变化

\[ r_{passivity} = -\max(0, -T_t) \cdot w_{tank} \]

其中 \(T_t\) 是能量储罐的当前值。\(T_t < 0\) 意味着系统违反了无源性，应该被惩罚。

这种方法在训练中软性保证无源性，在部署中可以用硬约束保证（储罐为空时冻结参数）。

---

95.7 触觉传感集成：力/力矩传感器到控制闭环 ⭐⭐⭐

力传感器在控制链中的位置

力敏感 loco-manipulation 中，力传感器提供关键的闭环信号。根据安装位置和类型，可以分为：

传感器类型	安装位置	测量量	精度	延迟
六维力/力矩传感器	腕部	末端 6D wrench	高 (0.1 N)	低 (1 ms)
足底压力传感器	脚底	法向接触力分布	中 (1 N)	低 (1 ms)
关节力矩传感器	关节	单轴力矩	高 (0.01 Nm)	低
触觉皮肤	手指/手掌	多点压力分布	中	中 (5 ms)
电流传感器	电机驱动器	力矩估计	低	低

力传感器在 RL 训练中的使用策略

在 RL 训练中，仿真器提供精确的接触力信息。但直接把仿真中的力传感器读数作为策略观测有一个问题：部署时真机的力传感器噪声、零偏和标定误差会与训练分布不同。

推荐的做法是分层处理：

信息	训练中给 critic	训练中给 actor	部署中给 actor
精确外力 \(f_{ext}\)	是	否（使用 DR 噪声版本）	否（使用传感器读数或估计值）
接触力 \(\lambda\)	是	可选	足底传感器
足底力分布	是	可选（加噪声）	足底压力阵列
关节力矩 \(\tau\)	是	可选（加噪声）	电流估计

这种 "asymmetric information" 设计让 critic 使用完整信息做更准确的价值估计，而 actor 只使用部署可用的观测，避免了信息差导致的 sim-to-real gap。

信号流：从原始测量到控制动作

传感器原始信号 → 滤波/标定 → 坐标变换 → 观测向量 → 策略网络 → 动作输出
     ↑                                                      |
     └──────────── 物理执行 ←── 电机驱动 ←── 安全限幅 ←──┘

每个环节的关键处理

环节	处理内容	注意事项
滤波	低通滤波去除高频噪声	截止频率不能太低，否则延迟增加
标定	零偏补偿、增益校正	温度漂移需要定期重标定
坐标变换	传感器坐标系 → 身体坐标系	安装角度误差会导致系统偏差
归一化	力值除以额定量程	保持与其他观测量的数量级一致

力传感器的域随机化策略

为了缩小力传感器观测的 sim-to-real gap，训练中应对力传感器信号施加如下域随机化：

随机化类型	参数范围	物理对应
加性零偏	\(\pm 2\) N	传感器安装后的静态偏移
乘性增益	0.9-1.1	传感器标定误差
高斯噪声	\(\sigma = 0.5\) N	电子噪声
低频漂移	正弦，幅值 1 N，周期 10-60 s	温度漂移
量化噪声	分辨率 0.1 N	ADC 量化
随机丢包	概率 1-5%	通信丢失

import numpy as np


def randomize_force_sensor(force_true: np.ndarray,
                           bias_range: float = 2.0,
                           gain_range: tuple = (0.9, 1.1),
                           noise_std: float = 0.5,
                           dropout_prob: float = 0.02) -> np.ndarray:
    """中文注释：力传感器域随机化，模拟真机传感器的非理想特性。"""
    # 中文注释：静态零偏
    bias = np.random.uniform(-bias_range, bias_range, size=force_true.shape)
    # 中文注释：增益误差
    gain = np.random.uniform(gain_range[0], gain_range[1], size=force_true.shape)
    # 中文注释：高斯噪声
    noise = np.random.randn(*force_true.shape) * noise_std
    # 中文注释：随机丢包（丢包时输出零或上一次值）
    mask = np.random.rand(*force_true.shape) > dropout_prob

    force_noisy = gain * force_true + bias + noise
    force_noisy = force_noisy * mask  # 丢包时置零
    return force_noisy

反事实推理：如果不做力传感器的域随机化会怎样？训练中策略看到的是理想的力值——精确、无噪声、无延迟。部署时传感器的零偏可能导致策略持续输出一个方向的补偿力，因为它把零偏当成了真实外力。这种系统性偏差在长时间运行中会积累，导致机器人逐渐偏离目标姿态。

无力传感器的力估计

很多人形机器人没有腕部力传感器（成本、重量、可靠性限制）。此时需要从本体感知中隐式估计外力。

方法一：基于动力学模型的力估计

\[ \hat{f}_{ext} = (J_{ee}^T)^{\dagger} \left(\tau_{measured} - M(q)\dot{v} - h(q, \dot{q}) - J_c^T\hat{\lambda}\right) \]

需要关节力矩传感器或电流估计，以及准确的动力学模型。

方法二：基于历史观测的神经网络估计

\[ \hat{f}_{ext} = f_{est}(o_{t-K:t}, a_{t-K:t-1}) \]

用 Teacher-Student 框架训练：teacher 使用真实外力标签，student 从本体历史估计。

陷阱警告 ⚠️

编程陷阱：直接把原始力传感器读数送入策略网络

错误做法：obs['force'] = raw_force_sensor_reading

现象：策略在训练中表现好，部署时因为传感器零偏漂移而产生持续偏置动作

根本原因：力传感器的零偏会随温度、安装状态和时间漂移，训练中的零偏和部署时的零偏不同

正确做法：每次启动前做零偏标定，运行中用高通滤波去除慢漂移，或使用差分力（相对于初始值的变化）而非绝对力

---

95.8 SoFTA：时间解耦与异步控制 ⭐⭐⭐

为什么上体要更高频

SoFTA（Slow-Fast Two-Agent）的论文标题是 "Hold My Beer"——端啤酒不洒。这个任务对末端加速度极其敏感：垂直加速度超过 ~3 m/s² 就会让杯中液体晃荡。

下体步态通常 50 Hz 足够，但上体末端稳定可能需要 100 Hz 或更高。因为行走的步态频率约 1-2 Hz，50 Hz 的采样率给出了 25-50 倍的奈奎斯特裕度，足以平滑控制。但末端振动的频率可能达到 5-20 Hz（机械共振、地面冲击传导），需要 100 Hz 以上的采样和控制频率才能有效抑制。

子系统	控制频率	目标	风险
上体	100 Hz	末端稳定和精细操作	与下体动作不同步
下体	50 Hz	鲁棒步态和速度跟踪	反应上体扰动较慢
仿真物理	200 Hz+	接触积分精度	decimation 设置错误会引入 artifact

异步控制的数学视角

设下体每两步更新一次动作，上体每步更新一次。用 decimation 因子 \(k\) 表示：

\[ a_t^u=\pi_u(o_t), \qquad a_t^l=\pi_l(o_{k\lfloor t/k\rfloor}) \]

其中 \(k=2\) 表示下体每两个上体周期更新一次。下体动作在两个上体周期之间保持不变（action hold）。

训练中必须模拟异步

\[ a_t^l = \begin{cases} \pi_l(o_t) & \text{if } t \mod k = 0 \\ a_{t-1}^l & \text{otherwise} \end{cases} \]

如果训练时上下体同频更新，部署时才引入异步，策略会因为下体动作的"冻结"而产生非预期行为。

反事实推理：如果训练时不模拟频率差异会怎样？假设训练中上下体都是 100 Hz。部署时下体降到 50 Hz，每两步只更新一次。策略在训练中学到的是"每步都有新的下体动作"——它可能依赖下体的高频微调来保持稳定。当下体每两步才更新一次时，中间那步的下体动作是"旧的"，策略没有见过这种情况，稳定性会退化。

末端加速度奖励

SoFTA 的核心奖励项是末端加速度惩罚。用有限差分近似加速度：

\[ r_{ee\text{-}acc} = -\left\|\frac{x_{ee,t} - 2x_{ee,t-1} + x_{ee,t-2}}{\Delta t^2}\right\|^2 \]

特别关注垂直方向（z 轴）的加速度，因为液体晃荡主要由垂直加速度引起。

SoFTA 的实验结果

SoFTA 报告在端杯子任务上，相比单策略单频基线：

指标	单策略 50Hz	双策略 50Hz	SoFTA (100/50 Hz)
EE 垂直加速度 RMS	2.8 m/s²	2.1 m/s²	1.3 m/s²
EE 水平位置误差	4.2 cm	3.1 cm	2.8 cm
行走速度跟踪	0.92	0.95	0.94

上体高频率将垂直加速度降低了 54%，这是端杯子不洒的关键。

SoFTA 的部署工程：双线程控制架构

在部署中，上体和下体策略以不同频率运行，需要仔细的线程和通信设计：

┌─────────────────────────────────────────────────┐
│                 部署控制架构                       │
├─────────────────────────────────────────────────┤
│                                                  │
│  [传感器线程] 200 Hz                              │
│  IMU + 编码器 → 观测缓冲区                        │
│       ↓                                          │
│  ┌──────────────┐  ┌──────────────┐              │
│  │ 上体线程 100Hz│  │ 下体线程 50Hz│              │
│  │ 读观测→推理→  │  │ 读观测→推理→ │              │
│  │ 写上体动作    │  │ 写下体动作   │              │
│  └──────┬───────┘  └──────┬───────┘              │
│         ↓                  ↓                      │
│  ┌──────────────────────────────┐                │
│  │     动作合并 + 安全检查 100Hz │                │
│  │  上体动作 + 下体动作(hold) → │                │
│  │  安全限幅 → 电机指令          │                │
│  └──────────────────────────────┘                │
│                                                  │
└─────────────────────────────────────────────────┘

关键实现细节

问题	解决方案	注意事项
观测同步	无锁环形缓冲区	上体和下体读取相同时刻的观测
下体 action hold	下体线程不更新时使用上一次输出	必须与训练中的 hold 一致
推理延迟	策略网络在 GPU 上推理 < 1 ms	CPU 推理可能需要 5-10 ms
动作合并	上体关节取上体输出，下体关节取下体输出	腰部关节归属需明确
安全限幅	100 Hz 检查所有限制	频率不能低于最高控制频率

腰部关节归属问题

人形机器人的腰部（yaw/pitch/roll）关节连接上体和下体。需要决定腰部归上体还是下体控制：

归属方案	优点	缺点
归上体	上体可以扭腰辅助手部操作	扭腰可能影响行走稳定
归下体	下体统一控制重心	上体操作范围受限
加权共享	最灵活	需要协调损失防止冲突

FALCON 的默认做法是把腰部归下体控制——因为腰部对 ZMP 的影响大于对手部操作的贡献。SoFTA 让两者都可以输出腰部动作，但通过加权合并——低频（下体）决定大趋势，高频（上体）做微调。

末端加速度的正确测量方法

验证 SoFTA 效果需要正确测量末端加速度。二阶有限差分 \(\ddot{x} \approx (x_t - 2x_{t-1} + x_{t-2})/\Delta t^2\) 会放大高频噪声——差分是高通滤波器。在 100 Hz 采样下，噪声被放大 \(1/\Delta t^2 = 10000\) 倍。

正确做法：先对位置信号做低通滤波（截止频率 20-30 Hz），再做差分。或者用频域方法——对位置做 FFT，乘以 \((j\omega)^2\)。

import numpy as np
from scipy.signal import butter, filtfilt


def compute_ee_acceleration(positions: np.ndarray, dt: float,
                            cutoff_hz: float = 25.0,
                            sample_hz: float = 100.0) -> np.ndarray:
    """中文注释：先低通滤波再差分，避免噪声放大。"""
    nyquist = sample_hz / 2.0
    b, a = butter(2, cutoff_hz / nyquist, btype='low')
    # 中文注释：零相位滤波，避免相位延迟
    pos_filtered = filtfilt(b, a, positions, axis=0)
    # 中文注释：二阶有限差分
    acc = np.diff(pos_filtered, n=2, axis=0) / (dt ** 2)
    return acc

---

95.9 力敏感 Loco-Manipulation 的奖励设计陷阱 ⭐⭐⭐

力控任务特有的 reward hacking

力敏感任务的奖励设计比普通行走更容易出现 reward hacking，因为策略有更多"作弊"的方式。

陷阱	策略的"作弊"行为	原因	修复
假接触	策略让手快速碰触目标后立即弹开	位置跟踪奖励只看瞬时位置	要求持续接触一段时间
身体借力	用身体其他部位碰撞物体	只约束了手的位置没约束接触力	限制非末端链接的接触
力过大	推门时用远超需要的力	没有力的上界惩罚	加入力大小的惩罚项
力振荡	反复施力-撤力	力跟踪奖励没有时间连续性要求	加入力变化率惩罚
忽略下体	上体完美完成任务但下体摔倒	上体奖励权重过大	摔倒即 episode 结束

奖励项的权重选择原则

力控任务的奖励设计中，权重选择比普通行走更困难，因为有更多互相竞争的目标。以下是系统化的权重选择框架：

优先级	奖励类别	权重范围	理由
P0 安全	摔倒终止、关节限位	终止信号（非权重）	安全是硬约束，不参与权重竞争
P1 稳定	基座姿态、步态节律	0.3-0.5	没有稳定性一切目标无意义
P2 任务	末端跟踪、力跟踪	0.2-0.4	力控的核心任务目标
P3 品质	动作平滑、能耗	0.05-0.15	提升部署品质但不影响功能
P4 正则	姿态偏移、对称性	0.02-0.08	防止不自然行为

总权重归一化：所有奖励项的权重之和应接近 1.0，避免奖励尺度随任务变化而漂移。如果总奖励尺度不稳定，PPO 的价值函数估计会变差。

奖励项之间的冲突检测

训练开始前，可以用"敏感度分析"检测哪些奖励项最可能冲突：

1. 只保留一个奖励项训练，记录该项策略的行为特征
2. 逐一引入其他奖励项，观察已有行为是否退化
3. 如果引入 \(r_B\) 后 \(r_A\) 的值下降超过 20%，说明 \(r_A\) 和 \(r_B\) 冲突
4. 冲突的奖励项需要调整权重比例或引入约束式处理

推荐的奖励调优流程

1. 先只训练下体行走，确认稳定
2. 加入上体位置跟踪（无外力），确认上体不干扰行走
3. 加入小外力课程，观察策略是否开始"作弊"
4. 针对观察到的作弊行为添加惩罚项
5. 逐步增大外力，重复 3-4
6. 最终阶段做消融实验——逐一移除每个奖励项，确认每个项都在贡献

跨领域类比：力控任务的奖励设计类似于多目标优化中的 Pareto 前沿搜索。每个奖励项是一个优化目标，改善一个目标通常需要牺牲另一个。权重的选择相当于在 Pareto 前沿上选择一个工作点——没有"全局最优"，只有"对当前任务最合适的权衡"。这与单纯的行走任务不同，行走只需要优化"不摔倒+跟速度"两个主要目标，力控任务需要同时平衡 5-8 个目标。

本质洞察：力控任务的奖励设计本质上是一个"约束满足"问题而非"优化"问题。策略需要同时满足多个约束（手到目标、力在范围内、身体平衡、动作平滑），而不是最大化某个单一目标。这就是为什么 FALCON 的双策略设计有效——它把一个多约束优化问题拆成了两个各有一两个主要约束的子问题。

---

95.10 双臂协调约束：双臂搬运的运动学约束推导 ⭐⭐⭐

为什么双臂任务比单臂难

单臂操作时，末端有 6 个自由度。双臂同时抓住一个刚性物体时，两个末端的相对位姿被物体约束——不能独立运动。这引入了闭链运动学约束。

双臂约束的数学推导

设左臂末端位姿为 \(T_L \in SE(3)\)，右臂末端位姿为 \(T_R \in SE(3)\)，物体位姿为 \(T_O \in SE(3)\)。两个抓取点相对于物体的固定变换为 \(T_{OL}\) 和 \(T_{OR}\)。

位姿约束

\[ T_L = T_O \cdot T_{OL}, \qquad T_R = T_O \cdot T_{OR} \]

这意味着两臂的相对位姿是固定的：

\[ T_L^{-1} T_R = T_{OL}^{-1} T_{OR} = \text{const} \]

速度约束

对位姿约束求时间导数：

\[ \dot{x}_L = J_L(q_L) \dot{q}_L, \qquad \dot{x}_R = J_R(q_R) \dot{q}_R \]

\[ \dot{x}_L - \dot{x}_R = \text{Ad}_{T_{OL}^{-1}} \cdot \dot{x}_O - \text{Ad}_{T_{OR}^{-1}} \cdot \dot{x}_O \]

简化为：

\[ J_L \dot{q}_L - J_R \dot{q}_R = C \dot{x}_O \]

其中 \(C\) 是由抓取几何决定的常数矩阵。

内力与运动力的分解

双臂施加在物体上的总力可以分解为：

1. 运动力（Motion Force）：导致物体运动的合力
2. 内力（Internal Force）：两臂互相挤压的力，不导致物体运动但维持抓取稳定

\[ f_{motion} = f_L + f_R, \qquad f_{internal} = \frac{f_L - f_R}{2} \]

内力需要足够大以防止物体滑脱，但不能太大以免压坏物体或浪费能量。

内力的最优选择

设物体和手之间的摩擦系数为 \(\mu\)，物体重力为 \(mg\)，运动力为 \(f_{motion}\)。防止滑脱的条件（Coulomb 摩擦锥约束）为：

\[ f_{internal} \geq \frac{1}{2\mu}\left(\frac{mg}{2} + \|f_{motion}\|\right) \]

最优内力选择是在满足摩擦锥约束的前提下最小化总关节力矩：

\[ f_{internal}^* = \frac{1}{2\mu}\left(\frac{mg}{2} + \|f_{motion}\|\right) \cdot \text{safety\_factor} \]

其中 safety_factor 通常取 1.2-1.5。

双臂协调的 RL 奖励设计

双臂任务的奖励需要额外约束闭链一致性：

\[ r_{bimanual} = r_{object\_track} + r_{internal\_force} + r_{constraint\_violation} + r_{smooth} \]

其中 \(r_{constraint\_violation} = -w_c \|T_L^{-1} T_R - T_{OL}^{-1} T_{OR}\|^2\) 惩罚违反闭链约束的行为。

思维陷阱 🧠

新手想法："双臂就是两个独立的单臂策略同时运行"

实际上：如果两个臂的策略独立训练和运行，它们不知道对方的存在。当左臂试图把物体往左拉，右臂同时试图把物体往右拉，结果是巨大的内力——物体没有移动，但两个电机都在疯狂输出力矩。更糟的是，如果一个臂比另一个强（参数不对称），物体会被拉向一边，导致抓取失败

正确思维：双臂策略必须要么在训练中共享信息（FALCON 式的共享观测），要么通过物体约束隐式协调（需要力传感器感知内力），或者由一个主臂领导、另一个从臂跟随

---

95.11 力控调参实战：Kp/Kd 选择、力范围标定、安全限制 ⭐⭐⭐

系统化调参方法

力控调参不是"试试看"——有系统的方法论。

Step 1：确定任务力范围

任务	法向力范围	切向力范围	力矩范围
端杯子	1-5 N（支撑重力）	< 1 N	< 0.5 Nm
推门	10-40 N	2-10 N	2-8 Nm
拉车	30-100 N	5-20 N	5-15 Nm
螺栓拧紧	5-20 N	2-5 N	1-5 Nm

Step 2：选择初始刚度

\[ K_{init} = \frac{f_{typical}}{\delta_{acceptable}} \]

其中 \(f_{typical}\) 是典型任务力，\(\delta_{acceptable}\) 是可接受的位置偏差。

例如：推门任务，典型力 20 N，可接受偏差 2 cm：

\[ K_{init} = \frac{20}{0.02} = 1000 \text{ N/m} \]

Step 3：选择阻尼比

临界阻尼给出无振荡的最快响应：

\[ D_{critical} = 2\sqrt{K \cdot m_{eff}} \]

其中 \(m_{eff}\) 是末端等效质量。实践中通常选择 0.7-1.0 倍临界阻尼。

\[ D = \zeta \cdot D_{critical}, \qquad \zeta \in [0.7, 1.0] \]

阻尼比 \(\zeta\)	行为	适用场景
< 0.7	欠阻尼，有振荡	不推荐用于接触任务
0.7-1.0	接近临界阻尼	大多数力控任务
> 1.0	过阻尼，响应慢	需要极高稳定性时

Step 4：安全限制设计

安全机制	实现方式	触发阈值（推荐）
力矩限幅	\(\tau_{cmd} = \text{clip}(\tau_{cmd}, -\tau_{max}, \tau_{max})\)	额定力矩的 85%
力限幅	\(f_{cmd} = \text{clip}(f_{cmd}, -f_{max}, f_{max})\)	任务力范围的 150%
速度限制	超过速度阈值时增大阻尼	关节速度的 80%
位置限位	接近限位时增加虚拟墙	距限位 5° 开始
碰撞检测	力突变时急停	\(\|f\| > 2 \times f_{expected}\)

import numpy as np


def impedance_control(x: np.ndarray, dx: np.ndarray,
                      x_des: np.ndarray, dx_des: np.ndarray,
                      K: np.ndarray, D: np.ndarray,
                      f_ff: np.ndarray,
                      f_max: float = 100.0) -> np.ndarray:
    """中文注释：带安全限幅的笛卡尔阻抗控制。"""
    # 中文注释：计算弹簧力 + 阻尼力 + 前馈力
    f_spring = K @ (x_des - x)
    f_damping = D @ (dx_des - dx)
    f_total = f_spring + f_damping + f_ff

    # 中文注释：安全限幅——防止力过大
    f_norm = np.linalg.norm(f_total)
    if f_norm > f_max:
        f_total = f_total * (f_max / f_norm)

    return f_total


def compute_critical_damping(K: np.ndarray, m_eff: float,
                             zeta: float = 0.9) -> np.ndarray:
    """中文注释：计算临界阻尼矩阵（对角假设）。"""
    # 中文注释：对对角刚度矩阵，临界阻尼也是对角的
    K_diag = np.diag(K) if K.ndim == 2 else K
    D_diag = 2.0 * zeta * np.sqrt(K_diag * m_eff)
    return np.diag(D_diag)

调参的常见失败模式

失败模式	表现	原因	修复
接触振荡	末端在接触面上来回弹跳	\(\zeta < 0.7\)（欠阻尼）	增大阻尼至临界阻尼
接触冲击	接触瞬间力尖峰很大	\(K\) 太高	降低刚度或加入接近检测
跟踪滞后	末端跟不上目标运动	\(K\) 太低或 \(\zeta > 1.2\)	增大刚度或降低阻尼
姿态不稳	接触后整个身体偏移	没有全身力补偿	让下体策略感知末端外力
抓取滑落	抓取力不够	前馈力 \(f_{ff}\) 设置不当	增大法向前馈力

本质洞察：力控调参的核心困难不是找到"最优参数"，而是找到"在任务变化范围内都安全的参数"。一个在推门时表现完美的 \((K, D)\) 组合，在端杯子时可能因为刚度太高导致杯子被挤碎。这就是为什么 FALCON 用 RL 学习参数而不是手动调——RL 可以在大量不同任务配置中自动找到鲁棒的参数组合。

力范围标定的实验方法

在设计力课程之前，需要先测量目标任务的实际力范围。方法如下：

1. 直接测量：操作者用力传感器手动执行任务，记录力的时间序列
2. 文献参考：查阅人体工程学文献中的典型操作力
3. 安全余量：训练时的力范围设为实测值的 1.5-2 倍，保证策略在极端情况下也鲁棒

参考数据来源	适用范围	精度
ISO 11228 人体搬运标准	搬运任务	高
建筑门把手标准	开门任务	高
实测（力传感器）	特定任务	最高
RL 训练中的力统计	仿真估计	中

---

95.12 HOMIE：同构外骨骼遥操作路线 ⭐⭐⭐

为什么同构能降低误差

普通 VR 手柄给的是末端位姿，机器人需要 IK 反解上体关节。同构外骨骼给的是与机器人关节一一对应的关节角，映射链路更短。

接口	映射方式	优点	局限
VR 手柄	末端位姿 → IK → 关节角	通用、便宜	IK 多解、奇异点、延迟
同构外骨骼	关节角 → 关节角	直观、低延迟、无奇异	需针对机器人设计
RGB 视觉追踪	视频姿态估计 → 重定向	无需穿戴	遮挡、尺度误差、高延迟
语言/高层命令	技能选择	自然交互	底层力控缺口

HOMIE（Humanoid Loco-Manipulation with Isomorphic Exoskeleton Cockpit, Bao et al. RSS 2025）系统包含三个组件：

1. 同构外骨骼臂：与机器人手臂运动学一致，直接映射关节角
2. 运动感知手套：15 个自由度的霍尔传感器手套，映射灵巧手
3. 脚踏板：控制移动速度和方向，解放操作者上体

HOMIE 的关键设计决策

为什么用脚踏板控制移动？ 如果用手柄控制移动，操作者的双手就被占用了——无法同时控制手臂和移动。脚踏板让操作者的双脚控制移动方向和速度，双手专注于手臂操作。

为什么不需要 IK？ 外骨骼的每个关节直接对应机器人的一个关节，映射就是 \(q_{robot} = q_{exo}\)（可能加一个增益和偏移）。没有 IK 意味着没有多解问题、没有奇异点、没有 IK 求解延迟。

成本考虑：HOMIE 报告整套系统成本约 $500，比传统 VR 遥操作系统便宜一个数量级。

不同遥操作接口的定量延迟对比

接口类型	映射延迟	端到端延迟	主要延迟来源
同构外骨骼（HOMIE）	< 1 ms	5-10 ms	通信和电机响应
VR 手柄 + IK	3-8 ms（IK 求解）	15-25 ms	IK 求解 + 滤波
RGB 视觉追踪	30-50 ms（姿态估计）	50-100 ms	推理 + 重定向
语言命令	100-500 ms（LLM 推理）	200-1000 ms	模型推理 + 规划

本质洞察：遥操作延迟不只影响操作精度，还影响数据质量。高延迟导致操作者必须放慢速度——慢速操作的动作分布与自主策略的快速动作分布不一致，用这种数据训练的策略可能在快速场景下表现不佳。HOMIE 的低延迟让操作者可以接近正常速度操作，产生的数据分布更接近自主策略的目标分布。

HOMIE 的映射标定流程

虽然同构外骨骼理论上是 \(q_{robot} = q_{exo}\)，但实际部署时仍需标定几个参数：

1. 零位对齐：外骨骼和机器人的零位定义可能不同，需要在标准姿态下对齐
2. 增益校正：外骨骼和机器人的关节角范围可能不同，需要线性缩放 \(q_{robot} = \alpha q_{exo} + \beta\)
3. 手长差异：操作者和机器人的臂长不同，影响末端工作空间的映射
4. 延迟补偿：通信延迟导致的相位差需要在控制端补偿

标定通常在每次使用前做一次，耗时约 2-3 分钟。标定质量直接影响采集数据的可用性。

任意上体姿态课程

遥操作时人类可能把手臂摆到各种位置。下体策略必须在上体姿态大范围变化时仍保持平衡。因此训练中要随机上体目标，并给下体高度、姿态和对称正则：

\[ r_{random\text{-}pose} = \sum_{j \in upper} \exp\left(-\frac{(q_j - q_j^{rand})^2}{\sigma_j^2}\right) \]

\(q^{rand}\) 在上体关节的安全范围内均匀采样。下体策略在这种"上体随机乱动"的条件下训练行走，自然学会了对上体扰动的鲁棒性。

遥操作安全接口

遥操作命令进入机器人前必须经过安全过滤：

安全层	功能	实现方式
速度限制	防止突然大动作	对指令差分限幅
关节限位	防止超出关节范围	裁剪到安全范围
自碰撞检查	防止手臂碰到身体	简化几何碰撞检测
力矩可行性	防止超出电机能力	RNEA 力矩估计
急停	操作者紧急情况	硬件急停按钮

HOMIE 与 FALCON 的集成路径

HOMIE 提供了高质量的遥操作数据采集能力，FALCON 提供了力自适应的全身控制策略。两者的集成路径是：

1. 数据采集阶段：操作者通过 HOMIE 外骨骼遥操作机器人执行各种力敏感任务
2. 动作库构建：把遥操作的动作数据整理为参考动作序列
3. 策略训练：用 FALCON 双策略架构，以遥操作数据作为参考，训练自主执行策略
4. 自主部署：训练好的策略可以无需遥操作自主执行任务

这条路径的优势是：遥操作阶段由人类保证任务完成质量，策略训练阶段由 RL 保证鲁棒性和泛化能力。

跨领域类比：HOMIE + FALCON 的路径类似于自动驾驶中的"先人工标注再模型训练"。人类驾驶员（操作者）在各种场景下驾驶（遥操作），产生高质量的驾驶数据（动作序列）。然后用这些数据训练自动驾驶模型（策略），使其能在没有人类驾驶员的情况下自主驾驶（自主部署）。

遥操作数据的质量评估

不是所有遥操作数据都适合用于策略训练。需要过滤：

过滤条件	原因	阈值（推荐）
任务成功	失败的数据可能包含不安全动作	只保留成功 episode
动作平滑度	人类操作的抖动不应被学习	关节角速度 \(< 5\) rad/s
力矩可行性	逆动力学计算力矩不超限	额定力矩 85% 以内
操作时长	太快或太慢的操作可能不自然	1-3 倍标准时间内
多样性	训练集应覆盖多种初始条件	至少 50 个不同初始位姿

---

95.13 SkillBlender 与可组合技能 ⭐⭐

为什么需要技能库

单一任务策略很难覆盖搬、推、蹲、走、伸手、避障等组合。技能库路线先训练一组 goal-conditioned primitive，再按任务动态混合：

\[ a_t=\sum_i\alpha_i(o,g)\pi_i(o,g_i),\qquad \sum_i\alpha_i=1 \]

其中 \(\alpha_i(o,g)\) 是第 \(i\) 个技能的混合权重，由一个门控网络（gating network）根据当前观测 \(o\) 和全局目标 \(g\) 预测。\(\pi_i(o,g_i)\) 是第 \(i\) 个技能策略，接收本体观测和技能特定子目标 \(g_i\)。

技能库的层次结构

SkillBlender（Liu et al. 2024 CoRL）将技能分为三个层次：

层次	技能类型	示例	训练方式
L0 原子技能	单一运动行为	站立、走步、蹲下、伸手	单任务 RL
L1 组合技能	两个原子技能同时执行	走+伸手、蹲+搬运	混合训练 + 协调损失
L2 序列技能	多个技能按时间排列	走向目标→蹲下→抓取→站起→搬运	高层调度器

阶段小结：到这里我们区分了三个层次的技能复杂度。L0 是单一行为，对应单任务策略。L1 是并行组合，对应 SkillBlender 的核心能力。L2 是时序调度，通常由更高层的规划器（如 LLM 或有限状态机）负责。

门控网络的设计

门控网络决定"现在应该执行哪个技能、权重多大"。其设计需要满足几个数学约束：

1. 归一化约束：\(\sum_i \alpha_i = 1\)，通过 softmax 实现
2. 稀疏性偏好：同时激活太多技能会导致动作模糊。加入熵正则 \(-\beta H(\alpha)\) 鼓励稀疏激活
3. 平滑性约束：\(\|\alpha_t - \alpha_{t-1}\| < \epsilon\)，防止混合权重的突然跳变

\[ \alpha_t = \text{softmax}\left(\frac{f_{gate}(o_t, g)}{\tau_{gate}}\right), \quad \tau_{gate} \in [0.1, 1.0] \]

温度参数 \(\tau_{gate}\) 控制混合的稀疏度——\(\tau_{gate} \to 0\) 时退化为硬选择（winner-take-all），\(\tau_{gate} \to \infty\) 时所有技能均匀混合。

技能混合风险与缓解

风险	表现	缓解方法
技能边界不连续	切换瞬间动作跳变	混合权重平滑过渡
技能目标冲突	一边蹲一边伸手导致失稳	高层可行性检查
分布外组合	组合状态在训练中从未出现	组合课程和回放
安全不可解释	不知道哪个技能导致失败	记录混合权重和各技能残差
动作幅值爆炸	多个技能输出同向叠加超限	混合后动作限幅

技能冲突的定量检测

两个技能是否冲突，可以用动作空间中的余弦相似度来衡量：

\[ \text{conflict}_{ij} = -\cos(\pi_i(o, g_i),\; \pi_j(o, g_j)) = -\frac{\pi_i^T \pi_j}{\|\pi_i\| \|\pi_j\|} \]

当 \(\text{conflict}_{ij} > 0.5\)（两个技能的动作方向接近相反）时，应降低两者的同时激活概率。实现方式是在门控损失中加入排斥项：

\[ L_{repel} = \sum_{i<j} \alpha_i \alpha_j \max(0, \text{conflict}_{ij} - \delta) \]

其中 \(\delta\) 是容忍的冲突阈值。

反事实推理：如果不做技能冲突检测会怎样？假设"走路"技能要求右腿向前迈步，而"蹲下"技能要求双腿弯曲下沉。当两个技能被等权混合时，右腿的动作是"向前迈步"和"弯曲下沉"的平均——既没有正确迈步也没有正确下蹲，机器人会出现一个非自然的姿态，可能导致失稳。SkillBlender 的协调损失正是为了检测并抑制这类动作冲突。

与 FALCON 的关系

FALCON 解决上体与下体的空间分工。SkillBlender 解决多个全身技能的任务组合。两者可以结合：每个 primitive 内部仍采用上下体解耦。

组合架构示意

             高层调度器（LLM / FSM）
                    |
                    v
           门控网络 α_i(o, g)
           /       |       \
     [走+搬运]  [蹲+抓取]  [站+推门]   ← L1 组合技能
       /  \       /  \       /  \
    上体  下体  上体  下体  上体  下体   ← FALCON 解耦

每个 L1 组合技能内部使用 FALCON 式的上下体解耦。门控网络在 L1 技能之间做软切换。这种"空间解耦 + 技能混合"的双重结构，既利用了 FALCON 的梯度分离优势，又通过 SkillBlender 实现了多任务泛化。

MaskedManipulator 的补充视角

MaskedManipulator（2025）提出了另一种技能组合思路——不是混合多个策略的输出，而是让一个统一策略根据任务掩码（task mask）选择性激活不同的网络分支。掩码向量 \(m \in \{0, 1\}^K\) 指定当前激活哪些技能分支，避免了混合权重的连续优化问题。

WholeBodyVLA：VLA 与全身控制的融合前沿

WholeBodyVLA（OpenDriveLab, ICLR 2026）代表了力敏感 loco-manipulation 的另一条前沿路线——用视觉-语言-动作基础模型（VLA）直接输出全身控制信号。与 FALCON 的纯 RL 路线不同，WholeBodyVLA 利用预训练视觉-语言模型的语义理解能力，在 latent 空间中统一表示任务目标和运动技能。

维度	FALCON + SkillBlender	WholeBodyVLA
任务规范	显式目标（末端位姿 + 速度命令）	自然语言指令 + 视觉目标
技能表示	独立策略网络	统一 latent 空间
力控精度	高（专门设计的力课程）	中（需要额外力控适配层）
泛化能力	技能库内泛化	语义级泛化（新任务描述）
训练数据	仿真 RL	大规模遥操作数据 + 预训练

本质洞察：FALCON 和 WholeBodyVLA 代表了力敏感 loco-manipulation 的两个极端——FALCON 从控制理论出发，用精心设计的奖励和课程学习力自适应行为；WholeBodyVLA 从数据驱动出发，用大规模预训练模型的泛化能力覆盖多样化任务。两者的融合方向可能是：用 VLA 做高层任务理解和运动规划，用 FALCON 式双策略做底层力敏感执行——高层告诉"做什么"，底层保证"怎么做得安全"。

---

95.14 BFM-Zero：行为基础模型与无监督控制 ⭐⭐⭐

动机

手工 reward 很难覆盖所有人形任务。每增加一个新任务就需要设计新的奖励函数——这个过程耗时、需要领域知识，而且不同任务的奖励可能互相冲突。

奖励工程的困境

考虑一个人形机器人需要完成10种不同的 loco-manipulation 任务（搬运、推门、拉车、端杯、擦桌、递物、按按钮、开抽屉、挂衣服、扫地）。每种任务需要设计 5-8 个奖励项，总计 50-80 个奖励项。每个奖励项的权重需要调优，奖励项之间可能互相冲突。这种组合爆炸让手工奖励工程变得不可持续。

行为基础模型（Behavioral Foundation Model）希望先用无监督或弱监督方式学习可控行为空间，再由 prompt、goal 或少量 reward 指定具体任务。

与传统方法的对比

维度	手工奖励 RL	模仿学习	BFM-Zero
新任务所需	设计新奖励 + 调权重	收集新数据 + 训练	指定 goal/prompt
人力成本	高（奖励工程）	中（数据收集）	低（prompt）
泛化能力	差（每个任务独立）	中（数据覆盖范围内）	好（latent 空间插值）
力控安全	可在奖励中约束	依赖数据质量	需额外安全层

Forward-Backward 表示

BFM-Zero（He et al. 2025）基于 Forward-Backward (FB) 表示学习。核心思想是学习两个函数：

Forward 表示 \(F(s, z)\)：从当前状态 \(s\) 执行 latent \(z\) 后，预计到达的状态分布的特征

Backward 表示 \(B(s')\)：从某个目标状态 \(s'\) 反推，需要什么 latent \(z\) 才能到达

\[ z^* = B(g), \qquad a = \pi(o, z^*) \]

给定目标 \(g\)（可以是位姿、动作片段或奖励函数），backward 编码器给出对应的 latent \(z^*\)，策略按 \(z^*\) 执行。

BFM-Zero 的能力

BFM-Zero 在 Unitree G1 上实现了：

推理模式	输入	输出	是否需要额外训练
零样本动作跟踪	参考动作序列	跟踪该动作	否
零样本目标到达	目标位姿	到达该位姿	否
零样本奖励优化	奖励函数	最大化奖励的行为	否
少样本适应	少量真机数据	适应新任务	少量微调

FB 表示的数学细节

Forward-Backward 表示的训练目标是让两个函数的内积逼近折扣状态占用度量（discounted state occupancy measure）：

\[ F(s, z)^T B(s') \approx \sum_{t=0}^{\infty} \gamma^t P(s_t = s' | s_0 = s, z) \]

其中 \(\gamma\) 是折扣因子，\(P(s_t = s' | s_0 = s, z)\) 是从状态 \(s\) 执行 latent 指令 \(z\) 后在第 \(t\) 步到达 \(s'\) 的概率。

训练损失使用对比学习框架：

\[ L_{FB} = -\mathbb{E}\left[\log \frac{\exp(F(s, z)^T B(s^+))}{\exp(F(s, z)^T B(s^+)) + \sum_{j} \exp(F(s, z)^T B(s_j^-))}\right] \]

其中 \(s^+\) 是从 \((s, z)\) 出发实际到达的状态（正样本），\(s_j^-\) 是随机采样的状态（负样本）。这个损失迫使 Forward 和 Backward 表示在正确的状态-目标对上产生高内积。

无监督技能发现

BFM-Zero 的一个关键特性是 latent 空间 \(z\) 的每个维度自动对应某种运动行为——不需要人工定义"走路""蹲下"等标签。这是因为训练目标隐式地鼓励 \(z\) 的不同值对应不同的状态占用分布。

在实践中，通过对 latent 空间进行线性插值，可以生成平滑过渡的运动行为：

\[ z_{interp} = (1 - \alpha) z_{walk} + \alpha z_{crouch}, \quad \alpha \in [0, 1] \]

这种插值在 FALCON 的离散技能库中是不可能的——FALCON 的每个技能是一个独立的策略网络，不共享连续的表示空间。

与力敏感任务的关系

如果 latent 空间能编码"稳端杯子""推门""搬重物"等行为，力敏感任务可以从手工 reward 转向 goal-conditioned 控制。但对于接触力安全，仍需要低层约束或安全过滤——BFM-Zero 的 latent 空间不能保证力的大小在安全范围内。

安全层的集成方案

\[ a_{safe} = \text{CBF\_filter}(\pi(o, z^*), \; f_{max}, \; \tau_{max}) \]

其中 CBF（Control Barrier Function）过滤器在 BFM-Zero 输出的动作违反力安全约束时进行最小修正。这种"学习生成 + 安全过滤"的架构在 96 章基础模型中有更详细的讨论。

陷阱警告 ⚠️

思维陷阱：认为 BFM-Zero 可以完全替代手工奖励设计

新手想法："既然 BFM-Zero 是零样本的，就不需要设计任何奖励了吧"

实际上：BFM-Zero 的 latent 空间在无监督训练阶段形成。如果训练数据中缺少某种行为模式（如大力推重物），latent 空间就无法编码该行为。对于力敏感任务中的极端工况（100 N 拉车、冲击负载吸收），仍然需要用针对性的力课程来扩展训练分布

正确做法：用 BFM-Zero 覆盖常规任务，用 FALCON 式专门训练覆盖极端力控场景，两者互补

---

95.15 FALCON 与 SoFTA 合并：空间 + 时间双重解耦 ⭐⭐⭐⭐

组合架构

空间上，上体和下体有不同策略头和奖励（FALCON）。时间上，上体更高频，下体更低频（SoFTA）。

\[ a_t^u=\pi_u(o_t), \qquad a_t^l=\pi_l(o_{k\lfloor t/k\rfloor}), \qquad r_t=r_t^u+r_{k\lfloor t/k\rfloor}^l \]

组合带来的新挑战

挑战	原因	可能的处理方法
信用分配	上体高频动作影响下体低频回报	分层 advantage 估计或延迟归因
Bellman 一致性	两个时间尺度的价值函数定义不同	多时间尺度 critic
观测同步	上体看到下体的"旧动作"	显式输入 action hold 状态
部署延迟	两个控制线程不同步	时间戳和无锁缓冲区
调试困难	三个变量同时变化（奖励、力课程、频率）	消融实验分别验证

工程判断

不要一开始就同时上空间和时间解耦。建议分步推进：

1. 第一步：训练单策略、单频率基线，确认任务可学
2. 第二步：引入 FALCON 式双策略（空间解耦），确认力课程有效
3. 第三步：把上体频率提高（时间解耦），确认末端加速度改善
4. 第四步：联合调优

否则训练失败时无法判断问题来自奖励设计、力课程、频率差异还是网络架构。

与 94 章 ASAP 的集成

FALCON+SoFTA 解决的是仿真中的训练问题，ASAP 解决的是从仿真到真机的迁移问题。两者的集成路径是：

1. 仿真训练：用 FALCON 双策略 + SoFTA 频率解耦训练力敏感 loco-manipulation 策略
2. Sim2sim 验证：按 94 章的方法跨仿真器验证策略
3. 真机 Rollout：在 G1 真机上部署策略，记录状态-动作转移数据
4. Delta-action 训练：用真机数据训练踝关节残差模型
5. 带残差微调：在修正后的仿真中继续训练双策略
6. 力敏感部署：在真机上执行推门、拉车等任务

集成中的特殊挑战

力敏感任务的 sim-to-real 比普通行走更困难，因为：

挑战	原因	处理
接触力的 sim-to-real gap	接触模型差异直接影响力控任务	ASAP delta-action + 接触模式条件化
力传感器标定	仿真中的力值与真机不一致	真机启动前标定力传感器零偏
外力引起的额外接触变化	推门时脚底摩擦需求增加	力课程中同时训练下体摩擦鲁棒性
物体参数不确定	门的阻力、车的重量未知	物体参数随机化 + RMA 式适应

本质洞察：力敏感任务的 sim-to-real 不只是"把行走策略迁移到真机"——还需要确保策略在真实接触条件下能输出正确的力。94 章的 ASAP 方法修正的是"动作到状态"的映射，但力敏感任务还需要关注"动作到力"的映射是否准确。如果仿真和真实的接触刚度差距大，即使状态跟踪准了，接触力也可能不对。

方法关系图谱

                     力敏感 Loco-Manipulation 方法图谱

    [阻抗控制理论]───→ 力-位移关系 → 接触安全
         │                              │
         ▼                              ▼
    [FALCON 双策略]──→ 空间解耦 ─→ 上体精度+下体稳定
         │                              │
         ▼                              ▼
    [SoFTA 频率解耦]─→ 时间解耦 ─→ 末端加速度抑制
         │                              │
         ├──→ [ASAP] ─────────────→ Sim-to-Real 迁移
         │                              │
         ├──→ [HOMIE] ────────────→ 高质量数据采集
         │                              │
         ├──→ [SkillBlender] ─────→ 多技能组合
         │                              │
         └──→ [BFM-Zero] ────────→ 零样本泛化

每条路线解决的是 pipeline 中不同的瓶颈： - 精度瓶颈 → FALCON 双策略 - 平滑度瓶颈 → SoFTA 频率解耦 - 部署瓶颈 → ASAP delta-action - 数据瓶颈 → HOMIE 遥操作 - 泛化瓶颈 → BFM-Zero / SkillBlender

选择时先诊断瓶颈在哪里，而不是盲目叠加所有方法。

诊断瓶颈的量化标准

瓶颈类型	诊断指标	阈值	推荐解决方案
精度瓶颈	EE 跟踪误差 > 5 cm	RMS 误差	FALCON 双策略
平滑度瓶颈	EE 加速度 RMS > 2 m/s²	频域分析	SoFTA 频率解耦
部署瓶颈	真机跟踪误差 > 仿真的 2 倍	跨域误差比	ASAP delta-action
数据瓶颈	任务演示 < 50 条	数据量	HOMIE 遥操作
泛化瓶颈	新任务需 > 1 天调奖励	工程时间	BFM-Zero / SkillBlender

实际部署的经验法则

基于 FALCON 和 SoFTA 论文的实验数据，以下是一些实用的经验法则：

经验法则	适用条件	量化标准
力课程速度	训练成功率 > 70% 才推进	每 10M 步检查
上体频率	不超过下体的 4 倍	2-4 倍最佳
外力范围	训练力范围是部署的 1.5-2 倍	留安全裕度
共享观测	至少共享 IMU、所有关节、末端目标	不能完全隔离
阻尼比	接触任务用 0.8-1.0	不欠阻尼
力矩裕度	保留 15-20% 裕度	安全第一
训练总步数	双策略需要 200-300M 步	比单策略多 50%
消融实验	每次只改一个变量	可解释性优先

---

常见故障与排查 ⭐⭐

🔧 故障排查手册

症状	可能原因	排查步骤	修复方向
末端跟踪准但机器人摔倒	上体奖励权重压过下体稳定	分开统计上下体奖励和成功率	提高下体稳定权重或限制上体动作幅度
外力一大就脚滑	力课程太快或摩擦随机化不足	画足底水平力和垂直力比值	放慢课程、增强摩擦扰动
端杯子抖动	末端加速度惩罚不足或上体频率低	计算 EE 垂直加速度 RMS	提高上体频率、加大加速度惩罚
双策略动作互相打架	腰部关节归属不清或共享观测不足	查看腰部和躯干动作频谱	明确腰部归上体或下体，加入协调损失
遥操作动作跳变	输入未限速或时间戳抖动	记录命令差分的时间序列	低通滤波 + 速度限幅
RNEA 检查频繁失败	力任务超过硬件能力	统计失败时的关节和外力方向分布	降低力范围或改变姿态课程
技能混合后动作怪异	primitive 的训练分布不重叠	记录混合权重和各 primitive 的状态覆盖	增加组合课程训练
力传感器漂移导致持续偏动	零偏未标定或温度漂移	对比启动时和运行中的传感器读数	启动前标定 + 运行中高通滤波

---

力敏感任务的调试工具箱

工具	用途	实现建议
奖励分项记录器	分开统计上体/下体/协调奖励	WandB 多曲线面板
力课程监视器	记录当前力级别和通过率	每 epoch 打印
ZMP/CMP 可视化	实时显示稳定裕度	仿真 viewer 叠加
末端力曲线	记录接触力时间序列	分 xyz 通道绘制
双策略动作对比	显示上下体动作的频谱和幅值	FFT + 时域图
接触时序图	标注脚底接触的时间模式	步态时钟对比
力矩裕度仪表盘	每个关节的力矩使用率	条形图实时更新
能量平衡图	验证无源性条件	\(V(t)\) vs \(\int f^T\dot{x}\,d\tau\)

def diagnose_dual_policy(upper_actions, lower_actions, rewards_upper,
                         rewards_lower, contact_flags, ee_positions, dt):
    """中文注释：双策略训练的诊断函数。"""
    import numpy as np

    # 中文注释：上下体动作的频谱分析
    from scipy.signal import welch
    f_upper, psd_upper = welch(upper_actions, fs=1.0/dt, axis=0)
    f_lower, psd_lower = welch(lower_actions, fs=1.0/dt, axis=0)

    # 中文注释：末端加速度计算（滤波后差分）
    from scipy.signal import butter, filtfilt
    b, a = butter(2, 25.0 / (0.5/dt), btype='low')
    ee_filtered = filtfilt(b, a, ee_positions, axis=0)
    ee_acc = np.diff(ee_filtered, n=2, axis=0) / (dt**2)

    # 中文注释：报告
    print(f"上体动作 RMS: {np.sqrt(np.mean(upper_actions**2)):.4f}")
    print(f"下体动作 RMS: {np.sqrt(np.mean(lower_actions**2)):.4f}")
    print(f"上体奖励均值: {np.mean(rewards_upper):.4f}")
    print(f"下体奖励均值: {np.mean(rewards_lower):.4f}")
    print(f"EE 垂直加速度 RMS: {np.sqrt(np.mean(ee_acc[:, 2]**2)):.4f} m/s^2")
    print(f"EE 垂直加速度 peak: {np.max(np.abs(ee_acc[:, 2])):.4f} m/s^2")
    print(f"接触率: {np.mean(contact_flags):.2%}")

    return {
        'psd_upper': (f_upper, psd_upper),
        'psd_lower': (f_lower, psd_lower),
        'ee_acc_rms': np.sqrt(np.mean(ee_acc**2, axis=0)),
    }

---

练习 ⭐⭐

练习 95.1 末端外力对全身动力学的影响 ⭐⭐⭐

推导 \(J_{ee}^T f_{ee}\) 对浮动基动力学的影响。具体要求：

1. 从全身动力学方程出发，写出当 \(f_{ee} = [0, 0, 0, 30, 0, 0]^T\)（30 N 水平推力）时，各关节所需的额外力矩
2. 计算这个外力对 ZMP 位置的影响
3. 分析在什么姿态下这个外力最容易导致失稳

练习 95.2 阻抗参数设计 ⭐⭐⭐

为以下三个任务设计阻抗参数 \((K, D, f_{ff})\)：

任务	典型力	可接受位置误差	接触速度
端杯子	3 N 垂直	1 cm	0
推门	25 N 水平	3 cm	0.2 m/s
擦桌子	10 N 垂直	0.5 cm	0.3 m/s
递物品	5 N 多方向	2 cm	0.1 m/s

计算每个任务的 \(K_{init}\)、\(D_{critical}\) 和推荐阻尼比 \(\zeta\)。

练习 95.3 FALCON 双策略实现 ⭐⭐⭐⭐

实现一个简化的 FALCON 风格双策略训练配置：

1. 定义上体和下体的观测空间、动作空间和奖励函数
2. 实现共享观测的拆分逻辑
3. 设计一个简单的协调损失
4. 训练并对比单策略 vs 双策略的末端跟踪和行走稳定性

练习 95.4 3D 力课程实验 ⭐⭐⭐

实现一个带力矩可行性检查的力课程：

1. 实现 \(f(t) = f_0 + A\sin(\omega t)\) 的时变外力
2. 加入 RNEA 力矩裕度检查
3. 观察不同课程速度（\(\alpha = 1.0, 1.5, 2.0\)）对训练效率的影响

练习 95.5 SoFTA 异步控制 ⭐⭐⭐

实现上体 100 Hz、下体 50 Hz 的 action hold：

1. 在训练中模拟异步更新
2. 加入末端加速度惩罚
3. 对比同频和异频训练在端杯子任务上的 EE 垂直加速度 RMS

练习 95.6 无源性验证 ⭐⭐⭐⭐

对一个简单的二维阻抗控制器：

1. 计算存储函数 \(V(x)\) 和耗散功率
2. 数值验证无源性条件（画 \(V(t)\) 和 \(\int_0^t f^T \dot{x} d\tau\) 的对比曲线）
3. 故意设置负阻尼，观察系统发散

练习 95.7 综合项目（跨章）⭐⭐⭐⭐

结合94章的 ASAP 方法和本章的 FALCON 方法，设计一个力敏感人形 loco-manipulation 的完整训练和部署方案。要求包括：(a) 双策略训练配置，(b) 力课程设计，(c) sim-to-real 的 delta-action 微调，(d) 部署安全检查。画出完整的系统框图。

---

公式速查

编号	公式	含义	使用位置
E1	\(M\dot v+h=S^T\tau+J_c^T\lambda+J_{ee}^T f_{ee}\)	含末端外力的全身动力学	力敏感任务
E2	\(f=K(x^d-x)+D(\dot x^d-\dot x)+f_{ff}\)	阻抗控制	接触操作
E3	\(V(t)-V(0)\leq\int_0^t f^T\dot x\,d\tau\)	无源性条件	稳定性证明
E4	\(a=[\pi_u(o),\pi_l(o)]\)	双策略动作拼接	FALCON 风格
E5	\(r^u=r_{ee}+r_{posture}+r_{smooth}\)	上体奖励	末端跟踪
E6	\(r^l=r_{vel}+r_{balance}+r_{gait}\)	下体奖励	稳定行走
E7	\(\tau=S(M\dot v+h-J_c^T\lambda-J_{ee}^T f_{ext})\)	外力可行性检查	RNEA 安全
E8	\(a_t^l=\pi_l(o_{k\lfloor t/k\rfloor})\)	低频 action hold	SoFTA
E9	\(a=\sum_i\alpha_i\pi_i(o,g_i)\)	技能混合	SkillBlender
E10	\(D_{critical}=2\sqrt{K\cdot m_{eff}}\)	临界阻尼	调参
E11	\(T_L^{-1}T_R = \text{const}\)	双臂约束	协调搬运

---

综合项目：力敏感 G1 Loco-Manipulation 小系统

• 阶段 1：实现单手 EE 位姿跟踪任务，下体保持速度命令行走。
• 阶段 2：加入 0-40 N 水平外力课程，记录末端误差和基座姿态变化。
• 阶段 3：拆分上体和下体动作头（FALCON 式），分别设计奖励并共享本体观测。
• 阶段 4：加入 RNEA 力矩裕度检查，过滤不可行课程样本。
• 阶段 5：把上体控制频率提高到下体两倍（SoFTA 式），对比末端加速度。
• 阶段 6：设计端杯子任务，要求 EE 垂直加速度 RMS < 1.5 m/s² 且峰值 < 3 m/s²。
• 阶段 7：输出 monolithic 单策略、FALCON 双策略、FALCON+SoFTA 三种配置的定量对比表。

---

本章小结

知识点	核心内容	与其他知识的关系
力敏感任务特性	外力是任务变量，不是扰动	区别于普通行走控制
阻抗控制理论	弹簧-阻尼模型 → 笛卡尔阻抗 → 无源性证明	力控的理论基础
无源性与能量储罐	被动性保证稳定性，能量储罐处理变阻抗	控制理论核心概念
FALCON 双策略	上下体空间解耦，共享观测，独立奖励	解决梯度冲突问题
力课程学习	渐进增加外力，力矩可行性检查	安全的课程学习设计
触觉传感集成	力传感器信号流和无传感器的力估计	闭环力控的硬件基础
SoFTA 频率解耦	上体高频、下体低频，训练必须模拟异步	端杯子等精细任务
双臂协调	闭链约束、运动力与内力分解	双手搬运任务
HOMIE 同构遥操作	关节到关节映射，无需 IK	降低遥操作延迟和误差
BFM-Zero 行为基础模型	Forward-Backward 表示，零样本任务适应	减少手工奖励设计

---

巩固卡片

卡片 01：末端外力与全身耦合

• 问题：为什么手受力会影响脚的稳定性？
• 直觉：力通过动力学方程 \(J_{ee}^T f_{ee}\) 影响所有关节加速度，包括腿部关节。
• 数学抓手：\(M\dot{v}+h=S^T\tau+J_c^T\lambda+J_{ee}^T f_{ee}\)
• 工程接口：全身动力学求解器（如 Pinocchio RNEA）
• 易错判断：只做上体 IK 不让下体感知外力，等于让下体策略面对未知扰动。
• 自测动作：计算 30 N 水平推力对 ZMP 的影响。

卡片 02：阻抗控制与无源性

• 问题：阻抗控制为什么能安全地接触物体？
• 直觉：弹簧-阻尼模型提供柔顺的力-位移关系。无源性保证系统不会自己产生能量。
• 数学抓手：\(V(t)-V(0)\leq\int_0^t f^T\dot{x}\,d\tau\)
• 工程接口：\(K \succeq 0, D \succeq 0\) 保证无源性
• 易错判断：阻抗控制不等于力控制——阻抗定义力-位移关系，力控制直接控制力。
• 自测动作：对一个简单系统数值验证无源性。

卡片 03：FALCON 双策略

• 问题：为什么拆上下体？
• 直觉：上体追末端精度、下体保步态稳定，两个目标的梯度会冲突。
• 数学抓手：\(L=L_{PPO}(\pi_u,r^u)+L_{PPO}(\pi_l,r^l)+\lambda L_{coord}\)
• 工程接口：共享观测 + 独立动作头 + 独立奖励
• 易错判断：双策略不是完全隔离——必须共享本体观测和命令信息。
• 自测动作：检查下体策略是否能看到末端目标。

卡片 04：力课程设计

• 问题：为什么渐进加力？
• 直觉：早期策略不够稳定，大外力直接导致大量摔倒，样本效率极低。
• 数学抓手：\(F_{max}(\rho) = F_{init}+(F_{target}-F_{init})\cdot\rho^\alpha\)
• 工程接口：力课程 + RNEA 力矩可行性检查
• 易错判断：课程太快（\(\alpha\) 太小）会让策略学不到，太慢会浪费训练时间。
• 自测动作：画力课程下的训练成功率曲线。

卡片 05：SoFTA 频率解耦

• 问题：为什么上体更高频？
• 直觉：末端稳定对加速度敏感，高频控制才能抑制振动。步态频率低，下体不需要那么快。
• 数学抓手：\(a_t^l=\pi_l(o_{k\lfloor t/k\rfloor})\)（action hold）
• 工程接口：训练中模拟异步更新，部署中双线程
• 易错判断：训练时不模拟频率差，部署时会出现非预期行为。
• 自测动作：对比同频和异频训练的 EE 垂直加速度。

卡片 06：双臂协调

• 问题：双臂抓一个物体有什么特殊约束？
• 直觉：物体把两个末端的相对位姿锁死了，形成闭链约束。
• 数学抓手：\(T_L^{-1}T_R = \text{const}\)
• 工程接口：运动力 vs 内力分解
• 易错判断：两个独立策略会产生巨大的内力而物体不动。
• 自测动作：推导双臂速度约束。

卡片 07：HOMIE 同构外骨骼

• 问题：同构映射相比 VR 手柄有什么优势？
• 直觉：关节到关节直接映射，省去 IK 的多解、奇异和延迟问题。
• 数学抓手：\(q_{robot} = q_{exo}\)
• 工程接口：外骨骼 + 手套 + 脚踏板
• 易错判断：同构外骨骼需要针对特定机器人设计，不通用。
• 自测动作：对比 VR 手柄映射和同构映射的末端位置误差。

---

本章常见误解汇总

误解	正确理解
阻抗控制 = 力控制	阻抗控制定义力和位移之间的动态关系，力控制直接调节力的大小
双策略 = 上下体隔离	双策略共享所有本体观测，只在动作输出和奖励上解耦
力课程越快收敛越快	课程过快导致早期大量摔倒，反而降低样本效率
力传感器越多越好	每增加一个传感器都引入新的标定需求、噪声源和 sim-to-real gap
变阻抗一定比固定阻抗好	变阻抗需要额外保证无源性，实现复杂度高，很多场景固定参数已足够
单策略永远不如双策略	简单任务（小力范围、低精度要求）中单策略更容易调试且性能接近
同构外骨骼可以通用	外骨骼的运动学必须匹配特定机器人，不同机器人需要不同外骨骼
BFM-Zero 不需要任何训练	无监督预训练阶段仍然需要大量计算资源，"零样本"指的是下游任务不需要额外训练

---

本章术语速查表

术语	英文	含义
阻抗控制	Impedance Control	控制位移和力之间动态关系的框架
无源性	Passivity	系统不产生能量的性质，保证与任何无源环境交互时稳定
能量储罐	Energy Tank	显式管理变阻抗系统能量预算的方法
双策略	Dual Policy	FALCON 中上体和下体使用独立策略网络的设计
频率解耦	Frequency Decoupling	SoFTA 中上体和下体使用不同控制频率的设计
力课程	Force Curriculum	训练中逐步增大外力幅值的课程学习策略
同构外骨骼	Isomorphic Exoskeleton	与机器人关节一一对应的遥操作外骨骼
技能混合	Skill Blending	多个技能策略按权重混合输出动作
行为基础模型	Behavioral Foundation Model	用无监督学习构建可控行为空间的大规模模型
内力	Internal Force	双臂抓握中两臂互相挤压的力，不导致物体运动
运动力	Motion Force	导致物体运动的合力
\(\mathcal{L}_2\) 增益	\(\mathcal{L}_2\) Gain	系统从输入到输出的最大放大倍数
临界阻尼	Critical Damping	无振荡的最快收敛阻尼设置
动作保持	Action Hold	低频策略在两次更新之间保持上一次输出不变
前馈力	Feedforward Force	主动施加的力（如推门），非由弹簧位移产生

---

延伸阅读

资料	难度	内容
Zhang et al., FALCON (arXiv:2505.06776, L4DC 2026)	⭐⭐⭐⭐	力自适应双策略 loco-manipulation
Li et al., SoFTA (arXiv:2505.24198, 2025)	⭐⭐⭐	端杯子不洒——慢快双代理控制
Bao et al., HOMIE (arXiv:2502.13013, RSS 2025)	⭐⭐⭐	同构外骨骼遥操作系统
Li et al., BFM-Zero (arXiv:2511.04131, 2025)	⭐⭐⭐⭐	行为基础模型，零样本控制
Hogan, "Impedance Control" (ASME 1985)	⭐⭐⭐	阻抗控制的开创性论文
Focchi et al., "Robot Impedance Control and Passivity Analysis" (2016)	⭐⭐⭐⭐	无源性证明的现代处理
Variable Impedance Control Survey (Frontiers 2020)	⭐⭐⭐	变阻抗控制综述
LeCAR-Lab/FALCON GitHub 仓库	⭐⭐	FALCON 代码实现
LeCAR-Lab/SoFTA GitHub 仓库	⭐⭐	SoFTA 代码实现
InternRobotics/OpenHomie GitHub 仓库	⭐⭐	HOMIE 开源代码
Liu et al., SkillBlender (CoRL 2024)	⭐⭐⭐	可组合技能的混合策略
OpenDriveLab, WholeBodyVLA (ICLR 2026)	⭐⭐⭐⭐	VLA + 全身控制的统一框架
Ott, "Cartesian Impedance Control of Redundant Robots" (2008)	⭐⭐⭐	冗余机器人笛卡尔阻抗控制的经典教材
Siciliano et al., "Robotics: Modelling, Planning and Control" Ch.9	⭐⭐⭐	力控制与阻抗控制的教科书级讲解

---

本章与后续章节的衔接

本章建立了力敏感 loco-manipulation 的完整理论和方法框架。接下来的学习路径如下：

后续方向	章节	与本章的关系
VLA 基础模型	96 章	WholeBodyVLA 路线需要本章的力控接口作为底层执行器
ASAP sim-to-real	94 章	FALCON 训练完成后需要 ASAP 方法迁移到真机
足式 RL 训练	足式/190	本章的力课程设计建立在足式 RL 训练的基础上
全身动力学与 WBC	92 章	本章使用的 \(M\dot{v}+h=S^T\tau+J_c^T\lambda+J_{ee}^T f_{ee}\) 在 92 章详细推导

学习路径建议

1. 如果你的目标是"在仿真中实现力敏感 loco-manipulation"→ 重点学习 95.2-95.6（阻抗理论 + FALCON + 力课程）
2. 如果你的目标是"在真机上部署"→ 还需要学习 94 章（ASAP sim-to-real）和 95.7（触觉传感集成）
3. 如果你的目标是"多任务泛化"→ 还需要学习 95.13-95.14（SkillBlender + BFM-Zero）和 96 章（基础模型）