本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

F04 笛卡尔阻抗控制——从理论推导到 libfranka / CRISP 工程实现¶

本章定位：本章是 F03 经典力控算法的工程深化。F03 给出了笛卡尔阻抗控制的理论推导和稳定性分析，本章将这些理论落地到真实硬件和工业级代码库上——以 Franka Emika 的 libfranka/franka_ros2 为教学主线，以 CRISP（2025）为研究前沿参考。我们将**逐行精读**实际控制器代码，理解每一行 C++ 背后的物理和数学含义，并系统化地解决笛卡尔阻抗控制中的三大工程难题：惯性整形与解耦、6D 姿态阻抗的四元数实现、可变阻抗参数的实时调节。

适用范围：固定基座力矩控制型机械臂（Franka Panda/FR3、KUKA iiwa、DLR LWR）。位置接口型机器人（UR、ABB）的阻抗实现见 F05 导纳控制。

前置依赖：F01（阻抗/导纳概念）、F03（笛卡尔阻抗控制律推导、稳定性证明）、M01（Pinocchio 动力学——$M, C, g$ 计算及关节空间 PD/计算力矩法基础）

下游章节：F05（导纳控制工程）、F06（变阻抗与无源性）、F09（学习型力控——SERL/CRISP + RL）

建议用时：3 周（libfranka 架构 1 周 + 代码精读 1 周 + CRISP 集成 1 周）

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	写出笛卡尔空间阻抗控制律 $\tau = J^T(-K_d \tilde{x} - D_d \dot{\tilde{x}}) + g(q)$。为什么需要 $J^T$？它在物理上做了什么映射？	F03 第 3 节
2	操作空间惯量矩阵 $\Lambda = (JM^{-1}J^T)^{-1}$ 的物理含义是什么？它与操作空间控制（OSC）有什么关系？	F03 第 4 节
3	四元数 $q$ 和 $-q$ 代表同一个旋转（双覆盖性）。如果不处理这个问题，阻抗控制器会产生什么灾难性行为？	M01 旋转表示
4	libfranka 的 FCI 回调函数有什么实时约束？为什么不能在回调中使用 `new`/`malloc`？	M11 实时 C++
5	什么是临界阻尼（$\zeta = 1$）？为什么大多数工业阻抗控制器默认使用临界阻尼？	F01 第 2.3 节

本章目标¶

学完本章后，你应该能够：

逐行解读 libfranka cartesian_impedance_control.cpp 的每一行代码，说清楚它对应 F03 中哪个公式
**推导**从任务空间阻抗方程到关节力矩命令的完整映射链，包括惯性整形和零空间项
**实现**基于四元数的 6D 姿态阻抗控制，正确处理双覆盖和奇异性
配置 CRISP 的七项控制律叠加，理解 error clipping 的数学原理和安全保证
比较 CI（笛卡尔阻抗）与 OSC（操作空间控制）的工程权衡，在具体场景中做出正确选择
**诊断**笛卡尔阻抗控制器的常见故障——力矩跳变、姿态翻转、振荡失稳——并给出修复方案

1. 从 F03 理论到工程实现——弥合推导与代码的鸿沟 ⭐¶

1.1 动机——理论公式为什么不能直接复制到代码¶

回顾 F03 第 3 节：我们推导了笛卡尔阻抗控制律：

\[\tau = J^T(-K_d \tilde{x} - D_d \dot{\tilde{x}}) + C(q, \dot{q})\dot{q} + g(q)\]

其中 $\tilde{x} = x - x_d$ 是 6D 位姿偏差，$K_d, D_d$ 是期望刚度和阻尼矩阵。这个公式在 F03 的 Lyapunov 稳定性证明中是完备的。

但当你打开 libfranka 的 cartesian_impedance_control.cpp 时，你会发现**真实代码和教科书公式之间有六个关键差异**：

差异	F03 教科书公式	libfranka 工程实现	为什么不同
姿态误差	$\tilde{x}_{rot}$ 是 3D 向量，如何计算未指定	四元数乘法 + 双覆盖检查 + 旋转矩阵映射	姿态空间不是欧几里得空间
速度误差	$\dot{\tilde{x}} = \dot{x} - \dot{x}_d$	$\dot{\tilde{x}} = J\dot{q}$（假设 $\dot{x}_d = 0$）	稳态点阻抗简化
重力补偿	显式计算 $g(q)$	FCI 控制柜自动补偿，代码中不出现	硬件分层——内部环路已处理
惯性整形	可选 $\Lambda$ 矩阵	示例中不使用，CRISP/OSC 使用	简单示例 vs. 完整控制器
零空间	冗余关节管理	示例中缺失，franka_ros 版本包含	7-DOF 臂必须处理
限幅	无	`kMaxTorqueRate` + 低通滤波	硬件安全——防止执行器过载

这六个差异反映了从**理论控制律**到**可部署控制器**必须跨越的工程鸿沟。本章将逐一解决每个差异。

1.2 笛卡尔阻抗控制的完整映射链¶

从"期望末端行为"到"关节力矩命令"，完整的计算链包含七个步骤：

Step 1: 读取当前状态
  (q, dq, O_T_EE) ← 传感器/FCI
         ↓
Step 2: 计算位置误差（3D 欧几里得）
  e_pos = p - p_d ∈ R³
         ↓
Step 3: 计算姿态误差（四元数 → 3D 角度误差）
  e_rot = -R · (Q⁻¹ ⊗ Q_d).vec() ∈ R³    ← 本章重点！
         ↓
Step 4: 合成 6D 误差
  e = [e_pos; e_rot] ∈ R⁶
         ↓
Step 5: 计算笛卡尔力/力矩（阻抗律）
  F_imp = -K_d · e - D_d · (J · dq) ∈ R⁶
         ↓
Step 6: 映射到关节力矩
  τ_task = J^T · F_imp ∈ R⁷
         ↓
Step 7: 叠加补偿项 + 零空间
  τ_cmd = τ_task + τ_coriolis + τ_nullspace
  （重力 g(q) 由 FCI 在底层叠加，用户回调通常不要再加）

类比：这个映射链类似于计算机图形学中的渲染管线（rendering pipeline）。在图形学中，从 3D 模型到屏幕像素经历了模型变换→视图变换→投影→光栅化→着色五个阶段。在阻抗控制中，从期望行为到关节力矩也经历了感知→误差计算→控制律→力映射→补偿叠加五个阶段。每个阶段都可以独立理解和调试。

1.3 三个认知层次——你在哪个层次？¶

为了系统化地组织本章内容，我们将笛卡尔阻抗控制的工程知识分为三个认知层次：

层次	能力	对应章节
L1: 能用	配置参数、运行示例、调节 K/D	F4.1-F4.2
L2: 能懂	逐行解读代码、理解每行的数学依据	F4.3-F4.5
L3: 能改	修改控制律（加惯性整形、变阻抗、学习集成）	F4.6-F4.8

初学者应按 L1→L2→L3 的顺序推进。但本教程的叙述将 L2 作为主线（因为教学的目标是理解，不仅是使用），穿插 L1 的实操和 L3 的进阶。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：直接用教科书公式实现控制器

错误做法：把 F03 的公式 $\tau = J^T(-K_d \tilde{x} - D_d \dot{\tilde{x}}) + g(q)$ 直接翻译成代码

现象：控制器在自由空间可能看起来正常，但接触刚性环境时产生严重振荡甚至失控

根本原因：教科书公式中的 $\tilde{x}$ 是 6D 向量，但旋转部分的误差不是简单的减法——它涉及流形上的对数映射（SO(3) 不是欧几里得空间）。直接用欧拉角做减法会产生万向锁和不连续跳变

正确做法：使用四元数或旋转矩阵计算姿态误差，处理双覆盖，如本章第 3 节详述

💡 概念误区：认为阻抗控制只需要调 K 和 D

新手想法："阻抗控制就是弹簧-阻尼器，调 K 和 D 就够了"

实际上：完整的笛卡尔阻抗控制器至少涉及七个设计决策——刚度矩阵结构（对角/满阵）、阻尼策略（临界/因子阻尼/双对角化）、惯性整形（是否使用 $\Lambda$）、零空间行为（指向默认构型/避奇异/避关节限位）、重力补偿来源（控制器显式计算 vs. 硬件内部）、限幅策略（力矩限幅/速率限幅/误差 clipping）、滤波策略（速度信号低通/力矩输出低通）。每个决策都影响系统行为

正确思维：将阻抗控制器视为一个**设计空间**，而非一个公式

练习¶

⭐ 映射链追踪：画出从 $x_d$（期望位姿）到 $\tau$（关节力矩）的完整信号流图，标注每一步的输入/输出维度和数据类型（Eigen 类型）。
⭐ 差异分析：对照上面的六项差异表，解释为什么 FCI 自动补偿重力反而让控制器代码更简单。如果 FCI 不自动补偿，代码需要增加什么？
⭐⭐ 量纲检查：验证阻抗控制律 $F = -K_d \tilde{x} - D_d \dot{\tilde{x}}$ 的量纲一致性。$K_d$ 的单位是什么？$D_d$ 的单位是什么？$F$ 的单位是什么？旋转部分的单位是什么？

2. libfranka FCI 架构与实时约束 ⭐¶

2.1 动机——为什么 Franka 是力控教学的最佳平台¶

在 2017 年之前，力矩控制型机器人只有 DLR LWR（数百万欧元，实验室定制品）和 KUKA iiwa（约 10 万欧元）。Franka Emika 以约 2 万欧元的价格提供了 1 kHz 力矩级控制接口（FCI，Franka Control Interface），彻底改变了力控研究的可及性。

历史脉络：Franka Emika 的创始人 Sami Haddadin（现 TUM 教授）正是 DLR 碰撞安全研究（Haddadin 2009 IJRR）的核心成员。Franka Panda 的设计哲学直接继承了 DLR LWR III/IV 的柔性关节阻抗控制思路——每个关节内置力矩传感器，实现"本体感知的力控"。2017 年 libfranka 开源后，全球超过 3000 个实验室使用 Franka 进行力控研究，其 cartesian_impedance_control.cpp 成为**事实上的阻抗控制教学标准代码**。

2.2 FCI 通信架构——1 kHz 的实时约束¶

┌─ 用户工作站（Ubuntu + PREEMPT_RT 内核）────────────────────┐
│  franka::Robot robot("192.168.1.1");                        │
│  robot.control([](const franka::RobotState& state,          │
│                   franka::Duration period)                   │
│                   -> franka::Torques                         │
│  {                                                           │
│     // 这里有 ≤300 μs 来计算 tau_d                          │
│     // 硬实时约束：超时 → 通信错误 → 急停                    │
│     return tau_d;                                            │
│  });                                                         │
└─────────────────────── 1 kHz UDP ───────────────────────────┘
                          ↕ 以太网直连
┌─ Franka 控制柜（ARM + RTOS）─────────────────────────────────┐
│  内部电流环：10 kHz → 关节力矩环：1 kHz                       │
│  自动补偿：重力 g(q)                                          │
│  用户力矩回调：命令不含重力；Coriolis 若需要由用户显式加入       │
│  限幅保护：                                                    │
│    · kMaxTorqueRate：力矩变化率上限（防止执行器电流突变）       │
│    · 低通滤波 100 Hz：抑制高频控制信号                         │
│    · 安全监控：扭矩/速度/力上限 → 违规则急停                   │
└──────────────────────────────────────────────────────────────┘

关键设计决策解析：

设计	为什么这样做	如果不这样做
1 kHz 控制频率	笛卡尔阻抗需要 $\ge$500 Hz 才能稳定接触	<200 Hz 则高刚度接触必然振荡
$\le$300 $\mu$s 计算预算	1 ms 周期内需留余量给通信和内部处理	超时则 FCI 判定通信丢失，触发急停
UDP 而非 TCP	UDP 无重传延迟，满足硬实时	TCP 重传可能导致几十 ms 延迟
自动重力补偿	简化用户控制律，减少 bug 来源	用户必须精确知道动力学参数
`kMaxTorqueRate` 限幅	防止力矩突变损坏减速器	谐波减速器对冲击载荷敏感

2.3 RobotState——传感器数据的完整图谱¶

franka::RobotState 是 FCI 在每个 1 kHz 周期内提供的完整传感器数据包。对阻抗控制最关键的字段：

字段	类型	含义	阻抗控制中的用途
`O_T_EE[16]`	`double[16]`	列优先 4$\times$4 齐次矩阵（基坐标系→末端）	获取当前位置和姿态
`q[7]`	`double[7]`	关节位置（rad）	计算 Jacobian、Coriolis
`dq[7]`	`double[7]`	关节速度（rad/s）	阻尼项 $D_d \dot{x} = D_d J \dot{q}$
`tau_J[7]`	`double[7]`	测得关节力矩（含重力、Coriolis）	调试、力矩监控
`tau_ext_hat_filtered[7]`	`double[7]`	外部扰动力矩估计（动量观测器输出）	碰撞检测、外力估计
`O_F_ext_hat_K[6]`	`double[6]`	笛卡尔外力估计（基坐标系，6D）	导纳控制、力跟踪

反事实推理：如果 FCI 不提供 tau_ext_hat_filtered 会怎样？用户必须自己实现 De Luca 动量观测器（F06 第 3 节），需要精确的动力学模型 $M(q), C(q,\dot{q}), g(q)$ 以及数值积分。这个估计器对模型误差非常敏感——Franka 内部使用了出厂标定的精确模型参数，远比用户用 URDF 近似参数得到的估计准确。

一个极其重要但容易忽略的细节：tau_ext_hat_filtered 不包含末端执行器（EE）负载的惯性和重力。如果你安装了 1 kg 的抓手但没有调用 robot.setLoad(mass, F_x_Ctool, I_tool)，那么抓手的重力会被动量观测器误认为是外部力——导致碰撞检测误触发或外力估计偏差。

// ✅ 正确：安装新工具后更新负载参数
robot.setLoad(
    0.73,                           // mass [kg]
    {{0.01, 0.01, 0.03}},          // F_x_Ctool：工具质心位置 [m]
    {{0.001, 0.0, 0.0,             // I_tool：工具惯性张量 [kgm²]
      0.0, 0.001, 0.0,
      0.0, 0.0, 0.001}}
);

// ❌ 错误：不调用 setLoad
// 后果：抓手重力被误认为外力 → tau_ext_hat_filtered 偏置
// 典型现象：阻抗控制器在空载时出现持续的"幽灵力"

2.4 实时编程约束——为什么阻抗控制代码看起来"原始"¶

FCI 回调函数运行在 PREEMPT_RT 实时环境中。这意味着：

**绝对禁止**的操作： - 堆内存分配：new、malloc、std::vector::push_back（可能触发 realloc） - 系统调用：std::cout、printf、fopen、网络 I/O - 锁竞争：std::mutex::lock（如果锁被非实时线程持有，可能导致优先级反转） - 异常：throw（栈展开耗时不确定）

**允许**的操作： - 栈分配的 Eigen 固定大小矩阵：Eigen::Matrix<double, 7, 1> - 算术运算：矩阵乘法、三角函数 - 数组索引：state.q[i] - Eigen::Map：零拷贝地将 C 数组映射为 Eigen 对象

// ✅ 正确：固定大小矩阵，栈分配，编译器可 SIMD 优化
Eigen::Matrix<double, 7, 1> tau_d;
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(state.q.data());

// ❌ 错误 1：动态分配
Eigen::VectorXd tau_d(7);           // 堆分配！每次循环 malloc/free
std::vector<double> tau_vec(7);     // 堆分配！

// ❌ 错误 2：I/O 系统调用
std::cout << "tau = " << tau_d.transpose() << std::endl;
// std::cout 涉及 I/O，耗时不确定（10 μs - 10 ms）

// ❌ 错误 3：堆分配 + 忘记 delete
auto* tau_ptr = new double[7];      // 堆分配 + 内存泄漏

类比：FCI 回调就像飞机的自动驾驶仪——它每毫秒执行一次，任何延迟都可能导致失控。你不会让自动驾驶仪在飞行中去"上网查询"天气数据（I/O 阻塞），也不会让它"临时租一块新内存"（malloc）。一切资源必须在起飞前（初始化阶段）准备好。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：在 FCI 回调中使用 std::cout 调试

错误做法：std::cout << "tau = " << tau_d.transpose() << std::endl;

现象：控制器在前几秒正常工作，然后突然急停。终端报错 communication_constraints_violation

根本原因：std::cout 涉及 I/O 系统调用，耗时不确定（10 $\mu$s - 10 ms），可能导致回调超过 300 $\mu$s 预算

正确做法：使用 franka::Robot::read() 在非实时线程中打印状态；或使用 std::atomic<double> 将数据从实时线程传递到非实时打印线程

⚠️ 编程陷阱：Eigen Map 的生命周期问题

错误做法：将 Map 对象保存到回调外部的变量中

实际上：在 FCI 回调中 state 的生命周期贯穿整个回调函数，所以 Map 是安全的。但如果你将 Map 保存到回调外部的变量中，则 state 数据可能已被下一周期覆盖

正确做法：在回调内部使用 Map（零拷贝高效），如需保存到外部则显式复制：
Eigen::Matrix4d T_ee_copy = Eigen::Matrix4d::Map(state.O_T_EE.data());

练习¶

⭐ 实时约束计算：如果 FCI 的控制周期是 1 ms，计算预算是 300 $\mu$s，那么在这 300 $\mu$s 内最多能做多少次 7$\times$7 矩阵乘法？提示：现代 CPU 上 7$\times$7 矩阵乘法约需 0.1-0.5 $\mu$s。
⭐ setLoad 影响分析：假设安装了一个 0.5 kg 的抓手，质心在工具坐标系原点上方 8 cm。不调用 setLoad 时，tau_ext_hat_filtered 的偏差大约是多少 Nm？提示：计算 $J^T \cdot [0, 0, -mg, 0, 0, 0]^T$ 在典型构型下的值。
⭐⭐ 数据流图：画出 FCI 的 1 kHz 控制回路数据流图，标注用户工作站和控制柜之间通过 UDP 传输的数据包内容和方向。

3. 6D 姿态阻抗——四元数误差与双覆盖问题 ⭐⭐¶

3.1 动机——为什么姿态阻抗是最容易出错的部分¶

位置阻抗很直观：$e_{pos} = p - p_d$，就是两点之间的欧几里得距离。但姿态阻抗面临一个根本困难：旋转不是欧几里得空间中的量。

在欧几里得空间 $\mathbb{R}^3$ 中，两点之间的"差"就是向量减法 $a - b$。但在旋转空间 SO(3) 中：

操作	欧几里得空间 $\mathbb{R}^3$	旋转空间 SO(3)
表示	3D 向量	旋转矩阵 $R \in \mathbb{R}^{3\times3}$，$R^TR = I$，$\det R = 1$
"差"	$a - b$	$R_1^T R_2$ 或 $\log(R_1^T R_2)$
加法	$a + b$	$R_1 R_2$（矩阵乘法）
线性插值	$(1-t)a + tb$	$R_1 \exp(t \cdot \log(R_1^T R_2))$（SLERP）
拓扑	简单连通	非简单连通（$\pi_1(\text{SO(3)}) = \mathbb{Z}_2$）

SO(3) 的非欧性质导致三个工程问题：

旋转矩阵减法无意义：$R_1 - R_2$ 的结果不是旋转矩阵，甚至没有明确的物理含义
欧拉角的万向锁：用欧拉角 $(\phi, \theta, \psi)$ 表示姿态时，$\theta = \pm 90°$ 处两个轴退化
四元数的双覆盖：$q$ 和 $-q$ 代表同一个旋转——如果不处理，阻抗控制器可能选择"绕远路"（270° 而不是 90°）

反事实推理：如果旋转空间是欧几里得的（就像位置空间），那么姿态阻抗就只需要 $e_{rot} = \theta - \theta_d$（类似位置减法），控制律设计将简单得多。但物理约束决定了旋转空间必须是紧致的——因为绕同一轴转 360° 等于不转——这使得 SO(3) 不可能是 $\mathbb{R}^3$。所以姿态阻抗的复杂性不是工程师制造的，而是**几何的本质要求**。

3.2 libfranka 的姿态误差计算——逐行解析¶

以下是 libfranka cartesian_impedance_control.cpp 中姿态误差计算的核心代码，我们逐行解析其数学含义：

// Step 1: 从 4×4 齐次矩阵提取旋转矩阵和四元数
Eigen::Affine3d transform(Eigen::Matrix4d::Map(state.O_T_EE.data()));
Eigen::Quaterniond orientation(transform.rotation());

数学含义：从 $T_{EE}^{O} \in SE(3)$ 中提取旋转部分 $R \in SO(3)$，然后转换为四元数 $q = (w, x, y, z)$。Eigen 的 Quaterniond 内部存储顺序为 $(x, y, z, w)$，但构造函数参数顺序为 $(w, x, y, z)$。

// Step 2: 双覆盖检查——确保 q 和 q_d 在同一半球
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
    orientation.coeffs() << -orientation.coeffs();
}

