本文档属于 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 控制频率	笛卡尔阻抗需要 ≥500 Hz 才能稳定接触	<200 Hz 则高刚度接触必然振荡
≤300 μ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×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 μs - 10 ms），可能导致回调超过 300 μ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 μs，那么在这 300 μs 内最多能做多少次 7×7 矩阵乘法？提示：现代 CPU 上 7×7 矩阵乘法约需 0.1-0.5 μ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×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×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();
自检方法：让目标姿态在 ±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×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×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×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×
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\) 是控制力，\(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×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
最大力矩抖动	低	低	中	中
计算量（μ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·s/rad	零空间"阻尼"——防止关节振荡
\(q_{nominal}\)	[0, -π/4, 0, -3π/4, 0, π/2, π/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×6 对角	完整 6×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×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 μ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×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）的集成路径。

本章小结¶

知识点	难度	核心结论	工程应用
FCI 架构与实时约束	⭐	1 kHz/300 μ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×6 K + rqt 调参	研究级日常使用
变阻抗策略	⭐⭐⭐	参数时变 → 能量注入风险 → F06	多阶段任务
阻抗/导纳选型	⭐	硬件接口决定范式	实验室/产线选型

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

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

章节	新增模块	功能
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	教材	⭐⭐	力控和阻抗控制的简明介绍

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
末端接触时高频振荡	1. 阻尼不足（欠阻尼） 2. 刚度过高 3. 控制频率过低	1. 计算实际阻尼比 ζ 2. 降低 K_d 3. 检查控制频率 ≥500 Hz	F04§5, F01§2.3
姿态突然旋转 360°	四元数双覆盖未处理	1. 检查 `coeffs().dot()` 判断 2. 在翻转处打断点 3. 绘制四元数各分量曲线	F04§3.2
`communication_constraints_violation` 急停	FCI 回调超时（>300 μ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° 时力矩跳变	奇异附近 Λ 条件数爆炸（仅 OSC）	1. 打印 Λ 条件数 2. 切换到 CI 模式 3. 添加阻尼伪逆正则化	F04§5.3
零空间关节漂移	未添加零空间项或零空间刚度过低	1. 检查零空间项代码 2. 提高 K_null 3. 检查投影矩阵计算	F04§9.1