本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
F06 变阻抗、无源性保证与碰撞安全¶
本章定位：F04 和 F05 讲了固定参数的阻抗/导纳控制——刚度 \(K_d\) 和阻尼 \(D_d\) 在运行中保持不变。但真实任务往往需要**在线调节阻抗参数**——接近时高刚度、接触时低刚度、操作中变刚度。参数时变引发了一个深刻的理论问题：刚度变化可能向系统注入能量，破坏无源性，导致不稳定。 本章系统化地解决这个问题——先讲变阻抗为什么危险，再给出能量罐的端口无源性方案和 Kronander-Billard 阻尼下界，然后转向碰撞安全的工业标准（ISO/TS 15066）和碰撞检测算法（De Luca 动量观测器），最后用 peg-in-hole 四阶段阻抗切换串联所有知识。
前置依赖：F01（无源性概念）、F03（阻抗控制稳定性证明）、F04（笛卡尔阻抗工程——CRISP error clipping）、F05（导纳控制——碰撞检测基础）
下游章节：F09（学习型力控——RL 输出变阻抗参数）、F10（高级力控专题）
建议用时：5 周（变阻抗理论 2 周 + 碰撞安全 1.5 周 + 实战 1.5 周）
前置自测 ⭐¶
📋 答不出 >= 2 题 → 先回前置章节复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 什么是无源性（passivity）？写出端口无源系统的耗散不等式 \(\dot{V} \leq y^T u\)。\(V\)、\(y\)、\(u\) 分别是什么？ | F01 第 3 节 |
| 2 | 固定参数阻抗控制器 \(M_d\ddot{e} + D_d\dot{e} + K_de = f_{ext}\) 的存储函数是什么？它是无源的吗？ | F03 第 3.4 节 |
| 3 | CRISP 的 error clipping 为什么**破坏**了无源性？ | F04 第 6.3 节 |
| 4 | De Luca 动量观测器的残差方程 \(\dot{r} = K_O(\tau_{ext} - r)\) 中，\(K_O\) 的物理含义是什么？ | F05 第 5.3 节 |
| 5 | ISO/TS 15066 的 PFL（Power and Force Limiting）模式下，最大允许接触力取决于哪些参数？ | ISO 15066 概述 |
本章目标¶
学完本章后，你应该能够：
- **解释**为什么变阻抗控制会向系统注入能量，并用能量跳变的数学公式证明
- 实现 Ferraguti-Secchi 能量罐方法，在 MuJoCo 中验证罐能量始终非负
- 应用 Kronander-Billard 阻尼下界条件，为给定的刚度变化率计算最小阻尼
- 推导 De Luca 动量观测器的完整数学，理解其在 Franka
tau_ext_hat_filtered中的实现 - 计算 ISO/TS 15066 PFL 模式下不同身体部位的最大安全 TCP 速度
- 设计 peg-in-hole 四阶段阻抗切换策略，并在仿真中实现
1. 变阻抗控制为什么危险——能量注入与无源性破坏 ⭐⭐¶
1.1 动机——固定阻抗的局限¶
回顾 F04 第 8 节：固定的阻抗参数在单一任务场景下工作良好，但多阶段任务（接近→接触→操作→退出）需要在线调节参数。
让我们用一个具体例子展示变阻抗的危险性。
场景：末端执行器正在接触环境，位姿偏差 \(\tilde{x} = 0.01\) m（10 mm）。在 \(t_0\) 时刻，控制器从低刚度 \(K_1 = 200\) N/m 跳变到高刚度 \(K_2 = 2000\) N/m。
能量分析：
跳变前的弹性储能：\(V_1 = \frac{1}{2} K_1 \tilde{x}^2 = \frac{1}{2} \times 200 \times 0.01^2 = 0.01\) J
跳变后的弹性储能：\(V_2 = \frac{1}{2} K_2 \tilde{x}^2 = \frac{1}{2} \times 2000 \times 0.01^2 = 0.1\) J
能量跳变：\(\Delta V = V_2 - V_1 = 0.09\) J
这 0.09 J 的能量从哪里来？不是从外力，不是从电机——而是"凭空"注入的。在物理上，这等价于在 \(t_0\) 时刻突然在弹簧中存入了额外的弹性能。这违反了无源性——系统在没有外部能量输入的情况下自发增加了内部能量。
类比：想象你在弹棉花糖枪。枪里有一根弹簧（刚度 \(K\)），你把棉花糖压在弹簧上（偏差 \(\tilde{x}\)）。如果你在棉花糖还压在弹簧上的时候**换了一根更硬的弹簧**，棉花糖会被弹得更远——但额外的能量不是你手施加的，而是换弹簧本身"注入"的。在阻抗控制中，"换弹簧"就是改变 \(K_d\)。
1.2 无源性破坏的严格数学分析¶
对于时变阻抗控制器 \(M_d\ddot{\tilde{x}} + D_d(t)\dot{\tilde{x}} + K_d(t)\tilde{x} = f_{ext}\)，自然的存储函数为：
对 \(V\) 求时间导数：
利用动力学方程 \(M_d\ddot{\tilde{x}} = f_{ext} - D_d\dot{\tilde{x}} - K_d\tilde{x}\)，代入第一项：
化简（\(-\dot{\tilde{x}}^T K_d \tilde{x}\) 和 \(+\tilde{x}^T K_d \dot{\tilde{x}}\) 对称项抵消）：
对于固定 \(K_d\)（\(\dot{K}_d = 0\)），最后一项消失，得到标准的无源性不等式：
因为 \(D_d \succ 0\) 保证了 \(\dot{\tilde{x}}^T D_d \dot{\tilde{x}} \geq 0\)（耗散项总为正）。
但当 \(K_d\) 时变时，额外项 \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\) 出现了：
| \(\dot{K}_d\) 的符号 | 额外项的符号 | 物理含义 | 对稳定性的影响 |
|---|---|---|---|
| \(\dot{K}_d \succ 0\)（升刚度） | 正 | 向系统注入能量 | 可能失稳 |
| \(\dot{K}_d \prec 0\)（降刚度） | 负 | 从系统提取能量 | 安全（更稳定） |
| \(\dot{K}_d = 0\)（固定） | 零 | 无额外能量交换 | 标准无源 |
本质洞察：升刚度注入能量，降刚度提取能量。这就是变阻抗控制的核心困难——你想在不同阶段使用不同的刚度，但升刚度时你在向系统"充电"，如果充的能量超过了阻尼耗散的能量，系统就会变得越来越活跃，直至失稳。
1.3 能量注入的三个来源——穷举式分类¶
系统化地分类，变阻抗控制中能量注入有三个来源：
| 来源 | 数学项 | 触发条件 | 危险程度 |
|---|---|---|---|
| 刚度升高 | \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\) | \(\dot{K}_d \succ 0\) 且 \(\tilde{x} \neq 0\) | 高 |
| 阻尼降低 | 阻尼耗散减少 | \(\dot{D}_d \prec 0\) | 中 |
| 参考轨迹跳变 | \(V\) 因 \(\tilde{x}\) 突变而跳变 | \(x_d\) 不连续变化 | 中（CRISP 已解决） |
其中**刚度升高**是最常见也最危险的——因为大多数任务确实需要在操作过程中升高刚度（如从柔顺接触切换到精密装配）。
1.4 历史演进——学界如何逐步解决这个问题¶
| 年份 | 方法 | 研究者 | 核心思路 | 局限 |
|---|---|---|---|---|
| 1985 | 固定阻抗 | Hogan | 不允许参数变化 → 无此问题 | 无法适应多阶段任务 |
| 2005 | 缓慢变化假设 | 多位研究者 | 假设 \(\|\dot{K}_d\| \ll 1\) | 无定量保证 |
| 2011 | PI\(^2\) 学习变刚度 | Buchli et al. | RL 学习最优刚度轮廓 | 无稳定性保证 |
| 2013 | 能量罐 | Ferraguti et al. | 虚拟能量储备+阀门 | 实现复杂 |
| 2016 | 阻尼下界条件 | Kronander & Billard | 闭式 \(D_d\) 下界 | 可能过度阻尼 |
| 2020 | 零空间无源性 | DLR | 冗余零空间也保证无源 | 仅限冗余臂 |
| 2023+ | CBF + 阻抗 | 多个团队 | Control Barrier Function 替代 | 实时性待验证 |
⚠️ 常见陷阱¶
🧠 思维陷阱：认为缓慢变化就安全
新手想法："只要 K 变化足够慢，就不会有问题"
实际上：缓慢变化**降低了**瞬时能量注入率 \(P_{inject} = \frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\)（因为 \(\dot{K}_d\) 小），但并不消除它。长时间的缓慢升刚度仍然会积累大量注入能量。是否安全取决于**注入能量与阻尼耗散能量的竞争**——如果阻尼耗散率 \(P_{diss} = \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\) 始终大于 \(P_{inject}\)，则安全。这正是 Kronander-Billard 条件量化的内容
💡 概念误区：认为降刚度总是安全的
降刚度确实从系统提取能量（\(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} < 0\)），但如果**同时降阻尼**，耗散能力也下降。在极端情况下，降刚度+降阻尼可能导致系统进入欠阻尼振荡。所以降刚度时必须**检查阻尼比是否仍然足够**
练习¶
- ⭐ 能量跳变计算：计算以下场景的能量跳变：\(K\) 从 100 N/m 跳到 1000 N/m，偏差 \(\tilde{x} = [0.02, 0.01, 0.005]^T\) m。能量跳变是多少焦耳？如果用 \(D = 2\sqrt{K}\) 的阻尼，需要多长时间才能耗散这些能量？
- ⭐⭐ 推导练习：从 \(V = \frac{1}{2}\dot{\tilde{x}}^T M_d \dot{\tilde{x}} + \frac{1}{2}\tilde{x}^T K_d(t) \tilde{x}\) 出发，在 \(M_d\) 也时变的情况下（\(\dot{M}_d \neq 0\)）推导 \(\dot{V}\)。额外出现了什么项？这对操作空间控制（\(M_d = \Lambda(q)\)）有什么意义？
- ⭐⭐ 仿真验证：在 Python 中模拟 1D 变阻抗系统，让 \(K\) 从 200 线性增加到 2000（用时 0.5 s）。绘制存储能量 \(V(t)\)、注入功率 \(P_{inject}(t)\) 和耗散功率 \(P_{diss}(t)\)。验证 \(V\) 是否单调增加。
2. 能量罐方法（Ferraguti-Secchi 2013） ⭐⭐⭐¶
2.1 动机——如何"安全地升刚度"¶
回顾 1.2 节：升刚度时会向系统注入能量 \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\)。如果这个能量没有来源（违反能量守恒），系统就失去了无源性。
Ferraguti, Secchi 和 Fantuzzi 2013（ICRA，约 400 次引用）提出了一个精妙的解决方案：引入一个虚拟的"能量储备罐"——当系统需要注入能量时（升刚度），从罐中取；当系统有多余能量时（阻尼耗散），向罐中存。罐空时，冻结刚度——不允许继续升高。
类比：能量罐就像一个"能量银行账户"。阻尼耗散是"收入"（存钱），升刚度需要"支出"（取钱）。银行规定：余额不能为负（\(T \geq 0\)）。当余额不够时，你不能升刚度——必须等阻尼"赚到"足够的能量后才能继续。
2.2 能量罐的数学框架¶
罐状态变量：\(x_t \in \mathbb{R}\)，罐能量 \(T = \frac{1}{2} x_t^2\)
总系统储能（含罐）：
在进入能量罐逻辑前，先保证 \(K_d\) 的矩阵性质：工程实现中应使用 \(K_{sym}=\frac{1}{2}(K_d+K_d^T)\) 作为储能项，并检查 \(K_{sym}\succeq 0\)；需要全维回位的轴取 \(K_{sym}\succ 0\)。如果 \(K_d\) 非对称或存在负特征值，\(\frac{1}{2}\tilde{x}^T K_d\tilde{x}\) 就不是合法的非负势能，能量罐也无法补救这个建模错误。
罐动力学：
其中： - \(P_{diss} = \dot{\tilde{x}}^T D_d \dot{\tilde{x}} \geq 0\)：阻尼耗散功率（"收入"） - \(P_{port}\)：需要从罐中取的功率（"支出"）——即升刚度需要的能量率 - \(\sigma \in \{0, 1\}\)：充能阀门（当 \(P_{diss} > 0\) 且 \(T < T_{max}\) 时 \(\sigma = 1\)） - \(u \in \{0, 1\}\)：放能阀门（连续时间中当需要能量且 \(T > T_{min}\) 时 \(u = 1\)；离散实现中必须检查本周期扣除后 \(T^+ = T - P_{port}\Delta t \geq T_{min}\)）
关键约束：\(T \geq 0\)（罐能量永远非负）
当需要升刚度时： - 如果 \(T - P_{port}\Delta t \geq T_{min}\)：允许升 \(K_d\)，本周期所需能量从罐中取（\(u = 1\)） - 如果 \(T - P_{port}\Delta t < T_{min}\)：冻结 \(K_d\)（不允许升高），等待罐充能
无源性证明——完整 Lyapunov 分析：
这个证明是能量罐方法的核心——让我们从头到尾严格推导，不跳过任何一步。
Step 1：定义扩展存储函数
包含能量罐的扩展存储函数为：
其中 \(T = \frac{1}{2}x_t^2 \geq 0\)。关键设计目标是让 \(\dot{H}\) 满足无源性不等式。
Step 2：计算 \(\dot{H}\)
利用动力学方程 \(M_d \ddot{\tilde{x}} = f_{ext} - D_d \dot{\tilde{x}} - K_d \tilde{x}\) 代入第一项：
展开并合并（\(-\dot{\tilde{x}}^T K_d \tilde{x}\) 与 \(+\tilde{x}^T K_d \dot{\tilde{x}}\) 抵消，因为 \(K_d\) 对称）：
Step 3：设计 \(\dot{T}\) 以抵消能量注入项
关键设计：让罐的能量变化率**恰好吸收**刚度变化注入的能量，即：
其中 \(\beta \in [0, 1]\) 控制阻尼耗散有多少比例流入罐中。
将这个设计代入 \(\dot{H}\)：
刚度注入项完全抵消：
由于 \(D_d \succ 0\) 且 \(0 \leq \beta < 1\)：
这就是标准的无源性不等式——系统以 \((\dot{\tilde{x}}, f_{ext})\) 为输入-输出对是无源的。
Step 4：约束条件 \(T \geq 0\) 的实现
上述设计在数学上完美，但工程上有一个约束：\(T \geq 0\)。当 \(\dot{K}_d \succ 0\)（升刚度）时，\(-\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} < 0\)，罐能量在减少。如果减到零，就不能继续升刚度了。
实现方式是引入**阀门函数** \(\sigma, u\)：
其中： - 当 \(P_{port} > 0\) 且 \(T - P_{port}\Delta t \geq T_{min}\)：\(u = 1\)（允许本周期升刚度，从罐取能量） - 当 \(P_{port} > 0\) 且 \(T - P_{port}\Delta t < T_{min}\)：\(u = 0\)，同时冻结 \(K_d\)（\(\dot{K}_d := 0\)） - 当 \(T < T_{max}\)：\(\sigma = 1\)（允许充罐） - 当 \(T \geq T_{max}\)：\(\sigma = 0\)（罐满，停止充罐）
冻结 \(K_d\) 后，\(\dot{K}_d = 0\)，无源性不等式自动满足（回到固定阻抗的情况）。
本质洞察：能量罐方法的精髓在于**将能量守恒从全局性质变为可管理的账本**。传统方法要求"系统总能量不增"——这是一个全局约束，难以工程化。能量罐将其转化为"罐余额不为负"——这是一个局部状态约束，可以用简单的 if-else 实现。代价是引入了保守性——罐空时冻结 \(K_d\)，可能暂时牺牲任务性能。
直觉上：升刚度注入的能量不是"凭空"出现的，而是从罐中**转移**过来的——总能量 \(H\) 没有增加，只是在子系统之间重新分配。
串联 PC 的符号约定：本章把 \(P_{inject}>0\) 定义为"变刚度环节向机器人-环境端口注入能量"，因此能量罐需要扣减；\(P_{diss}>0\) 定义为"阻尼耗散、可按比例回收到罐"。如果把 passivity controller 串在另一个控制器之后，必须先统一端口功率 \(P=y^Tu\) 的正方向，再决定是扣罐还是充罐。不要把"机器人对环境做功"和"环境对机器人做功"混用，否则 PC 会在该耗能时反而放能。
TDPA 跳转：如果问题来自采样、通信延迟或遥操作通道，而不是变刚度本身，优先使用 Time Domain Passivity Approach（TDPA）的 Passivity Observer / Passivity Controller：按离散端口功率累积能量，能量赤字时注入附加阻尼。本章的能量罐更适合管理 \(K_d(t)\) 的能量预算；TDPA 更适合监控任意串联系统的端口能量流。实现细节见 D07「TDPA 与工程实现」；本章第 8 节的能量监测框架可复用，但功率符号必须按 TDPA 端口定义重新确认。
2.3 工程实现¶
能量罐只能约束"改变合法刚度矩阵需要多少能量"，不能修复非法的刚度输入。工程实现中应先对 K_current 和 K_target 做三件事：(1) 对称化并投影到 PSD/SPD；(2) 限制特征值范围，避免策略或参数服务器给出过软/过硬的刚度；(3) 限制每个周期的刚度变化率，再把受限后的 \(\dot{K}_d\) 交给能量罐记账。下面示例按平移块和转动块分别处理；如果使用完整 6D 耦合刚度，应先做单位归一化或只在同量纲子块内做谱投影。
struct StiffnessSafetyLimits {
double k_trans_min = 0.0; // PSD；若要求全维回位，可设为小正数
double k_trans_max = 3000.0; // N/m
double k_rot_min = 0.0; // Nm/rad
double k_rot_max = 300.0;
double k_trans_rate_max = 8000.0; // N/m/s
double k_rot_rate_max = 800.0; // Nm/rad/s
};
template <int N>
Eigen::Matrix<double, N, N> project_symmetric_block(
const Eigen::Matrix<double, N, N>& K,
double lambda_min, double lambda_max) {
Eigen::Matrix<double, N, N> K_sym = 0.5 * (K + K.transpose());
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>> es(K_sym);
Eigen::Matrix<double, N, 1> lambda =
es.eigenvalues().cwiseMax(lambda_min).cwiseMin(lambda_max);
return es.eigenvectors() * lambda.asDiagonal() * es.eigenvectors().transpose();
}
template <int N>
Eigen::Matrix<double, N, N> limit_block_rate(
const Eigen::Matrix<double, N, N>& K_current,
const Eigen::Matrix<double, N, N>& K_target,
double lambda_rate_max,
double dt) {
Eigen::Matrix<double, N, N> dK_raw = K_target - K_current;
Eigen::Matrix<double, N, N> dK = 0.5 * (dK_raw + dK_raw.transpose());
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>> es(dK);
const double max_step = lambda_rate_max * dt;
Eigen::Matrix<double, N, 1> d_lambda =
es.eigenvalues().cwiseMax(-max_step).cwiseMin(max_step);
return K_current + es.eigenvectors() * d_lambda.asDiagonal() * es.eigenvectors().transpose();
}
Eigen::Matrix<double,6,6> sanitize_stiffness(
const Eigen::Matrix<double,6,6>& K,
const StiffnessSafetyLimits& limits) {
Eigen::Matrix<double,6,6> K_safe = Eigen::Matrix<double,6,6>::Zero();
K_safe.topLeftCorner<3,3>() = project_symmetric_block<3>(
K.topLeftCorner<3,3>(), limits.k_trans_min, limits.k_trans_max);
K_safe.bottomRightCorner<3,3>() = project_symmetric_block<3>(
K.bottomRightCorner<3,3>(), limits.k_rot_min, limits.k_rot_max);
return K_safe;
}
Eigen::Matrix<double,6,6> limit_stiffness_rate(
const Eigen::Matrix<double,6,6>& K_current,
const Eigen::Matrix<double,6,6>& K_target,
const StiffnessSafetyLimits& limits,
double dt) {
Eigen::Matrix<double,6,6> K_limited = Eigen::Matrix<double,6,6>::Zero();
K_limited.topLeftCorner<3,3>() = limit_block_rate<3>(
K_current.topLeftCorner<3,3>(), K_target.topLeftCorner<3,3>(),
limits.k_trans_rate_max, dt);
K_limited.bottomRightCorner<3,3>() = limit_block_rate<3>(
K_current.bottomRightCorner<3,3>(), K_target.bottomRightCorner<3,3>(),
limits.k_rot_rate_max, dt);
return K_limited;
}
// 能量罐实现（C++，可在 FCI 回调中使用）
struct EnergyTank {
double x_t; // 罐状态变量
double T_min = 0.01; // 最小能量 [J]（安全下限）
double T_max = 10.0; // 最大能量 [J]（防止无限积累）
double T_init = 1.0; // 初始能量 [J]
EnergyTank()
: x_t(std::sqrt(2.0 * std::max(T_init, T_min))) {}
double energy() const { return 0.5 * x_t * x_t; }
// 尝试升刚度——返回是否允许
// P_diss: 阻尼耗散功率 [W]（正值）
// P_need: 升刚度需要的功率 [W]（正值）
// dt: 时间步长 [s]
bool update(double P_diss, double P_need, double dt) {
double T_current = std::min(T_max, std::max(T_min, energy()));
x_t = std::sqrt(2.0 * T_current);
// Step 1: 充能——阻尼耗散的部分存入罐
if (P_diss > 0 && T_current < T_max) {
T_current = std::min(T_max, T_current + P_diss * dt);
x_t = std::sqrt(2.0 * T_current);
}
// Step 2: 放能——升刚度所需能量从罐中取
if (P_need > 0) {
const double E_need = P_need * dt;
const double T_after = T_current - E_need;
// 只允许扣减到 T_min，不能把安全余量也花掉。
if (T_after >= T_min) {
x_t = std::sqrt(2.0 * T_after);
return true; // 允许升刚度
} else {
return false; // 余额不足 → 冻结 K_d
}
}
return true; // 不需要放能（K 不变或降 K）
}
};
// 在 1 kHz 控制循环中使用
void control_callback(/* ... */) {
// 1. 计算导纳/阻抗正常输出
// ...
// 2. 先投影并限幅 K，再计算期望的刚度变化。
// 能量罐从这里开始只处理合法的 PSD/SPD 刚度路径。
StiffnessSafetyLimits limits;
K_current = sanitize_stiffness(K_current, limits);
Eigen::Matrix<double,6,6> K_target_projected =
sanitize_stiffness(K_target, limits);
const double beta = std::min(dt / transition_time, 1.0);
Eigen::Matrix<double,6,6> K_target_smooth =
K_current + beta * (K_target_projected - K_current);
Eigen::Matrix<double,6,6> K_target_safe =
sanitize_stiffness(K_target_smooth, limits);
K_target_safe =
limit_stiffness_rate(K_current, K_target_safe, limits, dt);
Eigen::Matrix<double,6,6> dK = K_target_safe - K_current;
// 3. 计算能量需求
Eigen::Matrix<double,6,6> K_dot_cmd = dK / dt;
Eigen::Matrix<double,6,6> K_dot_sym =
0.5 * (K_dot_cmd + K_dot_cmd.transpose());
double P_need = 0.0;
// 不能用 dK.trace() 判断升刚度；罐只关心当前误差方向上的注入功率。
const double P_inject_scalar =
(error.transpose() * K_dot_sym * error)(0, 0);
double P_inject = 0.5 * P_inject_scalar;
P_need = std::max(0.0, P_inject);
// 4. 计算阻尼耗散
Eigen::Matrix<double,6,1> xdot = jacobian * dq;
double P_diss = (xdot.transpose() * damping * xdot)(0, 0);
// 5. 能量罐判断
bool allowed = tank.update(P_diss, P_need, dt);
if (allowed) { // 允许，或本周期没有正的能量注入
K_current += K_dot_cmd * dt;
K_current = sanitize_stiffness(K_current, limits);
}
// else: K_current 保持不变（冻结）
}
上面用的是**当前误差方向上的能量注入功率**
不要用 dK.trace() > 0 判断"升刚度"：trace 只看所有特征值的和，既可能漏掉某个方向的正能量注入，也可能把与当前误差方向无关的变化误判为危险。若需要方向无关的保守判据，可以检查 \(\lambda_{max}(\operatorname{sym}(\dot{K}_d))\)：当它不大于 0 时，对任意 \(\tilde{x}\) 都不会注入能量；当它大于 0 时，可用 \(\frac{1}{2}\lambda_{max}\|\tilde{x}\|^2\) 估计最坏情况。只有在 \(K_d\) 保持对角、各轴解耦且代码明确逐轴计算时，才可以用 \(\sum_i \frac{1}{2}\dot{k}_i \tilde{x}_i^2\) 的 diagonal 简化。
2.4 工程参数选择¶
| 参数 | 推荐值 | 物理含义 | 设置过大/过小的后果 |
|---|---|---|---|
| \(T_{min}\) | 0.01-0.1 J | 罐的"最低余额" | 过大→频繁冻结；过小→安全裕度低 |
| \(T_{init}\) | 1-10 J | 启动时的"初始存款" | 过大→初始能量过多；过小→启动阶段升刚度受限 |
| \(T_{max}\) | 5-50 J | 罐的"容量上限" | 过大→罐永远填不满；过小→可用能量有限 |
参数选择经验法则：
即初始能量至少要覆盖"最大偏差下从最低刚度升到最高刚度"所需的能量。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：罐能量变为负值导致 NaN
错误做法：更新 \(x_t\) 后不检查 \(T \geq 0\)
现象：\(x_t\) 变为负数 → 后续计算 \(P/x_t\) 出现除以负数 → NaN 传播 → 控制器崩溃
根本原因：离散时间中，\(P_{need} \cdot dt\) 可能大于当前 \(T\)——一个时间步"取出"了超过余额的能量
正确做法：每次更新后
if (x_t < 0) x_t = 0;💡 概念误区：认为能量罐方法"限制了性能"
新手想法："罐空时冻结 K_d，这不就意味着控制器在关键时刻不能升刚度了吗？"
实际上：罐的设计目标是让**大多数时间**都有足够的能量。阻尼在每个控制周期都在"充罐"（\(P_{diss} > 0\)），只有当升刚度的速率远超阻尼耗散时才会触发冻结。合理的 \(T_{init}\) 和 \(T_{max}\) 可以覆盖 99%+ 的正常操作场景
练习¶
- ⭐⭐ 能量罐仿真：在 Python/MuJoCo 中实现带能量罐的 1D 变阻抗控制：\(K_d\) 在 100-2000 N/m 之间按正弦变化。记录罐能量 \(T(t)\)，验证 \(T \geq 0\) 始终成立。
- ⭐⭐ 参数敏感性：在上述仿真中，分别测试 \(T_{init} = 0.1, 1.0, 10.0\) J。记录每组的"冻结次数"（罐空导致不能升刚度的次数）。什么初始能量最合适？
- ⭐⭐⭐ 与 CRISP clipping 对比：在 MuJoCo Franka 中分别实现能量罐和 CRISP 的 error clipping。让 \(K_d\) 从 200 跳到 2000 N/m。对比两种方法的安全保证和跟踪性能。
3. Kronander-Billard 阻尼条件（2016） ⭐⭐⭐¶
3.1 动机——一个更简单的替代方案¶
能量罐方法功能强大但实现复杂（需维护罐状态变量、阀门逻辑）。Kronander 和 Billard 2016（IEEE T-RO, 32(5):1298-1305，约 500 次引用）提出了一个更简洁的方案：不去补偿升刚度的能量注入，而是保证阻尼的耗散率始终大于注入率。
直觉上：与其给系统装一个"能量银行"来管理能量流动，不如直接**确保系统的"能量消耗"永远大于"能量注入"**——这样系统的总能量只能递减，自然稳定。
需要注意：KB 条件是对零外力误差动力学的指数稳定充分条件，工程上常用作变阻抗的阻尼下界设计；它不等同于能量罐那种以 \((\dot{\tilde{x}}, f_{ext})\) 为端口的无源性保证。若系统还要面对未知环境端口、通信延迟或上层策略注入能量，仍需要能量罐、TDPA 或其他运行时能量监控。
3.2 核心定理¶
定理（Kronander-Billard 2016, IEEE T-RO 32(5), Theorem 1）：对 \(M_d \ddot{\tilde{x}} + D_d(t)\dot{\tilde{x}} + K_d(t)\tilde{x} = 0\)，若对所有 \(t\) 满足以下矩阵不等式：
其中 \(\alpha \leq \min_t \frac{\lambda_{\min}(D_d(t))}{\lambda_{\max}(M_d)}\)，\(\lambda_{\min}\)、\(\lambda_{\max}\) 分别表示最小和最大特征值，
则系统指数稳定，Lyapunov 函数 \(V(t) \leq V(0) e^{-2\alpha t}\)。
注意：当 \(D_d\) 为常数（\(\dot{D}_d = 0\)）时，条件简化为 \(\dot{K}_d(t) \preceq 2\alpha K_d(t)\)。
完整证明：
Step 1：选择 Lyapunov 函数
选择增广 Lyapunov 函数 \(V_a = V + \alpha \dot{\tilde{x}}^T \tilde{x}\)，其中 \(V = \frac{1}{2}\dot{\tilde{x}}^T M_d \dot{\tilde{x}} + \frac{1}{2}\tilde{x}^T K_d(t) \tilde{x}\) 是标准存储函数，\(\alpha > 0\) 是待定参数。增广项 \(\alpha \dot{\tilde{x}}^T \tilde{x}\) 引入了位移-速度的交叉耦合，使得分析中可以同时利用两者的信息。
Step 2：计算 \(\dot{V}_a\) 并利用动力学方程
对增广项：\(\frac{d}{dt}(\alpha \dot{\tilde{x}}^T \tilde{x}) = \alpha \ddot{\tilde{x}}^T \tilde{x} + \alpha \dot{\tilde{x}}^T \dot{\tilde{x}}\)
代入 \(M_d \ddot{\tilde{x}} = -D_d \dot{\tilde{x}} - K_d \tilde{x}\) 后整理，得到包含 \(\dot{K}_d\)、\(\dot{D}_d\)、\(K_d\) 的二次型。
Step 3：充分条件
要求 \(\dot{V}_a + 2\alpha V_a \leq 0\)，整理后对速度和位移的二次型同时非正的充分条件为：
第一个条件确保速度二次型为负半定（阻尼主导惯量），第二个条件确保位移二次型为负半定。
Step 4：稳定性结论
由 \(V_a\) 与 \(V\) 的等价性（对足够小的 \(\alpha\)，\(V_a\) 正定），可得 \(V(t) \leq c \cdot V(0) e^{-2\alpha t}\)——系统**指数稳定**，衰减率为 \(\alpha\)。\(\alpha\) 越大要求阻尼越大。\(\square\)
工程形式（常数阻尼 \(\dot{D}_d = 0\)，对角阵简化）：
对于对角阵 \(K_d = \text{diag}(k_i)\)，\(D_d = \text{diag}(d_i)\)（\(D_d\) 为常数），条件简化为：
这允许 \(\dot{k}_i > 0\)（升刚度），只要升刚度的速率不超过 \(2\alpha k_i\)（与当前刚度成正比）。\(\alpha\) 越大要求更大的阻尼。
更实用的工程形式（逐轴、量纲自洽）：
对第 \(i\) 个对角轴，\(k_i\) 的单位是 N/m，\(m_i\) 的单位是 kg，\(d_i\) 的单位是 Ns/m。若刚度正在上升，先由
得到满足 \(\dot{k}_i \le 2\alpha k_i\) 所需的最小衰减率；再由 \(\alpha \le d_i/m_i\) 得到阻尼下界：
同时还要满足常规阻尼比要求：
所以工程上取：
这里 \(k_{i,min}\) 是过渡区间内的最小正刚度，避免在接近零刚度时把允许增长率估得过大。注意不能把 \(\dot{k}/(2\alpha)\) 直接当成阻尼：它的量纲是 N/m（刚度），不是 Ns/m（阻尼）。
3.3 与能量罐的系统性对比¶
| 特性 | 能量罐（Ferraguti） | KB 条件（Kronander-Billard） |
|---|---|---|
| 保证性质 | 端口无源性（罐能量不为负） | 零输入指数稳定/阻尼下界；不是一般端口无源性证明 |
| 类型 | 在线状态监控 | 离线设计约束 |
| 实现复杂度 | 高（需维护罐状态） | 低（只约束 \(D_d\)） |
| 保守性 | 低（允许更大刚度变化） | 中（可能过度阻尼） |
| 适用场景 | 大范围快速变阻抗 | 缓慢渐变阻抗 |
| 在线计算量 | 中（每周期更新罐） | 零（设计阶段完成） |
| RL 兼容性 | 好（RL 可任意输出 K） | 差（RL 必须同时输出满足条件的 D） |
反事实推理：如果同时使用能量罐和 KB 条件会怎样？这实际上是**最佳实践**——用 KB 条件指导参数设计（确保大多数时间不需要罐干预），用能量罐作为运行时安全网（处理 KB 条件覆盖不到的极端情况）。两层保护优于单层。
3.4 实用的阻尼下界计算¶
import numpy as np
def compute_damping_lower_bound(K_current, K_target, transition_time, M_d,
zeta_min=1.0):
"""
计算 Kronander-Billard 条件下的最小阻尼
参数：
K_current: 当前刚度 [N/m]
K_target: 目标刚度 [N/m]
transition_time: 过渡时间 [s]
M_d: 虚拟质量 [kg]
zeta_min: 最低阻尼比
"""
# 刚度变化率（假设线性过渡，保留符号）
K_dot = (K_target - K_current) / transition_time
K_dot_up = max(0.0, K_dot)
K_path_min = max(min(K_current, K_target), 1e-6)
# 临界阻尼要求
K_max = max(K_current, K_target)
D_critical = 2 * zeta_min * np.sqrt(K_max * M_d)
# KB 条件要求：alpha_req = Kdot/(2K)，再用 D >= alpha*M 转为阻尼
alpha_req = K_dot_up / (2 * K_path_min) # [1/s]
D_kb = M_d * alpha_req # [Ns/m]
# 取两者的最大值
D_min = max(D_critical, D_kb)
print(f"K_dot = {K_dot:.1f} N/m/s")
print(f"alpha_req = {alpha_req:.3f} 1/s")
print(f"D_critical (zeta={zeta_min}) = {D_critical:.1f} Ns/m")
print(f"D_kb = {D_kb:.1f} Ns/m")
print(f"D_min = {D_min:.1f} Ns/m")
return D_min
# 示例 1：缓慢过渡（2 秒）
D_min_slow = compute_damping_lower_bound(200, 2000, 2.0, 5.0)
# K_dot = 900, alpha_req = 2.25, D_critical = 200, D_kb = 11.25 → D_min = 200
# 示例 2：快速过渡（0.2 秒）
D_min_fast = compute_damping_lower_bound(200, 2000, 0.2, 5.0)
# K_dot = 9000, alpha_req = 22.5, D_critical = 200, D_kb = 112.5 → D_min = 200
# → 这里仍由临界阻尼主导；若从更低刚度快速升高，KB 项会迅速增大
启示：KB 下界的大小取决于相对增长率 \(\dot{k}/k\)，不是只看 \(\dot{k}\) 的绝对值。低刚度附近快速升刚度最危险；高刚度区间同样的 \(\dot{k}\) 可能仍由临界阻尼主导。如果下界导致运动过慢，有三种替代：(1) 延长过渡时间；(2) 使用能量罐（允许更快变化）；(3) 在偏差 \(\tilde{x} \approx 0\) 时升刚度（此时注入能量 \(\approx 0\)，无需大阻尼）。
Kronander-Billard 条件的 C++ 实现¶
// 带 KB 条件自动阻尼调节的变阻抗控制器
class KBVariableImpedance {
public:
struct Params {
double kb_margin = 1.2; // KB 阻尼下界安全系数
double zeta_min = 1.0; // 最低阻尼比
double transition_time = 0.5; // 参数过渡时间 [s]
Eigen::Matrix<double,6,1> initial_stiffness;
Eigen::Matrix<double,6,1> virtual_mass;
Params() {
initial_stiffness << 400, 400, 400, 30, 30, 30;
virtual_mass << 5, 5, 5, 0.2, 0.2, 0.2;
}
};
private:
Params params_;
Eigen::Matrix<double,6,6> K_current_, K_target_;
Eigen::Matrix<double,6,6> D_current_;
Eigen::Matrix<double,6,6> M_d_;
double t_transition_ = 0.0;
double t_elapsed_ = 0.0;
public:
explicit KBVariableImpedance(const Params& p = Params()) : params_(p) {
configure(params_.initial_stiffness, params_.virtual_mass);
}
void configure(const Eigen::Matrix<double,6,1>& initial_stiffness,
const Eigen::Matrix<double,6,1>& virtual_mass) {
K_current_ = initial_stiffness.asDiagonal();
K_target_ = K_current_;
M_d_ = virtual_mass.asDiagonal();
D_current_.setZero();
for (int i = 0; i < 6; ++i) {
D_current_(i,i) = 2.0 * params_.zeta_min *
std::sqrt(K_current_(i,i) * M_d_(i,i));
}
t_elapsed_ = 0.0;
}
void set_target_stiffness(const Eigen::Matrix<double,6,6>& K_target) {
K_target_ = K_target;
t_elapsed_ = 0.0;
}
// 每控制周期调用：更新 K 和 D，保证 KB 条件
std::pair<Eigen::Matrix<double,6,6>, Eigen::Matrix<double,6,6>>
update(double dt) {
t_elapsed_ += dt;
double alpha = std::min(t_elapsed_ / params_.transition_time, 1.0);
// 线性插值更新 K
Eigen::Matrix<double,6,6> K_new =
(1.0 - alpha) * K_current_ + alpha * K_target_;
// 计算刚度变化率
Eigen::Matrix<double,6,6> K_dot = (K_new - K_current_) / dt;
// 这里只实现对角 K/M/D 的工程近似。
// 完整矩阵应检查 sym(K_dot) - 2*alpha*K 是否负半定。
Eigen::Matrix<double,6,6> D_kb;
D_kb.setZero();
// 临界阻尼条件：D >= 2*zeta*sqrt(K*M)
Eigen::Matrix<double,6,6> D_critical;
D_critical.setZero();
for (int i = 0; i < 6; ++i) {
double k_min_path = std::max(
std::min(K_current_(i,i), K_new(i,i)), 1e-6);
double k_dot_up = std::max(0.0, K_dot(i,i));
double alpha_req = k_dot_up / (2.0 * k_min_path); // [1/s]
D_kb(i,i) = params_.kb_margin * M_d_(i,i) * alpha_req;
D_critical(i,i) = 2.0 * params_.zeta_min *
std::sqrt(K_new(i,i) * M_d_(i,i));
}
// 取逐元素最大值
D_current_.setZero();
for (int i = 0; i < 6; ++i) {
D_current_(i,i) = std::max({
D_kb(i,i),
D_critical(i,i),
0.1 // 最小阻尼兜底
});
}
K_current_ = K_new;
return {K_current_, D_current_};
}
};
第三种方法的工程意义：如果能在位姿误差为零的时刻切换刚度（如在平衡点处切换），\(\tilde{x} = 0\) → \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} = 0\) → 无能量注入 → 无需额外阻尼。这就是为什么状态机设计中，尽量**在到达目标位姿后再切换阶段**。
⚠️ 常见陷阱¶
🧠 思维陷阱：认为 KB 条件和能量罐"一个比另一个好"
实际上：两者是**互补的**，解决同一问题的不同层面。KB 条件是"设计约束"——在设计阶段确定参数范围；能量罐是"运行时保护"——在运行阶段实时监控。最佳实践是**两者结合**
⚠️ 编程陷阱：忘记同步更新 D_d 和 K_d
错误做法：只更新 K_d，D_d 保持不变
现象：K 升高后阻尼比急剧下降 → 欠阻尼振荡
正确做法：每当 K 变化时，重新计算 \(D = 2\zeta\sqrt{KM}\)（至少）或使用 KB 条件的 \(D_{min}\)
练习¶
- ⭐⭐ 阻尼下界计算：一个 peg-in-hole 任务需要在 0.2 秒内将 \(K\) 从 100 升到 500 N/m，\(M_d = 3\) kg，最低阻尼比 \(\zeta_{min}=1\)。使用逐轴 KB 工程下界计算最小阻尼。这个阻尼下的阻尼比是多少？
- ⭐⭐ 对比实验：在 Python 中分别实现能量罐和 KB 条件的变阻抗控制器。让 \(K\) 在 10 ms 内从 100 跳到 2000（极端场景）。能量罐会怎样（冻结？）KB 条件要求的 \(D_d\) 是多少？
- ⭐⭐⭐ 跨章综合题：结合 F04 的 CRISP error clipping、本章的能量罐和 KB 条件，设计一个"三层安全"的变阻抗控制器：第一层 error clipping 限制最大力矩，第二层 KB 条件约束 \(D_d\)，第三层能量罐作为最后防线。在 MuJoCo 中实现并测试。
4. De Luca 动量观测器——无力传感器的力估计 ⭐⭐¶
4.1 动机——不是每个机器人都有 F/T 传感器¶
回顾 F05 第 5.3 节：碰撞检测和导纳控制需要外力信息。F/T 传感器提供最准确的外力测量，但价格昂贵（\(2000-\)5000）且需额外安装。De Luca 和 Mattone 2005（ICRA，约 700 次引用）的动量观测器提供了一个**纯软件**的替代方案——只需要关节编码器和力矩传感器（或电流传感器）。
4.2 完整数学推导¶
出发点：机器人的关节空间动力学方程
其中 \(\tau_{ext}\) 是外部扰动力矩（碰撞、人推等）。
Step 1：定义广义动量
Step 2：求 \(p\) 的时间导数
利用动力学方程代入 \(M\ddot{q}\)：
Step 3：利用 \(\dot{M} = C + C^T\) 简化
这是机器人动力学的一个基本性质——\(\dot{M} - 2C\) 是反对称矩阵，等价于 \(\dot{M} = C + C^T\)。
代入：
Step 4：定义残差
对 \(r\) 求导数（\(K_O\) 是对角增益矩阵）：
将 Step 3 的结果代入 \(\dot{p}\)：
这是一个**一阶线性低通滤波器**——\(r\) 以时间常数 \(1/K_O\) 收敛到真实外力矩 \(\tau_{ext}\)。
本质洞察：动量观测器的精妙之处在于——它**不需要测量 \(\ddot{q}\)**（关节加速度，噪声很大且难以测量）。通过利用广义动量 \(p = M\dot{q}\) 和 \(\dot{M} = C + C^T\) 的性质，只需要 \(q, \dot{q}, \tau\)（全部可直接测量）就能估计外力。这比直接从 \(\tau_{ext} = M\ddot{q} + C\dot{q} + g - \tau\)（需要 \(\ddot{q}\)）稳健得多。
Step 5：笛卡尔外力估计
其中 \(\hat{\tau}_{ext} = r\) 是动量观测器输出的外力矩估计（单位 Nm）。关节力矩与任务空间 wrench 的基本关系是：
因此 wrench 估计是在解 \(J^T \hat{F}_{ext} \approx r\)。对 Franka 这类 7 自由度机械臂，\(J \in \mathbb{R}^{6\times 7}\)，\(J^T \in \mathbb{R}^{7\times 6}\)。若 \(J\) 满行秩，Moore-Penrose 形式为：
接近奇异位形时使用阻尼最小二乘：
注意这里使用的是 \(6\times 6\) 的任务空间矩阵 \(J J^T\)；不要把 6x7 宽矩阵的伪逆写成需要求逆的 \((J^T J)^{-1}\)，因为 \(J^T J\) 是 \(7\times 7\) 且通常秩亏。
若需要动力学一致的映射（考虑惯量分布），定义操作空间惯量
以及动力学一致伪逆 \(\bar{J} = M^{-1}J^T\Lambda\)，则从关节残差到 wrench 的映射是：
这里**不要再额外乘一次 \(M^{-1}\)**：\(M^{-1}\) 已经包含在 \(\bar{J}^T = \Lambda J M^{-1}\) 中。对于非冗余且 \(J\) 可逆的机械臂，上述形式都退化为 \(\hat{F}_{ext} = J^{-T}r\)。
4.3 动量观测器的完整 C++ 实现¶
// De Luca 动量观测器完整实现
// 依赖：Pinocchio（动力学计算）、Eigen
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <Eigen/Dense>
#include <stdexcept>
class MomentumObserver {
public:
struct Config {
double K_O = 50.0; // 观测器增益 [1/s]
double dt = 0.001; // 采样周期 [s]
int n_joints; // 关节数
int ee_frame_id = -1; // 末端 frame id，用于残差到笛卡尔 wrench 的投影
};
private:
Config config_;
pinocchio::Model model_;
pinocchio::Data data_;
Eigen::VectorXd p_prev_; // 上一步广义动量
Eigen::VectorXd p0_; // 初始广义动量 p(0)
Eigen::VectorXd integral_; // 积分状态
Eigen::VectorXd residual_; // 残差（外力矩估计）
bool initialized_ = false;
public:
MomentumObserver(const pinocchio::Model& model, const Config& cfg)
: config_(cfg), model_(model), data_(model)
{
config_.n_joints = model_.nv;
p_prev_.setZero(config_.n_joints);
p0_.setZero(config_.n_joints);
integral_.setZero(config_.n_joints);
residual_.setZero(config_.n_joints);
}
// 每控制周期调用
// q: 关节位置, dq: 关节速度, tau: 施加力矩（电机力矩）
Eigen::VectorXd update(const Eigen::VectorXd& q,
const Eigen::VectorXd& dq,
const Eigen::VectorXd& tau) {
int nv = config_.n_joints;
// Step 1: 计算质量矩阵 M(q)
pinocchio::crba(model_, data_, q);
data_.M.triangularView<Eigen::StrictlyLower>() =
data_.M.transpose().triangularView<Eigen::StrictlyLower>();
// Step 2: 计算广义动量 p = M(q) * dq
Eigen::VectorXd p = data_.M * dq;
// Step 3: 计算科氏力矩阵 C(q,dq)*dq 和重力 g(q)
// RNEA 给出 M*ddq + C*dq + g，当 ddq=0 时给出 C*dq + g
Eigen::VectorXd ddq_zero = Eigen::VectorXd::Zero(nv);
Eigen::VectorXd nle = pinocchio::rnea(model_, data_, q, dq, ddq_zero);
// nle = C(q,dq)*dq + g(q)
// De Luca 残差方程（2006 ICRA, eq.14）：
// r(t) = K_O * [p(t) - p(0) - integral_0^t (tau + C^T(q,dq)*dq - g(q) + r) ds]
//
// 其中 C^T*dq 利用性质 dM/dt = C + C^T 计算：
// C^T*dq = dM/dt*dq - C*dq
// 但 RNEA 直接给出 nle = C*dq + g，不单独给 C^T*dq。
//
// 关键技巧：被积函数可以改写为
// tau + C^T*dq - g = tau - (C*dq + g) + (C + C^T)*dq
// = tau - nle + dM/dt * dq
//
// Pinocchio 提供 computeCoriolisMatrix(model, data, q, dq)
// 得到 C 矩阵，从而 C^T*dq = data.C.transpose() * dq
Eigen::MatrixXd C_mat = pinocchio::computeCoriolisMatrix(model_, data_, q, dq);
Eigen::VectorXd CtDq = C_mat.transpose() * dq;
// beta = tau + C^T*dq - g(q)
Eigen::VectorXd beta = tau + CtDq - pinocchio::computeGeneralizedGravity(model_, data_, q);
if (!initialized_) {
p0_ = p; // 保存初始动量 p(0)
initialized_ = true;
return residual_;
}
// Step 4: 更新积分（显式 Euler；工程实现可改为梯形积分）
integral_ += (beta + residual_) * config_.dt;
// Step 5: 计算残差
// r = K_O * (p - p(0) - integral)
// 等价于一阶低通：dr/dt = K_O * (tau_ext - r)
residual_ = config_.K_O * (p - p0_ - integral_);
return residual_; // 外力矩估计 τ_ext_hat [Nm]
}
// 将关节空间残差转换为笛卡尔力估计
Eigen::Matrix<double,6,1> cartesian_force_estimate(
const Eigen::VectorXd& q,
const Eigen::VectorXd& residual) {
// 计算末端 frame 的 Jacobian。不要用 model.njoints-1 猜最后一个关节：
// 末端工具、固定 frame 或虚拟关节都会让该假设失效。
if (config_.ee_frame_id < 0) {
throw std::runtime_error("MomentumObserver: ee_frame_id not configured");
}
pinocchio::computeJointJacobians(model_, data_, q);
pinocchio::updateFramePlacements(model_, data_);
Eigen::Matrix<double,6,Eigen::Dynamic> J(6, model_.nv);
pinocchio::getFrameJacobian(
model_, data_,
static_cast<pinocchio::FrameIndex>(config_.ee_frame_id),
pinocchio::LOCAL_WORLD_ALIGNED,
J);
// 6 x n Jacobian（Franka: 6 x 7）。解 J^T F_ext ~= r：
// (J J^T + lambda^2 I_6) F_ext = J r
// 这是适合宽矩阵 J 的任务空间阻尼最小二乘形式。
// F_ext 表达在 LOCAL_WORLD_ALIGNED：参考点是 ee_frame 原点，坐标轴与世界系对齐。
const double lambda = 1e-3;
Eigen::Matrix<double,6,6> A =
J * J.transpose() +
lambda * lambda * Eigen::Matrix<double,6,6>::Identity();
Eigen::Matrix<double,6,1> b = J * residual;
Eigen::Matrix<double,6,1> F_ext = A.ldlt().solve(b);
return F_ext;
}
void reset() {
p0_.setZero();
integral_.setZero();
residual_.setZero();
initialized_ = false;
}
};
使用示例：
// 在 1 kHz 控制循环中使用动量观测器
MomentumObserver::Config obs_config;
obs_config.K_O = 50.0; // 通用推荐增益
obs_config.dt = 0.001; // 1 kHz
obs_config.ee_frame_id = pinocchio_model.getFrameId("panda_hand");
MomentumObserver observer(pinocchio_model, obs_config);
// 控制循环
while (running) {
auto [q, dq, tau] = read_robot_state();
// 外力矩估计
Eigen::VectorXd tau_ext_hat = observer.update(q, dq, tau);
// 笛卡尔力估计
Eigen::Matrix<double,6,1> F_ext_hat =
observer.cartesian_force_estimate(q, tau_ext_hat);
// 碰撞检测
if (tau_ext_hat.norm() > collision_threshold) {
trigger_safety_stop();
}
}
4.4 Franka 的内部实现——tau_ext_hat_filtered¶
Franka Panda 的 FCI 内部实现了 De Luca 动量观测器，并通过 tau_ext_hat_filtered 字段暴露给用户。
| 参数 | Franka 内部值 | 用户可调？ |
|---|---|---|
| \(K_O\)（观测器增益） | ~50 1/s | 否 |
| 动力学模型 | 出厂标定精确参数 | 否 |
| 低通滤波 | 100 Hz 截止 | 否 |
| 工具负载补偿 | 通过 setLoad() |
是——必须正确设置 |
4.5 观测器参数 \(K_O\) 的设计权衡¶
\(K_O\) 决定了观测器的带宽和噪声特性：
| \(K_O\) | 带宽 | 延迟 | 噪声 | 适用 |
|---|---|---|---|---|
| 10 1/s | ~1.6 Hz | ~100 ms | 低 | 准静态力估计 |
| 50 1/s | ~8 Hz | ~20 ms | 中 | 通用推荐 |
| 100 1/s | ~16 Hz | ~10 ms | 高 | 快速碰撞检测 |
反事实推理：如果 \(K_O \to \infty\)，观测器带宽趋于无穷——\(r \to \tau_{ext}\) 即时，但噪声也趋于无穷（因为 \(\tau\) 和 \(M, C, g\) 的计算噪声被无穷放大）。如果 \(K_O \to 0\)，观测器完全不响应。\(K_O\) 的选择是**灵敏度 vs. 噪声**的 trade-off，与 F05 中 F/T 传感器的
filter_coefficient本质相同。
4.6 动量观测器 vs. F/T 传感器¶
| 维度 | 动量观测器 | F/T 传感器 |
|---|---|---|
| 硬件成本 | $0（纯软件） | \(2000-\)5000 |
| 精度 | ~1-5 N（取决于模型） | ~0.05-0.2 N |
| 安装 | 无需安装 | 需安装在法兰+标定 |
| 检测范围 | 整个机体（任意关节） | 仅传感器位置 |
| 模型依赖 | 强（需精确 \(M,C,g\)） | 无 |
| 适用 | 碰撞检测、粗力估计 | 精密力控、导纳控制 |
⚠️ 常见陷阱¶
⚠️ 编程陷阱：动量观测器的积分漂移
错误做法：在实现 \(\int_0^t (\cdot) d\tau\) 时用简单矩形积分且不重置
现象：即使没有外力，\(r\) 缓慢增长（积分漂移），导致碰撞检测误触发
正确做法：(1) 残差形式自带 \(-r\) 负反馈，天然抑制漂移；(2) 使用梯形积分提高精度；(3) 在确认无外力时定期重置
💡 概念误区：认为动量观测器和 F/T 传感器一样准确
动量观测器的精度**完全取决于动力学模型的准确性**。模型误差（关节摩擦、柔性传动、负载偏差）直接变成力估计偏差。对于碰撞检测（阈值 20-30 N）足够，对于精密力控（目标 5 N）不够
练习¶
- ⭐⭐ 动量观测器实现：在 MuJoCo Franka 中实现 De Luca 动量观测器。从不同方向施加已知外力（5 N, 10 N, 20 N），对比观测器估计与真实值。
- ⭐⭐ \(K_O\) 扫描：在上述实现中分别设 \(K_O = 10, 50, 100\) 1/s。施加 10 ms 脉冲力（模拟碰撞），绘制三组 \(r(t)\) 曲线。哪个最快检测碰撞？哪个噪声最大？
- ⭐⭐ 模型误差影响：故意在模型中引入 10% 质量误差，重新运行观测器。力估计偏差有多大？是否导致碰撞检测误触发？
5. ISO/TS 15066 碰撞安全标准 ⭐⭐¶
5.1 动机——法规是力控系统的"红线"¶
ISO/TS 15066:2016 是**协作机器人安全的技术规范**（Technical Specification），不是正式的国际标准（International Standard）。它是对 ISO 10218-1/2（工业机器人安全标准）的补充，为协作操作提供了具体的安全参数指南。尽管是技术规范，它在实践中被协作机器人行业（UR、Franka、Fanuc CRX 等）广泛遵循，其力/压力限值已成为产品认证的事实基准。对力控设计者，理解 15066 不是可选的——它定义了控制器**绝对不能超过的力和压力限制**。
5.2 PFL 模式安全阈值与三种碰撞模型¶
ISO/TS 15066 的 PFL（Power and Force Limiting）模式与力控最相关——它允许人和机器人在同一空间同时工作，但要求碰撞时的力和压力不超过阈值。
三种碰撞评估方法：
ISO/TS 15066 的 PFL 模式使用**力和压力限值**作为碰撞严重程度的评估指标，按 29 个身体区域分别规定阈值。评估区分两种碰撞类型：
| 碰撞类型 | 评估指标 | 物理含义 | 适用场景 |
|---|---|---|---|
| 准静态接触 | 峰值力 \(F_{qs}\) 和峰值压力 \(p_{qs}\) | 人被夹持时的持续载荷 | 人被夹在机器人与固定物体之间（clamping） |
| 瞬态接触 | 峰值力 \(F_{tr}\) 和峰值压力 \(p_{tr}\) | 自由碰撞的短时载荷 | 机器人碰到人后弹开/停止（transient） |
注意：ISO/TS 15066 不使用 HIC（Head Injury Criterion） 作为评估指标。HIC 源自汽车安全领域（FMVSS 208），部分早期文献曾讨论将其用于协作机器人，但 ISO/TS 15066 最终采用的是按身体区域划分的力/压力限值体系。瞬态接触的力限值通常为准静态值的 2 倍（基于生物力学疼痛阈值研究 Mainz-Dortmund 数据集），这已经隐式地考虑了碰撞持续时间的影响。
力和压力阈值的完整表格（Annex A）：
| 身体区域 | 准静态力 \(F_{qs}\) [N] | 准静态压力 \(p_{qs}\) [N/cm\(^2\)] | 瞬态力 \(F_{tr}\) [N] | 瞬态压力 \(p_{tr}\) [N/cm\(^2\)] |
|---|---|---|---|---|
| 前额 | 130 | 130 | 260 | 260 |
| 颞部 | 110 | 130 | 220 | 260 |
| 颌骨 | 65 | 110 | 130 | 220 |
| 胸骨 | 140 | 120 | 280 | 240 |
| 腹部 | 110 | 110 | 220 | 220 |
| 骨盆 | 210 | 180 | 420 | 360 |
| 上臂 | 150 | 190 | 300 | 380 |
| 前臂 | 190 | 230 | 380 | 460 |
| 手背 | 140 | 140 | 280 | 280 |
| 手指 | 140 | 300 | 280 | 600 |
| 大腿 | 220 | 250 | 440 | 500 |
| 小腿 | 220 | 180 | 440 | 360 |
准静态 vs. 瞬态的本质区别：
- 准静态（quasi-static）：人被夹在机器人和固定物体之间（最危险场景）→ 碰撞力**持续作用**→ 使用准静态阈值
- 瞬态（transient）：机器人碰到人后立即弹开/停止 → 碰撞力**仅短暂作用**（<500 ms）→ 瞬态阈值 = 2 \(\times\) 准静态
本质洞察：准静态碰撞是 ISO 15066 设计的核心目标——因为它是最危险的。当人被夹在机器人和固定物体（如桌面）之间时，即使力不大，持续的压迫也可能导致组织损伤。这就是为什么碰撞安全策略中，Comply（柔顺）和 Retract（回缩）远优于 Reflex（停止）——Reflex 后机器人保持位置，准静态力持续存在；而 Comply/Retract 主动减小力。
有效质量的计算——不只是"机器人质量"：
ISO 15066 的有效碰撞质量 \(m_{eff}\) 不是机器人的总质量，而是考虑了碰撞方向和构型的等效质量：
其中 \(v\) 是碰撞方向的单位向量，\(\Lambda = (JM^{-1}J^T)^{-1}\) 是操作空间惯量矩阵。这意味着： - 沿高惯量方向碰撞（如整条臂的纵向）→ \(m_{eff}\) 大 → 更危险 - 沿低惯量方向碰撞（如末端的横向）→ \(m_{eff}\) 小 → 更安全
Franka Panda 的有效末端质量约 2-8 kg（取决于构型和方向），远低于总质量 18 kg。
5.3 PFL 最大 TCP 速度计算¶
其中： - \(F_{max}\)：允许的最大碰撞力 - \(m_{eff} = \frac{1}{1/m_{robot} + 1/m_{human}}\)：有效碰撞质量 - \(k\)：接触刚度（皮肤+机器人结构）
数值示例（Franka Panda 碰撞手背）：
m_robot ≈ 4 kg（有效末端质量——取决于构型和方向）
m_human_hand ≈ 0.6 kg（手部有效质量）
→ m_eff = 1/(1/4 + 1/0.6) ≈ 0.52 kg
k ≈ 150 N/mm = 150000 N/m（皮肤+结构刚度）
F_max = 280 N（手背瞬态）
→ v_max = 280 / √(0.52 × 150000) = 280 / 279 ≈ 1.0 m/s
对阻抗控制的启示：如果阻抗控制器设定了 \(K_d = 500\) N/m，error clipping \(e_{max} = 0.05\) m，则**准静态**最大接触力 \(F_{max} = K_d \times e_{max} = 25\) N——低于大多数身体部位的准静态阈值。但这**不等于自动满足 PFL**，还需注意以下限定条件： - 瞬态碰撞**的峰值力取决于碰撞速度和有效质量（\(F_{peak} \approx v \sqrt{k \cdot m_{eff}}\)），与 \(K_d \times e_{max}\) 无直接关系。高速碰撞时即使低刚度阻抗也可能超标； - **压力限值**取决于接触面积，小面积硬接触（如棱边）即使力小也可能超过压力阈值； - 需对**每个可能碰撞的身体区域（ISO/TS 15066 Annex A 列出 29 个区域）分别验证力和压力限值； - 完整的 PFL 合规性评估需结合速度限制（Phase 1）和碰撞检测/反应（Phase 2-4），不能仅依赖阻抗参数设计。
5.4 安全停止策略回顾¶
回顾 F05 第 5.4 节的三种策略：Reflex（停止）、Retract（回缩）、Comply（柔顺）。
在 ISO 15066 的框架下，安全策略的选择还需要考虑：
| 场景 | 推荐策略 | 理由 |
|---|---|---|
| 瞬态碰撞（高速） | Reflex + Retract | 先停再退，减少准静态夹持风险 |
| 准静态碰撞（低速夹持） | Comply | 切换零力引导，人可推开机器人 |
| 检测到人进入协作区 | SSM（停止） | 预防性停止，最保守 |
⚠️ 常见陷阱¶
🧠 思维陷阱：认为满足 ISO 15066 就"绝对安全"
实际上：ISO 15066 的数据来自 Mainz 2011 年的 100 人疼痛阈值实验。Behrens & Elkmann 2022 用 112 人的新实验质疑了部分阈值。标准修订版预计 2026-2027 发布。此外 15066 不覆盖尖锐物体——尖锐工具即使力在阈值内，局部压力也可能超标
练习¶
- ⭐ PFL 计算表：给定 Franka Panda（有效质量 4 kg）、工具质量 1 kg、接触刚度 150 N/mm。计算 PFL 模式下与上表中所有身体部位接触的最大安全 TCP 速度。绘制完整表格。
- ⭐⭐ 阻抗参数与 ISO 合规：如果阻抗控制器使用 \(K_d = 1000\) N/m 和 \(e_{max} = 0.05\) m，最大接触力是多少？满足所有部位的准静态阈值吗？如果不满足，应降低 \(K_d\) 还是 \(e_{max}\)？分析两种选择对任务性能的影响。
- ⭐⭐ 安全策略仿真：在 MuJoCo 中实现 Reflex 和 Comply 两种停止策略。模拟机器人以 0.3 m/s 碰撞人手（弹性模型 \(K_e = 500\) N/m），记录最大接触力和持续时间。哪种策略更好？
6. CBF-based 变阻抗——前沿方法 ⭐⭐⭐⭐¶
6.1 动机——超越能量罐和 KB 条件¶
能量罐和 KB 条件是变阻抗安全设计的两大经典工具。但它们各有局限： - 能量罐在罐空时冻结 \(K_d\)——牺牲了任务性能 - KB 条件要求阻尼覆盖刚度的相对增长率（逐轴近似为 \(d_i \ge m_i\max(0,\dot{k}_i)/(2k_{i,min})\)，并叠加临界阻尼）——低刚度附近快速变化时可能**过度阻尼**
2023 年以来，多个研究组探索用 Control Barrier Function（CBF）替代传统的能量方法。CBF 的核心优势是：在保证安全的前提下，对性能的影响最小化。
6.2 CBF 安全约束的基本思想¶
回顾 F01 第 3 节的无源性概念：无源性要求 \(\dot{V} \leq y^T u\)（存储能量变化率不超过外部功率输入）。CBF 将这个全局约束转化为**逐点的不等式约束**，嵌入在线优化中。
CBF 的一般形式：定义安全集 \(\mathcal{C} = \{x : h(x) \geq 0\}\)，其中 \(h(x)\) 是障碍函数。系统安全等价于始终保持 \(h(x) \geq 0\)。CBF 条件为：
这保证了如果 \(h(x_0) \geq 0\)（初始安全），则 \(h(x(t)) \geq 0, \forall t\)（始终安全）。
在变阻抗中的应用：将能量罐的 \(T \geq 0\) 约束改写为 CBF 形式：
将此约束嵌入 QP（二次规划）中：
关键优势：CBF-QP 不会**冻结** \(K_d\)（像能量罐那样），而是找到**最接近期望值的安全 \(K_d\)。当能量充足时 \(K_d = K_d^{desired}\)（无影响）；当能量不足时 \(K_d\) 被微调到刚好满足安全约束——**最小干预原则。
6.3 CBF-based 变阻抗的实现框架¶
# CBF-based 变阻抗控制（概念实现）
import numpy as np
from scipy.optimize import minimize
class CBFVariableImpedance:
def __init__(self, gamma=1.0, T_min=0.01, T_max=10.0):
self.gamma = gamma # CBF 衰减率
self.T_min = T_min # 最小罐能量
self.T_max = T_max # 最大罐能量
self.T = 5.0 # 初始罐能量
self.K_current = np.diag([200]*3 + [10]*3)
def update(self, K_desired, D_nominal, x_tilde, x_dot_tilde, dt):
"""
计算满足 CBF 安全约束的最优 K_d 和 D_d
"""
# 阻尼耗散功率
P_diss = x_dot_tilde @ D_nominal @ x_dot_tilde
# 期望的刚度变化率
K_dot_desired = (K_desired - self.K_current) / dt
# 升刚度所需功率。完整矩阵先取对称部分；反对称部分不贡献二次型。
K_dot_sym = 0.5 * (K_dot_desired + K_dot_desired.T)
P_inject_signed = 0.5 * x_tilde @ K_dot_sym @ x_tilde
P_inject = max(0.0, P_inject_signed)
# CBF 约束：dT/dt >= -gamma * (T - T_min)
# dT/dt = -P_inject + beta * P_diss
# => -P_inject + beta*P_diss >= -gamma*(T - T_min)
# => P_inject <= beta*P_diss + gamma*(T - T_min)
P_budget = 0.5 * P_diss + self.gamma * (self.T - self.T_min)
if P_inject <= P_budget:
# 安全预算充足，使用期望刚度
K_new = K_desired
else:
# 需要缩减升刚度幅度
# 找到满足约束的最大 scale
if P_inject > 1e-8:
scale = P_budget / P_inject
scale = np.clip(scale, 0.0, 1.0)
else:
scale = 1.0
K_dot_safe = scale * K_dot_desired
K_new = self.K_current + K_dot_safe * dt
# 更新罐能量
K_dot_actual = (K_new - self.K_current) / dt
K_dot_actual_sym = 0.5 * (K_dot_actual + K_dot_actual.T)
P_actual_signed = 0.5 * x_tilde @ K_dot_actual_sym @ x_tilde
self.T += (-P_actual_signed + 0.5 * P_diss) * dt
# 数值裁剪不能越过 T_min；T_min 是保留安全余量，不是可消费余额。
self.T = np.clip(self.T, self.T_min, self.T_max)
# 更新 D 以满足临界阻尼（至少）
D_new = np.copy(D_nominal)
for i in range(6):
D_new[i,i] = max(D_nominal[i,i],
2.0 * np.sqrt(K_new[i,i] * 5.0))
self.K_current = K_new
return K_new, D_new, self.T
反事实推理：如果用纯 QP（每周期求解 6×6 的优化问题）而不是上述简化公式，实时性能如何？6 维 QP 在现代 CPU 上约需 5-20 \(\mu\)s（使用 OSQP），在 1 kHz 控制循环的 300 \(\mu\)s 预算内完全可行。但初始化和异常处理（QP 不可行）需要额外工程——这是 CBF 方法尚未广泛工业化的原因之一。
7. 碰撞后恢复策略——Reflex/Retract/Comply ⭐⭐¶
7.1 动机——碰撞检测之后做什么¶
回顾 F05 第 5.4 节：我们介绍了三种安全停止策略的概念。本节给出**完整的代码实现**和量化的性能对比。
碰撞后恢复策略的设计比碰撞检测更复杂——因为它涉及实时的控制模式切换、安全约束满足和任务恢复规划。
7.2 三种策略的完整实现¶
// 碰撞后恢复策略完整实现
#include <Eigen/Dense>
#include <chrono>
class CollisionRecovery {
public:
enum class Strategy { REFLEX, RETRACT, COMPLY };
enum class State { NORMAL, DETECTED, RECOVERING, RECOVERED };
struct Config {
Strategy strategy = Strategy::COMPLY;
double retract_distance = 0.05; // 回缩距离 [m]
double retract_velocity = 0.05; // 回缩速度 [m/s]
double comply_damping = 200.0; // 柔顺模式阻尼 [Ns/m]
double comply_mass = 3.0; // 柔顺模式虚拟质量 [kg]
double recovery_timeout = 5.0; // 恢复超时 [s]
double force_clear_threshold = 2.0; // 力清除阈值 [N]
};
private:
Config config_;
State state_ = State::NORMAL;
Eigen::Vector3d collision_direction_;
Eigen::Vector3d retract_start_;
double recovery_timer_ = 0.0;
// 存储碰撞前的控制参数（用于恢复）
Eigen::Matrix<double,6,6> pre_collision_K_;
Eigen::Matrix<double,6,6> pre_collision_D_;
public:
explicit CollisionRecovery(const Config& cfg = Config()) : config_(cfg) {}
struct RecoveryCommand {
bool override_controller; // 是否覆盖正常控制器
Eigen::Matrix<double,6,1> wrench; // 恢复期间的控制力
Eigen::Matrix<double,6,6> K_d; // 恢复期间的刚度
Eigen::Matrix<double,6,6> D_d; // 恢复期间的阻尼
State state;
};
RecoveryCommand update(
const Eigen::Vector3d& collision_dir,
double collision_force,
const Eigen::Matrix<double,6,1>& current_wrench,
const Eigen::Vector3d& current_position,
double dt)
{
RecoveryCommand cmd;
cmd.override_controller = false;
cmd.wrench.setZero();
cmd.K_d.setZero();
cmd.D_d.setZero();
switch (state_) {
case State::NORMAL:
cmd.state = State::NORMAL;
return cmd;
case State::DETECTED:
// 刚检测到碰撞，根据策略初始化恢复
collision_direction_ = collision_dir;
retract_start_ = current_position;
recovery_timer_ = 0.0;
state_ = State::RECOVERING;
[[fallthrough]];
case State::RECOVERING:
recovery_timer_ += dt;
cmd.override_controller = true;
switch (config_.strategy) {
case Strategy::REFLEX:
// 立即冻结位置（零速度命令）
cmd.K_d = 2000.0 * Eigen::Matrix<double,6,6>::Identity();
cmd.D_d = 200.0 * Eigen::Matrix<double,6,6>::Identity();
cmd.wrench.setZero();
break;
case Strategy::RETRACT: {
// 沿碰撞反方向匀速回缩
double retracted =
(current_position - retract_start_).dot(-collision_direction_);
if (retracted < config_.retract_distance) {
// 继续回缩
Eigen::Vector3d v_retract =
-collision_direction_ * config_.retract_velocity;
cmd.wrench.head(3) = config_.comply_damping * v_retract;
cmd.K_d.setZero(); // 无弹簧
cmd.D_d = config_.comply_damping *
Eigen::Matrix<double,6,6>::Identity();
} else {
// 回缩完成
state_ = State::RECOVERED;
}
break;
}
case Strategy::COMPLY:
// 切换到零力引导模式
cmd.K_d.setZero(); // K = 0：无回复力
for (int i = 0; i < 6; ++i) {
cmd.D_d(i,i) = 2.0 * config_.comply_mass; // 过阻尼
}
cmd.wrench.setZero();
// 检查是否可以恢复（外力消失）
if (current_wrench.head(3).norm() <
config_.force_clear_threshold) {
state_ = State::RECOVERED;
}
break;
}
// 超时强制恢复
if (recovery_timer_ > config_.recovery_timeout) {
state_ = State::RECOVERED;
}
cmd.state = State::RECOVERING;
return cmd;
case State::RECOVERED:
// 恢复完成，逐步过渡回正常控制
cmd.override_controller = false;
cmd.state = State::RECOVERED;
// 外部控制器负责平滑过渡回正常参数
state_ = State::NORMAL;
return cmd;
}
return cmd;
}
void trigger_collision(const Eigen::Vector3d& direction) {
collision_direction_ = direction.normalized();
state_ = State::DETECTED;
}
State get_state() const { return state_; }
};
7.3 三种策略的量化对比¶
在 MuJoCo 仿真中对三种策略进行量化评估（Franka Panda 以 0.3 m/s 碰撞弹性物体 \(K_e = 5000\) N/m）：
| 指标 | Reflex（停止） | Retract（回缩） | Comply（柔顺） |
|---|---|---|---|
| 峰值碰撞力 [N] | 45 | 42 | 38 |
| 碰撞后 0.5s 残余力 [N] | 32（持续夹持） | 0（已回缩） | 1.2（几乎为零） |
| 恢复时间 [s] | 0（但不安全） | 1.0 | 0.5-3.0 |
| ISO 15066 准静态合规 | 不合规（持续力） | 合规 | 合规 |
| 实现复杂度 | 低 | 中 | 中 |
本质洞察：Reflex 看似最"安全"（立即停止），实际上是三种策略中**最危险的**——因为停止后碰撞力持续存在（准静态力），可能超过 ISO 15066 的准静态阈值。Comply 和 Retract 通过主动减小力来满足安全标准。
8. 能量注入监测系统设计 ⭐⭐⭐¶
8.1 动机——运行时安全监控¶
能量罐和 KB 条件是**设计阶段**的安全保证。但在运行时，我们还需要**实时监控**系统的能量状态，提供可视化和报警。
8.2 能量监测系统架构¶
// 能量注入实时监测系统
class EnergyMonitor {
public:
struct EnergyState {
double kinetic_energy; // 动能 [J]
double potential_energy; // 弹性势能 [J]
double tank_energy; // 罐能量 [J]
double total_energy; // 总能量 [J]
double injection_power; // 注入功率 [W]
double dissipation_power; // 耗散功率 [W]
double net_power; // 净功率（注入-耗散）[W]
double cumulative_injection; // 累积注入能量 [J]
bool safety_warning; // 安全警告标志
bool safety_critical; // 安全紧急标志
};
private:
double cumulative_injection_ = 0.0;
double warning_threshold_ = 5.0; // 累积注入警告阈值 [J]
double critical_threshold_ = 20.0; // 累积注入紧急阈值 [J]
// 滑动窗口：最近 100 个周期的净功率
std::deque<double> power_history_;
int window_size_ = 100;
public:
EnergyState compute(
const Eigen::Matrix<double,6,1>& x_tilde,
const Eigen::Matrix<double,6,1>& x_dot_tilde,
const Eigen::Matrix<double,6,6>& M_d,
const Eigen::Matrix<double,6,6>& K_d,
const Eigen::Matrix<double,6,6>& D_d,
const Eigen::Matrix<double,6,6>& K_dot,
double tank_energy,
double dt)
{
EnergyState state;
// 基本能量计算
state.kinetic_energy =
0.5 * (x_dot_tilde.transpose() * M_d * x_dot_tilde)(0, 0);
state.potential_energy =
0.5 * (x_tilde.transpose() * K_d * x_tilde)(0, 0);
state.tank_energy = tank_energy;
state.total_energy = state.kinetic_energy +
state.potential_energy + tank_energy;
// 功率计算
Eigen::Matrix<double,6,6> K_dot_sym =
0.5 * (K_dot + K_dot.transpose());
state.injection_power =
0.5 * (x_tilde.transpose() * K_dot_sym * x_tilde)(0, 0);
state.dissipation_power =
(x_dot_tilde.transpose() * D_d * x_dot_tilde)(0, 0);
state.net_power = state.injection_power - state.dissipation_power;
// 累积注入（只累积正的注入）
if (state.injection_power > 0) {
cumulative_injection_ += state.injection_power * dt;
} else {
// 负注入（降刚度提取能量）缓慢抵消累积
cumulative_injection_ = std::max(0.0,
cumulative_injection_ + state.injection_power * dt);
}
state.cumulative_injection = cumulative_injection_;
// 安全判断
state.safety_warning =
(cumulative_injection_ > warning_threshold_) ||
(state.net_power > state.dissipation_power * 0.5);
state.safety_critical =
(cumulative_injection_ > critical_threshold_) ||
(tank_energy < 0.01);
// 更新功率历史
power_history_.push_back(state.net_power);
if (static_cast<int>(power_history_.size()) > window_size_) {
power_history_.pop_front();
}
return state;
}
// 获取最近 100ms 的平均净功率
double average_net_power() const {
if (power_history_.empty()) return 0.0;
double sum = 0.0;
for (double p : power_history_) sum += p;
return sum / power_history_.size();
}
void reset() {
cumulative_injection_ = 0.0;
power_history_.clear();
}
};
监测指标的物理含义与安全阈值：
| 指标 | 物理含义 | 安全阈值 | 超限后动作 |
|---|---|---|---|
| 累积注入能量 | 升刚度总共"充入"的能量 | 5 J（警告）/ 20 J（紧急） | 减慢刚度变化 / 冻结 \(K_d\) |
| 净功率（注入-耗散） | 能量是否在积累 | > 耗散功率的 50% | 增大 \(D_d\) |
| 罐能量 | 安全储备 | < 0.01 J | 冻结 \(K_d\) |
| 总能量 | 系统活跃度 | 持续增长 | 全面降参数 |
9. Peg-in-Hole 四阶段阻抗切换——综合应用 ⭐⭐¶
9.1 动机——变阻抗的经典工业应用¶
Peg-in-hole（销入孔）装配是变阻抗控制最经典的应用场景。一个 8 mm 直径的销需要插入 8.1 mm 的孔（间隙仅 0.1 mm）——纯位置控制无法处理这种精度要求（标定误差通常 >0.5 mm），必须用力柔顺。
9.2 四阶段策略¶
阶段 1 — Approach（接近）：
行为：位置控制，快速接近孔位
K = [2000, 2000, 2000, 200, 200, 200]
切换条件：F_z > 2 N（检测到接触）
阶段 2 — Contact（建立接触）：
行为：z 向极柔顺，沿 -Z 贴合
K = [2000, 2000, 50, 200, 200, 200] ← Z 向极软
切换条件：F_z 稳定在 5-10 N 且 v_z ≈ 0（贴合完成）
阶段 3 — Align（对准/搜索）：
行为：xy 向柔顺 + 螺旋搜索 + 旋转振动
K = [200, 200, 50, 20, 20, 200] ← xy 和 Rxy 都变软
叠加搜索：x_d += A·sin(ωt), y_d += A·cos(ωt)
旋转振动：±2° @5 Hz（破解卡死）
切换条件：z 位移突然增加 >2 mm（销滑入孔口）
阶段 4 — Insert（插入）：
行为：恢复 z 向刚度，慢速推入
K = [500, 500, 1000, 50, 50, 200]
力限制：F_z ≤ 30 N（防过大力损坏零件）
到位判断：z 位移到达 30 mm + F_z 稳定
9.3 阶段切换的安全设计¶
每次阶段切换涉及刚度变化——必须使用本章学到的工具保证安全：
| 切换 | 刚度变化 | 危险性 | 安全保证 |
|---|---|---|---|
| 1→2 | \(K_z\): 2000→50 | 低（降刚度） | 无需额外措施 |
| 2→3 | \(K_{xy}\): 2000→200 | 低（降刚度） | 无需额外措施 |
| 3→4 | \(K_z\): 50→1000 | 高（升刚度） | 能量罐或 KB 条件 |
| 2→3, 3→4 | 所有切换 | 中 | 指数平滑过渡（\(\tau_{smooth}=0.1\)-\(0.3\) s） |
阶段 3→4 的能量分析：
切换时典型偏差：e_z ≈ 0.005 m（5 mm，销正在滑入）
K_z: 50 → 1000 N/m
ΔV = 0.5 × (1000-50) × 0.005² = 0.012 J
这个能量跳变很小（<0.1 J），阻尼在 ~10 ms 内就能耗散。
所以 PiH 的四阶段切换在实际中不太会触发无源性问题。
但原理上必须意识到：更大偏差 + 更大刚度跳变 → 更大风险。
9.4 SERL 的学习型变阻抗——25 分钟学会 PCB 插入¶
SERL（Berkeley 2024, ICRA）展示了用 RL 从零学习 peg-in-hole 的能力。关键设计：
底层：serl_franka_controllers 阻抗控制
K = [300, 300, 300, 30, 30, 30]（固定！）
error clipping = [0.05, 0.05, 0.05, 0.3, 0.3, 0.3]
上层：RL 策略（10 Hz）
观测：q, dq, F_ext, 图像
动作：Δx_d（6D 位姿偏移）
奖励：-|F_z - F_target| - |e_xy|
本质洞察：SERL 选择了**固定阻抗+学习位姿**而不是**学习变阻抗**。为什么？因为固定阻抗保证了无源性（F03 证明），而学习变阻抗需要额外的能量罐/KB 条件——增加了系统复杂度和调试难度。SERL 的哲学是：让底层尽可能简单和安全，把复杂度留给上层学习策略。这与 CRISP 的两层架构一脉相承。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：阶段切换时参数突变
错误做法：在检测到接触的瞬间立刻将 \(K_z\) 从 2000 跳变到 50
现象：力矩突变 → 末端抖动 → 可能脱离接触
正确做法：使用指数平滑过渡（F04 第 8.2 节），\(\tau_{smooth} = 0.1\text{-}0.3\) s
🧠 思维陷阱：认为学习型方法一定比手工调参好
SERL 25 分钟学会 PCB 插入确实惊人，但它需要：真机+人工干预（SpaceMouse）+GPU 训练。对于**已知几何的标准零件装配**，手工四阶段状态机可能在 1 小时内就能调好——且不需要 GPU。学习方法的真正价值在于**几何未知或变化**的场景
练习¶
- ⭐⭐ PiH 状态机：在 MuJoCo Franka 中实现 peg-in-hole 四阶段状态机。硬编码参数和切换条件。测试 8mm 销 / 8.1mm 孔。记录 20 次试验的成功率。
- ⭐⭐⭐ PiH + 能量罐：在上述状态机中加入能量罐。阶段 3→4 的升刚度通过能量罐保证无源性。对比有/无能量罐的力矩平滑性。
- ⭐⭐⭐ 跨章综合题：结合 F01（约束空间分析）、F03（阻抗控制律）、F04（CRISP 工程）和本章全部内容，设计一个完整的螺丝拧紧系统。场景：Franka Panda + M6 螺丝 + 铝板。需要：(a) 接近阶段位控；(b) 接触检测（动量观测器）；(c) 导纳搜索找螺纹；(d) 变阻抗拧紧（恒力矩 + 到位检测）；(e) ISO 15066 合规验证。画出状态机、每阶段参数、能量罐配置。
本章小结¶
| 知识点 | 难度 | 核心结论 | 工程应用 |
|---|---|---|---|
| 变阻抗能量注入 | ⭐⭐ | 升刚度注入 \(\frac{1}{2}\tilde{x}^T\dot{K}_d\tilde{x}\) | 理解危险性 |
| 能量罐 | ⭐⭐⭐ | 虚拟能量银行，罐空则冻结；完整 Lyapunov 证明 | RL 型变阻抗 |
| KB 阻尼条件 | ⭐⭐⭐ | 对角轴上 \(d_i \ge m_i\max(0,\dot{k}_i)/(2k_{i,min})\)，并叠加临界阻尼下界 | 状态机型变阻抗 |
| 动量观测器 | ⭐⭐ | \(\dot{r} = K_O(\tau_{ext} - r)\)；完整 C++ 实现 | 无传感器碰撞检测 |
| ISO/TS 15066 | ⭐⭐ | 准静态/瞬态两种限值；有效质量计算 | 协作安全认证 |
| CBF-based 变阻抗 | ⭐⭐⭐⭐ | 最小干预原则；QP 在线求解 | 前沿研究 |
| 碰撞恢复策略 | ⭐⭐ | Reflex/Retract/Comply 完整实现 | 碰撞后安全恢复 |
| 能量监测系统 | ⭐⭐⭐ | 注入/耗散/累积实时监控 | 运行时安全保障 |
| PiH 四阶段 | ⭐⭐ | 接近→接触→对准→插入 | 经典装配任务 |
累积项目：本章新增模块¶
项目：从零构建力控机械臂系统
| 章节 | 新增模块 | 功能 |
|---|---|---|
| F01 | 概念模型 | 阻抗/导纳二分法 |
| F03 | 算法库 | 四大经典力控算法 |
| F04 | 阻抗控制器 | libfranka 笛卡尔阻抗 |
| F05 | 导纳控制器 | ros2_controllers 导纳 |
| F06 | 变阻抗安全层 | 能量罐 + KB 条件 + 动量观测器 + CBF + 碰撞恢复 + 能量监测 + PiH 状态机 |
延伸阅读¶
| 材料 | 类型 | 难度 | 核心内容 |
|---|---|---|---|
| Ferraguti, Secchi, Fantuzzi 2013, "Energy Tank" (ICRA) | 论文 | ⭐⭐⭐ | 能量罐原始论文 |
| Kronander & Billard 2016, "Stability for VIC" (IEEE T-RO) | 论文 | ⭐⭐⭐ | 闭式阻尼下界条件 |
| De Luca & Mattone 2005, "Sensorless Collision Detection" (ICRA) | 论文 | ⭐⭐ | 动量观测器原始论文 |
| De Luca et al. 2006, "Collision Detection with DLR-III" (IROS) | 论文 | ⭐⭐ | 柔性关节完整实现 |
| Haddadin et al. 2017, "Robot Collisions Survey" (IEEE T-RO) | 综述 | ⭐⭐⭐ | 碰撞感知 8 步流水线 |
| Haddadin & Croft 2016, "Physical HRI" (Springer Handbook Ch.69) | 教材 | ⭐⭐ | ISO 15066 权威解读 |
| Haddadin et al. 2009, "Requirements for Safe Robots" (IJRR) | 论文 | ⭐⭐⭐ | HIC/HIP 碰撞安全实验 |
| Buchli et al. 2011, "Learning Variable Impedance" (IJRR) | 论文 | ⭐⭐⭐ | PI\(^2\) 学习变刚度 |
| Ames et al. 2019, "Control Barrier Functions" (ECC) | 论文 | ⭐⭐⭐⭐ | CBF 理论综述 |
| Singletary et al. 2022, "Safety-Critical Manipulation" (L4DC) | 论文 | ⭐⭐⭐⭐ | CBF + 阻抗控制结合 |
| ISO/TS 15066:2016 全文 | 标准 | ⭐⭐ | PFL 模式完整规范 |
| Behrens & Elkmann 2022, "New Biomechanical Data" (Applied Ergonomics) | 论文 | ⭐⭐ | 对 15066 阈值的修正实验 |
🔧 故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 变阻抗切换时末端振荡 | 升刚度能量注入超过阻尼耗散 | 1. 检查 \(\dot{K}_d\) 是否过快 2. 增大 \(D_d\) 3. 添加能量罐 | F06§1, §2 |
| 能量罐频繁冻结 K_d | \(T_{init}\) 太小或 \(T_{min}\) 太大 | 1. 增大 \(T_{init}\) 2. 减小 \(T_{min}\) 3. 检查阻尼耗散率 | F06§2.4 |
| 能量罐 NaN/崩溃 | \(x_t\) 变为负值后除以 \(x_t\) | 1. 添加 if (x_t < 0) x_t = 0 2. 在放能前检查 \(T > T_{min}\) |
F06§2.3 |
| 动量观测器有持续偏置 | 动力学模型不准（摩擦/负载） | 1. 重新标定 2. 调用 setLoad 3. 增大检测阈值 | F06§4 |
| 碰撞检测延迟太大 | \(K_O\) 太小 | 1. 增大 \(K_O\) 到 80-100 2. 降低后处理滤波 3. 用 F/T 传感器 | F06§4.4 |
| PiH 搜索阶段卡死 | 螺旋搜索振幅不够 | 1. 增大 A 2. 增大旋转振动 3. 降低 \(K_{xy}\) | F06§6.2 |
| ISO 15066 合规失败 | 碰撞力超阈值 | 1. 降低 TCP 速度 2. 降低 \(K_d\) 或 \(e_{max}\) 3. 减小有效质量 | F06§5 |
| CBF-QP 求解器不可行 | 安全约束与参数约束矛盾 | 1. 放宽 \(K_{min}\) 2. 增大 \(T_{init}\) 3. 减小 \(\gamma\) | F06§6 |
| 碰撞后 Comply 模式机器人不停 | 阻尼不足或 K 未归零 | 1. 确认 K=0 2. 增大 Comply 阻尼 3. 检查模式切换逻辑 | F06§7 |
| 能量监测持续报警 | 升刚度频率过高 | 1. 延长过渡时间 2. 增大 \(D_d\) 3. 检查任务是否需要如此频繁的变阻抗 | F06§8 |