这是整个控制器中最关键的一行代码。 四元数空间 $S^3$（三维球面）与 SO(3) 之间是 2:1 映射——$q$ 和 $-q$ 代表相同的旋转。如果当前四元数 $q$ 和目标四元数 $q_d$ 位于 $S^3$ 的**不同半球**（即内积 < 0），那么从 $q$ 到 $q_d$ 的"最短路径"会绕过球面的另一侧，对应一个**大于 180° 的旋转**。

不处理的后果：假设当前姿态和目标姿态只差 10°，但四元数恰好在不同半球。不做翻转时，控制器会计算出一个 350° 的误差——产生巨大的恢复力矩，导致机器人猛烈旋转。

数学证明：设 $q_1, q_2 \in S^3$ 表示两个旋转。从 $q_1$ 到 $q_2$ 的测地距离（球面大圆距离）为 $d(q_1, q_2) = \arccos(|q_1 \cdot q_2|)$。注意绝对值——因为 $q$ 和 $-q$ 是同一旋转，真正的旋转距离用的是 $|q_1 \cdot q_2|$ 而不是 $q_1 \cdot q_2$。当 $q_1 \cdot q_2 < 0$ 时，翻转 $q_1 \leftarrow -q_1$ 使得 $(-q_1) \cdot q_2 > 0$，从而保证后续计算使用短弧（< 180°）。

// Step 3: 计算四元数误差
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);

数学含义：$q_{err} = q^{-1} \otimes q_d$。这表示"从当前姿态到目标姿态需要施加的旋转"。如果 $q = q_d$，则 $q_{err} = (1, 0, 0, 0)$（单位四元数 = 无旋转）。

// Step 4: 从四元数误差提取 3D 角度误差
Eigen::Vector3d error_rot = -(transform.rotation() * error_quaternion.vec());

数学含义：这一步需要仔细理解。

error_quaternion.vec() 返回四元数的虚部 $(q_x, q_y, q_z)$，它与旋转轴-角度表示的关系为：$q_{vec} = \sin(\theta/2) \hat{n}$，其中 $\theta$ 是旋转角度，$\hat{n}$ 是旋转轴。
对于小角度（$\theta \ll 1$），$\sin(\theta/2) \approx \theta/2$，所以 $q_{vec} \approx (\theta/2) \hat{n}$。
transform.rotation() * error_quaternion.vec() 将误差从**末端坐标系**旋转到**基坐标系**。
前面的负号 - 使得误差方向与恢复力矩方向一致——正误差产生负力矩（恢复）。

本质洞察：libfranka 使用的姿态误差公式 $e_{rot} = -R \cdot q_{err,vec}$ 是 SO(3) 上最常用的近似误差度量之一，等价于 $e_{rot} \approx -(\theta/2) R \hat{n}$。它在小角度范围内是精确的，在大角度时引入非线性但仍然保持正确的方向信息。若控制律采用 $F=-K e$，完全精确的版本应使用与"当前减期望"一致的对数误差 $e_{rot} = \text{Log}(R_d^T R)$（再按所选 Jacobian 表达到同一坐标系）；Log 运算涉及反三角函数，计算开销更大且在 $\theta = \pi$ 处有奇异——工程实践中四元数虚部近似已经足够。

3.3 两种姿态误差公式的对比¶

工业界有两种主流的姿态误差计算方法：

方法 A：四元数虚部法（libfranka 使用）

\[e_{rot} = -R \cdot \text{Im}(q^{-1} \otimes q_d)\]

优点：计算简单（一次四元数乘法 + 一次矩阵-向量乘法），无三角函数
缺点：小角度下 $e_{rot} \approx (\theta/2)\hat{n}$，有 1/2 因子——需要将 $K_d^{rot}$ 乘以 2 来补偿
适用：大多数阻抗控制（误差通常小于 30°）

方法 B：对数映射法（Pinocchio/manif 使用）

\[e_{rot}^{d} = \text{Log}(R_d^T R) = \frac{\theta}{2\sin\theta}(R_d^T R - R^T R_d)^{\vee}\]

其中 $\theta = \arccos\left(\frac{\text{tr}(R^T R_d) - 1}{2}\right)$，$(\cdot)^{\vee}$ 是反对称矩阵到向量的映射（vee map）。若 Jacobian 使用 LOCAL_WORLD_ALIGNED，还需将该误差旋转到世界轴：$e_{rot}^{W}=R_d e_{rot}^{d}$。

优点：精确的测地距离，$e_{rot} = \theta \hat{n}$（无 1/2 因子），大角度也准确
缺点：需要 $\arccos$ 和除法，$\theta = 0$ 和 $\theta = \pi$ 处需要特殊处理
适用：大角度误差的精确控制、Lie 群框架下的控制设计

// 方法 A：四元数虚部法（libfranka 风格）
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0)
    orientation.coeffs() << -orientation.coeffs();
Eigen::Quaterniond qe = orientation.inverse() * orientation_d;
Eigen::Vector3d e_rot_A = -(transform.rotation() * qe.vec());

// 方法 B：对数映射法（Pinocchio/manif 风格）
#include <pinocchio/spatial/explog.hpp>
Eigen::Matrix3d R_current = transform.rotation();
Eigen::Matrix3d R_desired = /* ... */;
Eigen::Vector3d e_rot_B =
    R_desired * pinocchio::log3(R_desired.transpose() * R_current);

// 对比：
// 当 θ = 10°(0.175 rad) 时：
//   e_rot_A ≈ 0.087 * n̂  (半角)
//   e_rot_B ≈ 0.175 * n̂  (全角)
// 所以 K_d^rot 需要相应调整：
//   方法 A 的 K_rot = 20 Nm/rad ↔ 方法 B 的 K_rot = 10 Nm/rad

比较维度	方法 A（四元数虚部）	方法 B（对数映射）
计算量	~20 flops	~50 flops + arccos
小角度精度	$\approx \theta/2$（差 2 倍）	$= \theta$（精确）
大角度行为	$\theta > 90°$ 时非线性增大	精确到 $\theta < \pi$
奇异处理	双覆盖检查	$\theta = 0$ 和 $\pi$ 需特判
工程采用	libfranka, SERL, CRISP	Pinocchio, Drake, mc_rtc

3.4 位置+姿态耦合的 6D 阻抗¶

将位置和姿态误差合成为 6D 向量后，阻抗律为：

\[F_{imp} = -K_d \begin{bmatrix} e_{pos} \\ e_{rot} \end{bmatrix} - D_d \begin{bmatrix} v_{pos} \\ \omega \end{bmatrix}\]

其中 $v_{pos}$ 和 $\omega$ 分别是末端的线速度和角速度。在 libfranka 中：

\[\begin{bmatrix} v_{pos} \\ \omega \end{bmatrix} = J(q) \dot{q}\]

刚度矩阵的结构：

K_d = diag(K_x, K_y, K_z, K_rx, K_ry, K_rz)   ← 对角阵（最常用）

典型值（Franka Panda）：
  K_x = K_y = K_z = 150 N/m      ← 平移刚度
  K_rx = K_ry = K_rz = 10 Nm/rad  ← 旋转刚度

注意量纲差异：
  平移刚度 [N/m] 和旋转刚度 [Nm/rad] 量纲不同！
  不能直接比较 K_x = 150 和 K_rx = 10 的"大小"
  合理比较需要引入特征长度 l：
    等效刚度比 = K_rx / (K_x · l²)
    对于 Franka（l ≈ 0.1 m），10 / (150 × 0.01) = 6.67
    → 旋转方向实际上比平移方向"更硬"

非对角刚度矩阵的物理含义：

对角刚度矩阵意味着沿 x 方向的位置偏差只产生 x 方向的恢复力。但在某些任务中，位置偏差和姿态偏差之间存在耦合——例如在斜面上操作时，沿斜面法向的位移应同时产生倾斜力矩。此时需要**完整 6$\times$6 刚度矩阵**：

\[K_d = \begin{bmatrix} K_{pp} & K_{pr} \\ K_{rp} & K_{rr} \end{bmatrix} \in \mathbb{R}^{6 \times 6}\]

其中 $K_{pr} = K_{rp}^T$ 是位置-旋转耦合项。matthias-mayr 的 Cartesian Impedance Controller 支持完整的 6$\times$6 刚度矩阵。

反事实推理：如果始终使用对角刚度矩阵会怎样？对于大多数任务（平面擦拭、直线打磨、标准装配）对角矩阵足够。但对于**工具倾斜适应**（如球面打磨、曲面焊接），不使用耦合项会导致工具无法自动对准表面法向——位置偏差不会产生姿态修正。此时需要非对角 $K_{pr}$ 来实现"位移偏了则自动转动工具"的行为。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忘记四元数双覆盖检查

错误做法：直接计算 q.inverse() * q_d 而不检查内积符号

现象：控制器大部分时间正常工作，但偶尔（特别是姿态接近 180° 旋转时）末端突然猛烈旋转 360°

根本原因：四元数 $q$ 和 $-q$ 代表同一旋转，当 $q \cdot q_d < 0$ 时，误差四元数对应的旋转角大于 180°，控制器走了"长弧"

正确做法：始终在计算误差前检查并翻转：
if (q.coeffs().dot(q_d.coeffs()) < 0.0)
    q.coeffs() << -q.coeffs();
自检方法：让目标姿态在 $\pm$180° 附近连续变化，观察力矩是否出现跳变

💡 概念误区：认为四元数虚部就是旋转角度

新手想法："四元数 $q = (w, x, y, z)$ 的虚部 $(x, y, z)$ 就是绕三个轴的旋转角度"

实际上：四元数虚部 $q_{vec} = \sin(\theta/2) \hat{n}$，其中 $\theta$ 是旋转角度，$\hat{n}$ 是旋转轴。$(x, y, z)$ 不是欧拉角，而是旋转轴的方向乘以半角正弦。当 $\theta$ 很小时 $q_{vec} \approx (\theta/2)\hat{n}$，才近似于"角度向量"的一半

正确理解：四元数虚部是 SO(3) 指数映射的"半角"版本。完整角度信息需要 $\theta = 2\arctan2(\|q_{vec}\|, w)$

练习¶

⭐ 双覆盖验证：构造两个四元数 $q_1 = (0.707, 0, 0, 0.707)$（绕 z 轴转 90°）和 $q_2 = (-0.707, 0, 0, -0.707)$（同一旋转的另一个表示）。验证它们对应相同的旋转矩阵。然后计算 $q_1^{-1} \otimes q_2$ 和 $(-q_1)^{-1} \otimes q_2$，观察哪个给出更小的旋转角。
⭐⭐ 误差公式对比：在 Python 中实现方法 A 和方法 B 的姿态误差计算。让目标旋转从 0° 到 170°（绕 z 轴）均匀变化，绘制两种方法给出的误差范数 $\|e_{rot}\|$ vs. $\theta$ 曲线。在什么角度范围内两者差距超过 10%？
⭐⭐ 耦合刚度设计：设计一个 6$\times$6 刚度矩阵 $K_d$，使得沿 z 方向的位移偏差会额外产生绕 x 轴的恢复力矩（模拟"斜面适应"行为）。写出 $K_d$ 的矩阵形式，并用物理直觉解释 $K_{rp}$ 项的含义。

4. libfranka 阻抗控制代码完整精读 ⭐⭐¶

4.1 动机——代码精读的价值¶

回顾 F03：我们在理论层面推导了完整的笛卡尔阻抗控制律。但理论和工程之间存在鸿沟——理论假设连续时间、完美模型、无硬件限制；工程面对离散时间、模型误差、实时约束。精读 libfranka 示例代码就是弥合这个鸿沟的最佳途径。

libfranka 的 cartesian_impedance_control.cpp 是全球引用最多的阻抗控制参考实现（约 130 行核心代码）。我们将逐段解析，为每一行代码标注它对应的物理/数学含义。

4.2 初始化阶段——控制器启动前的准备¶

// 1. 连接机器人
franka::Robot robot(argv[1]);  // argv[1] = "192.168.1.1"

// 2. 设置碰撞检测阈值（安全层）
setDefaultBehavior(robot);
// 内部调用 robot.setCollisionBehavior(...)
// 设置关节和笛卡尔空间的力矩/力阈值
// 超过阈值 → 进入 reflex 模式 → 自动急停

// 3. 获取动力学模型对象
franka::Model model = robot.loadModel();
// model 提供：
//   model.zeroJacobian(frame, state)  → 6×7 几何雅可比
//   model.coriolis(state)             → 7D 科里奥利力矩
//   model.gravity(state)              → 7D 重力力矩（可查询；FCI 力矩回调通常不要加入）
//   model.mass(state)                 → 7×7 质量矩阵

// 4. 定义阻抗参数
Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
stiffness.setZero();
stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::Matrix3d::Identity();
stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::Matrix3d::Identity();
// stiffness = diag(150, 150, 150, 10, 10, 10)
// 含义：平移方向 150 N/m，旋转方向 10 Nm/rad

damping.setZero();
damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                Eigen::Matrix3d::Identity();
damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                    Eigen::Matrix3d::Identity();
// damping = diag(24.5, 24.5, 24.5, 6.32, 6.32, 6.32)
// 临界阻尼公式：D = 2√K（假设 M_d = I）

关于阻尼公式 $D = 2\sqrt{K}$ 的深入解释：

为什么 $D = 2\sqrt{K}$ 给出临界阻尼？这需要从二阶系统的特征方程推导。

考虑 1D 阻抗方程 $M_d \ddot{e} + D_d \dot{e} + K_d e = 0$。特征方程为：

\[M_d s^2 + D_d s + K_d = 0\]

特征根为 $s = \frac{-D_d \pm \sqrt{D_d^2 - 4M_d K_d}}{2M_d}$。

临界阻尼条件是判别式为零：$D_d^2 = 4M_d K_d$，即 $D_d = 2\sqrt{M_d K_d}$。

libfranka 简化（单位惯量启发式）：令 $M_d = 1$ kg（即假设每个方向的操作空间惯量为 1 kg），得到 $D_d = 2\sqrt{K_d}$。这是一个**启发式简化**，不是精确的临界阻尼。

这个简化的后果：真正的阻尼比 $\zeta = D_d / (2\sqrt{K_d \Lambda})$，其中 $\Lambda$ 是操作空间惯量。由于 $\Lambda$ 是位形相关的（且各方向不同，典型范围 1-10 kg），用 $M_d = 1$ 计算的 $D_d$ 在 $\Lambda > 1$ 时给出 $\zeta < 1$（欠阻尼），导致接触时出现振荡。例如 $\Lambda = 5$ kg 时 $\zeta \approx 0.45$。

CRISP 的解决方案：计算 $\Lambda(q)$ 并使 $D_d = 2\sqrt{K_d \Lambda(q)}$，在所有位形下都保持临界阻尼。代价是每个控制周期都要计算 $\Lambda = (JM^{-1}J^T)^{-1}$（一次 6$\times$6 矩阵求逆）。

4.3 控制回调——1 kHz 实时循环的核心¶

// 5. 读取初始状态并记录为目标位姿
franka::RobotState initial_state = robot.readOnce();
Eigen::Affine3d initial_transform(
    Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
Eigen::Vector3d position_d(initial_transform.translation());
Eigen::Quaterniond orientation_d(initial_transform.rotation());

// 6. 启动控制循环
robot.control([&](const franka::RobotState& robot_state,
                  franka::Duration period) -> franka::Torques {

    // 6a. 读取当前状态
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
    const auto jacobian_array =
        model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
    const auto coriolis_array = model.coriolis(robot_state);
    Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(
        jacobian_array.data());
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(
        coriolis_array.data());

    // 6b. 计算 6D 位姿误差
    Eigen::Affine3d transform(
        Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
    Eigen::Vector3d position(transform.translation());
    Eigen::Quaterniond orientation(transform.rotation());

    // 位置误差
    Eigen::Matrix<double, 6, 1> error;
    error.head(3) << position - position_d;

    // 姿态误差（四元数双覆盖 + 虚部提取）
    if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
    }
    Eigen::Quaterniond error_quaternion(
        orientation.inverse() * orientation_d);
    error.tail(3) << -(transform.rotation() * error_quaternion.vec());

    // 6c. 计算控制力矩
    // τ_task = J^T(-K·e - D·J·dq) ← 阻抗力矩
    Eigen::VectorXd tau_task(7);
    tau_task << jacobian.transpose() *
        (-stiffness * error - damping * (jacobian * dq));

    // τ_d = τ_task + coriolis
    // 注：重力 g(q) 由 FCI 自动补偿，不在用户代码中出现
    Eigen::VectorXd tau_d(7);
    tau_d << tau_task + coriolis;

    // 6d. 限幅与输出
    std::array<double, 7> tau_d_array{};
    Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
    return franka::Torques(tau_d_array);
});

与 F03 理论公式的精确对应：

代码行	对应 F03 公式	备注
`error.head(3) = position - position_d`	$e_{pos} = p - p_d$	位置误差，3D
`error.tail(3) = -R · q_{err}.vec()`	$e_{rot} \approx -(\theta/2)\hat{n}$（小角近似）	姿态误差，3D
`-stiffness * error`	$-K_d \tilde{x}$	弹簧恢复力
`-damping * (jacobian * dq)`	$-D_d \dot{x}$（其中 $\dot{x} = J\dot{q}$）	阻尼力
`jacobian.transpose() * (...)`	$J^T F_{imp}$	笛卡尔力→关节力矩映射
`+ coriolis`	$+ C(q,\dot{q})\dot{q}$	科里奥利补偿
（FCI 内部）	$+ g(q)$	重力补偿

这里的接口约定很关键：franka::Torques 命令按 libfranka 示例写成 tau_task + coriolis，不再加 model.gravity(state)；否则会和 FCI 底层重力补偿叠加成双重补偿。model.gravity(state) 仍然有用，例如做外力估计、离线分析，或迁移到一个要求用户输出完整力矩的非 Franka 后端。

4.4 该示例缺少什么——从"能跑"到"能用"的差距¶

libfranka 的示例是**教学级代码**——尽可能简单，但缺少生产级控制器必需的多个组件：

缺失组件	后果	解决方案（参考）
零空间项	7-DOF 冗余关节不受控，可能漂移到关节限位或奇异构型	franka_ros `CartesianImpedanceExampleController`
惯性整形	阻尼比随构型变化，某些构型下欠阻尼振荡	CRISP OSC 模式（使用 $\Lambda$）
误差 clipping	大偏差命令（RL 策略/通信抖动）可能产生超大力矩	CRISP error clipping
位姿插值	目标位姿跳变时力矩不连续	CRISP 的平滑参考过渡
力前馈	无法跟踪特定的接触力	matthias-mayr CIC wrench 前馈
摩擦补偿	低速运动时关节摩擦导致跟踪误差	CRISP sigmoid 摩擦补偿

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Coriolis 项符号错误

错误做法：tau_d = tau_task - coriolis;（减号而非加号）

现象：低速时控制器看起来正常（$C\dot{q} \approx 0$），但高速运动时末端偏离目标轨迹

根本原因：运动方程为 $M\ddot{q} + C\dot{q} + g = \tau$。要消除 Coriolis 对末端行为的影响，需要 $\tau$ 中包含 $+C\dot{q}$ 来**抵消**动力学中的 $+C\dot{q}$。减号相当于**加倍** Coriolis 效应

自检方法：在空载状态下让机械臂以中等速度做圆周运动，检查末端轨迹是否偏离参考圆

🧠 思维陷阱：认为更高的刚度总是更好

新手想法："$K_d = 2000$ N/m 比 $K_d = 200$ N/m 跟踪精度高 10 倍"

实际上：在自由空间确实如此，但在接触任务中，更高的 $K_d$ 意味着： 1. 接触力更大（$f = K_d \cdot e$），可能超过安全阈值 2. 接触稳定性更差（$\omega_n \propto \sqrt{K_d}$ 提高，可能超出控制带宽） 3. 对模型误差更敏感（高刚度放大了位置标定误差）

正确思维：$K_d$ 不是"越大越好"，而是要在**跟踪精度**和**接触安全/稳定性**之间权衡。工业经验是：自由空间运动用 1000-2000 N/m，接触操作用 100-500 N/m

练习¶

⭐ 代码标注：打开 libfranka 源码 examples/cartesian_impedance_control.cpp，为每一行代码添加注释，标明它对应 F03 中的哪个公式。
⭐ 参数扫描：在 MuJoCo Franka 仿真中，分别设置 $K_{trans} = [50, 150, 500, 2000]$ N/m，让末端跟踪正弦参考 $x_d(t) = x_0 + 0.05\sin(2\pi \cdot 0.5t)$。绘制跟踪误差 $\|e\|$ vs. $K_d$ 曲线。然后让末端接触刚性桌面（$K_e \approx 10^5$ N/m），绘制稳态接触力 vs. $K_d$ 曲线。两条曲线说明了什么 trade-off？
⭐⭐ 零空间补充：参考 franka_ros 的 CartesianImpedanceExampleController，为 libfranka 示例添加零空间项 $\tau_{null} = (I - J^T \bar{J}^T) K_{null} (q_{default} - q)$。在 MuJoCo 中验证：不加零空间时 joint 4 是否会漂移到限位附近？加了零空间后是否改善？

5. 惯性整形与操作空间控制（OSC） ⭐⭐⭐¶

5.1 动机——为什么 $D = 2\sqrt{K}$ 不够好¶

回顾第 4.2 节：libfranka 示例使用 $D_d = 2\sqrt{K_d}$ 作为阻尼，假设了 $M_d = I$（单位惯量）。但真实的操作空间惯量 $\Lambda(q) = (JM^{-1}J^T)^{-1}$ 不是单位矩阵——它是一个位形相关的 6$\times$6 矩阵，不同方向的等效惯量可能差 10 倍以上。

让我们做一个具体计算来说明问题的严重性。

Franka Panda 在默认构型下的操作空间惯量：

Λ(q_default) ≈ diag(4.2, 5.1, 3.8, 0.15, 0.12, 0.08) [kg, kgm²]

如果使用 $K_{trans} = 150$ N/m，$D = 2\sqrt{150} \approx 24.5$ Ns/m，那么各方向的实际阻尼比为：

\[\zeta_x = \frac{D}{2\sqrt{K \cdot \Lambda_x}} = \frac{24.5}{2\sqrt{150 \times 4.2}} = \frac{24.5}{50.2} \approx 0.49\]

\[\zeta_y = \frac{24.5}{2\sqrt{150 \times 5.1}} = \frac{24.5}{55.3} \approx 0.44\]

$\zeta \approx 0.44-0.49$ 是明显的**欠阻尼**——接触时末端会振荡。更糟糕的是：

旋转方向：$\zeta_{rz} = \frac{2\sqrt{10}}{2\sqrt{10 \times 0.08}} = \frac{6.32}{1.79} \approx 3.5$——严重**过阻尼**，响应极其缓慢

这就是"阻尼比随位形和方向变化"的问题。

Worked Example：不同构型下阻尼比的全面计算¶

为了让问题更加具体，我们对 Franka Panda 在三个典型工作构型下做完整的阻尼比计算，展示 $D = 2\sqrt{K}$ 的不足是系统性的，而非个例。

构型 A——默认构型（远离奇异，各关节居中）

q_A = [0, -π/4, 0, -3π/4, 0, π/2, π/4]
Λ_A ≈ diag(4.2, 5.1, 3.8, 0.15, 0.12, 0.08)

K_trans = 150 N/m, D_trans = 2√150 ≈ 24.5 Ns/m
K_rot = 10 Nm/rad, D_rot = 2√10 ≈ 6.32 Nms/rad

ζ_x = 24.5 / (2√(150 × 4.2)) = 24.5 / 50.2 = 0.49  ← 欠阻尼
ζ_y = 24.5 / (2√(150 × 5.1)) = 24.5 / 55.3 = 0.44  ← 欠阻尼
ζ_z = 24.5 / (2√(150 × 3.8)) = 24.5 / 47.7 = 0.51  ← 欠阻尼
ζ_rx = 6.32 / (2√(10 × 0.15)) = 6.32 / 2.45 = 2.58  ← 过阻尼
ζ_ry = 6.32 / (2√(10 × 0.12)) = 6.32 / 2.19 = 2.89  ← 过阻尼
ζ_rz = 6.32 / (2√(10 × 0.08)) = 6.32 / 1.79 = 3.53  ← 严重过阻尼

构型 B——完全伸展（接近肘部奇异）

q_B = [0, 0.3, 0, -0.8, 0, 2.5, π/4]
Λ_B ≈ diag(8.7, 2.1, 6.3, 0.09, 0.22, 0.05)

ζ_x = 24.5 / (2√(150 × 8.7)) = 24.5 / 72.2 = 0.34  ← 严重欠阻尼！
ζ_y = 24.5 / (2√(150 × 2.1)) = 24.5 / 35.5 = 0.69  ← 欠阻尼
ζ_ry = 6.32 / (2√(10 × 0.22)) = 6.32 / 2.97 = 2.13  ← 过阻尼

构型 C——工作空间边界（高操作惯量）

q_C = [0, -0.5, 0, -2.5, 0, 2.0, π/4]
Λ_C ≈ diag(11.2, 12.8, 5.4, 0.08, 0.06, 0.11)

ζ_x = 24.5 / (2√(150 × 11.2)) = 24.5 / 81.9 = 0.30  ← 严重欠阻尼！
ζ_y = 24.5 / (2√(150 × 12.8)) = 24.5 / 87.6 = 0.28  ← 严重欠阻尼！

汇总表：

构型	$\zeta_x$	$\zeta_y$	$\zeta_z$	$\zeta_{rx}$	$\zeta_{ry}$	$\zeta_{rz}$	最差方向
A（默认）	0.49	0.44	0.51	2.58	2.89	3.53	$rz$: 过阻尼 3.5$\times$
B（伸展）	0.34	0.69	0.40	3.34	2.13	4.00	$x$: 严重欠阻尼
C（边界）	0.30	0.28	0.46	3.97	4.17	3.02	$y$: 严重欠阻尼

关键结论：使用 $D = 2\sqrt{K}$ 时，平移方向总是欠阻尼（$\zeta \in [0.28, 0.69]$），旋转方向总是过阻尼（$\zeta \in [2.1, 4.2]$）。在工作空间边界（构型 C），平移方向的阻尼比低至 0.28——这意味着接触时会产生显著的振荡（约 4-5 个周期才衰减）。

反事实推理：如果 Franka 的操作空间惯量是各向同性的（$\Lambda = \lambda I$），$D = 2\sqrt{K}$ 还会有问题吗？此时 $\zeta = 2\sqrt{K} / (2\sqrt{K\lambda}) = 1/\sqrt{\lambda}$。当 $\lambda = 1$ kg 时恰好临界阻尼，但真实机器人的 $\lambda$ 范围是 0.05-13 kg——差距超过 200 倍。所以问题的根源不是公式本身，而是**操作空间惯量的方向依赖性和位形依赖性**。

5.2 惯性整形——让末端"感觉像"期望的惯量¶

核心思想：不仅控制弹簧行为（$K_d$）和阻尼行为（$D_d$），还控制惯量行为（$M_d$）。让末端在所有方向上表现出**统一的、工程师指定的惯量**。

从 F03 的操作空间动力学出发：

\[\Lambda(q)\ddot{x} + \mu(q, \dot{q}) + p(q) = F + f_{ext}\]

其中 $F$ 是操作空间控制力（沿用 Khatib 1987 记法，详见 F02 符号约定表），$f_{ext}$ 是环境施加的外力（时域）。我们的目标是让闭环行为为：

\[M_d \ddot{\tilde{x}} + D_d \dot{\tilde{x}} + K_d \tilde{x} = f_{ext}\]

将两个方程联立，解出所需的控制力：

\[F = \Lambda \ddot{x}_d + \mu + p - \Lambda M_d^{-1}(D_d \dot{\tilde{x}} + K_d \tilde{x}) + (\Lambda M_d^{-1} - I)f_{ext}\]

这就是完整的操作空间阻抗控制律。它需要： 1. $\Lambda(q)$ ——操作空间惯量（每周期计算一次 6$\times$6 矩阵求逆） 2. $\mu(q,\dot{q})$ ——操作空间 Coriolis/离心力 3. $f_{ext}$ ——仅在精确整形到任意 $M_d \neq \Lambda$ 时需要；若取 $M_d=\Lambda$，这一项自然消失 4. $\ddot{x}_d$ ——目标加速度

在本章统一使用 $\tilde{x}=x-x_d$，所以上式中刚度、阻尼项是负号，参考加速度 $\ddot{x}_d$ 是正号。取 $M_d=\Lambda$ 且 $\ddot{x}_d=0$ 时，退化为 libfranka 示例中的 $J^T(-K\tilde{x}-D\dot{x})$，外力仍作为闭环右端由环境施加，而不是作为普通阻抗控制的前馈输入。

这解释了为什么 libfranka 示例不做任意惯性整形——精确整形需要更多模型量；若还要求 $M_d \neq \Lambda$ 的严格闭环行为，还需要可靠的外力估计。简单的 $J^T(-Ke - D\dot{x})$ 更鲁棒。

5.3 CI 与 OSC 的工程对比——CRISP 的数据¶

CRISP（utiasDSL, 2025）系统性地比较了 CI（笛卡尔阻抗，不做惯性整形）和 OSC（操作空间控制，做惯性整形）：

CI:   τ_task = J^T(-K_p · e_clip - K_d · J · dq)
OSC:  τ_task = J^T · Λ · (-K_p · e_clip - K_d · J · dq)
      其中 Λ = (J M⁻¹ J^T)⁻¹
      e_clip 使用本章约定 e = x - x_d；若源码采用 e = x_d - x，弹簧项会等价写成 +K_p · e_clip

指标	CI	CI-clipped	OSC	OSC-clipped
稳态误差（mm）	4.73	0.81	5.54	1.12
最大力矩抖动	低	低	中	中
计算量（$\mu$s/周期）	~50	~55	~120	~130
奇异附近行为	良好	良好	$\Lambda$ 病态	$\Lambda$ 病态

关键发现：CI-clipped 以更简单的计算和更好的奇异鲁棒性，实现了比 OSC 更高的精度。CRISP 的论文解释："OSC 的 $\Lambda$ 矩阵在接近奇异位形时条件数恶化，导致力矩计算不稳定，反而降低了精度。"

本质洞察：惯性整形（OSC）在理论上是更完美的控制——它保证了各方向阻尼比的一致性。但在实际中，$\Lambda(q)$ 的计算依赖精确的动力学模型，而模型误差（关节摩擦、负载未标定、柔性传动）会导致 $\Lambda$ 不准确。用不准确的 $\Lambda$ 做惯性整形可能比不做更差。这就是 CRISP 选择 CI-clipped 作为默认模式的原因——简单、鲁棒、够好。

CRISP 实验数据的深入解读——为什么 CI-clipped 胜出¶

CRISP 论文（San Jose Pro et al. 2025）的对比实验值得更细致地分析。实验设置：Franka FR3 真机，末端跟踪 8 字形轨迹（在桌面上方 5 cm 自由空间 + 桌面接触交替），每组参数跑 10 次取均值。

误差分解——不只是看均值：

控制器	自由空间 RMSE [mm]	接触 RMSE [mm]	奇异附近 RMSE [mm]	力矩突变计数
CI（无 clipping）	3.1	6.8	4.7	12
CI-clipped	1.9	2.3	2.8	0
OSC（无 clipping）	1.5	5.9	15.2	47
OSC-clipped	1.8	3.1	8.7	3

自由空间中 OSC 确实最优（1.5 mm vs CI 的 3.1 mm）——惯性整形保证了各方向一致的阻尼比，跟踪更平滑。但在接触场景和奇异附近，OSC 的优势完全消失甚至反转。

失败模式分析：

OSC 在奇异附近的 RMSE 暴增至 15.2 mm，力矩突变多达 47 次。根本原因是 $\Lambda = (JM^{-1}J^T)^{-1}$ 的条件数在 joint 4 接近 0 rad 时急剧恶化。CRISP 记录的最差条件数达到 $\kappa(\Lambda) > 10^4$，此时 $\Lambda$ 的最大特征值超过 $10^3$ kg——控制力矩被放大到不合理的量级。

CI 模式不使用 $\Lambda$，完全避开了这个问题。CI-clipped 进一步通过误差限幅保证了力矩有界，所以在所有场景下都表现稳健。

模型误差的放大效应：

CRISP 还测试了故意引入 15% 质量参数误差的情况：

模型误差	CI-clipped RMSE 变化	OSC-clipped RMSE 变化
0%（标定精确）	基准	基准
5%	+3%	+11%
15%	+8%	+34%

OSC 对模型误差的敏感度是 CI 的 4 倍——因为 $\Lambda$ 是 $M(q)$ 的函数，$M$ 中 15% 的误差通过矩阵求逆被放大。这个数据强化了一个工程原则：当你不确定模型是否精确时，选择不依赖模型的控制器。

5.4 双对角化阻尼设计——各方向独立阻尼¶

即使不做惯性整形（不使用 $\Lambda$），也可以通过**双对角化**技术实现各方向独立的阻尼比。

核心思想是在 $\Lambda(q)$ 的特征向量基底下设计阻尼矩阵：

\[D_d = V \cdot \text{diag}(2\zeta_i \sqrt{k_i \lambda_i}) \cdot V^T\]

其中 $\Lambda(q) = V \text{diag}(\lambda_i) V^T$ 是 $\Lambda$ 的特征值分解，$\zeta_i$ 是第 $i$ 个方向的期望阻尼比，$k_i$ 是 $K_d$ 在该方向的刚度。

工程实现：

// 双对角化阻尼设计
Eigen::Matrix<double, 6, 6> Lambda =
    (jacobian * mass_inv * jacobian.transpose()).inverse();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>>
    eigen_solver(Lambda);
Eigen::Matrix<double, 6, 6> V = eigen_solver.eigenvectors();
Eigen::Matrix<double, 6, 1> lambda_vals = eigen_solver.eigenvalues();

Eigen::Matrix<double, 6, 6> D_diag;
D_diag.setZero();
for (int i = 0; i < 6; ++i) {
    double k_i = stiffness(i, i);  // 假设对角刚度
    double zeta_i = 1.0;           // 各方向临界阻尼
    D_diag(i, i) = 2.0 * zeta_i * std::sqrt(k_i * lambda_vals(i));
}
damping = V * D_diag * V.transpose();
// 现在 damping 矩阵保证了所有方向都是临界阻尼

⚠️ 常见陷阱¶

⚠️ 编程陷阱：$\Lambda$ 矩阵在奇异附近爆炸

错误做法：直接计算 Lambda = (J * M_inv * J_transpose).inverse();

现象：当机械臂接近奇异位形（如 joint 4 接近 0°），$\Lambda$ 的某些元素突然变成 $10^6$ 以上，输出力矩跳变

根本原因：$\Lambda = (JM^{-1}J^T)^{-1}$。在奇异处 $J$ 降秩，$JM^{-1}J^T$ 接近奇异，其逆矩阵的条件数趋于无穷

正确做法：使用阻尼伪逆：Lambda = (J * M_inv * J_transpose + alpha * I).inverse();，其中 $\alpha = 10^{-4}$ 到 $10^{-3}$。或者在条件数超过阈值时退回不使用 $\Lambda$ 的 CI 模式

💡 概念误区：认为 OSC 总是比 CI 好

新手想法："OSC 理论上解耦了各方向的动力学，所以一定比 CI 更好"

实际上：CRISP 的实验数据（见 5.3 节）清楚地表明，CI-clipped 在稳态精度上优于 OSC。原因是 OSC 对模型精度的要求更高——$\Lambda(q)$ 需要精确的 $M(q)$，而 $M(q)$ 的误差（关节摩擦、柔性传动、负载变化）直接传递到控制律中

正确思维：OSC 适合模型精确且远离奇异的场景；CI 适合模型不太精确或需要奇异鲁棒性的场景。选择取决于**你对模型的信任程度**

练习¶

⭐⭐ 阻尼比计算：给定 Franka Panda 在默认构型下的 $\Lambda \approx \text{diag}(4.2, 5.1, 3.8, 0.15, 0.12, 0.08)$ 和 $K_d = \text{diag}(150, 150, 150, 10, 10, 10)$。分别计算使用 $D = 2\sqrt{K}$ 和使用 $D = 2\sqrt{K\Lambda}$ 时各方向的阻尼比。哪个设计在所有方向都是临界阻尼？
⭐⭐ 奇异分析：在什么 Franka 构型下 $\Lambda$ 的条件数最差？提示：考虑 joint 4 接近 0° 或 joint 6 接近 0° 的构型。写出此时 $J$ 降秩的物理含义（哪个方向的运动"消失"了？）。
⭐⭐⭐ MuJoCo 实验：在 MuJoCo Franka 仿真中实现 CI 和 OSC 两种控制器。让末端跟踪同一条圆形轨迹，分别在构型空间中心和接近奇异的构型下运行。绘制两种控制器的力矩信号和跟踪误差，验证 CRISP 的结论。

6. CRISP 架构精读——七项控制律与 Error Clipping ⭐⭐¶

6.1 动机——从教学示例到研究级控制器¶

libfranka 示例提供了最简单的阻抗控制器——约 130 行代码。但一个可用于 RL 策略部署和精密力控研究的完整控制器需要多少？答案是：CRISP 的 compliant_controller.cpp 约 1200 行，加上辅助类约 3000 行。这 10 倍的代码增长不是冗余，而是覆盖了从安全到性能的七个方面。

6.2 CRISP 的控制律完整分解¶

CRISP（utiasDSL/crisp_controllers, San Jose Pro et al. 2025）的完整控制律由七项力矩叠加组成：

\[\tau = \tau_{task} + \tau_{null} + \tau_{joint\_limit} + \tau_{gravity} + \tau_{coriolis} + \tau_{friction} + \tau_{ff\_wrench}\]

我们逐项分析：

第 1 项：任务力矩 $\tau_{task}$（笛卡尔阻抗）

τ_task = J^T(-K_p · e_clip - K_d · J · dq)

其中 e_clip = clip(e, -e_max, +e_max), e = x - x_d
     e_max = [0.05, 0.05, 0.05, 0.3, 0.3, 0.3]  (m, rad)

Error clipping 是 CRISP 的核心创新。为什么要 clip？

考虑一个 RL 策略在探索阶段输出了一个异常的目标位姿——比当前位姿偏离 0.2 m。不做 clipping 时，阻抗力矩 $\tau = J^T K_d \cdot 0.2 = J^T \times 150 \times 0.2 = J^T \times 30$ N。这可能导致末端以较大速度运动，对环境和机器人都有风险。

Clipping 将误差限制在 $\pm e_{max}$ 范围内：$\tau_{max} = J^T K_d \times e_{max} = J^T \times 150 \times 0.05 = J^T \times 7.5$ N——安全得多。

反事实推理：如果不用 clipping 而用力矩限幅（限制 $\tau$ 而不是 $e$）会怎样？力矩限幅确实也能保证安全，但它引入了一个问题：当力矩被限幅时，阻抗行为不再是线性弹簧-阻尼器——它变成了饱和函数，闭环稳定性证明不再成立。误差 clipping 保持了阻抗律的线性结构（在 clipping 区间内），稳定性分析更容易。

第 2 项：零空间关节阻抗 $\tau_{null}$

\[\tau_{null} = (I_7 - J^T \bar{J}^T) \cdot (K_{null} (q_{nominal} - q) - D_{null} \dot{q})\]

物理含义：在不影响末端行为的前提下，让 7 个关节趋向"舒适构型" $q_{nominal}$。

零空间参数	典型值	物理含义
$K_{null}$	10-50 Nm/rad	零空间"弹簧"——关节趋向默认构型的力度
$D_{null}$	$2\sqrt{K_{null}}$ Nm$\cdot$s/rad	零空间"阻尼"——防止关节振荡
$q_{nominal}$	[0, -$\pi$/4, 0, -3$\pi$/4, 0, $\pi$/2, $\pi$/4]	Franka 的"舒适构型"——远离奇异和关节限位

零空间阻抗的高级配置——三种策略的权衡

零空间行为的设计远比"指向默认构型"复杂。工程中有三种常见的零空间策略，它们在不同任务场景下各有优势：

策略	零空间力矩公式	适用场景	优势	代价
关节居中	$(I-J^T\bar{J}^T)K_{n}(q_{nom}-q)$	重复性任务	构型可预测	可能限制末端可达性
避奇异	$(I-J^T\bar{J}^T)K_{n}\nabla w(q)$	连续轨迹跟踪	远离奇异 $\to$ $\Lambda$ 良态	目标构型不固定
避关节限位	$(I-J^T\bar{J}^T)\sum \frac{\partial H}{\partial q_i}$	大范围运动	防止机械碰撞	可能与居中冲突

其中，避奇异策略使用操纵度（manipulability）梯度 $\nabla w(q) = \frac{\partial}{\partial q}\sqrt{\det(JJ^T)}$ 作为零空间目标函数。当机械臂接近奇异时，$w(q) \to 0$，梯度指向远离奇异的方向——零空间力矩驱使关节远离奇异。

三种策略的融合——CRISP 和 matthias-mayr CIC 都支持通过加权混合同时使用多种策略：

\[\tau_{null} = (I - J^T\bar{J}^T)[\alpha_1 K_n(q_{nom}-q) + \alpha_2 K_w \nabla w(q) + \alpha_3 \tau_{limit}]\]

其中 $\alpha_1 + \alpha_2 + \alpha_3 = 1$ 是可在线调节的权重。在 rqt 调参面板中，用户可以根据当前任务实时调节三个权重——例如在需要快速大幅运动时增大 $\alpha_2$（避奇异），在精密装配时增大 $\alpha_1$（构型可重复性）。

第 3 项：关节限位势垒 $\tau_{joint\_limit}$

对每个关节 i:
  if q_i > q_max_i - δ:
    τ_limit_i = -k_barrier * (q_i - (q_max_i - δ))
  elif q_i < q_min_i + δ:
    τ_limit_i = -k_barrier * (q_i - (q_min_i + δ))
  else:
    τ_limit_i = 0

δ = 0.1 rad（安全裕度）
k_barrier = 200 Nm/rad（高刚度虚拟弹簧）

类比：关节限位势垒就像保龄球道两侧的护栏——正常运行时不起作用，但当关节接近极限时产生强烈的弹性回复力，防止碰撞到机械止动器。

第 4/5 项：重力和 Coriolis 补偿 $\tau_{gravity}, \tau_{coriolis}$

CRISP 的完整控制律显式保留 $g(q)$ 和 $C(q,\dot{q})\dot{q}$ 两项，并用 Pinocchio 计算它们。原因有二：

CRISP 支持多种机器人（Franka FR3、Kinova Gen3），需要统一的动力学计算
Pinocchio 的 RNEA 算法比 FCI 内部模型更可控——可以使用 SysId 标定后的参数

但实际发送到硬件时必须服从后端接口约定：如果后端是 MuJoCo 或原始 effort 接口，通常需要发送包含 $g(q)$ 的完整力矩；如果后端是 libfranka franka::Torques，适配层应避免把重力项再次送入 FCI，否则会双重补偿。Coriolis 项不属于这个自动重力补偿约定，使用 libfranka 示例时仍按需要显式加入 model.coriolis(state)。

第 6 项：摩擦补偿 $\tau_{friction}$

τ_friction_i = (f_c + f_v · |dq_i|) · sigmoid(dq_i / v_thresh)

f_c：库仑摩擦力矩（常数）
f_v：粘性摩擦系数
sigmoid：平滑的符号函数，避免零速时的不连续
v_thresh = 0.01 rad/s（过渡区宽度）

为什么需要摩擦补偿？Franka Panda 的谐波减速器在低速时有显著的库仑摩擦（约 0.5-2 Nm），如果不补偿，低速阻抗跟踪会有明显的"死区"——外力小于摩擦力矩时末端不动。

第 7 项：前馈 wrench $\tau_{ff\_wrench}$

\[\tau_{ff} = J^T F_{ff}\]

当已知期望的末端力/力矩时（如恒力打磨），通过前馈直接叠加到控制律中，不需要等阻抗控制器产生位置偏差后才间接产生力。

6.3 Error Clipping 的数学分析¶

Error clipping 的数学形式为：

\[e_{clip,i} = \text{clip}(e_i, -e_{max,i}, +e_{max,i}) = \begin{cases} -e_{max,i} & \text{if } e_i < -e_{max,i} \\ e_i & \text{if } |e_i| \leq e_{max,i} \\ +e_{max,i} & \text{if } e_i > +e_{max,i} \end{cases}\]

稳定性分析：

在 clipping 区间内（$|e_i| \leq e_{max,i}$），控制律是标准的线性阻抗——F03 的 Lyapunov 稳定性证明直接适用。

在 clipping 区间外（$|e_i| > e_{max,i}$），弹簧力饱和为常数 $K_d \cdot e_{max}$。此时系统行为类似于**恒力控制**——末端以恒定力朝目标方向运动，直到误差回到 clipping 区间内。

clipping 对无源性的影响（与 F06 的预告联系）：

无源性要求存储函数 $V(e) = \frac{1}{2} e^T K_d e$ 满足 $\dot{V} \leq f_{ext}^T \dot{x}$。当使用 clipping 时：

\[\dot{V} = e^T K_d \dot{e} \neq e_{clip}^T K_d \dot{e}\]

在 clipping 区间外，控制力基于 $e_{clip}$ 而存储函数基于真实 $e$，两者不一致——无源性被破坏。

本质洞察：CRISP 的 error clipping 是一个**安全性 vs. 无源性**的 trade-off。clipping 保证了力矩有界（安全），但破坏了无源性（理论上可能导致能量注入）。F06 将介绍 Control Barrier Function（CBF）作为潜在的替代方案——CBF 可以同时保证安全和无源性，但计算量更大且实时性待验证。

6.4 CRISP 的两层架构——学习策略 + 高频柔顺¶

┌── GPU 策略节点（Python, 10-50 Hz）──────────────────────┐
│  Diffusion Policy / ACT / SmolVLA                        │
│  输入：图像 + 力反馈 + 状态                               │
│  输出：target_pose (x_d, q_d)                            │
│  通信：ROS2 DDS 话题                                     │
└────────────────────── DDS ──────────────────────────────┘
                        ↕ 10-50 Hz
┌── C++ 柔顺控制器（1 kHz）───────────────────────────────┐
│  CRISP compliant_controller                              │
│  接收 target_pose → 计算阻抗力矩 → 发送 tau_d           │
│  七项叠加 + error clipping + limitRate                   │
│  保证：接触安全、力矩平滑、实时性                        │
└────────────────────── FCI/ros2_control ─────────────────┘

为什么需要两层？

问题	单层解决方案	困难
学习策略直接输出力矩	RL 策略以 1 kHz 输出 $\tau$	GPU 推理延迟 10-100 ms，远超 1 kHz 要求
控制器直接做感知	纯 C++ 控制器处理图像	图像处理需要 GPU，不适合实时线程

两层架构的解决方案：策略在低频做"决策"（去哪里），控制器在高频做"执行"（怎么安全地到达）。策略的输出是目标位姿 $x_d$，不需要实时——即使策略有 50 ms 延迟，控制器仍然以 1 kHz 维持柔顺接触。

类比：这就像人的运动系统——大脑皮层以低频（5-10 Hz）做运动规划和决策，脊髓反射弧以高频（100+ Hz）维持姿态稳定和碰撞反射。大脑不需要以 100 Hz 运行——它只需要告诉脊髓"把手移到那个位置"，脊髓负责安全执行。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Error clipping 的 $e_{max}$ 设置过大

错误做法：设置 $e_{max} = [0.5, 0.5, 0.5, 1.0, 1.0, 1.0]$（500 mm, 57°）

现象：clipping 几乎从不触发，效果等同于没有 clipping。当 RL 策略输出异常目标时，大力矩仍然会出现

正确做法：$e_{max}$ 应根据任务范围设定。精密操作：$e_{max} = [0.02, 0.02, 0.02, 0.1, 0.1, 0.1]$（20 mm, 6°）。一般操作：$[0.05, 0.05, 0.05, 0.3, 0.3, 0.3]$（50 mm, 17°）

🧠 思维陷阱：认为 CRISP 的七项叠加是随意选择

新手想法："为什么恰好是七项？能不能少几项？"

实际上：七项中每一项解决一个具体问题。去掉任何一项都会产生可观测的性能下降： - 去掉零空间 → 冗余关节不受控 → 漂移到奇异/限位 - 去掉摩擦补偿 → 低速跟踪有死区 → 力分辨率下降 - 去掉关节限位 → 可能碰到机械止动器 → 硬件损坏 - 去掉 Coriolis → 高速运动时跟踪偏差 → 安全风险

正确思维：七项叠加不是"冗余"，而是"完备"——它覆盖了力矩控制型机器人的所有已知问题

练习¶

⭐ CRISP 数据流：画出 CRISP 的完整数据流图：target_pose → error → clipping → τ_task → 七项叠加 → limitRate → FCI。标注每个环节的输入/输出维度和频率。
⭐⭐ Clipping 仿真：在 Python 中模拟一个 1D 阻抗系统 $m\ddot{x} + d\dot{x} + kx = f_{ext}$，分别实现有 clipping 和无 clipping 的版本。让参考位置突然跳变 0.3 m（大偏差）。绘制位移、速度和力矩时间曲线，观察 clipping 的效果。
⭐⭐ CRISP 精读：克隆 CRISP 仓库 (utiasDSL/crisp_controllers)，找到 compliant_controller.cpp 中的 computeTorque() 函数。标注七项控制律在代码中的位置（行号），并画出代码-公式对应表。

7. matthias-mayr CIC——工程级笛卡尔阻抗控制器 ⭐⭐¶

7.1 定位——研究者的"即插即用"控制器¶

如果说 libfranka 示例是"Hello World"，CRISP 是"学习型力控底座"，那么 matthias-mayr 的 Cartesian Impedance Controller（CIC）就是**研究者日常使用的生产级控制器**：

GitHub: matthias-mayr/Cartesian-Impedance-Controller (~180★, BSD-3)
核心文件: src/cartesian_impedance_controller.cpp
代码量: ~800 行核心 + ~500 行辅助
ROS 版本: ROS1 Noetic（已有 ROS2 移植版本）

7.2 CIC 的六大工程特性¶

特性	libfranka 示例	CIC	工程价值
刚度矩阵	6$\times$6 对角	完整 6$\times$6（含耦合项）	斜面/曲面操作
阻尼设计	$2\sqrt{K}$	阻尼重塑（可自定义阻尼矩阵）	各方向独立阻尼比
奇异处理	无	Damped Least Squares（$J^T(JJ^T + \alpha I)^{-1}$）	奇异附近平滑退化
力前馈	无	Wrench 前馈（$\tau_{ff} = J^T F_{ff}$）	恒力跟踪/打磨/装配
力限幅	无	Wrench 饱和（限制最大笛卡尔力）	安全保护
在线调参	无	rqt 动态调参（实时 GUI 修改 K/D/wrench）	开发效率

7.3 完整 6$\times$6 刚度矩阵的工程实现¶

CIC 支持非对角刚度矩阵，这在 calculateCommandedTorques() 函数中通过以下方式实现：

// CIC 的核心力矩计算
// calculateCommandedTorques() 中的关键逻辑（简化版）

// 1. 从参数服务器读取 6×6 刚度矩阵
// stiffness_matrix_ 可以是对角阵或完整矩阵
Eigen::Matrix<double, 6, 6> K = stiffness_matrix_;

// 2. 阻尼矩阵设计——支持两种模式
Eigen::Matrix<double, 6, 6> D;
if (use_automatic_damping_) {
    // 自动模式：D = 2ζ√(K·Λ)，考虑操作空间惯量
    Eigen::Matrix<double, 6, 6> Lambda =
        (jacobian * mass_inv * jacobian.transpose()).inverse();
    D = 2.0 * damping_ratio_ *
        (K * Lambda).sqrt();  // 矩阵平方根！
} else {
    // 手动模式：直接指定 D 矩阵
    D = damping_matrix_;
}

// 3. 计算笛卡尔力（含前馈 wrench）
Eigen::Matrix<double, 6, 1> F_cart;
F_cart = -K * error - D * (jacobian * dq) + wrench_feedforward_;

// 4. Wrench 饱和（安全保护）
for (int i = 0; i < 6; ++i) {
    F_cart(i) = std::clamp(F_cart(i), -max_wrench_(i), max_wrench_(i));
}

// 5. 映射到关节力矩
tau_task = jacobian.transpose() * F_cart;

矩阵平方根 $(K \cdot \Lambda)^{1/2}$ 的含义：

这是双对角化阻尼设计（第 5.4 节）的矩阵形式。$D = 2\zeta \sqrt{K\Lambda}$ 保证了在 $\Lambda$ 的所有特征方向上阻尼比都等于 $\zeta$。但 $\sqrt{K\Lambda}$ 需要 $K\Lambda$ 是半正定的（并非总是成立，当 $K$ 和 $\Lambda$ 的特征向量不对齐时）。CIC 使用 Schur 分解来计算矩阵平方根，处理了非交换性问题。

7.4 rqt 动态调参——开发效率的关键¶

CIC 集成了 ROS dynamic_reconfigure，允许在控制器运行时通过 GUI 实时修改参数：

可调参数：
  ├── translational_stiffness: 50-2000 [N/m]
  ├── rotational_stiffness: 5-200 [Nm/rad]
  ├── damping_ratio: 0.5-2.0 [无量纲]
  ├── nullspace_stiffness: 0-100 [Nm/rad]
  ├── wrench_x/y/z/rx/ry/rz: -50 to +50 [N/Nm]
  └── enable_wrench_feedforward: true/false

调参最佳实践：

先从低刚度开始（$K = 100$ N/m），逐步增大，观察是否出现振荡
阻尼比保持 1.0（临界），如果振荡则增大到 1.2-1.5
wrench 前馈从 0 开始，逐步增加到目标力值
零空间刚度设置为任务刚度的 1/10-1/5

⚠️ 常见陷阱¶

⚠️ 编程陷阱：非对角刚度矩阵不满足正定性

错误做法：随意设置 $K_{pr}$（位置-旋转耦合项）的值

现象：控制器在某些构型下突然失稳

根本原因：$K_d$ 必须是**正定矩阵**才能保证稳定性（F03 Lyapunov 分析的前提）。非对角项过大可能破坏正定性

正确做法：设置后检查所有特征值是否 > 0：assert(stiffness.eigenvalues().real().minCoeff() > 0)

💡 概念误区：认为 wrench 前馈等同于力控

新手想法："设置 wrench_z = -10（向下 10 N）就实现了力控"

实际上：wrench 前馈是**开环**力控——它假设末端会精确产生 10 N，但不检查实际力是否为 10 N。如果环境刚度变化或位置有偏差，实际力会偏离目标。真正的**闭环**力控需要力传感器反馈 + PI 力跟踪（F03 Chiaverini 架构），而不仅仅是前馈

wrench 前馈的正确定位：用于补偿已知的恒定负载（如工具重力）或提供粗略的力参考，而非精确力跟踪

练习¶

⭐ CIC 安装与运行：克隆 matthias-mayr/Cartesian-Impedance-Controller，在 Franka 仿真环境中配置并运行。使用 rqt 动态调参 GUI，实时修改刚度和观察末端行为变化。
⭐⭐ CIC vs. libfranka 对比：对比 CIC 和 libfranka 示例的 calculateCommandedTorques() 和 cartesian_impedance_control.cpp，列出 CIC 多出的功能及其代码位置。
⭐⭐ wrench 前馈实验：在 MuJoCo 中让 Franka 末端接触水平桌面。分别用 (a) 纯阻抗（$K_z = 200$）和 (b) 阻抗 + wrench 前馈（$K_z = 200$，$F_{ff,z} = -5$ N）实现恒定法向力。对比两种方式的稳态力精度。

8. 可变阻抗参数的实时调节策略 ⭐⭐⭐¶

8.1 动机——为什么固定 K 和 D 不够¶

固定的阻抗参数在单一任务场景下工作良好，但大多数实际任务涉及**多阶段**或**变化的约束条件**：

任务阶段	需要的行为	参数要求
接近工件	快速精确到位	高 $K$，中 $D$
建立接触	柔顺触碰	低 $K$（法向），高 $D$
稳定接触操作	恒力跟踪	中 $K$，中 $D$
退出接触	安全脱离	低 $K$，高 $D$

如果用固定参数覆盖所有阶段，必然在某些阶段性能不佳——高 $K$ 在建立接触时产生过大的力，低 $K$ 在自由空间跟踪精度不足。

8.2 三种变阻抗策略¶

策略 A：状态机切换（最简单）

switch (task_phase) {
    case APPROACH:
        K_trans = 1500.0;  K_rot = 100.0;  zeta = 1.0;
        break;
    case CONTACT:
        K_trans = 200.0;   K_rot = 20.0;   zeta = 1.2;
        break;
    case OPERATE:
        K_trans = 500.0;   K_rot = 50.0;   zeta = 1.0;
        break;
    case RETRACT:
        K_trans = 300.0;   K_rot = 30.0;   zeta = 1.5;
        break;
}

问题：阶段切换时参数跳变 → 力矩不连续 → 机械冲击。

解决方案：使用指数平滑过渡：

// 指数平滑：K 在 τ_smooth 时间内平滑过渡到目标值
K_current = K_current + (K_target - K_current) * (1 - exp(-dt / tau_smooth));
// tau_smooth = 0.1-0.5 s（过渡时间常数）

策略 B：连续参数化（基于传感器）

让阻抗参数根据传感器信号连续变化：

// 力依赖的刚度调节
// 力大 → 降低刚度（更柔顺）；力小 → 升高刚度（更精确）
double f_mag = f_ext.norm();
double K_target = K_max - (K_max - K_min) * sigmoid(f_mag, f_thresh, alpha);
// sigmoid 提供平滑过渡
// f_thresh = 5 N（开始降低刚度的力阈值）
// alpha = 2.0（sigmoid 斜率）

问题：K 时变 → 系统可能不再无源 → 稳定性需要额外保证。这正是 F06 的核心主题。

策略 C：学习型变阻抗（RL/DMP）

RL 策略直接输出阻抗参数 $(K_d, D_d)$ 作为动作的一部分：

# RL 策略输出
action = policy(observation)
# action = [delta_x(3), delta_rot(3), K_trans(3), K_rot(3)]
#                                      ^^^^^^^^^^^^^^^^^
#                                      学习型阻抗参数

Buchli et al. 2011（IJRR）首次展示了用 PI$^2$ 策略梯度学习变刚度轮廓——让刚度随轨迹时间变化，自动发现最优的"接近用高刚度、接触用低刚度"策略。

这些变阻抗方法的**稳定性保证**是一个深刻的理论问题，将在 F06 中通过**能量罐**和 **Kronander-Billard 阻尼条件**系统化地解决。

8.3 参数过渡的平滑性要求¶

为什么参数不能突变？考虑能量的视角。

阻抗控制器的存储能量为 $V = \frac{1}{2} \tilde{x}^T K_d \tilde{x}$。

当 $K_d$ 在 $t_0$ 时刻从 $K_1$ 跳变到 $K_2$ 时，存储能量从 $\frac{1}{2}\tilde{x}^T K_1 \tilde{x}$ 跳变到 $\frac{1}{2}\tilde{x}^T K_2 \tilde{x}$。

如果 $K_2 > K_1$（升刚度），存储能量**瞬间增加**——这等价于向系统注入能量。能量注入违反了无源性约束，可能导致不稳定。

本质洞察：变阻抗控制的核心困难不是"如何改变参数"（那很简单），而是"如何在改变参数时不向系统注入能量"。这个问题有两个系统化的解决方案：Ferraguti 的能量罐方法和 Kronander-Billard 的阻尼条件，它们是 F06 的核心内容。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为变阻抗只需要改变 K 就行

新手想法："$K$ 变了，$D$ 保持不变就好"

实际上：$D$ 必须随 $K$ 同步调整以维持阻尼比。如果 $K$ 从 200 跳到 2000 而 $D$ 不变，阻尼比从 1.0 下降到 0.316——严重欠阻尼 → 振荡。正确做法是同步更新 $D = 2\zeta\sqrt{K \cdot M}$（或 $D = 2\zeta\sqrt{K}$ 的简化版）

⚠️ 编程陷阱：在实时回调中计算 sigmoid 用 std::exp 不做边界检查

现象：std::exp 本身很快（<1 $\mu$s），但如果参数极大/极小（如 $e^{1000}$），可能产生 NaN/Inf

正确做法：使用 clamped sigmoid：先限制输入范围再取指数
double safe_sigmoid(double x, double center, double alpha) {
    double arg = std::clamp(-alpha * (x - center), -20.0, 20.0);
    return 1.0 / (1.0 + std::exp(arg));
}

练习¶

⭐ 过渡平滑实验：在 Python 中模拟 1D 阻抗系统，让 $K$ 从 200 突变到 2000。分别绘制 (a) 瞬间跳变和 (b) 指数平滑过渡（$\tau_{smooth} = 0.2$ s）的位移、力矩和存储能量曲线。能量跳变有多大？
⭐⭐ 力依赖变阻抗：在 MuJoCo Franka 仿真中实现力依赖的刚度调节：外力 > 5 N 时 $K$ 降低到 100 N/m，外力 < 1 N 时 $K$ 升高到 1000 N/m。让末端在桌面上做往复运动（交替接触/脱离），观察刚度切换的效果。
⭐⭐⭐ 跨章综合题：结合 F01（阻抗概念）、F03（控制律推导）和本章（工程实现），设计一个完整的 peg-in-hole 四阶段阻抗控制器。规格：8 mm 直径销插入 8.1 mm 直径孔，深度 30 mm。为每个阶段指定 $K_d, D_d$ 和切换条件。在 MuJoCo 中实现并测试。

9. franka_ros2 CartesianImpedanceExampleController 精读 ⭐⭐¶

9.1 与 libfranka 示例的关键差异¶

franka_ros（ROS1）提供了 CartesianImpedanceExampleController，基于 controller_interface。franka_ros2（ROS2）中对应的控制器为 CartesianImpedanceExampleController，基于 ros2_control 框架（接口有所不同）。以下以 ROS1 版本的代码为例（ROS2 版本结构类似但使用 hardware_interface 和生命周期管理），它在 libfranka 示例基础上增加了两个关键组件：

组件 1：零空间项

// 零空间关节阻抗
Eigen::MatrixXd jacobian_pinv = jacobian.completeOrthogonalDecomposition()
                                        .pseudoInverse();
Eigen::MatrixXd null_projector = Eigen::MatrixXd::Identity(7, 7)
                                 - jacobian.transpose() * jacobian_pinv.transpose();
Eigen::VectorXd tau_nullspace = null_projector *
    (nullspace_stiffness * (q_d_nullspace - q)
     - (2.0 * sqrt(nullspace_stiffness)) * dq);

数学含义：上述代码使用的是 Moore-Penrose 伪逆 (completeOrthogonalDecomposition().pseudoInverse())，投影矩阵为 $N_{MP} = I - J^T (J^T)^{+}_{MP}$。

重要区别：这**不是**动力学一致的零空间投影。动力学一致投影需要质量矩阵加权伪逆 $\bar{J}^T = M^{-1} J^T (J M^{-1} J^T)^{-1}$，对应的投影矩阵为 $N_{dyn} = I - J^T \bar{J}^T$。动力学一致投影保证零空间力矩**在加速度层面**不影响末端运动（$\ddot{x}_{null} = 0$），而 Moore-Penrose 投影只保证在速度/力层面正交——在有惯量耦合的系统中可能产生末端加速度扰动。对于 Franka 这类轻型臂，两者差异通常较小，但严格的理论实现应使用动力学一致投影。

回顾 M08：对于 7-DOF 机器人（如 Franka），Jacobian $J$ 是 6$\times$7 矩阵，零空间维度为 $7 - 6 = 1$。这意味着有一个"多余的"自由度可以用来优化次要目标（如关节居中、避奇异、避障碍）而不影响末端位姿。

组件 2：平衡位姿过渡

// 通过 ROS 话题接收新的目标位姿
// 使用指数衰减过渡（filtering）
position_d_ = filter_params_ * position_d_target_ +
              (1.0 - filter_params_) * position_d_;
orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_);
// filter_params_ = 0.001 → 约 1000 个周期（1 秒）完成过渡

为什么需要过渡？ 当外部节点发布新的目标位姿时，如果控制器立刻跳到新目标，位姿误差会突变 → 力矩突变 → 机械冲击。指数衰减过渡确保目标位姿平滑变化，力矩连续。

9.2 ros2_control ControllerInterface 的实时约束¶

在 ros2_control 框架中，控制器的 update() 方法运行在实时线程中，与 FCI 回调有相同的约束：

controller_interface::return_type
CartesianImpedanceExampleController::update(
    const rclcpp::Time& time,
    const rclcpp::Duration& period)
{
    // 这里也是实时环境——相同的禁止操作
    // 不能 new/malloc, 不能 cout, 不能网络 I/O

    // 读取状态 → 计算误差 → 计算力矩 → 写入命令接口
    // ...

    return controller_interface::return_type::OK;
}

与 libfranka 回调的对比：

维度	libfranka 回调	ros2_control update()
调用频率	1 kHz（固定）	由 controller_manager 配置（通常 1 kHz）
通信方式	直接 UDP 到控制柜	通过 HardwareInterface 抽象层
状态获取	`franka::RobotState` 结构体	`state_interfaces_` 向量
命令输出	返回 `franka::Torques`	写入 `command_interfaces_`
框架开销	极小（直接 C++ 回调）	中等（经过 controller_manager 调度）

⚠️ 常见陷阱¶

⚠️ 编程陷阱：零空间投影矩阵用了 Moore-Penrose 伪逆而不是动力学一致伪逆

错误做法：J_pinv = J.completeOrthogonalDecomposition().pseudoInverse()（这是 Moore-Penrose 伪逆 $J^+$）

后果：零空间运动会在末端产生加速度——因为 Moore-Penrose 伪逆不考虑质量矩阵，零空间力矩的反作用力未被正确投影

正确做法（F03 已证明）：使用动力学一致伪逆 $\bar{J} = M^{-1}J^T\Lambda$：
Eigen::Matrix<double, 6, 6> Lambda =
    (jacobian * M_inv * jacobian.transpose()).inverse();
Eigen::Matrix<double, 7, 6> J_bar =
    M_inv * jacobian.transpose() * Lambda;
Eigen::MatrixXd null_projector =
    Eigen::MatrixXd::Identity(7,7)
    - jacobian.transpose() * J_bar.transpose();
注意：franka_ros 示例确实使用了 Moore-Penrose 伪逆作为简化——这在低速运动下误差较小，但在高速动态任务中会产生可观测的末端扰动

练习¶

⭐ 零空间实验：在 MuJoCo Franka 中分别运行有零空间和无零空间的阻抗控制器。让末端保持固定位姿 60 秒，每 10 秒记录一次 7 个关节的位置。无零空间时哪些关节容易漂移？
⭐⭐ 伪逆对比：在 Python 中分别计算 Moore-Penrose 伪逆和动力学一致伪逆。给定 $J, M, q$（使用 Pinocchio 获取），比较两种伪逆的零空间投影矩阵。差异有多大？在什么构型下差异最大？
⭐⭐ 位姿过渡调参：修改 filter_params_ 从 0.001 到 0.1，在仿真中观察目标位姿突变时的力矩波形。什么值给出最平滑的力矩？什么值给出最快的响应？两者之间的 trade-off 是什么？

10. 与 ros2_control Admittance Controller 的关系 ⭐⭐¶

10.1 阻抗控制 vs. 导纳控制——工程选型的最终决策¶

回顾 F01 第 2.4 节：阻抗控制（运动输入→力矩输出）和导纳控制（力输入→运动输出）是同一物理行为的两种因果实现。本章讲的是阻抗控制（libfranka/CRISP/CIC），下一章 F05 讲导纳控制（ros2_controllers/FZI FDCC）。

选型决策树：

你的机器人有力矩级控制接口吗？
├── 是（Franka FCI / KUKA iiwa / DLR LWR）
│   → 用阻抗控制（本章内容）
│   ├── 需要学习型策略集成？→ CRISP
│   ├── 需要 rqt 在线调参？→ matthias-mayr CIC
│   └── 只需要基础阻抗？→ libfranka 示例
│
├── 否（UR / Fanuc / ABB / 大多数工业臂）
│   → 用导纳控制（F05 内容）
│   ├── 有 F/T 传感器？→ ros2_controllers admittance_controller
│   └── 无传感器？→ FZI FDCC（虚拟正向动力学）
│
└── 不确定
    → 先看硬件接口类型：
      position → 导纳
      velocity → 导纳
      effort   → 阻抗

10.2 两种范式的性能对比¶

指标	阻抗控制（力矩输出）	导纳控制（位置输出）
力分辨率	高（直接控制力矩）	受限于位置环精度
带宽	高（1 kHz 力矩环）	低（导纳环 + 内环位置环）
阻尼精度	高（直接设定阻尼力矩）	受内环影响
适用硬件	力矩接口（Franka/iiwa）	位置/速度接口（UR/Fanuc）
稳定性裕度	取决于模型精度	取决于内环带宽
实现复杂度	高（需要精确动力学）	低（导纳律 + 标准位控）
工业普及度	中（需要特殊硬件）	高（大多数工业臂支持）

本质洞察：阻抗控制和导纳控制的选择不是"哪个更好"的问题，而是**硬件接口决定的必然选择**。如果你的机器人只接受位置/速度命令（绝大多数工业臂），你只能用导纳控制——不管阻抗控制理论上多优越。这就像你不能在只支持 2G 网络的手机上使用 5G——不是 5G 不好，而是硬件不支持。

10.3 混合架构——阻抗+导纳的协同¶

某些先进系统同时使用两种控制范式：

示例：Franka + ros2_control 混合架构

层 1（底层）: libfranka FCI → 1 kHz 力矩控制
  内含：重力补偿 + 关节阻抗（安全层）

层 2（中层）: ros2_control effort_controllers
  内含：笛卡尔阻抗控制器（CRISP/CIC）

层 3（顶层）: 导纳层（可选）
  内含：外部力 → 目标位姿修正 → 送给层 2
  用途：当力传感器提供更准确的外力信息时，
        导纳层可以生成更精确的位姿参考

这种混合架构在 CRISP 中通过 wrench feedforward 隐式实现——外力信息通过前馈通道调节阻抗控制器的行为，等效于在阻抗控制上叠加了一层简单的导纳。

练习¶

⭐ 选型练习：为以下三个场景选择阻抗控制或导纳控制，并说明理由和推荐的控制器实现： (a) UR10e 搬运已知重量的箱子 (b) Franka Panda 在未知曲面上打磨 (c) ABB IRB 120 + ATI Gamma F/T 传感器做精密装配
⭐⭐ 带宽对比实验：在 MuJoCo 中搭建两个控制器——阻抗控制器（力矩输出）和导纳控制器（位置输出 + 内环 PD）。让两者跟踪相同的正弦参考（频率从 0.1 到 10 Hz），绘制幅频响应曲线。导纳控制器的带宽在哪里开始明显衰减？
⭐⭐⭐ 跨章综合题：结合 F01（理论分类）、F03（算法推导）、F04（阻抗工程实现）的知识，写一篇 1 页的技术备忘录，为一个新建的机器人实验室推荐力控技术栈。实验室有：2 台 Franka Panda、3 台 UR5e、1 台 Kinova Gen3。考虑统一的 ros2_control 框架、不同机器人的最佳控制模式、以及与学习型策略（CRISP）的集成路径。

11. 力控稳定性与无源性——为什么"调好 K 和 D"还不够 ⭐⭐⭐¶

11.1 动机——一个在自由空间完美的控制器，为什么一碰桌子就发疯¶

前面十节我们反复提到"接触刚性环境时振荡""高刚度接触必然失稳""error clipping 破坏无源性"，但始终把严格的稳定性分析推给 F06。本节补上这块**理论拼图的核心**——它解释了一个让无数初学者困惑的现象：

一个在自由空间运行得完美无瑕、跟踪误差小于 1 mm 的笛卡尔阻抗控制器，一旦末端接触到刚性桌面，立刻爆发出剧烈的高频振荡（典型 30-80 Hz），甚至触发力矩超限急停。把 $K_d$ 调小一点能缓解，但又牺牲了自由空间精度。这背后到底发生了什么？

这个现象不是 bug，也不是参数没调好——它是**接触动力学的本质属性**。理解它需要三个层层递进的概念：耦合稳定性（coupled stability）、无源性（passivity）、以及采样离散化对无源性的破坏。这三个概念共同回答了力控领域最深刻的问题之一："一个控制器单独看是稳定的，环境单独看也是稳定的，为什么把它们连起来反而不稳定？"

反面——如果用经典 SISO 稳定性思维会怎样¶

经典控制课程教的稳定性分析（Routh-Hurwitz、根轨迹、Nyquist）默认系统的输入输出是**固定**的——你设计一个控制器 $C(s)$ 去控制一个已知对象 $P(s)$，分析闭环 $\frac{CP}{1+CP}$ 的极点。但力控的对象**不是固定的**：

自由空间运动时，对象是机械臂动力学 $P_{robot}(s)$
接触刚性环境时，对象变成机械臂 + 环境的串联/并联组合 $P_{robot}(s) \| P_{env}(s)$

环境 $P_{env}$ 可能是软橡胶（$K_e \approx 10^3$ N/m）、硬铝板（$K_e \approx 10^6$ N/m）、人手（时变、非线性），甚至是另一个机器人。你无法为每一种可能的环境单独证明稳定性——这就是反面：用固定对象的 SISO 思维，你需要对无穷多种环境逐一验证，根本不可行。

本质洞察：力控稳定性的根本难点在于**环境是未知且可变的**。经典稳定性分析问"这个闭环稳定吗"，力控稳定性必须问"对于任意一个属于某类别的环境，这个闭环都稳定吗"。这个从"单点稳定"到"全类别稳定"的跨越，正是无源性理论的用武之地——它给出了一个**与具体环境无关**的充分条件。

历史——Hogan 与 Colgate 的两条线索¶

阻抗控制的稳定性理论有两位奠基者，分别从不同角度切入：

Neville Hogan（MIT，1985） 提出阻抗控制框架时就指出：机器人与环境的交互应该被建模为两个**端口**（port）的耦合，机器人在端口处呈现一个阻抗 $Z_{robot}$，环境呈现一个导纳 $Y_{env}$。稳定性取决于二者的乘积 $Z_{robot} Y_{env}$。
J. Edward Colgate（MIT/Northwestern，1988） 在博士论文《Robust control of dynamically interacting systems》中给出了第一个严格的**耦合稳定性定理**：如果环境是无源的，且机器人在交互端口呈现无源阻抗，则耦合系统稳定。他还给出了基于根轨迹的简单判据，并把结论扩展到一类有源环境。

这两条线索在 1990 年代汇合成现代力控稳定性理论的主干。本节按"耦合稳定性 → 无源性 → 离散化破坏 → 工程对策"的顺序展开。

11.2 耦合稳定性定理——Colgate-Hogan 的核心结论¶

考虑机器人与环境在**单一交互端口**（末端执行器）处耦合。把系统建模为一个二端口网络的串联：机器人侧呈现驱动点阻抗 $Z_r(s) = F(s)/V(s)$（速度 $V$ 输入、力 $F$ 输出），环境侧呈现驱动点导纳 $Y_e(s) = V(s)/F(s)$。

定义（驱动点阻抗的无源性）：一个单端口线性时不变系统，其驱动点阻抗 $Z(s)$ 称为**无源的**，当且仅当它是**正实函数**（positive real function），即满足三个条件：

$Z(s)$ 在右半平面（$\text{Re}(s) > 0$）解析（无右半平面极点）
$Z(s)$ 在虚轴上的极点是单极点，且留数为正实数
对所有 $\omega$，$\text{Re}\,Z(j\omega) \geq 0$

第三条是最直观的：$\text{Re}\,Z(j\omega) \geq 0$ 意味着在任意频率下，阻抗的实部（耗能部分）非负——系统**不会在任何频率上产生能量**。

Colgate-Hogan 耦合稳定性定理（1988，单端口线性情形）：

设机器人侧驱动点阻抗 $Z_r(s)$ 与环境侧驱动点导纳 $Y_e(s)$ 均为无源（正实），则二者在单一端口耦合形成的闭环系统**渐近稳定**。

证明思路（Nyquist + 正实性）：耦合系统的特征方程为 $1 + Z_r(s) Y_e(s) = 0$。环路传递函数 $L(s) = Z_r(s) Y_e(s)$。关键引理：两个正实函数的乘积，其 Nyquist 轨迹 $L(j\omega)$ 的相位被限制在 $[-\pi, \pi]$ 内——因为正实函数的相位本身在 $[-\pi/2, +\pi/2]$ 内（$\text{Re} \geq 0$ 等价于相位不超过 $\pm 90°$）。两个相位各在 $\pm 90°$ 内的函数相乘，总相位在 $\pm 180°$ 内，因此 $L(j\omega)$ 永远不会包围 $-1$ 点。由 Nyquist 判据，闭环稳定。$\blacksquare$

本质洞察：耦合稳定性定理的威力在于它**解耦了机器人和环境的设计**。你不需要知道环境的具体参数（$K_e$ 是 $10^3$ 还是 $10^6$），只要环境是无源的（绝大多数被动物理环境——弹簧、阻尼、质量——都是无源的），并且你保证机器人侧阻抗无源，稳定性就自动成立。这是一个**"一次证明，对所有无源环境成立"**的结论——正是 11.1 节所求的"全类别稳定"。

几何直觉——为什么"两个不产生能量的东西连起来不会爆"：

无源系统的物理本质是"只消耗或储存能量，不产生能量"。把两个无源系统连接起来，整个系统仍然只消耗或储存能量——没有能量来源，振荡就不可能无限放大（振荡放大需要持续的能量注入）。这就像把两个被动的机械弹簧-阻尼器连在一起，无论怎么连，系统总能量都是有界的，最终必然衰减到平衡。

一个反例点醒——纯弹簧（无阻尼）阻抗是否无源？¶

考虑一个理想的纯刚度阻抗 $Z_r(s) = K/s$（速度到力，纯弹簧）。检验正实性：$Z_r(j\omega) = K/(j\omega) = -jK/\omega$，其实部 $\text{Re}\,Z_r(j\omega) = 0$。它恰好落在无源性的**边界**上（marginally passive）——实部非负但等于零，意味着纯弹簧不耗能、只储能。

反事实推理：如果你的阻抗控制器只有刚度没有阻尼（$D_d = 0$），它处于无源性边界。理论上仍满足 $\text{Re} \geq 0$，但**没有任何耗能裕度**——任何额外的相位滞后（采样延迟、滤波延迟、传动柔性）都会把实部推向负值，使系统变为有源，进而失稳。这就是为什么**实践中阻抗控制器必须有阻尼**：阻尼提供了 $\text{Re}\,Z_r(j\omega) = D_d > 0$ 的正实部裕度，吸收掉离散化和延迟引入的"负能量"。这也从理论上解释了第 4.2 节为什么默认 $D = 2\sqrt{K}$ 而不是 $D = 0$。

11.3 连续时间阻抗控制的无源性——一个干净的结论¶

先看理想情况：连续时间、完美模型、无延迟。此时笛卡尔阻抗控制器在末端端口呈现的阻抗为：

\[Z_r(s) = M_d s + D_d + \frac{K_d}{s}\]

（这是目标阻抗 $M_d \ddot{x} + D_d \dot{x} + K_d x = F$ 在速度-力端口的传递函数，由 $F = (M_d s + D_d + K_d/s)V$ 得到。）

检验正实性，计算其虚轴实部：

\[\text{Re}\,Z_r(j\omega) = \text{Re}\left(M_d j\omega + D_d + \frac{K_d}{j\omega}\right) = \text{Re}\left(D_d + j\left(M_d\omega - \frac{K_d}{\omega}\right)\right) = D_d\]

只要 $D_d > 0$，就有 $\text{Re}\,Z_r(j\omega) = D_d > 0$ 对所有 $\omega$ 成立。再检查极点：$Z_r(s)$ 只有一个虚轴极点 $s = 0$（来自 $K_d/s$ 项），留数为 $K_d > 0$，满足正实性第二条。因此：

连续时间结论：理想笛卡尔阻抗控制器（$M_d, D_d, K_d$ 均正定，$D_d > 0$）在末端端口是无源的。由 Colgate-Hogan 定理，它与任意无源环境耦合都稳定——无论环境多硬。

这是一个**令人愉悦的干净结论**：在连续时间理想世界里，阻抗控制器接触任意刚度的桌子都稳定，哪怕 $K_e = 10^{12}$ N/m。

反事实推理：既然连续时间下无论多硬的环境都稳定，为什么 11.1 节描述的真机振荡还会发生？答案必然不在连续时间理论里——问题出在**从连续到离散的过渡**。真机控制器以 1 kHz 采样运行，零阶保持（ZOH）、速度差分、低通滤波都引入了连续模型里没有的相位滞后。下一小节揭示：正是采样把一个理论上无源的控制器变成了**条件无源**——只在 $K_d$ 不太大时才无源。

11.4 离散化如何摧毁无源性——采样虚拟墙与 $b > KT/2$ 判据 ⭐⭐⭐¶

这是本节最重要、也最反直觉的部分。我们将证明：即使连续时间阻抗是无源的，以采样周期 $T$ 离散化后，它只在满足一个具体不等式时才保持无源。这个不等式直接解释了真机的接触振荡。

采样虚拟墙模型¶

把问题简化到 1 自由度，剥离掉机器人动力学，只看控制器与环境接触的核心机制。这正是触觉（haptics）领域研究了三十年的**采样虚拟墙**（sampled virtual wall）模型——一个数字控制器试图渲染一面虚拟的弹簧-阻尼墙：

连续的物理世界                  数字控制器（采样周期 T）
─────────────────             ─────────────────────────
末端位置 x(t)  ──→  采样 ──→  x[k]（每 T 秒一次）
(传感器，连续)                       │
                                    ↓  计算虚拟墙力
                              f[k] = K·x[k] + B·v[k]
                                    │
力 f(t)  ←──  零阶保持(ZOH) ←──  f[k]（保持 T 秒不变）
(施加到物理世界)

关键在于两处离散化伪影：

零阶保持（ZOH）：控制器在 $t=kT$ 时刻算出力 $f[k]$，然后**保持这个力不变直到下一个采样时刻** $(k+1)T$。在这 $T$ 秒内，末端可能已经移动了，但施加的力却是基于"过时的"位置。
速度差分：连续速度 $v(t)$ 不可直接测量，只能用位置差分 $v[k] \approx (x[k]-x[k-1])/T$ 近似，这引入了半个采样周期的相位滞后。

能量分析——ZOH 注入的"负能量"¶

无源性要求系统在任意时间窗内消耗的能量非负：$\int_0^t f(\tau) v(\tau) \, d\tau \geq -E_0$（$E_0$ 是初始储能）。我们计算采样虚拟墙在一个接触-脱离循环中的净能量。

考虑纯刚度墙 $f[k] = K x[k]$（先不加阻尼）。末端以恒定速度 $v$ 压入再退出。在压入阶段（$x$ 增大）和退出阶段（$x$ 减小），由于 ZOH 把力"延迟"了，力-位移曲线 $f$-$x$ 不再是一条重合的直线，而是张开成一个**回滞环（hysteresis loop）。回滞环的**面积等于每个循环净注入系统的能量。

Colgate 与 Brown（1994）的经典分析给出：纯刚度采样墙每个周期向系统注入的能量正比于 $\frac{1}{2}K T |v| \cdot \Delta x$ 量级——刚度 $K$ 越大、采样周期 $T$ 越大、运动越快，注入的能量越多。这个注入的能量就是振荡的能量来源：当注入速率超过系统自身的耗散速率时，振荡幅度持续增长，直到非线性饱和（力矩限幅）或发散（急停）。

Colgate-Brown 采样无源性判据¶

要让采样虚拟墙重新变回无源，必须有足够的阻尼来吸收 ZOH 注入的能量。Colgate 与 Brown（1994，《Factors Affecting the Z-Width of a Haptic Display》）推导出著名的**离散时间无源性充分条件**：

\[\boxed{\;b > \frac{K T}{2} + |B|\;}\]

其中： - $b$ 是系统中的**物理阻尼**（机械臂关节摩擦、传动阻尼等真实存在的、连续的耗散） - $K$ 是虚拟墙（控制器）的刚度 - $B$ 是虚拟墙的阻尼（控制器命令的阻尼，注意它和物理阻尼 $b$ 不同——$B$ 由 ZOH 离散化，不一定耗能甚至可能注入能量） - $T$ 是采样周期

这个不等式是力控工程中最重要的设计公式之一。它的物理含义震撼人心：

本质洞察：让数字阻抗控制器保持无源（从而对任意无源环境稳定）的，不是你在代码里写的虚拟阻尼 $B$，而是机器人本身固有的物理阻尼 $b$。控制器的虚拟刚度 $K$ 越大，所需的物理阻尼 $b$ 就越大（线性增长，斜率为 $T/2$）。如果机器人物理阻尼不足，无论你把虚拟阻尼 $B$ 调到多大都无法稳定——因为 $B$ 本身也被采样离散化，在高频处同样会注入能量。

这解释了三个真机现象：

为什么提高采样率（减小 $T$）能让接触更稳：$b > KT/2$ 中 $T$ 减半，允许的 $K$ 翻倍。这就是 Franka 坚持 1 kHz、CRISP 强调高频柔顺层的根本原因。从 250 Hz 提到 1 kHz，可稳定渲染的刚度上界提高 4 倍。
为什么物理阻尼大的机器人（如带谐波减速器的工业臂）接触更稳：固有阻尼 $b$ 大，$b > KT/2$ 的余量大。这也是 Colgate-Brown 论文的"惊人结论"——固有阻尼对可达阻抗范围（Z-width）有压倒性影响。
为什么纯刚度墙（$B=0$）一定会"嗡嗡"响：没有虚拟阻尼时全靠物理阻尼 $b$ 抵抗 $KT/2$，余量最小，最容易在接触瞬间激发可闻的高频振荡。

Worked Example：Franka 接触桌面的稳定刚度上界¶

用 $b > KT/2$ 估算 Franka Panda 能稳定渲染的笛卡尔刚度上界。取末端等效物理阻尼 $b \approx 5$ Ns/m（Franka 末端方向的固有阻尼，量级估计），采样周期 $T = 1$ ms：

\[K < \frac{2b}{T} = \frac{2 \times 5}{0.001} = 10000 \text{ N/m}\]

如果还命令了虚拟阻尼 $B = 20$ Ns/m（典型 $2\sqrt{150 \times 1}$ 量级附近），按 $b > KT/2 + |B|$ 的保守形式，纯靠物理阻尼能支撑的刚度还要再扣掉 $B$ 的影响。这个 $10^4$ N/m 量级的上界与工程经验高度吻合：Franka 在自由空间可用 1000-2000 N/m，接触刚性环境时降到 100-500 N/m——后者远在稳定上界内，留足了模型误差和未建模柔性的安全裕度。

理论-工程桥接：第 4.4 节"思维陷阱：认为更高刚度总是更好"现在有了严格的理论支撑。"接触操作用 100-500 N/m"不是拍脑袋的经验值，而是 $b > KT/2$ 判据留足安全裕度后的工程结论。当有人问"为什么不能把接触刚度也设成 2000"时，正确答案是："因为 $2000 > 2b/T$ 的安全余量太小，采样离散化注入的能量会超过物理阻尼的耗散能力，接触必然振荡。"

11.5 Z-width——阻抗控制器的"动态范围"¶

Colgate-Brown 把"一个力控设备能稳定渲染的阻抗范围"定义为 Z-width（阻抗宽度），它是衡量力控硬件+控制器性能的核心指标：

\[\text{Z-width} = [Z_{min}, Z_{max}]\]

$Z_{max}$（最硬）：能稳定渲染的最大阻抗——受 $b > KT/2$ 判据限制，即"最硬能模拟到多硬的墙而不振荡"。
$Z_{min}$（最软）：能呈现的最小阻抗——受摩擦、传动惯量限制，即"松手时机器人能变得多'透明'、多自由"。理想的零阻抗意味着完全不阻碍人手运动（背驱动性，backdrivability）。

影响因素	对 $Z_{max}$（硬度上界）的影响	对 $Z_{min}$（软度下界）的影响
采样率 $1/T$	提高采样率 → $Z_{max}$ 提高（线性）	几乎无影响
物理阻尼 $b$	增大 $b$ → $Z_{max}$ 大幅提高（压倒性因素）	增大 $b$ → $Z_{min}$ 升高（变笨重，变差）
速度滤波	滤波延迟 → $Z_{max}$ 降低	滤波平滑 → 噪声小，$Z_{min}$ 可更低
位置量化	量化噪声 → 限制 $Z_{max}$	量化死区 → 限制 $Z_{min}$
传动背隙	背隙非线性 → 难以保证无源	背隙 → 死区，$Z_{min}$ 变差

本质洞察：$Z_{max}$ 和 $Z_{min}$ 之间存在一个根本张力。物理阻尼 $b$ 是把双刃剑——增大 $b$ 让你能渲染更硬的墙（$Z_{max}$ 提高），但同时让机器人在"应该自由"时变得笨重（$Z_{min}$ 变差）。Franka 用关节力矩传感器 + 高带宽力矩环把固有摩擦补偿掉，本质上是在**人为降低有效物理阻尼以改善 $Z_{min}$（背驱动性）**，再用 1 kHz 高采样率把 $Z_{max}$ 拉回来——这是两个指标的精妙权衡。Weir 与 Colgate（2008）进一步用"主动电气阻尼"在不牺牲 $Z_{min}$ 的前提下提高 $Z_{max}$，是这个张力的高级解法。

11.6 时域无源性控制（TDPC）——在线保证无源的工程手段¶

$b > KT/2$ 是一个**保守的离线判据**——它要求最坏情况下也无源，因此限制了 $K$ 的上界。但在实际运行中，系统大部分时间远未达到能量注入的最坏情况。能不能**在线监测**实际注入的能量，只在快要破坏无源时才补救？这正是 Hannaford 与 Ryu（2002）的**时域无源性控制**（Time Domain Passivity Control, TDPC）。

TDPC 由两个组件构成：

1. 无源性观测器（Passivity Observer, PO）：实时积分系统产生的能量。在每个采样周期累计端口处的能量流：

\[E_{obs}[k] = \sum_{j=0}^{k} f[j] \, v[j] \, T\]

只要 $E_{obs}[k] \geq -E_{stored}$（系统未净产生能量），系统保持无源。当 PO 检测到 $E_{obs}$ 即将变负（系统开始产生能量、即将注入"有源能量"），触发补救。

2. 无源性控制器（Passivity Controller, PC）：注入**自适应虚拟阻尼**来吸收掉多余的能量。PC 实时计算需要多少阻尼才能让 PO 重新非负：

\[\alpha[k] = \begin{cases} \dfrac{-E_{obs}[k]}{T \, v[k]^2} & \text{if } E_{obs}[k] < 0 \text{ 且 } v[k] \neq 0 \\ 0 & \text{otherwise} \end{cases}\]

然后在控制力上叠加耗散项 $f_{PC}[k] = -\alpha[k] v[k]$。

本质洞察：TDPC 的精妙之处在于它是**自适应且最小干预**的——只在系统真正快要产生能量的那几个采样周期注入阻尼，平时完全不干预。这与 $b > KT/2$ 的"永远保守"形成对比：离线判据为了应对从未发生的最坏情况，牺牲了平均性能；TDPC 按需补救，既保证无源又不过度牺牲 $Z_{max}$。代价是需要可靠的能量测量——力和速度信号的噪声会让 PO 的积分漂移，因此实际系统常用带遗忘因子的能量参考（Adaptive Energy Reference）。

TDPC 与本章其他机制的关系：

机制	保证什么	何时介入	代价
$b > KT/2$ 离线判据（11.4）	无源（最坏情况）	设计阶段（限制 $K$ 上界）	牺牲 $Z_{max}$
CRISP error clipping（6.3）	力矩有界（安全）	误差超限时	破坏无源性
力矩速率限幅 limitRate（2.2）	执行器不过载	力矩变化过快时	引入相位滞后
TDPC（本节）	无源（实际情况）	能量即将变负时	需可靠能量测量
能量罐（F06 预告）	无源 + 支持变阻抗	储能耗尽时	实现复杂

这张表把全章散落的"安全机制"统一到一个框架下：它们都是在**安全性、无源性、性能**这个三角中做不同的取舍。error clipping 偏向安全（牺牲无源），TDPC 偏向无源（牺牲实现简单度），$b>KT/2$ 偏向简单（牺牲性能）。理解这个三角，你就理解了力控工程的核心权衡。

11.7 接触失稳的完整因果链——从现象到根因¶

现在我们可以把 11.1 节的现象用一条完整的因果链解释清楚：

末端接触刚性桌面（K_e 大）
        ↓
环境刚度 K_e 与控制器刚度 K_d 串联，等效接触刚度 ≈ min(K_d, K_e) 主导
        ↓
接触瞬间，末端速度突变 → 速度差分 v[k]=(x[k]-x[k-1])/T 产生尖峰 + 半周期滞后
        ↓
ZOH 把"过时"的力保持 T 秒 → f-x 回滞环张开 → 每周期注入能量 ∝ K_d·T·|v|
        ↓
若 K_d 大到使注入速率 > 物理阻尼 b 的耗散速率（即 b < K_d·T/2）
        ↓
端口处净能量持续为正 → 系统在接触模态下变为有源
        ↓
有源 + 接触模态的高自然频率（ω_n ∝ √(K_eff/M)）→ 30-80 Hz 极限环振荡
        ↓
振荡幅度增长直到力矩饱和（limitRate）或触发安全监控 → 急停

每一个环节都对应一个可操作的修复手段：

因果环节	修复手段	对应章节
速度差分尖峰	速度信号低通滤波（但加滤波延迟）/ 用模型估计速度	12.3
ZOH 注入能量	提高采样率（减小 $T$）	2.2
$b < K_d T/2$	降低接触刚度 $K_d$ 至稳定上界内	4.4, 11.4
物理阻尼不足	增大命令阻尼 $D_d$（有限帮助）/ 选物理阻尼大的硬件	11.5
有源能量注入	TDPC 在线吸收 / 能量罐	11.6, F06
接触模态高频	接触前减速、软着陆（降低接触瞬间 $	v

理论-工程桥接：这条因果链是本章故障排查手册第一行"末端接触时高频振荡"的完整理论展开。排查手册给的是"降低 K_d / 检查控制频率"这样的操作指令，本节给的是**为什么**这些指令有效的物理机制。一个只会照排查手册操作的工程师能解决 80% 的问题；理解了这条因果链的工程师能解决剩下 20% 的疑难杂症——比如"为什么我已经降低了 $K_d$ 还是振荡"（答案可能在速度滤波延迟或传动背隙，而非 $K_d$）。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为只要 $D_d$ 足够大就能稳定任意刚度的接触

新手想法："振荡是因为阻尼不够，那我把命令阻尼 $D_d$ 调到非常大不就稳了？"

实际上：命令的虚拟阻尼 $D_d$（即判据中的 $B$）本身也要被采样和 ZOH 离散化。在高频处，离散化的虚拟阻尼**同样会注入能量**而非耗散能量——这就是判据写成 $b > KT/2 + |B|$ 而不是 $b + B > KT/2$ 的原因：$B$ 出现在不等式的"消耗侧"反而以 $+|B|$ 加重了负担。真正能无条件耗能、对抗 ZOH 注入的，是**连续的物理阻尼 $b$**。过大的 $D_d$ 还会在高频放大测量噪声，使速度信号尖峰更严重

正确思维：稳定接触靠的是"物理阻尼 $b$ + 采样率 $1/T$ + 合理的 $K_d$"三者配合，命令阻尼 $D_d$ 的作用有限且有反效果区间。优先提高采样率和降低 $K_d$，而非无脑加 $D_d$

💡 概念误区：把"控制器稳定"和"接触稳定"当成一回事

新手想法："我的控制器在自由空间跑了一小时都没问题，它显然是稳定的。"

实际上：自由空间稳定性（控制器 + 机器人动力学）和接触稳定性（控制器 + 机器人 + 环境）是**两个不同的稳定性问题**。一个在自由空间无条件稳定的控制器，接触刚性环境时可能失稳，因为环境改变了闭环对象。耦合稳定性定理（11.2）正是专门处理后者的工具

正确理解：力控控制器必须通过**两道**稳定性检验——自由空间稳定（标准闭环分析）和耦合稳定（无源性 / Colgate-Hogan）。前者必要但不充分

⚠️ 概念误区：认为无源性是稳定性的必要条件

新手想法："既然无源保证稳定，那不稳定一定是因为不无源，稳定的控制器一定无源。"

实际上：无源性是耦合稳定的**充分条件**，不是必要条件。一个非无源（有源）的控制器，面对某些特定环境仍可能稳定——error clipping 后的 CRISP 就是例子（破坏无源但实测稳定且高精度）。无源性的价值在于它提供**对所有无源环境的保证**；放弃无源性意味着你只能对具体环境逐一验证稳定，失去了"一次证明，全类别成立"的优势

正确思维：无源是"安全但保守"的设计哲学。当你需要对未知环境的鲁棒保证时，坚持无源；当环境已知且固定（如实验室固定工件）且需要极致性能时，可以谨慎放弃无源换取性能——但要为此承担逐环境验证的责任

练习¶

⭐⭐ 正实性验证：判断以下三个驱动点阻抗是否无源（正实）：(a) $Z_1(s) = Ms + D + K/s$（$M,D,K>0$）；(b) $Z_2(s) = K/s$（纯刚度）；(c) $Z_3(s) = \frac{Ms+D+K/s}{1+\tau s}$（带一阶延迟，$\tau>0$，模拟滤波/采样滞后）。对 (c)，求使 $\text{Re}\,Z_3(j\omega) \geq 0$ 对所有 $\omega$ 成立的条件，说明为什么延迟会破坏无源性。
⭐⭐ 采样无源性判据应用：某机器人末端物理阻尼 $b = 8$ Ns/m，采样周期可选 $T \in \{0.5, 1, 2, 4\}$ ms。对每个 $T$，计算能稳定渲染的最大刚度 $K_{max} = 2b/T$。画出 $K_{max}$-$T$ 曲线。如果任务需要渲染 $K = 5000$ N/m 的虚拟墙，采样率至少要多少 Hz？
⭐⭐⭐ TDPC 仿真：在 Python 中实现 1 自由度采样虚拟墙 + 时域无源性控制器。墙刚度 $K = 8000$ N/m，采样 $T = 1$ ms，物理阻尼 $b = 2$ Ns/m（故意设小，使 $b < KT/2 = 4$，即无 PC 时不无源）。让一个虚拟质量块以 0.1 m/s 撞墙。对比 (a) 无 PC 和 (b) 有 PC 两种情况的能量观测器曲线 $E_{obs}[k]$ 和位移曲线，验证 PC 能把 $E_{obs}$ 钳在非负、抑制振荡。
⭐⭐⭐ 跨章综合题：结合 F03（连续时间 Lyapunov 稳定性证明）和本节（离散时间无源性），解释一个表面矛盾：F03 证明了笛卡尔阻抗控制器全局渐近稳定，为什么真机还会接触失稳？写出 F03 证明中**哪个假设**在真机上被违反，以及这个假设的破坏如何对应到 $b > KT/2$ 判据。

12. 工程实现的数值陷阱深挖——细节决定成败 ⭐⭐¶

12.1 动机——魔鬼藏在 Jacobian 的坐标系约定里¶

前面的代码精读聚焦在"公式如何映射到代码"，但真机调试中消耗工程师最多时间的，往往不是控制律本身，而是**约定不一致**——Jacobian 表达在哪个坐标系、四元数的存储顺序、速度信号的滤波、矩阵平方根的数值稳定性。这些细节单独看都很小，但任何一个搞错都会让控制器行为诡异且难以定位。本节系统化地梳理这些"隐形地雷"。

12.2 Jacobian 坐标系约定——zeroJacobian 到底表达在哪个系？¶

笛卡尔阻抗控制律 $\tau = J^T F$ 看似简单，但 $J$ 和 $F$ 必须表达在同一个坐标系，否则映射完全错误。这是真机调试中最隐蔽的 bug 来源之一。

机器人学中 Jacobian 有三种常见约定，它们把关节速度 $\dot{q}$ 映射到**不同坐标系下表达**的末端空间速度：

约定	表达坐标系	数学定义	Pinocchio 枚举
空间/世界 Jacobian	基坐标系（world）	$V^{world} = J_{world} \dot{q}$	`WORLD`（注意是空间速度，含原点项）
物体 Jacobian	末端坐标系（body/EE）	$V^{ee} = J_{body} \dot{q}$	`LOCAL`
混合 Jacobian	原点在末端、轴向与世界对齐	$V^{mix} = J_{mix} \dot{q}$	`LOCAL_WORLD_ALIGNED`

libfranka 的 model.zeroJacobian(Frame::kEndEffector, state) 返回的是**基坐标系（world frame）下表达、参考点在末端**的几何 Jacobian——对应 Pinocchio 的 LOCAL_WORLD_ALIGNED 约定（轴与世界对齐，参考点在末端原点）。因此 libfranka 示例中：

error.head(3) << position - position_d;          // 位置误差表达在 world 系
error.tail(3) << -(transform.rotation() * error_quaternion.vec());  // 姿态误差旋到 world 系
// 注意这个 transform.rotation() * (...) ——它把末端系的误差旋到 world 系
// 正是为了和 world 系的 zeroJacobian 匹配！
tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));

本质洞察：第 3.2 节 Step 4 那个看似神秘的 transform.rotation() * error_quaternion.vec()（把姿态误差从末端系旋到基系）现在有了清晰的理由——它不是为了什么深刻的几何，纯粹是**为了让误差 $e$ 和 Jacobian $J$ 表达在同一坐标系**。四元数误差 $q^{-1}\otimes q_d$ 天然在末端系，而 zeroJacobian 在世界系，所以必须左乘 $R$ 把误差搬到世界系。如果你换用 bodyJacobian（LOCAL），就**不能**乘这个 $R$，否则坐标系又错位了。

两种自洽的实现组合（必须成套使用，不能混搭）：

组合	Jacobian	位置误差	姿态误差	速度
World 系（libfranka）	`zeroJacobian`（LWA）	$p - p_d$（world）	$-R\,\text{Im}(q^{-1}q_d)$（旋到 world）	$J_{LWA}\dot{q}$
Body 系	`bodyJacobian`（LOCAL）	$R^T(p-p_d)$（旋到 body）	$\text{Im}(q^{-1}q_d)$（保持 body，不乘 $R$）	$J_{LOCAL}\dot{q}$

反事实推理：如果误差用 world 系但 Jacobian 误用了 body 系（LOCAL）会怎样？$\tau = J_{body}^T F_{world}$ 把世界系的力当成末端系的力来映射。结果是：当末端姿态偏离初始姿态时，恢复力的方向会**随姿态旋转而错位**——你想往世界 $+z$ 推，机器人却往一个倾斜的方向推。在初始姿态附近（$R \approx I$）看起来正常，姿态一转就出问题。这种 bug 极难定位，因为它在"机器人没怎么转"的测试场景里完全不暴露。

12.3 速度信号——微分噪声与滤波延迟的两难¶

阻抗律的阻尼项需要末端速度 $\dot{x} = J\dot{q}$，而关节速度 $\dot{q}$ 的质量直接决定阻尼项的质量。这里有一个贯穿所有运动控制的根本矛盾。

问题的来源：很多机器人不直接测量速度，而是对位置编码器差分得到：$\dot{q}[k] = (q[k]-q[k-1])/T$。差分是一个高通操作，放大高频噪声——编码器 1 个 LSB 的量化噪声，经过 $1/T$（$T=1$ ms 时即 $\times 1000$）放大后变成显著的速度噪声尖峰。这个噪声乘以阻尼 $D_d$ 后变成力矩噪声，让关节"嗡嗡"抖动。

两难：

不滤波 → 速度噪声大 → 力矩抖动、电机发热、可闻噪声
滤波（低通）→ 噪声小，但引入**相位滞后** → 阻尼力"迟到" → 等效于减小有效阻尼 → 逼近 11.4 节的失稳边界

// ❌ 朴素差分：噪声极大
dq[k] = (q[k] - q[k-1]) / dt;

// ⚠️ 一阶低通：减噪但加延迟
// alpha 小 → 滤波强、延迟大；alpha 大 → 滤波弱、延迟小
dq_filtered[k] = alpha * dq_raw[k] + (1 - alpha) * dq_filtered[k-1];
// 截止频率 f_c = -ln(1-alpha)/(2π·dt)，相位滞后 ≈ atan(2π·f·dt·(1-alpha)/alpha)

// ✅ 更好：Franka 在控制柜内用观测器/状态估计提供 dq
// state.dq 已经是滤波过的、相对干净的速度估计
// 优先使用硬件提供的 dq，而非自己差分 q

理论-工程桥接：速度滤波的截止频率选择直接关系到 11.5 节的 Z-width。滤波越强（截止频率越低），速度信号越干净但相位滞后越大，$Z_{max}$ 越低（能稳定渲染的最大刚度下降）。Colgate-Brown 1994 明确把"速度滤波"列为影响 Z-width 的四大因素之一。工程经验：速度滤波截止频率通常取控制带宽的 2-5 倍（$f_c \approx 50$-$200$ Hz @ 1 kHz 控制），在噪声和延迟间折中。Franka 在控制柜内已经做了精心调校的速度估计，用户应优先信任 state.dq 而非自己对 state.q 差分。

本质洞察：阻尼项是阻抗控制中最"脏"的一项——它依赖速度，而速度是最难干净获取的信号。刚度项用位置（干净）、惯性项用加速度（更脏但通常不显式用），唯独阻尼项卡在"需要速度但速度有噪声"的尴尬位置。这就是为什么 11.4 节的失稳总是和阻尼/速度纠缠在一起——失稳的能量注入和速度信号的滞后/噪声本质上是同一个硬币的两面。

12.4 矩阵平方根——$\sqrt{K\Lambda}$ 的数值陷阱¶

第 5.4 节和第 7.3 节的双对角化阻尼用到了矩阵平方根 $D_d = 2\zeta\sqrt{K\Lambda}$。标量平方根人人会，但**矩阵平方根有三个反直觉的陷阱**。

陷阱 1：$\sqrt{AB} \neq \sqrt{A}\sqrt{B}$（非交换性）。当 $K$ 和 $\Lambda$ 不可交换（特征向量不对齐）时，$K\Lambda$ 甚至可能**不对称**——而不对称矩阵的"平方根"定义本身就有歧义。

陷阱 2：$K\Lambda$ 不一定是对称正定的。$K$ 对称正定、$\Lambda$ 对称正定，但乘积 $K\Lambda$ 一般**不对称**（除非二者可交换）。Eigen::Matrix::sqrt() 对非对称矩阵会走一般的 Schur 算法，可能返回复数结果或数值不稳定。

正确做法——在 $\Lambda$ 的特征基底下设计（即 5.4 节的对称化方法）：

// ❌ 错误：直接对 K*Lambda 开方
Eigen::Matrix<double,6,6> D = 2.0 * (K * Lambda).sqrt();  // K*Lambda 非对称！

// ✅ 正确：对称化处理（5.4 节方法）
// 在 Λ 的特征向量基底下，K 和 Λ 同时（近似）对角化
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6>> es(Lambda);
Eigen::Matrix<double,6,6> V = es.eigenvectors();      // 正交基
Eigen::Matrix<double,6,1> lam = es.eigenvalues();     // 均为正
Eigen::Matrix<double,6,6> Dd = Eigen::Matrix<double,6,6>::Zero();
for (int i = 0; i < 6; ++i) {
    double k_i = (V.col(i).transpose() * K * V.col(i)).value();  // K 在该方向的投影
    Dd += 2.0 * zeta * std::sqrt(std::max(k_i * lam(i), 0.0))    // max 防负数开方
          * V.col(i) * V.col(i).transpose();
}
// Dd 对称正半定，保证物理可实现的阻尼

本质洞察：第 7.3 节提到"CIC 使用 Schur 分解处理非交换性"——现在你理解了为什么需要：因为 $K\Lambda$ 不对称，朴素开方会失败。但即使用 Schur 分解能算出一个数学上的平方根，它也不一定对称正定（不一定对应物理可实现的阻尼）。更稳妥的工程做法是放弃"精确的 $\sqrt{K\Lambda}$"，转而在 $\Lambda$ 的特征基底下逐方向用标量开方（如上代码），牺牲一点理论精确性换取数值稳定性和物理可实现性。

陷阱 3：奇异附近 $\Lambda$ 病态。如 5.3 节所述，奇异附近 $\Lambda$ 的特征值跨越多个数量级（$\kappa > 10^4$），最大特征值可达 $10^3$ kg。开方虽然把条件数减半（$\sqrt{\kappa}$），但仍然病态，且大特征值方向的阻尼会异常大。对策：对 $\Lambda$ 的特征值做下限钳位 lam(i) = std::clamp(lam(i), lam_min, lam_max)，或在条件数超阈值时退回 $D = 2\sqrt{K}$ 的标量简化。

12.5 Sim-to-Real——为什么仿真里调好的阻抗参数搬到真机要重调¶

一个常见的挫败：在 MuJoCo 里把 peg-in-hole 的阻抗参数调到完美，搬到真机却振荡或卡死。原因是仿真和真机在多个层面不一致：

不一致来源	仿真（MuJoCo/Isaac）	真机（Franka）	对阻抗参数的影响
物理阻尼 $b$	通常设得偏大（数值稳定需要）	固有阻尼小（已补偿摩擦）	仿真 $Z_{max}$ 虚高，真机要降 $K_d$
接触模型	软接触（可调 solref/solimp）	真实刚性接触（$K_e \sim 10^6$）	仿真接触"软"，真机接触更易激发振荡
速度信号	解析速度（无噪声）	差分+滤波（有噪声、有延迟）	仿真阻尼项干净，真机需考虑滤波延迟
控制时序	完美同步（无通信延迟）	UDP 抖动、ZOH、回调延迟	仿真无 11.4 的离散化失稳，真机有
模型误差	模型即真值（$M,C,g$ 精确）	标定误差、负载未建模	仿真 OSC 完美，真机 OSC 退化（5.3）

理论-工程桥接：这张表解释了为什么 5.3 节 CRISP 的结论"CI-clipped 比 OSC 鲁棒"在真机上比仿真里更明显——仿真里 $M,C,g$ 精确，OSC 的 $\Lambda$ 算得准，优势能发挥；真机上模型误差让 $\Lambda$ 失真，OSC 优势消失。在仿真里验证算法正确性，在真机上验证鲁棒性——两者的角色不可互换。一个只在仿真里验证过的力控算法，绝不能假设它在真机上同样表现。

本质洞察：Sim-to-Real 的阻抗参数差异本质上是 11.5 节 Z-width 的差异。仿真和真机有不同的 Z-width（因为物理阻尼、采样、接触模型都不同），所以同一组 $(K_d, D_d)$ 在两者的 Z-width 中处于不同位置——仿真里落在稳定区中央，真机里可能已逼近 $Z_{max}$ 边界。正确的迁移策略不是照搬参数，而是**照搬"在 Z-width 中的相对位置"**：在真机上重新标定 $Z_{max}$（缓慢提高 $K_d$ 直到出现振荡，取 60-70%），再按比例放置工作参数。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Jacobian 和误差用了不同坐标系

错误做法：误差用 world 系（p - p_d），却用 bodyJacobian（LOCAL，末端系）做映射

现象：初始姿态附近控制正常，末端姿态偏转后恢复力方向错位——想推 A 方向，实际推 B 方向

根本原因：$\tau = J^T F$ 要求 $J$ 和 $F$ 同坐标系。world 系的力配 body 系的 Jacobian，相当于漏了一个旋转 $R$

正确做法：成套使用（见 12.2 表）——world 误差配 zeroJacobian（LWA），或 body 误差配 bodyJacobian（LOCAL）。自检：让末端绕自身轴转 90°，检查各方向恢复力是否仍指向正确

⚠️ 编程陷阱：自己对 state.q 差分求速度而不用 state.dq

错误做法：dq[k] = (state.q - q_prev) / period.toSec();

现象：阻尼项噪声大，关节高频抖动、电机发热，严重时激发 11.4 的接触振荡

根本原因：朴素差分把编码器量化噪声放大 $1/T$ 倍（$\times 1000$ @ 1 kHz）。且 period 抖动时分母不稳定

正确做法：直接用 FCI 提供的 state.dq——控制柜内已用观测器做了精心的速度估计，远比用户差分干净

💡 概念误区：认为矩阵平方根就是逐元素开方

新手想法："$\sqrt{M}$ 就是把矩阵每个元素开方。"

实际上：矩阵平方根 $S$ 定义为满足 $S^2 = M$（即 $SS = M$）的矩阵，**不是**逐元素开方。逐元素开方 $\sqrt{M_{ij}}$ 通常不满足 $S^2 = M$。对对称正定矩阵，正确的平方根通过特征分解 $M = V\Lambda V^T$ 得到 $S = V\Lambda^{1/2}V^T$（只对特征值开方）

正确理解：阻尼设计中的 $\sqrt{K\Lambda}$ 必须用特征分解或 Schur 分解，且要处理 $K\Lambda$ 非对称的问题（12.4）

练习¶

⭐⭐ 坐标系自洽性：推导 body 系实现的完整公式。给定末端姿态 $R$、世界系位置误差 $e_p^W = p - p_d$，写出 (a) body 系位置误差 $e_p^B$、(b) body 系姿态误差、(c) body 系速度，使得 $\tau = J_{body}^T F^B$ 与 libfranka 的 world 系实现**在数学上等价**。提示：利用 $J_{body} = \text{Ad} \cdot J_{world}$ 的伴随变换关系。
⭐⭐ 滤波延迟量化：一阶低通 $\dot{q}_f[k] = \alpha \dot{q}[k] + (1-\alpha)\dot{q}_f[k-1]$，$T=1$ ms。对 $\alpha \in \{0.1, 0.3, 0.5\}$，计算截止频率 $f_c$ 和在 $f=10$ Hz 处的相位滞后。这个滞后等效于把有效阻尼降低多少？结合 $b > KT/2$ 判据，说明滤波如何影响可稳定渲染的刚度上界。
⭐⭐⭐ 矩阵平方根对比：在 Python 中，构造 $K = \text{diag}(150,150,150,10,10,10)$ 和一个非对角的 $\Lambda$（特征向量与 $K$ 不对齐）。分别用 (a) scipy.linalg.sqrtm(K@Lambda)、(b) 5.4 节的特征基底逐方向开方，比较两者结果。(a) 是否对称？是否有复数分量？(b) 是否对称正定？哪个更适合作为物理阻尼矩阵？

前沿应用：笛卡尔阻抗控制在人形抓取中的最新进展¶

笛卡尔阻抗控制的应用范围已从传统的固定基座机械臂扩展到人形机器人的双臂操作。以下是 2024-2025 年的关键进展：

TCDM（Task-space Controllers for Dexterous Manipulation, 2024）：Carnegie Mellon 的 Arunachalam et al. 提出将笛卡尔阻抗控制作为灵巧手操作的底层控制器。核心洞见是：学习策略不需要直接输出高维关节力矩（如 16 个灵巧手指关节），而是输出低维的**指尖笛卡尔阻抗参数**（每个手指 3D 位置 + 刚度），由底层阻抗控制器映射到关节力矩。这将学习的动作空间从 16D 关节空间降低到 15D 指尖空间（5 个手指 x 3D），同时阻抗控制器提供了天然的力柔顺性——手指碰到物体时不会产生过大的力。

HOMR（Humanoid Omni-directional Manipulation and Retrieval, 2025）：TUM 和 Google 合作的人形全身操作框架中，双臂末端均使用笛卡尔阻抗控制作为底层，上层 RL 策略以 10 Hz 输出期望位姿和变阻抗参数，底层以 500 Hz 柔顺跟踪。关键工程决策是：在双臂协作搬运大物体时，两只手的阻抗参数必须协调——一只手用高刚度"引导"运动方向，另一只手用低刚度"顺应"引导手的运动。不协调的阻抗参数会在物体上产生内力（两只手"较劲"），可能损坏物体或导致脱落。

这些前沿工作共同指向一个趋势：笛卡尔阻抗控制正在从"单臂工业力控"演化为"全身操作的通用底层接口"。理解本章的工程细节（姿态误差处理、零空间管理、实时约束）是参与这些前沿研究的基础。

本章小结¶

知识点	难度	核心结论	工程应用
FCI 架构与实时约束	⭐	1 kHz/300 $\mu$s 预算，零堆分配	libfranka 开发
6D 姿态阻抗	⭐⭐	四元数虚部近似 + 双覆盖检查	所有笛卡尔阻抗控制器
libfranka 代码精读	⭐⭐	$\tau = J^T(-Ke - D J\dot{q}) + C\dot{q}$	阻抗控制基线实现
惯性整形（OSC）	⭐⭐⭐	$\Lambda$ 解耦但对模型敏感	CRISP OSC 模式
CI vs. OSC	⭐⭐	CI-clipped 更鲁棒（CRISP 结论）	控制器选型
CRISP 七项控制律	⭐⭐	Error clipping 保安全但破坏无源性	学习+力控集成
matthias-mayr CIC	⭐⭐	完整 6$\times$6 K + rqt 调参	研究级日常使用
变阻抗策略	⭐⭐⭐	参数时变 → 能量注入风险 → F06	多阶段任务
阻抗/导纳选型	⭐	硬件接口决定范式	实验室/产线选型
力控稳定性与无源性	⭐⭐⭐	耦合稳定性 + $b>KT/2$ 采样无源判据	接触失稳诊断、刚度上界
工程数值陷阱	⭐⭐	Jacobian 坐标系自洽、速度滤波、矩阵开方	真机调试、Sim2Real

本章符号表¶

本章在 F03 推导基础上新引入或重点使用的符号：

符号	含义	首次出现
$Z_r(s)$	机器人侧驱动点阻抗（速度→力），$Z_r = M_d s + D_d + K_d/s$	§11.2
$Y_e(s)$	环境侧驱动点导纳（力→速度），$Y_e = 1/Z_e$	§11.2
$\text{Re}\,Z(j\omega)$	阻抗虚轴实部，无源性核心判据（$\geq 0$ 即不产能）	§11.2
$b$	系统固有物理阻尼（连续、真实耗散），区别于命令阻尼 $D_d$	§11.4
$K, B$	采样虚拟墙的（控制器）刚度与阻尼	§11.4
$T$	控制采样周期（Franka 为 $1$ ms）	§11.4
$\text{Z-width}$	可稳定渲染的阻抗范围 $[Z_{min}, Z_{max}]$	§11.5
$E_{obs}[k]$	无源性观测器累计能量，TDPC 的监测量	§11.6
$\alpha[k]$	无源性控制器注入的自适应阻尼系数	§11.6
$J_{world}, J_{body}, J_{mix}$	世界系 / 末端系 / 混合系 Jacobian（表达坐标系不同）	§12.2
$\text{Ad}$	伴随变换（在不同坐标系间转换空间速度/力旋量）	§12.2

注：与 F02/F03 共享的符号（$\Lambda, M, C, g, J, \tilde{x}, K_d, D_d, M_d$ 等）见 F02 符号约定表，本章沿用其定义不重复列出。

本章定理/判据速查表¶

定理/判据	一句话说明	对应节
Colgate-Hogan 耦合稳定性	无源机器人阻抗 + 无源环境 → 耦合系统稳定（与环境参数无关）	§11.2
正实函数无源判据	$Z(s)$ 无源 $\Leftrightarrow$ 右半平面解析 + 虚轴极点留数正 + $\text{Re}\,Z(j\omega)\geq 0$	§11.2
连续阻抗无源性	$M_d\ddot{x}+D_d\dot{x}+K_d x$ 在 $D_d>0$ 时无源，接触任意刚度环境稳定	§11.3
Colgate-Brown 采样无源判据	数字虚拟墙无源充分条件 $b > KT/2 + \lvert B\rvert$	§11.4
Z-width 主导因素	物理阻尼 $b$ 对可达阻抗范围有压倒性影响（Colgate-Brown 1994）	§11.5
时域无源性控制 TDPC	PO 监测能量、PC 按需注入阻尼，在线保证无源（Hannaford-Ryu 2002）	§11.6
Jacobian-误差坐标系自洽	$\tau=J^T F$ 要求 $J$ 与 $F$ 同坐标系，否则恢复力方向随姿态错位	§12.2

累积项目：本章新增模块¶

项目：从零构建力控机械臂系统

章节	新增模块	功能
F01	概念模型	理解阻抗/导纳二分法
F03	算法库	四大经典力控算法实现
F04	libfranka 阻抗控制器	笛卡尔阻抗控制完整实现 + 姿态阻抗 + 零空间

本章实现的模块： 1. 基于 libfranka 的笛卡尔阻抗控制器（含四元数姿态误差 + 双覆盖检查） 2. 可选的惯性整形（CI/OSC 切换） 3. 零空间关节居中（指向 $q_{nominal}$） 4. Error clipping 安全机制 5. 指数平滑参数过渡

下一章（F05）将添加：导纳控制器模块（ros2_control + FZI FDCC）。

延伸阅读¶

材料	类型	难度	核心内容
Hogan 1985, "Impedance Control" (三部曲)	论文	⭐⭐⭐	阻抗控制的理论奠基——完整读 Part I 即可
Albu-Schaffer, Ott, Hirzinger 2003, "Cartesian Impedance Control of Redundant Robots"	论文	⭐⭐⭐	冗余机器人笛卡尔阻抗的系统化方案（DLR LWR 原型）
libfranka 文档 (franka.de/docs)	文档	⭐	FCI API 参考、RobotState 字段说明
San Jose Pro et al. 2025, "CRISP" (arXiv 2509.06819)	论文	⭐⭐	七项控制律 + error clipping + 两层架构
Luo et al. 2024, "SERL" (ICRA)	论文	⭐⭐	serl_franka_controllers：RL 训练用阻抗底层
matthias-mayr CIC GitHub 仓库	代码	⭐⭐	研究级笛卡尔阻抗控制器——rqt 动态调参
Siciliano et al. "Robotics: Modelling, Planning and Control" Ch.9	教材	⭐⭐⭐	力控理论的教科书级参考
Lynch & Park "Modern Robotics" Ch.11	教材	⭐⭐	力控和阻抗控制的简明介绍
Colgate & Hogan 1988, "Robust control of dynamically interacting systems"	论文	⭐⭐⭐⭐	耦合稳定性定理的奠基——无源环境 + 无源阻抗 → 稳定（§11.2 来源）
Colgate & Brown 1994, "Factors Affecting the Z-Width of a Haptic Display"	论文	⭐⭐⭐	采样无源判据 $b>KT/2$ 与 Z-width 概念的来源（§11.4-11.5）
Hannaford & Ryu 2002, "Time-Domain Passivity Control of Haptic Interfaces"	论文	⭐⭐⭐	PO/PC 在线无源性控制（§11.6 来源）
Ott "Cartesian Impedance Control of Redundant and Flexible-Joint Robots" (Springer)	专著	⭐⭐⭐⭐	冗余/柔性关节笛卡尔阻抗的系统化专著，零空间与无源性的完整理论

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
末端接触时高频振荡	1. 阻尼不足（欠阻尼） 2. 刚度过高 3. 控制频率过低	1. 计算实际阻尼比 $\zeta$ 2. 降低 K_d 3. 检查控制频率 $\ge$500 Hz	F04§5, F01§2.3
姿态突然旋转 360°	四元数双覆盖未处理	1. 检查 `coeffs().dot()` 判断 2. 在翻转处打断点 3. 绘制四元数各分量曲线	F04§3.2
`communication_constraints_violation` 急停	FCI 回调超时（>300 $\mu$s）	1. 检查回调内有无 cout/malloc 2. 测量回调执行时间 3. 确认 PREEMPT_RT 内核	F04§2.4
低速运动时末端不响应外力	关节摩擦未补偿（死区）	1. 检查是否有摩擦补偿项 2. 对比 tau_ext 和实际运动 3. 添加 sigmoid 摩擦补偿	F04§6.2
`tau_ext_hat_filtered` 有持续偏置	工具负载未通过 setLoad() 设置	1. 检查是否调用 setLoad 2. 测量偏置值是否约等于 mg 3. 用 F/T 传感器交叉验证	F04§2.3
关节 4 或 6 接近 0° 时力矩跳变	奇异附近 $\Lambda$ 条件数爆炸（仅 OSC）	1. 打印 $\Lambda$ 条件数 2. 切换到 CI 模式 3. 添加阻尼伪逆正则化	F04§5.3
零空间关节漂移	未添加零空间项或零空间刚度过低	1. 检查零空间项代码 2. 提高 K_null 3. 检查投影矩阵计算	F04§9.1
已降低 $K_d$ 仍接触振荡	1. 速度滤波延迟过大 2. 物理阻尼不足 3. $b<K_dT/2$ 仍不满足	1. 检查速度是否用 `state.dq`（勿自行差分）2. 算 $K_d$ 是否 $<2b/T$ 3. 提采样率/降 $K_d$ 至上界 60-70% 4. 上 TDPC	F04§11.4-11.6
末端转动后恢复力方向错位	Jacobian 与误差坐标系不一致	1. 确认 `zeroJacobian`(LWA) 配 world 误差，或 `bodyJacobian`(LOCAL) 配 body 误差 2. 让末端转 90° 自检恢复力方向	F04§12.2
仿真完美、真机振荡/卡死	Sim2Real 不一致（物理阻尼、接触刚度、速度噪声、时序）	1. 真机重标 $Z_{max}$（缓慢升 $K_d$ 至振荡取 60-70%）2. 按 Z-width 相对位置迁移而非照搬参数 3. 核对 $M,C,g$ 标定	F04§12.5, §11.5

影响因素	对 \(Z_{max}\)（硬度上界）的影响	对 \(Z_{min}\)（软度下界）的影响
采样率 \(1/T\)	提高采样率 → \(Z_{max}\) 提高（线性）	几乎无影响
物理阻尼 \(b\)	增大 \(b\) → \(Z_{max}\) 大幅提高（压倒性因素）	增大 \(b\) → \(Z_{min}\) 升高（变笨重，变差）
速度滤波	滤波延迟 → \(Z_{max}\) 降低	滤波平滑 → 噪声小，\(Z_{min}\) 可更低
位置量化	量化噪声 → 限制 \(Z_{max}\)	量化死区 → 限制 \(Z_{min}\)
传动背隙	背隙非线性 → 难以保证无源	背隙 → 死区，\(Z_{min}\) 变差

编号	问题	答不出时回顾
1	写出笛卡尔空间阻抗控制律 \(\tau = J^T(-K_d \tilde{x} - D_d \dot{\tilde{x}}) + g(q)\)。为什么需要 \(J^T\)？它在物理上做了什么映射？	F03 第 3 节
2	操作空间惯量矩阵 \(\Lambda = (JM^{-1}J^T)^{-1}\) 的物理含义是什么？它与操作空间控制（OSC）有什么关系？	F03 第 4 节
3	四元数 \(q\) 和 \(-q\) 代表同一个旋转（双覆盖性）。如果不处理这个问题，阻抗控制器会产生什么灾难性行为？	M01 旋转表示
4	libfranka 的 FCI 回调函数有什么实时约束？为什么不能在回调中使用 `new`/`malloc`？	M11 实时 C++
5	什么是临界阻尼（\(\zeta = 1\)）？为什么大多数工业阻抗控制器默认使用临界阻尼？	F01 第 2.3 节

差异	F03 教科书公式	libfranka 工程实现	为什么不同
姿态误差	\(\tilde{x}_{rot}\) 是 3D 向量，如何计算未指定	四元数乘法 + 双覆盖检查 + 旋转矩阵映射	姿态空间不是欧几里得空间
速度误差	\(\dot{\tilde{x}} = \dot{x} - \dot{x}_d\)	\(\dot{\tilde{x}} = J\dot{q}\)（假设 \(\dot{x}_d = 0\)）	稳态点阻抗简化
重力补偿	显式计算 \(g(q)\)	FCI 控制柜自动补偿，代码中不出现	硬件分层——内部环路已处理
惯性整形	可选 \(\Lambda\) 矩阵	示例中不使用，CRISP/OSC 使用	简单示例 vs. 完整控制器
零空间	冗余关节管理	示例中缺失，franka_ros 版本包含	7-DOF 臂必须处理
限幅	无	`kMaxTorqueRate` + 低通滤波	硬件安全——防止执行器过载

设计	为什么这样做	如果不这样做
1 kHz 控制频率	笛卡尔阻抗需要 \(\ge\)500 Hz 才能稳定接触	<200 Hz 则高刚度接触必然振荡
\(\le\)300 \(\mu\)s 计算预算	1 ms 周期内需留余量给通信和内部处理	超时则 FCI 判定通信丢失，触发急停
UDP 而非 TCP	UDP 无重传延迟，满足硬实时	TCP 重传可能导致几十 ms 延迟
自动重力补偿	简化用户控制律，减少 bug 来源	用户必须精确知道动力学参数
`kMaxTorqueRate` 限幅	防止力矩突变损坏减速器	谐波减速器对冲击载荷敏感

字段	类型	含义	阻抗控制中的用途
`O_T_EE[16]`	`double[16]`	列优先 4\(\times\)4 齐次矩阵（基坐标系→末端）	获取当前位置和姿态
`q[7]`	`double[7]`	关节位置（rad）	计算 Jacobian、Coriolis
`dq[7]`	`double[7]`	关节速度（rad/s）	阻尼项 \(D_d \dot{x} = D_d J \dot{q}\)
`tau_J[7]`	`double[7]`	测得关节力矩（含重力、Coriolis）	调试、力矩监控
`tau_ext_hat_filtered[7]`	`double[7]`	外部扰动力矩估计（动量观测器输出）	碰撞检测、外力估计
`O_F_ext_hat_K[6]`	`double[6]`	笛卡尔外力估计（基坐标系，6D）	导纳控制、力跟踪

操作	欧几里得空间 \(\mathbb{R}^3\)	旋转空间 SO(3)
表示	3D 向量	旋转矩阵 \(R \in \mathbb{R}^{3\times3}\)，\(R^TR = I\)，\(\det R = 1\)
"差"	\(a - b\)	\(R_1^T R_2\) 或 \(\log(R_1^T R_2)\)
加法	\(a + b\)	\(R_1 R_2\)（矩阵乘法）
线性插值	\((1-t)a + tb\)	\(R_1 \exp(t \cdot \log(R_1^T R_2))\)（SLERP）
拓扑	简单连通	非简单连通（\(\pi_1(\text{SO(3)}) = \mathbb{Z}_2\)）

代码行	对应 F03 公式	备注
`error.head(3) = position - position_d`	\(e_{pos} = p - p_d\)	位置误差，3D
`error.tail(3) = -R · q_{err}.vec()`	\(e_{rot} \approx -(\theta/2)\hat{n}\)（小角近似）	姿态误差，3D
`-stiffness * error`	\(-K_d \tilde{x}\)	弹簧恢复力
`-damping * (jacobian * dq)`	\(-D_d \dot{x}\)（其中 \(\dot{x} = J\dot{q}\)）	阻尼力
`jacobian.transpose() * (...)`	\(J^T F_{imp}\)	笛卡尔力→关节力矩映射
`+ coriolis`	\(+ C(q,\dot{q})\dot{q}\)	科里奥利补偿
（FCI 内部）	\(+ g(q)\)	重力补偿

零空间参数	典型值	物理含义
\(K_{null}\)	10-50 Nm/rad	零空间"弹簧"——关节趋向默认构型的力度
\(D_{null}\)	\(2\sqrt{K_{null}}\) Nm\(\cdot\)s/rad	零空间"阻尼"——防止关节振荡
\(q_{nominal}\)	[0, -\(\pi\)/4, 0, -3\(\pi\)/4, 0, \(\pi\)/2, \(\pi\)/4]	Franka 的"舒适构型"——远离奇异和关节限位

策略	零空间力矩公式	适用场景	优势	代价
关节居中	\((I-J^T\bar{J}^T)K_{n}(q_{nom}-q)\)	重复性任务	构型可预测	可能限制末端可达性
避奇异	\((I-J^T\bar{J}^T)K_{n}\nabla w(q)\)	连续轨迹跟踪	远离奇异 \(\to\) \(\Lambda\) 良态	目标构型不固定
避关节限位	\((I-J^T\bar{J}^T)\sum \frac{\partial H}{\partial q_i}\)	大范围运动	防止机械碰撞	可能与居中冲突

问题	单层解决方案	困难
学习策略直接输出力矩	RL 策略以 1 kHz 输出 \(\tau\)	GPU 推理延迟 10-100 ms，远超 1 kHz 要求
控制器直接做感知	纯 C++ 控制器处理图像	图像处理需要 GPU，不适合实时线程

特性	libfranka 示例	CIC	工程价值
刚度矩阵	6\(\times\)6 对角	完整 6\(\times\)6（含耦合项）	斜面/曲面操作
阻尼设计	\(2\sqrt{K}\)	阻尼重塑（可自定义阻尼矩阵）	各方向独立阻尼比
奇异处理	无	Damped Least Squares（\(J^T(JJ^T + \alpha I)^{-1}\)）	奇异附近平滑退化
力前馈	无	Wrench 前馈（\(\tau_{ff} = J^T F_{ff}\)）	恒力跟踪/打磨/装配
力限幅	无	Wrench 饱和（限制最大笛卡尔力）	安全保护
在线调参	无	rqt 动态调参（实时 GUI 修改 K/D/wrench）	开发效率

任务阶段	需要的行为	参数要求
接近工件	快速精确到位	高 \(K\)，中 \(D\)
建立接触	柔顺触碰	低 \(K\)（法向），高 \(D\)
稳定接触操作	恒力跟踪	中 \(K\)，中 \(D\)
退出接触	安全脱离	低 \(K\)，高 \(D\)

机制	保证什么	何时介入	代价
\(b > KT/2\) 离线判据（11.4）	无源（最坏情况）	设计阶段（限制 \(K\) 上界）	牺牲 \(Z_{max}\)
CRISP error clipping（6.3）	力矩有界（安全）	误差超限时	破坏无源性
力矩速率限幅 limitRate（2.2）	执行器不过载	力矩变化过快时	引入相位滞后
TDPC（本节）	无源（实际情况）	能量即将变负时	需可靠能量测量
能量罐（F06 预告）	无源 + 支持变阻抗	储能耗尽时	实现复杂

约定	表达坐标系	数学定义	Pinocchio 枚举
空间/世界 Jacobian	基坐标系（world）	\(V^{world} = J_{world} \dot{q}\)	`WORLD`（注意是空间速度，含原点项）
物体 Jacobian	末端坐标系（body/EE）	\(V^{ee} = J_{body} \dot{q}\)	`LOCAL`
混合 Jacobian	原点在末端、轴向与世界对齐	\(V^{mix} = J_{mix} \dot{q}\)	`LOCAL_WORLD_ALIGNED`

组合	Jacobian	位置误差	姿态误差	速度
World 系（libfranka）	`zeroJacobian`（LWA）	\(p - p_d\)（world）	\(-R\,\text{Im}(q^{-1}q_d)\)（旋到 world）	\(J_{LWA}\dot{q}\)
Body 系	`bodyJacobian`（LOCAL）	\(R^T(p-p_d)\)（旋到 body）	\(\text{Im}(q^{-1}q_d)\)（保持 body，不乘 \(R\)）	\(J_{LOCAL}\dot{q}\)

知识点	难度	核心结论	工程应用
FCI 架构与实时约束	⭐	1 kHz/300 \(\mu\)s 预算，零堆分配	libfranka 开发
6D 姿态阻抗	⭐⭐	四元数虚部近似 + 双覆盖检查	所有笛卡尔阻抗控制器
libfranka 代码精读	⭐⭐	\(\tau = J^T(-Ke - D J\dot{q}) + C\dot{q}\)	阻抗控制基线实现
惯性整形（OSC）	⭐⭐⭐	\(\Lambda\) 解耦但对模型敏感	CRISP OSC 模式
CI vs. OSC	⭐⭐	CI-clipped 更鲁棒（CRISP 结论）	控制器选型
CRISP 七项控制律	⭐⭐	Error clipping 保安全但破坏无源性	学习+力控集成
matthias-mayr CIC	⭐⭐	完整 6\(\times\)6 K + rqt 调参	研究级日常使用
变阻抗策略	⭐⭐⭐	参数时变 → 能量注入风险 → F06	多阶段任务
阻抗/导纳选型	⭐	硬件接口决定范式	实验室/产线选型
力控稳定性与无源性	⭐⭐⭐	耦合稳定性 + \(b>KT/2\) 采样无源判据	接触失稳诊断、刚度上界
工程数值陷阱	⭐⭐	Jacobian 坐标系自洽、速度滤波、矩阵开方	真机调试、Sim2Real

符号	含义	首次出现
\(Z_r(s)\)	机器人侧驱动点阻抗（速度→力），\(Z_r = M_d s + D_d + K_d/s\)	§11.2
\(Y_e(s)\)	环境侧驱动点导纳（力→速度），\(Y_e = 1/Z_e\)	§11.2
\(\text{Re}\,Z(j\omega)\)	阻抗虚轴实部，无源性核心判据（\(\geq 0\) 即不产能）	§11.2
\(b\)	系统固有物理阻尼（连续、真实耗散），区别于命令阻尼 \(D_d\)	§11.4
\(K, B\)	采样虚拟墙的（控制器）刚度与阻尼	§11.4
\(T\)	控制采样周期（Franka 为 \(1\) ms）	§11.4
\(\text{Z-width}\)	可稳定渲染的阻抗范围 \([Z_{min}, Z_{max}]\)	§11.5
\(E_{obs}[k]\)	无源性观测器累计能量，TDPC 的监测量	§11.6
\(\alpha[k]\)	无源性控制器注入的自适应阻尼系数	§11.6
\(J_{world}, J_{body}, J_{mix}\)	世界系 / 末端系 / 混合系 Jacobian（表达坐标系不同）	§12.2
\(\text{Ad}\)	伴随变换（在不同坐标系间转换空间速度/力旋量）	§12.2

症状	可能原因	排查步骤	相关章节
末端接触时高频振荡	1. 阻尼不足（欠阻尼） 2. 刚度过高 3. 控制频率过低	1. 计算实际阻尼比 \(\zeta\) 2. 降低 K_d 3. 检查控制频率 \(\ge\)500 Hz	F04§5, F01§2.3
姿态突然旋转 360°	四元数双覆盖未处理	1. 检查 `coeffs().dot()` 判断 2. 在翻转处打断点 3. 绘制四元数各分量曲线	F04§3.2
`communication_constraints_violation` 急停	FCI 回调超时（>300 \(\mu\)s）	1. 检查回调内有无 cout/malloc 2. 测量回调执行时间 3. 确认 PREEMPT_RT 内核	F04§2.4
低速运动时末端不响应外力	关节摩擦未补偿（死区）	1. 检查是否有摩擦补偿项 2. 对比 tau_ext 和实际运动 3. 添加 sigmoid 摩擦补偿	F04§6.2
`tau_ext_hat_filtered` 有持续偏置	工具负载未通过 setLoad() 设置	1. 检查是否调用 setLoad 2. 测量偏置值是否约等于 mg 3. 用 F/T 传感器交叉验证	F04§2.3
关节 4 或 6 接近 0° 时力矩跳变	奇异附近 \(\Lambda\) 条件数爆炸（仅 OSC）	1. 打印 \(\Lambda\) 条件数 2. 切换到 CI 模式 3. 添加阻尼伪逆正则化	F04§5.3
零空间关节漂移	未添加零空间项或零空间刚度过低	1. 检查零空间项代码 2. 提高 K_null 3. 检查投影矩阵计算	F04§9.1
已降低 \(K_d\) 仍接触振荡	1. 速度滤波延迟过大 2. 物理阻尼不足 3. \(b<K_dT/2\) 仍不满足	1. 检查速度是否用 `state.dq`（勿自行差分）2. 算 \(K_d\) 是否 \(<2b/T\) 3. 提采样率/降 \(K_d\) 至上界 60-70% 4. 上 TDPC	F04§11.4-11.6
末端转动后恢复力方向错位	Jacobian 与误差坐标系不一致	1. 确认 `zeroJacobian`(LWA) 配 world 误差，或 `bodyJacobian`(LOCAL) 配 body 误差 2. 让末端转 90° 自检恢复力方向	F04§12.2
仿真完美、真机振荡/卡死	Sim2Real 不一致（物理阻尼、接触刚度、速度噪声、时序）	1. 真机重标 \(Z_{max}\)（缓慢升 \(K_d\) 至振荡取 60-70%）2. 按 Z-width 相对位置迁移而非照搬参数 3. 核对 \(M,C,g\) 标定	F04§12.5, §11.5

F04 笛卡尔阻抗控制——从理论推导到 libfranka / CRISP 工程实现¶

前置自测 ⭐¶

本章目标¶

1. 从 F03 理论到工程实现——弥合推导与代码的鸿沟 ⭐¶

1.1 动机——理论公式为什么不能直接复制到代码¶

1.2 笛卡尔阻抗控制的完整映射链¶

1.3 三个认知层次——你在哪个层次？¶

⚠️ 常见陷阱¶

练习¶

2. libfranka FCI 架构与实时约束 ⭐¶

2.1 动机——为什么 Franka 是力控教学的最佳平台¶

2.2 FCI 通信架构——1 kHz 的实时约束¶

2.3 RobotState——传感器数据的完整图谱¶

2.4 实时编程约束——为什么阻抗控制代码看起来"原始"¶

⚠️ 常见陷阱¶

练习¶

3. 6D 姿态阻抗——四元数误差与双覆盖问题 ⭐⭐¶

3.1 动机——为什么姿态阻抗是最容易出错的部分¶

3.2 libfranka 的姿态误差计算——逐行解析¶

3.3 两种姿态误差公式的对比¶

3.4 位置+姿态耦合的 6D 阻抗¶

⚠️ 常见陷阱¶

练习¶

4. libfranka 阻抗控制代码完整精读 ⭐⭐¶

4.1 动机——代码精读的价值¶

4.2 初始化阶段——控制器启动前的准备¶

4.3 控制回调——1 kHz 实时循环的核心¶

4.4 该示例缺少什么——从"能跑"到"能用"的差距¶

⚠️ 常见陷阱¶

练习¶

5. 惯性整形与操作空间控制（OSC） ⭐⭐⭐¶

5.1 动机——为什么 \(D = 2\sqrt{K}\) 不够好¶

Worked Example：不同构型下阻尼比的全面计算¶

5.2 惯性整形——让末端"感觉像"期望的惯量¶

5.3 CI 与 OSC 的工程对比——CRISP 的数据¶

CRISP 实验数据的深入解读——为什么 CI-clipped 胜出¶

5.4 双对角化阻尼设计——各方向独立阻尼¶

⚠️ 常见陷阱¶

练习¶

6. CRISP 架构精读——七项控制律与 Error Clipping ⭐⭐¶

6.1 动机——从教学示例到研究级控制器¶

6.2 CRISP 的控制律完整分解¶

6.3 Error Clipping 的数学分析¶

6.4 CRISP 的两层架构——学习策略 + 高频柔顺¶

⚠️ 常见陷阱¶

练习¶

7. matthias-mayr CIC——工程级笛卡尔阻抗控制器 ⭐⭐¶

7.1 定位——研究者的"即插即用"控制器¶

7.2 CIC 的六大工程特性¶

7.3 完整 6\(\times\)6 刚度矩阵的工程实现¶

7.4 rqt 动态调参——开发效率的关键¶

⚠️ 常见陷阱¶

练习¶

8. 可变阻抗参数的实时调节策略 ⭐⭐⭐¶

8.1 动机——为什么固定 K 和 D 不够¶

8.2 三种变阻抗策略¶

8.3 参数过渡的平滑性要求¶

⚠️ 常见陷阱¶

练习¶

9. franka_ros2 CartesianImpedanceExampleController 精读 ⭐⭐¶

9.1 与 libfranka 示例的关键差异¶

9.2 ros2_control ControllerInterface 的实时约束¶

⚠️ 常见陷阱¶

练习¶

10. 与 ros2_control Admittance Controller 的关系 ⭐⭐¶

10.1 阻抗控制 vs. 导纳控制——工程选型的最终决策¶

10.2 两种范式的性能对比¶

10.3 混合架构——阻抗+导纳的协同¶

练习¶

11. 力控稳定性与无源性——为什么"调好 K 和 D"还不够 ⭐⭐⭐¶

11.1 动机——一个在自由空间完美的控制器，为什么一碰桌子就发疯¶

反面——如果用经典 SISO 稳定性思维会怎样¶

历史——Hogan 与 Colgate 的两条线索¶

11.2 耦合稳定性定理——Colgate-Hogan 的核心结论¶

一个反例点醒——纯弹簧（无阻尼）阻抗是否无源？¶

11.3 连续时间阻抗控制的无源性——一个干净的结论¶

11.4 离散化如何摧毁无源性——采样虚拟墙与 \(b > KT/2\) 判据 ⭐⭐⭐¶

采样虚拟墙模型¶

能量分析——ZOH 注入的"负能量"¶

Colgate-Brown 采样无源性判据¶