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

F01 力控导论——阻抗控制与导纳控制的统一视角

本章定位：本章是力控与柔顺控制系列（F01-F10，28 周）的起点。从"为什么位置控制不够"出发，建立力控的第一性原理——Hogan 1985 的阻抗/导纳二分法，然后通过二端口网络模型统一理解所有力控范式，最终给出工业应用场景地图和与足式方向（足式/90_WBC分层优化与TSID WBC）的力控对偶关系。本章的目标是**建立直觉和全景视野**，具体数学工具由 F02 提供，算法实现由 F03 展开。

适用范围：阻抗/导纳概念对机械臂、腿足、人形、遥操作均适用。本章以固定基座机械臂为主线，但会明确标注哪些概念直接推广到浮动基座。

前置依赖：M01（Pinocchio 刚体动力学）；关节空间 PD/计算力矩法基础知识（详见 F03 §1）。F02 是后续数学展开，也可以在读完本章后回看。

下游章节：F02（数学基础）、F03（经典力控算法）、F04（笛卡尔阻抗工程）、F05（导纳控制工程）、D05（遥操作二端口网络）

建议用时：1 周（理论 3 天 + 文献精读 2 天 + 练习 2 天）

---

前置自测 ⭐

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

编号	问题	答不出时回顾
1	刚体动力学方程：写出 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\) 并解释各项物理含义。\(M(q)\) 的正定性意味着什么？	M01 Pinocchio 深度精读
2	雅可比矩阵：\(J(q)\) 将什么映射到什么？\(\tau = J^T F\) 这个公式的物理意义是什么？	M01 第 7 节雅可比
3	PD 控制：写出关节空间 PD 控制律 \(\tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q})\)。它在接触刚性环境时会出什么问题？	机器人控制基础 / F03
4	操作空间概念：什么是操作空间惯量矩阵 \(\Lambda = (J M^{-1} J^T)^{-1}\)？为什么需要它？	F02 操作空间动力学
5	传递函数基础：二阶系统 \(G(s) = \omega_n^2 / (s^2 + 2\zeta\omega_n s + \omega_n^2)\) 中 \(\zeta\) 和 \(\omega_n\) 的物理含义？	控制理论基础

---

本章目标

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

1. **解释**为什么纯位置控制在接触任务中注定失败，以及力控制解决了什么根本性问题
2. 复述 Hogan 1985 的核心哲学："不控力也不控位，控两者之间的关系"，并用二端口网络模型精确表述
3. **区分**阻抗控制（力矩输出型）与导纳控制（位置输出型）的因果性差异，并能根据硬件接口正确选型
4. **画出**力控技术栈的完整分类树，标注每个分支的代表性论文、适用场景和硬件要求
5. **分析**机器人-环境耦合系统的接触稳定性，理解环境阻抗如何与控制器阻抗相互作用
6. **说明**力控在工业场景（打磨/装配/人机协作）中的具体应用，以及与足式方向 WBC 的力控对偶关系

---

知识导航

本章涵盖力控制的**导论全景**，知识点及其逻辑关系如下：

§1 位置控制的局限 ⭐
  → "为什么需要力控？"——从一个失败的打磨实验出发
  |
  v
§2 Hogan 1985 阻抗控制框架 ⭐⭐
  → "力控的正确思路是什么？"——控制力-运动关系
  → 阻抗 vs 导纳的因果性对偶
  → 统一力-阻抗控制 (UFIC, 2024)
  |
  v
§3 二端口网络与接触稳定性 ⭐⭐⭐
  → "怎么保证稳定？"——无源性→耦合稳定
  → 能量函数、Lyapunov 分析、Nyquist 直觉
  |
  v
§4 力传感器方案 ⭐⭐                §5 技术栈全景 ⭐⭐
  → "硬件怎么选？"                    → "算法怎么分类？"
  → F/T vs 关节力矩 vs 观测器          → 被动/主动/变/学习型
  |                                   |
  v                                   v
§6 工业应用场景 ⭐⭐                 §7 与足式对偶 ⭐⭐⭐
  → 打磨/装配/协作/检测                → 阻抗控制 ↔ WBC-QP
  |
  v
§8 历史脉络与前沿 ⭐⭐
  → 40年演化：Hogan→DLR→EPFL→TUM→Berkeley

如果跳过本章会怎样

1. 直接学 F03（经典力控算法）：你会看到阻抗/导纳/混合控制律的公式，但不理解它们**为什么**要这样设计——不知道 Hogan 的核心哲学"控制关系而非变量"，参数调节就变成了无头绪的试错
2. 直接学 F07（浮动基座 WBC）：你会在 QP 约束中看到接触力 \(\lambda\) 和力矩限制，但不理解这些约束的物理含义——不知道无源性为什么重要，就无法判断 WBC 参数是否安全

预计阅读时间

模式	时间	适用人群
精读（含推导和练习）	8-10 小时	初学者，首次接触力控
速读（跳过证明和练习）	3-4 小时	有控制理论基础，快速建立全景
速查（仅看表格和总结）	30 分钟	已学过力控，查阅特定知识点

---

1. 为什么需要力控制——位置控制的根本局限 ⭐

1.1 一个失败的实验

考虑一个看似简单的工业任务：让 7-DOF Franka Panda 用砂纸打磨一个金属工件表面。

你可能首先想到用位置控制：规划一条沿工件表面的轨迹，让机械臂精确跟踪。于是你用经典的计算力矩法（computed torque）稍加修改：

// 计算力矩法 + 重力补偿：位置控制方案
Eigen::VectorXd tau_cmd = M * (q_ddot_ref + Kp * (q_ref - q) + Kd * (q_dot_ref - q_dot))
                          + C * q_dot + g;
// 看起来很完美？

启动仿真，砂纸接触工件表面的瞬间，你会观察到以下灾难性现象：

现象	原因	后果
接触力瞬间飙升到数百牛	工件比轨迹位置高 0.5mm，位置误差产生巨大恢复力	工件损坏/传感器过载
机械臂剧烈振荡	高增益 PD + 刚性环境 = 不稳定	控制器停机
砂纸跳离表面	弹跳效应——高力→反弹→脱离接触→零力→回落→再次高力	打磨质量为零
表面划伤	接触力不可控，不均匀	工件报废

这个实验揭示了位置控制的根本问题：当机器人与环境接触时，位置误差不再产生"恢复运动"，而是产生**接触力**。位置控制器不理解力，它只知道"位置偏了就使劲推"——在自由空间这是对的，但在接触中这是灾难。

1.2 接触过渡问题——自由空间与约束空间的不连续性

在深入物理分析之前，有必要理解一个更深层次的概念：接触过渡（Contact Transition）问题。

当机械臂从自由空间运动进入接触状态时，系统发生了一次**拓扑突变**——自由度数从 \(n\)（全自由度）降为 \(n - k\)（\(k\) 个约束方向被锁定）。这不是一个连续渐变过程，而是一个瞬态的不连续切换：

阶段	自由度	动力学	控制目标
自由空间	\(n\)	\(M\ddot{q} + h = \tau\)	位置跟踪
接触瞬间	\(n \to n-k\)	冲击动力学（\(\Delta t \to 0\)）	冲击力限制
稳态接触	\(n-k\)	\(M\ddot{q} + h = \tau + J_c^T \lambda\)	力/运动协调

接触瞬间的物理过程极其剧烈。考虑末端以速度 \(v_0\) 撞击刚性表面，接触持续时间 \(\Delta t \sim 1\)-\(5\) ms（取决于材料弹性），冲击力峰值：

\[f_{impact} = \frac{m_{eff} \cdot v_0}{\Delta t}\]

对于 Franka Panda（\(m_{eff} \approx 3\) kg）以 \(v_0 = 100\) mm/s 接触钢件（\(\Delta t \approx 2\) ms），冲击力峰值约为 \(3 \times 0.1 / 0.002 = 150\) N——远超任何位置控制器的预期工作范围。

反事实推理：如果接触过渡是一个连续过程（比如环境是无限厚的柔软凝胶，刚度从 0 渐变到最终值），位置控制也许还能应付——力会缓慢上升，给控制器时间反应。但真实世界的接触几乎都是**不连续的**：空气→金属、空气→桌面、空气→人体皮肤——刚度在接触瞬间从零跳变到有限值。这个不连续性是位置控制失败的物理根源之一。

接触过渡对控制器的要求：

1. 极短的响应时间：冲击发生在毫秒级，控制器必须以 \(\geq 1\) kHz 运行才能在冲击过程中做出反应
2. 力信息获取：没有力信息的纯位置控制器在接触瞬间是"盲"的——它感知到位置误差增大，以为末端"没到位"，于是加大力矩——这正是灾难的开始
3. 阻抗匹配：控制器的输出阻抗必须与环境阻抗"兼容"——这正是 Hogan 1985 阻抗控制要解决的核心问题

1.3 位置控制失败的物理分析

让我们从物理层面严格分析为什么位置控制在接触时必然失败。

自由空间运动：末端执行器不受环境约束，运动方程为

\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\]

PD 控制器 \(\tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}) + g(q)\) 使闭环误差动力学为

\[M\ddot{e} + K_d \dot{e} + K_p e = 0\]

这是一个渐近稳定的二阶系统，误差指数衰减。一切正常。

接触状态：当末端执行器与环境接触时，运动方程变为

\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + J^T f_{ext}\]

其中 \(f_{ext}\) 是环境施加在末端的接触力。关键问题在于：\(f_{ext}\) 不是控制器能决定的——它由环境的物理性质决定。如果环境是一面刚性墙壁：

\[f_{ext} = K_e (x_{wall} - x)\]

其中 \(K_e\) 是环境刚度。对于钢铁工件，\(K_e\) 可达 \(10^6 - 10^8\) N/m。

类比：想象你用手推一块海绵和推一面墙壁。推海绵时，你的手臂肌肉（"控制器"）决定了手指的运动轨迹；推墙壁时，无论你多用力，手指位置由墙壁决定——你唯一能控制的是你施加的力。位置控制就像一个只会"推到指定位置"的蛮力系统，它不知道自己正在推的是海绵还是墙壁。

将环境模型代入闭环方程，在接触方向上的等效动力学为：

\[m_{eff}\ddot{x} + d_{eff}\dot{x} + (K_p^{cart} + K_e)x = K_p^{cart} x_d + K_e x_{wall}\]

灾难来了：等效刚度变成了 \(K_p^{cart} + K_e\)。当 \(K_e \gg K_p^{cart}\)（刚性环境），系统的自然频率

\[\omega_n = \sqrt{\frac{K_p^{cart} + K_e}{m_{eff}}} \approx \sqrt{\frac{K_e}{m_{eff}}}\]

远远超出控制器的带宽，导致不可控的高频振荡。同时，接触力

\[f_{contact} = K_e(x_{wall} - x) \approx K_e(x_{wall} - x_d)\]

完全由环境刚度和位置误差决定——0.1mm 的标定误差在 \(K_e = 10^6\) N/m 的环境中就产生 100N 的力。

反事实推理：如果环境是完美柔软的（\(K_e = 0\)），位置控制永远有效——因为没有接触力。但现实中的工业任务几乎都涉及刚性或半刚性环境（金属工件、装配零件、人体骨骼）。所以问题不在于位置控制"不好"，而在于它**根本不是为接触场景设计的**。

1.4 从三个认知跨越理解力控的必要性

从位置控制到力控制不是简单的"换个控制律"，而是需要三个根本性的认知跨越：

跨越	位置控制世界观	力控制世界观
目标函数	最小化位置误差 \(\|q - q_d\|\)	调控力-运动关系（阻抗/导纳）
环境模型	环境是障碍物——要绕开	环境是交互伙伴——要配合
控制变量	关节角度/速度	关节力矩或期望阻抗参数
成功标准	轨迹跟踪精度（mm 级）	力跟踪精度（N 级）+ 稳定接触
失败模式	碰到东西 = 失败	碰不到东西 = 失败

本质洞察：位置控制解决的是"怎么从 A 到 B"的**几何问题**；力控制解决的是"碰到东西以后怎么办"的**物理交互问题**。机器人学的前 30 年（1960-1990）主要解决几何问题，后 30 年（1990-2020）才逐步转向交互问题。2020 年后，学习型力控（SERL、Diffusion Policy + 底层阻抗）正在把这两个问题统一起来。

1.5 Mason 的约束空间分析——力控的几何基础

在具体讨论力控算法之前，我们需要 Mason 1981 的约束空间分析作为理论基础。这个分析回答了一个关键问题：在接触任务中，哪些自由度应该用位置控制，哪些应该用力控制？

Mason 的核心观察：当末端执行器与环境接触时，6 维操作空间被分解为两个正交的子空间：

自然约束（Natural Constraints）：由物理接触几何**决定**，不可违反。 - 沿法向不能运动（位移被约束），但可以施力 - 例：手指按在桌面上，z 方向位移 = 0（自然位置约束），但法向力自由

人工约束（Artificial Constraints）：由工程师**选择**，用控制实现。 - 沿切向可以运动（力为零或受控），位置需要跟踪 - 例：手指沿桌面滑动，xy 方向跟踪目标轨迹（人工位置约束）

经典示例——擦黑板：

坐标系：z = 黑板法向（向外），x/y = 黑板切平面

z 轴（法向）：
  自然约束 → 位移受限（不能穿过黑板）
  控制选择 → 力控制（维持 5N 法向力）

x 轴（水平切向）：
  自然约束 → 无位移约束（可自由滑动）
  控制选择 → 位置控制（跟踪擦除轨迹）

y 轴（垂直切向）：
  自然约束 → 无位移约束（可自由滑动）
  控制选择 → 位置控制（跟踪擦除轨迹）

Rx, Ry（绕切向旋转）：
  控制选择 → 力矩控制 τ = 0（让工具自由倾斜适应表面）

Rz（绕法向旋转）：
  控制选择 → 位置控制（保持工具朝向）

数学形式化——选择矩阵：

\[S = \text{diag}(s_1, s_2, ..., s_6), \quad s_i \in \{0, 1\}\]

当 \(s_i = 1\) 时，第 \(i\) 个自由度用位置控制；当 \(s_i = 0\) 时用力控制。定义互补矩阵 \(\bar{S} = I_6 - S\)，则：

\[S + \bar{S} = I_6 \quad \text{（完备分解）}\]

擦黑板的选择矩阵为 \(S = \text{diag}(1, 1, 0, 0, 0, 1)\)，即 x/y/Rz 位控、z/Rx/Ry 力控。

这就是 Raibert & Craig 1981 混合力/位控制的理论基础，将在 F03 中完整推导。

反事实推理：如果不做约束空间分析，直接对所有 6 个自由度都用阻抗控制会怎样？答案是：在自由方向（切向）阻抗控制退化为位置控制（因为无外力），在约束方向（法向）阻抗控制控制力-位移关系。所以阻抗控制其实**隐式地**处理了约束分解——它是比混合控制更通用的框架。但代价是：你无法独立精确地控制约束方向上的力（阻抗控制控制的是"关系"，不是"绝对力值"）。

⚠️ 常见陷阱

💡 概念误区：认为降低 \(K_p\) 就能让位置控制器在接触中安全

新手想法："位置控制碰到东西力太大？降低 \(K_p\) 不就行了吗？"

实际上：降低 \(K_p\) 确实能降低接触力，但代价是自由空间的轨迹跟踪精度**同时恶化**。这是位置控制的根本 trade-off——\(K_p\) 高则自由空间精度好但接触力大，\(K_p\) 低则接触力小但轨迹偏差大。你无法同时优化两者，因为同一个 \(K_p\) 既决定了自由空间的恢复刚度，又决定了接触中的力生成率。阻抗控制的突破正在于**用两组独立参数分别处理自由运动和接触场景**

正确做法：接触任务必须使用力控（阻抗/导纳/混合），而非试图"调低"位置控制器来凑合

🧠 思维陷阱：将"力大"等同于"控制器有 bug"

新手想法："机械臂一碰到东西力就飙升，一定是代码写错了"

实际上：这不是 bug，而是位置控制在接触场景中的**正常行为**。\(f = K_e \cdot \Delta x\) 是物理定律，不是软件缺陷。0.5 mm 的标定误差在 \(K_e = 10^6\) N/m 的钢件上就产生 500 N 的力——这是物理学，不是编程错误。解决方案不在于修 bug，而在于**切换控制范式**

练习

1. ⭐ 接触力计算：一个位置控制器（\(K_p^{cart} = 500\) N/m）让末端跟踪一条经过刚性桌面内部 2mm 的轨迹。环境刚度 \(K_e = 10^5\) N/m。计算稳态接触力约为多少？如果标定误差是 1mm 呢？
2. ⭐ 约束分析：画出以下三个任务的约束空间分解和选择矩阵 \(S\)：(a) 拧瓶盖（6D），(b) 插 USB（6D），(c) 按电梯按钮（3D）。
3. ⭐⭐ 位控极限：为什么不能通过降低位置控制增益 \(K_p\) 来解决接触力过大的问题？提示：想一想低 \(K_p\) 对自由空间轨迹跟踪精度的影响。这是否引出了一个根本的 trade-off？

---

2. Hogan 1985 的阻抗控制框架——力控的第一性原理 ⭐⭐

2.1 历史背景：从 Whitney 到 Hogan

力控制的发展并非一蹴而就，而是经历了约 10 年的探索：

年份	里程碑	核心贡献	局限
1976	Paul & Shimano	最早讨论主动柔顺 + 力反馈的必要性（Stanford Arm）	无系统理论
1977	Whitney	RCC（Remote Center Compliance）被动柔顺装置	纯机械，不可编程
1980	Salisbury	笛卡尔刚度矩阵 \(K_x\)，经 Jacobian 映射为关节刚度	只有弹簧项，无阻尼/惯量
1981	Mason	约束空间分析（Natural/Artificial Constraints）	几何理论，无控制律
1981	Raibert & Craig	选择矩阵混合力/位控制	需精确约束几何
1985	Hogan	阻抗控制三部曲——统一框架	理论里程碑
1987	Khatib	操作空间动力学，动力学一致伪逆	计算量大

Hogan 之前的所有工作都试图**分别**控制力和位置——力在约束方向，位置在自由方向。Hogan 的革命性贡献是认识到：你不应该控制力或位置的任何一个，而应该控制它们之间的关系。

2.2 Hogan 的核心哲学

Hogan 1985 年在 ASME Journal of Dynamic Systems, Measurement, and Control 上发表了三篇连续论文（共约 24 页，累计引用约 9000 次），标题统一为 "Impedance Control: An Approach to Manipulation"。这三篇论文的核心论点可以用一句话概括：

本质洞察：与环境交互的机器人不应控制运动或力中的任何一个，而应控制**两者之间的动态关系**——这个关系就是**机械阻抗**（或其逆，机械导纳）。

为什么这个观点如此革命性？让我们从物理学的角度理解。

考虑一个物理端口（port），即机器人末端与环境的接触点。在这个端口上，**有且仅有两个物理量**可以测量和控制：

• 力（force）\(f\)——广义力，包括力和力矩
• 速度（velocity）\(v\)——广义速度，包括线速度和角速度

这两个量的乘积 \(P = f^T v\) 就是**端口功率**——能量流过端口的速率。

关键约束：在任何物理交互中，机器人和环境**共享**同一个端口。这意味着 \(f\) 和 \(v\) 不能同时被独立控制——它们之间存在约束关系，由环境的物理性质决定。

如果你控制	那么另一个由谁决定	问题
位置/速度 \(v\)	力 \(f\) 由环境决定	力不可控（刚性环境→无穷大力）
力 \(f\)	位置/速度由环境决定	位置不可控（自由空间→飘移）
力-速度关系 \(Z: v \to f\)	两者都由关系+环境共同决定	两者都被调控

这就是 Hogan 的核心洞察：控制阻抗（力-速度关系）是控制交互行为的正确方式。

类比：想象你在驾驶一辆汽车。你不直接控制轮胎与地面之间的摩擦力（那由路面决定），也不直接控制车辆的绝对位置（那由惯性和外力共同决定）。你控制的是**油门和方向盘**——它们定义了你的驱动力与车辆运动之间的关系。阻抗控制器就像油门和方向盘：它不直接控制力或位置，而是定义了"推我一下，我如何响应"的行为策略。

2.3 阻抗方程——核心数学

Hogan 将期望的末端行为定义为一个**目标阻抗模型**：

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

其中： - \(\tilde{x} = x - x_d\) 是末端位置偏差（当前位置减去平衡位置） - \(M_d \in \mathbb{R}^{6\times6}\) 是**期望惯量矩阵**（决定末端对力的加速响应） - \(D_d \in \mathbb{R}^{6\times6}\) 是**期望阻尼矩阵**（决定能量耗散速率） - \(K_d \in \mathbb{R}^{6\times6}\) 是**期望刚度矩阵**（决定弹簧回复力大小） - \(f_{ext}\) 是环境施加的外力

这个方程的物理含义**是：让末端执行器表现得**像一个弹簧-阻尼器-质量系统，其物理参数（\(M_d, D_d, K_d\)）可以由工程师自由编程设定。

参数的物理意义：

参数	物理类比	工程含义	单位
\(K_d\)	弹簧刚度	偏离平衡位置时的恢复力大小	N/m（平移）、Nm/rad（旋转）
\(D_d\)	阻尼器系数	运动时的粘滞阻力大小	Ns/m、Nms/rad
\(M_d\)	惯量/质量	对外力的加速响应灵敏度	kg、kgm\(^2\)

设计参数之间的关系：

自然频率：\(\omega_n = \sqrt{K_d / M_d}\)（rad/s）

阻尼比：\(\zeta = D_d / (2\sqrt{K_d M_d})\)（无量纲）

工业推荐范围：

阻尼比 \(\zeta\)	行为	适用场景
\(< 0.5\)	欠阻尼，接触时振荡	几乎不用
\(0.7 - 0.9\)	快速响应，轻微过冲	精密装配的接近阶段
\(1.0\)	临界阻尼，最快无过冲	大多数接触任务的默认选择
\(> 1.0\)	过阻尼，响应缓慢	安全优先的人机协作

典型参数范围（Franka Panda 7-DOF）：

平移刚度 K_trans:  100 - 2000 N/m
旋转刚度 K_rot:    5 - 200 Nm/rad
阻尼比 ζ:          0.7 - 1.0（几乎总是取临界阻尼或略过阻尼）
惯量 M_d:          通常取操作空间惯量 Λ(x)（不需要额外设定）

2.4 阻抗参数的快速选择法则——工程师的第一步

在深入阻抗/导纳对偶之前，这里给出一个面向工程师的参数快速选择法则。详细的参数设计方法将在 F03 和 F04 中展开，但掌握以下"拇指法则"可以让你在第一次实验中就获得合理的结果：

Step 1: 选择 \(K_d\)（从任务需求出发）

\[K_d \approx \frac{f_{task}}{x_{allow}}\]

其中 \(f_{task}\) 是任务期望力（N），\(x_{allow}\) 是允许的位置偏差（m）。例如：打磨任务期望 20 N 力、允许 2 mm 偏差 → \(K_d = 20/0.002 = 10000\) N/m。但注意这只是初始估计——如果 \(K_d \gg K_e\)（环境刚度），接触不稳定；如果 \(K_d \ll K_e\)，力由弹簧产生足够大。

Step 2: 选择 \(D_d\)（从稳定性出发）

\[D_d = 2\zeta\sqrt{K_d M_d}\]

几乎总是取 \(\zeta = 1\)（临界阻尼）作为起点。如果接触振荡，增大到 \(\zeta = 1.2\)-\(1.5\)。

Step 3: 选择 \(M_d\)（通常不需要选）

大多数实现中 \(M_d\) 取操作空间惯量 \(\Lambda(q)\)（即不做惯量整形），这避免了惯量整形带来的动力学补偿误差。只有在需要特别低的虚拟质量（人机协作透明度）时才手动设定 \(M_d < \Lambda\)。

类比：选择阻抗参数就像选择汽车悬挂——\(K_d\) 是弹簧硬度（太软颠簸太大，太硬颠到骨头），\(D_d\) 是减震器（没有减震器弹簧会持续弹跳），\(M_d\) 是簧载质量（载重越大悬挂响应越迟钝）。大多数工程师先定弹簧（\(K_d\)），再配减震器（\(D_d\)），最后看质量（\(M_d\)）——力控参数选择的顺序一模一样。

2.5 阻抗控制与导纳控制——因果性对偶

Hogan 同一框架中定义了两种**对偶的**控制范式。理解它们的区别是本章最重要的知识点之一。

机械阻抗 \(Z(s)\)：力-速度关系的传递函数

\[Z(s) = \frac{F(s)}{V(s)} = M_d s + D_d + \frac{K_d}{s}\]

物理含义：给定速度输入，输出力。阻抗大 → 输入相同速度时产生的力大（"硬"）。

机械导纳 \(Y(s)\)：阻抗的逆

\[Y(s) = \frac{V(s)}{F(s)} = Z(s)^{-1} = \frac{s}{M_d s^2 + D_d s + K_d}\]

物理含义：给定力输入，输出速度。导纳大 → 输入相同力时产生的运动大（"软"）。

本质洞察：阻抗和导纳描述的是**同一个物理行为**的两种因果描述。就像同一个方程 \(F = ma\) 可以写成 \(a = F/m\)（给定力求加速度）或 \(F = ma\)（给定加速度求力）。数学上完全等价，但在工程实现上有本质差异——因为**控制器的输出接口不同**。

两种控制范式的对比：

维度	阻抗控制 (Impedance Control)	导纳控制 (Admittance Control)
因果方向	运动输入 → 力矩输出	力输入 → 运动输出
控制器输出	关节力矩 \(\tau\)	位置/速度修正 \(\Delta x\)
传感器需求	可选（可无力传感器）	必须有力/力矩传感器
硬件接口	需要力矩接口（Franka FCI, KUKA FRI）	仅需位置/速度接口（UR, Fanuc, ABB）
控制框图	直接计算 \(\tau = J^T(-K_d\tilde{x} - D_d\dot{x}) + g\)	读取 \(f_{ext}\) → 积分出 \(\Delta x\) → 喂给位置控制器
带宽	高（直接力矩控制，~1 kHz）	低（受内环位置控制器带宽限制，~100-500 Hz）
典型平台	Franka Panda, KUKA iiwa, DLR LWR	UR e-Series, Fanuc CRX, ABB YuMi
实现复杂度	高（需动力学模型补偿）	中（内环位控由硬件处理）
稳定性风险	高刚度+高环境刚度可能不稳定	内环位控保证了基本稳定性

阻抗控制框图：
  ┌────────────┐         ┌────────┐
  │ 阻抗控制器  │ τ       │ 机器人  │   x, v
  │ Z: v → f   │────────→│ 动力学  │────────→ 环境
  │ τ = J^T F_d │         │        │   f_ext
  └────────────┘         └────────┘ ←────────

导纳控制框图：
  ┌────────────┐   Δx    ┌──────┐   τ    ┌────────┐
  │ 导纳控制器  │────────→│ 位置  │──────→│ 机器人  │   x, v
  │ Y: f → v   │         │ 控制器│       │ 动力学  │────────→ 环境
  │ Δx = Y f   │         └──────┘       │        │   f_ext
  └────────────┘ ←──────────────────────└────────┘
        f_ext

反事实推理：如果你在一个只有位置接口的工业机器人（如 UR5e）上尝试做阻抗控制会怎样？你无法直接命令关节力矩，只能命令关节位置/速度。这意味着你**必须**用导纳控制：读取外力传感器→计算位置修正→命令位置控制器。如果强行用软件模拟力矩接口（以 8ms 的位置控制周期间接实现），力控带宽会被限制到约 50 Hz，远低于接触动力学的需求。这就是为什么 Franka/iiwa 等协作臂专门暴露了 1kHz 力矩接口——硬件决定了可选的控制范式。

2.6 "控制关系"的深层含义

很多初学者读到"控制阻抗/导纳"时，会疑惑："这和位置控制+力限幅有什么区别？"

区别是根本性的。考虑同一个场景——末端接触刚性表面：

位置控制+力限幅：控制器试图到达目标位置，如果检测到力超过阈值就停止。这是一个**开关逻辑**（力大就停，力小就推），在接触面上会出现"推-停-推-停"的不连续行为。

阻抗控制：控制器让末端表现为弹簧-阻尼器。当接触力增大时，末端**自动后退**一个与力成正比的距离（\(\tilde{x} = K_d^{-1} f_{ext}\)）。没有开关、没有阈值、没有不连续——末端平滑地在力和位移之间取得平衡。

位置控制+力限幅（不连续）：
  force  |
         |    / 刚到阈值 -> 停
  F_max  |---/
         |  / 持续推
         | /
         +--------------> time
            不连续切换

阻抗控制（连续）：
  force  |
         |     / 力随位移线性增长
         |    /   直到 F = K_d * x_error
         |   /
         |  / 平衡点
         +--------------> displacement
            连续、平滑

"不是X而是Y"：阻抗控制**不是**"柔软的位置控制"，而是一种根本不同的控制哲学——它控制的是力与运动的**动态关系**，而非力或运动本身。

2.7 统一力-阻抗控制——Haddadin 2024 的前沿推进

Hogan 1985 的阻抗控制框架有一个根本性的局限：它不能精确跟踪期望的接触力。阻抗控制控制的是力-运动"关系"（\(f = K_d \tilde{x} + D_d \dot{\tilde{x}} + M_d \ddot{\tilde{x}}\)），稳态力 \(f_{ss} = K_d(x_d - x_{eq})\) 取决于平衡位置偏差——而偏差又取决于环境刚度。如果环境刚度未知，稳态力就无法精确预测。

这个问题困扰了力控领域将近 40 年。2024 年，Haddadin 和 Shahriari 在 IJRR 上发表了"Unified Force-Impedance Control"（UFIC），提出了一个统一框架，将阻抗控制和力控制融合为一个**基于无源性的连续谱**：

UFIC 的核心思想：

传统方法中，阻抗控制和力控制是两个独立的控制器，需要通过选择矩阵 \(S\) 切换（Raibert-Craig 1981）。UFIC 将两者统一为单一控制律，通过一个连续参数 \(\alpha \in [0,1]\) 在"纯阻抗"（\(\alpha = 0\)）和"纯力控"（\(\alpha = 1\)）之间平滑过渡：

\[\tau_{UFIC} = J^T \left[ (1-\alpha) \cdot f_{impedance} + \alpha \cdot f_{force} \right]\]

其中 \(f_{impedance} = -K_d \tilde{x} - D_d \dot{\tilde{x}}\) 是阻抗项，\(f_{force}\) 是力跟踪项。

UFIC 的无源性保证：

UFIC 框架使用能量罐（Energy Tank）方法（将在 F06 中详细讨论）来保证在参数 \(\alpha\) 切换过程中系统的无源性。当 \(\alpha\) 变化时，阻抗参数的切换可能向系统注入能量——能量罐提供了一个有限的能量预算，确保注入的能量不超过预算。

与经典方法的对比：

维度	混合力/位控 (Raibert-Craig)	并行力/位控 (Chiaverini)	UFIC (Haddadin 2024)
力/位切换	硬切换（选择矩阵）	软切换（加权叠加）	连续谱（\(\alpha\) 参数）
无源性保证	无	无	有（能量罐）
接触/非接触切换	需显式检测	需显式检测	自动平滑过渡
柔性关节支持	否	否	是
实验验证	大量	大量	Franka Panda

"不是X而是Y"：UFIC **不是**阻抗控制和力控制的简单加权平均，而是一个在能量层面保证安全性的统一框架。简单加权（\(\tau = w_1 \tau_{imp} + w_2 \tau_{force}\)）可能在切换时注入无穷能量；UFIC 通过能量罐限制了可注入的能量总量。

教学要点：UFIC 代表了力控理论从"分类讨论"到"统一连续谱"的演进方向。F06 将详细讨论能量罐方法，F09 将讨论如何用学习方法自动选择 \(\alpha\)。

2.8 Salisbury 1980 主动刚度控制——从被动到主动的桥梁

在 Hogan 之前，Salisbury 1980 提出了用 Jacobian 映射**主动实现**笛卡尔空间刚度的方法：

\[\tau = J^T K_x (x_d - x)\]

其中 \(K_x \in \mathbb{R}^{6\times6}\) 是笛卡尔刚度矩阵。实际实现需要关节空间等效刚度 \(K_q = J^T K_x J\)。

这是 Hogan 阻抗控制的**简化前身**（只有弹簧项 \(K_d\)，无阻尼 \(D_d\) 和惯量 \(M_d\)）。Hogan 1985 的贡献是加上 \(M_d\) 和 \(D_d\) 使行为完整——不仅有弹簧回复力，还有阻尼耗能和惯量响应。

教学要点：理解 Salisbury→Hogan 的演进逻辑——刚度控制解决了"柔软"的需求，但没有解决"阻尼"（无阻尼的弹簧会振荡）和"惯量整形"（操作者感受到的惯量不可调）。Hogan 的阻抗控制是刚度控制的完整版。

从 Salisbury 到 Hogan 的三步演进：

演进阶段	控制律	能做什么	不能做什么
Salisbury 1980	\(\tau = J^T K_x \tilde{x}\)	弹簧回复力→基本柔顺	无阻尼→接触振荡；无惯量整形→操作者感受到真实惯量
+ 阻尼项	\(\tau = J^T(K_x \tilde{x} + D_x \dot{\tilde{x}})\)	阻尼耗能→振荡衰减	无惯量整形→快速冲击时响应不可控
Hogan 1985	\(\tau = J^T(K_d \tilde{x} + D_d \dot{\tilde{x}} + M_d \ddot{\tilde{x}})\)	完整弹簧-阻尼-惯量系统→所有参数可编程	刚性关节假设→柔性关节需 DLR 扩展

每一步都是为了解决前一步遗留的问题。这种"问题驱动"的演进逻辑在整个力控领域反复出现——Hogan 的固定参数→变阻抗（解决适应性）→学习型力控（解决自动化），遵循的是同一种模式。

⚠️ 常见陷阱

💡 概念误区：把阻抗控制等同于"柔软的位置控制"

新手想法："阻抗控制就是 PD 控制加了个弹簧？和位置控制差不多吧？"

实际上：位置控制的目标是**最小化位置误差**（\(e \to 0\)），阻抗控制的目标是**建立期望的力-运动动态关系**（\(M_d\ddot{e} + D_d\dot{e} + K_d e = f_{ext}\)）。两者的区别不在于公式形式的相似性，而在于**设计意图**——位置控制"不允许"偏差，阻抗控制"利用"偏差来产生合理的接触力。当 \(K_d\) 很大时阻抗控制退化为位置控制，但这恰好是你**不应该**在接触场景中做的事

正确理解：阻抗控制是一种**交互控制范式**，它的核心在于"允许偏差以换取力的可控性"

⚠️ 编程陷阱：在 Laplace 域传递函数中混淆 \(F(s)\) 与时域 \(f(t)\) 的符号

错误做法：在代码中直接用 Laplace 域的 \(F_{ext}(s)\)（大写，频域）公式去写时域控制律，忘记 \(s\) 运算符对应的是微分/积分运算

现象：阻抗方程在频域写成 \((Ms^2 + Ds + K)\tilde{X} = F_{ext}\)，但时域实现必须用差分近似 \(\ddot{x}\)，\(\dot{x}\)——直接把 \(s\) 当成变量相乘是概念错误

正确做法：时域用小写 \(f_{ext}(t)\)，频域用大写 \(F_{ext}(s)\)；实现时将频域设计转换为差分方程再编码

练习

1. ⭐ 频率响应：对阻抗传递函数 \(Z(s) = M_d s + D_d + K_d/s\)，在 \(M_d = 1\) kg, \(D_d = 20\) Ns/m, \(K_d = 100\) N/m 下画 Bode 图（幅频和相频）。标注自然频率和阻尼比。
2. ⭐⭐ 导纳实现：写出导纳控制器的离散化伪代码：从读取力传感器到输出位置修正 \(\Delta x\)。考虑采样周期 \(T = 1\) ms。
3. ⭐⭐ 哲学辨析：用你自己的话解释"控制阻抗"与"控制力"的根本区别。举一个日常生活中你实际使用"阻抗控制"的例子（提示：想想你如何拧开一个不确定松紧程度的瓶盖）。

---

3. 二端口网络模型——统一视角 ⭐⭐⭐

3.1 从单端口到二端口

第 2 节中，我们把机器人-环境交互看作一个**单端口**系统：机器人和环境通过一个端口交换能量。但更精确的模型是**二端口网络**——它来自电气工程中的网络理论，被 Hogan 和后续研究者（特别是 Colgate & Hogan 1988）引入机器人力控制。

类比：二端口网络就像一个电气变压器——输入端有电压和电流（类比力和速度），输出端也有电压和电流。变压器定义了输入和输出之间的关系。在力控中，"变压器"就是控制器——它把人类操作者（主端）或期望轨迹的力/速度转换到机器人末端（从端），或者把环境的力/速度反馈回来。

力-速度对偶（机器人学与电路的类比）：

机器人力控	电路网络	物理意义
力 \(f\)	电压 \(V\)（或电流 \(I\)）	势变量（effort）
速度 \(v\)	电流 \(I\)（或电压 \(V\)）	流变量（flow）
阻抗 \(Z = f/v\)	电阻抗 \(Z = V/I\)	势-流关系
导纳 \(Y = v/f\)	电导纳 \(Y = I/V\)	流-势关系
功率 \(P = f^T v\)	功率 \(P = V \cdot I\)	能量传输速率

3.2 二端口阻抗矩阵

将机器人-环境系统抽象为二端口网络：

    端口 1 (控制器/操作者)     端口 2 (环境)
    +--------------------------------------+
    |                                      |
  f1 -->|                            |<-- f2
    |     二端口网络 (机器人+控制器)     |
  v1 <--|                            |--> v2
    |                                      |
    +--------------------------------------+

**阻抗矩阵**描述力与速度的关系：

\[\begin{bmatrix} F_1 \\ F_2 \end{bmatrix} = \begin{bmatrix} Z_{11} & Z_{12} \\ Z_{21} & Z_{22} \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \end{bmatrix}\]

• \(Z_{11}\)：端口 1 的输入阻抗（操作者感受到的阻抗）
• \(Z_{22}\)：端口 2 的输出阻抗（环境感受到的阻抗）
• \(Z_{12}, Z_{21}\)：端口间的耦合（透明度的关键）

**导纳矩阵**是阻抗矩阵的逆：

\[\begin{bmatrix} V_1 \\ V_2 \end{bmatrix} = \begin{bmatrix} Y_{11} & Y_{12} \\ Y_{21} & Y_{22} \end{bmatrix} \begin{bmatrix} F_1 \\ F_2 \end{bmatrix}\]

\[Y = Z^{-1}\]

混合矩阵 \(H\)（遥操作中最常用）：

\[\begin{bmatrix} F_1 \\ V_2 \end{bmatrix} = \begin{bmatrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{bmatrix} \begin{bmatrix} V_1 \\ F_2 \end{bmatrix}\]

混合矩阵的对角元素直接反映操作者端阻抗（\(H_{11}\)）和环境端导纳（\(H_{22}\)），非对角元素反映力/速度的传递透明度。理想透明度要求 \(H = \begin{bmatrix} 0 & I \\ I & 0 \end{bmatrix}\)——操作者直接"感受到"环境。这将在 D05（遥操作二端口网络）中深入展开。

3.3 统一框架：所有力控范式都是端口行为的选择

用二端口语言，我们可以统一理解所有力控范式：

控制范式	端口行为	因果方向	\(Z\) 矩阵特征
位置控制	端口 2 是速度源	\(v_2\) 给定 → \(f_2\) 由环境决定	\(Z_{22} \to \infty\)（无穷大输出阻抗）
力控制	端口 2 是力源	\(f_2\) 给定 → \(v_2\) 由环境决定	\(Z_{22} \to 0\)（零输出阻抗）
阻抗控制	端口 2 是阻抗	\(v_2 \to f_2 = Z_{22} v_2\)	\(Z_{22}\) 可编程（有限值）
导纳控制	端口 2 是导纳	\(f_2 \to v_2 = Y_{22} f_2\)	\(Y_{22}\) 可编程

本质洞察：位置控制是阻抗控制在 \(K_d \to \infty\) 时的极限（无穷大刚度弹簧 = 刚性位置约束）；纯力控是阻抗控制在 \(K_d \to 0, D_d \to 0, M_d \to 0\) 时的极限（零阻抗 = 纯力源）。阻抗控制是一个**连续谱**，位置控制和力控制是它的两个端点。这就是 Hogan 所说的"统一框架"的含义。

3.4 环境阻抗与耦合稳定性

当机器人（阻抗 \(Z_r\)）与环境（阻抗 \(Z_e\)）通过端口耦合时，耦合系统的稳定性由两者的**阻抗匹配**决定。

环境模型：

最常见的环境模型是弹簧-阻尼器（Kelvin-Voigt 模型）：

\[Z_e(s) = K_e / s + D_e\]

其中 \(K_e\) 是环境刚度，\(D_e\) 是环境阻尼。

工业中常见的环境刚度范围极大，这正是力控设计的核心挑战之一：

环境材料	\(K_e\) (N/m)	\(D_e\) (Ns/m)	典型应用	力控难度
海绵/泡沫	\(10^2 - 10^3\)	\(1-10\)	表面清洁	低
人体软组织	\(10^3 - 10^4\)	\(10-50\)	医疗康复	中
木材	\(10^4 - 10^5\)	\(50-200\)	木工打磨	中
硬塑料/铝	\(10^5 - 10^6\)	\(100-500\)	去毛刺	高
钢铁/花岗岩	\(10^6 - 10^8\)	\(500-5000\)	精密装配	极高

关键观察：环境刚度跨越了 6 个数量级。如果控制器稳定性依赖于已知的 \(K_e\)，那你需要为每种材料重新设计控制器——这在"一条产线多种工件"的工业场景中完全不可行。这正是无源性方法的价值所在。

耦合系统分析——完整推导：

在接触点，力平衡要求 \(f_r = -f_e\)（牛顿第三定律），速度连续性要求 \(v_r = v_e\)。从而：

\[Z_r(s) \cdot v = -Z_e(s) \cdot v \implies (Z_r + Z_e) v = 0\]

系统稳定当且仅当 \(Z_r(s) + Z_e(s)\) 的所有零点在左半平面。

展开耦合特征方程（1-DOF 简化，机器人阻抗 \(Z_r = M_d s + D_d + K_d/s\)，环境 \(Z_e = K_e/s + D_e\)）：

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

乘以 \(s\) 得特征多项式：

\[M_d s^2 + (D_d + D_e) s + (K_d + K_e) = 0\]

由 Routh-Hurwitz 判据，稳定的充分必要条件是所有系数为正：

• \(M_d > 0\)（惯量正——物理自然满足）
• \(D_d + D_e > 0\)（阻尼之和为正——只要控制器或环境有阻尼就满足）
• \(K_d + K_e > 0\)（刚度之和为正——只要控制器或环境有弹性就满足）

对于被动环境（\(D_e \geq 0, K_e \geq 0\)），只要 \(M_d, D_d, K_d > 0\)（标准阻抗参数），所有条件自动满足。但这个分析仅限于线性环境——非线性环境（如 Coulomb 摩擦、间隙）需要无源性方法。

耦合系统的闭环固有频率与阻尼比：

\[\omega_{cl} = \sqrt{\frac{K_d + K_e}{M_d}}, \quad \zeta_{cl} = \frac{D_d + D_e}{2\sqrt{M_d(K_d + K_e)}}\]

当 \(K_e \gg K_d\) 时： - \(\omega_{cl} \approx \sqrt{K_e/M_d}\)——频率由环境主导，可能极高 - \(\zeta_{cl} \approx (D_d + D_e)/(2\sqrt{M_d K_e})\)——阻尼比急剧下降

反事实推理：如果你为海绵环境（\(K_e = 500\) N/m）设计了 \(\zeta = 1\) 的阻抗控制器（\(D_d = 2\sqrt{K_d M_d}\)），然后用同样的参数接触钢铁（\(K_e = 10^6\) N/m），闭环阻尼比会变为 \(\zeta_{cl} \approx D_d / (2\sqrt{M_d \cdot 10^6}) \approx 0.03\)——严重欠阻尼，接触瞬间会剧烈振荡。这解释了为什么"为软环境调好的参数碰到硬物就炸"是新手最常遇到的问题。

环境刚度对控制器参数的影响——设计指导：

环境特性	\(K_d\) 选择	\(D_d\) 选择	理由
\(K_e\) 未知/变化大	取低值（100-500 N/m）	取高值（\(\zeta = 1\)-\(1.5\)）	低 \(K_d\) 保证 \(K_d \ll K_e\)，高阻尼防振荡
\(K_e\) 已知且固定	可取较高值	按闭环 \(\zeta_{cl} = 0.8\)-\(1\) 设计	已知 \(K_e\) 可精确调参
\(K_e\) 极高（刚性装配）	取极低值（50-100 N/m）	过阻尼（\(\zeta > 1\)）	安全优先，宁可响应慢
\(K_e\) 极低（软体操作）	取中等值（500-1000 N/m）	临界阻尼	低 \(K_e\) 时振荡风险小

Colgate-Hogan 1988 耦合稳定性定理：

如果机器人是**无源的**（\(\text{Re}[Z_r(j\omega)] \geq 0, \forall \omega\)）且环境也是**无源的**（\(\text{Re}[Z_e(j\omega)] \geq 0, \forall \omega\)），则耦合系统**保证稳定**。

这个定理的重要性怎么强调都不为过——它提供了力控稳定性的**充分条件**，不依赖于环境的具体参数。只要设计控制器使其满足无源性，无论碰到什么样的被动环境（从海绵到钢板），都保证稳定。

定理的直觉理解：

两个无源系统的耦合为什么必然稳定？考虑能量： - 系统 1（机器人 + 控制器）初始存储能量 \(V_1(0)\)，且不产生额外能量（无源性） - 系统 2（环境）初始存储能量 \(V_2(0)\)，且不产生额外能量（被动环境） - 两系统通过端口交换能量，但总能量 \(V_1 + V_2\) 只能单调下降（因为都不产生能量） - 有界的总能量 + 单调下降 = 有界稳定

更精确地说：\(V_1(T) + V_2(T) \leq V_1(0) + V_2(0)\)，\(\forall T > 0\)。如果再加上阻尼耗散（严格无源性），能量会趋于零——渐近稳定。

无源性在力控中的核心地位：

无源性条件: 系统不能凭空产生能量
  |
  v
数学表达: integral_0^t f(s)^T v(s) ds >= -V(x(0))   (存储函数有下界)
  |
  v
频域等价: Re[Z(jw)] >= 0, for all w                  (正实阻抗)
  |
  v
Colgate-Hogan 定理: 两个无源系统耦合 -> 稳定
  |
  v
工程含义: 设计无源的控制器 -> 对所有被动环境都稳定

这个"无源性→稳定性"的链条是整个力控理论的骨架，将在 F02 中完整推导。

3.5 能量函数与 Lyapunov 稳定性——无源性的严格数学基础

上一节从物理直觉理解了无源性。本节给出更严格的数学表述，为后续章节（F02、F06）的深入分析奠定基础。

存储函数（Storage Function）：

对于阻抗控制系统，定义存储函数（即 Lyapunov 候选函数）：

\[V(\tilde{x}, \dot{\tilde{x}}) = \frac{1}{2} \dot{\tilde{x}}^T M_d \dot{\tilde{x}} + \frac{1}{2} \tilde{x}^T K_d \tilde{x}\]

第一项是期望惯量中的动能，第二项是期望弹簧中的势能。\(V \geq 0\)（正半定），且 \(V = 0\) 当且仅当 \(\tilde{x} = 0, \dot{\tilde{x}} = 0\)（正定，在 \(M_d, K_d > 0\) 时）。

能量耗散率：

对 \(V\) 求时间导数，利用阻抗方程 \(M_d \ddot{\tilde{x}} + D_d \dot{\tilde{x}} + K_d \tilde{x} = f_{ext}\)：

\[\dot{V} = \dot{\tilde{x}}^T M_d \ddot{\tilde{x}} + \tilde{x}^T K_d \dot{\tilde{x}}\]

\[= \dot{\tilde{x}}^T (f_{ext} - D_d \dot{\tilde{x}} - K_d \tilde{x}) + \tilde{x}^T K_d \dot{\tilde{x}}\]

\[= \dot{\tilde{x}}^T f_{ext} - \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\]

**无源性条件**要求 \(\dot{V} \leq \dot{\tilde{x}}^T f_{ext}\)（能量变化率不超过外部功率输入），即：

\[- \dot{\tilde{x}}^T D_d \dot{\tilde{x}} \leq 0\]

这要求 \(D_d \geq 0\)（阻尼矩阵半正定）。当 \(D_d > 0\) 时，系统是**严格无源的**——阻尼持续耗散能量。

Lyapunov 稳定性结论：

当 \(f_{ext} = 0\)（无外力）时：

\[\dot{V} = -\dot{\tilde{x}}^T D_d \dot{\tilde{x}} \leq 0\]

由 LaSalle 不变性原理，\(\dot{V} = 0\) 的最大不变集是 \(\{\dot{\tilde{x}} = 0\}\)；在 \(\dot{\tilde{x}} = 0\) 上，阻抗方程退化为 \(K_d \tilde{x} = 0\)，即 \(\tilde{x} = 0\)。因此系统渐近稳定——末端最终回到平衡位置。

能量积分形式——无源性的等价表述：

对 \(\dot{V} \leq \dot{\tilde{x}}^T f_{ext}\) 从 \(0\) 到 \(T\) 积分：

\[V(T) - V(0) \leq \int_0^T \dot{\tilde{x}}^T f_{ext} \, dt\]

即：最终存储能量 \(\leq\) 初始存储能量 + 外部输入的总能量。

本质洞察：无源性的物理含义是"系统不能凭空创造能量"。阻抗控制器被设计为一个虚拟的弹簧-阻尼器-质量系统——这些都是被动元件（弹簧存储能量、阻尼器耗散能量、质量块存储动能），不产生额外能量。只要虚拟参数都为正（\(M_d, D_d, K_d > 0\)），控制器就保持无源性。问题出在变阻抗和离散化——这两者都可能破坏无源性，将在 F06 和下一小节中分析。

3.6 离散化与采样对无源性的破坏

在连续时间中，标准阻抗控制器是无源的。但当控制器以离散时间实现（采样周期 \(T\)）时，无源性可能被破坏。

直觉解释：假设在采样间隔 \([kT, (k+1)T)\) 内，环境力发生了变化，但控制器直到 \((k+1)T\) 才"看到"这个变化。在这个间隔内，控制器输出的力矩是基于旧信息计算的——这可能导致系统在一个采样间隔内"注入"多余的能量。

临界稳定性条件（Colgate & Schenkel 1997, ASME JDSMC）：

\[K_d < \frac{2(B + D_d)}{T}\]

其中 \(B\) 是机器人的物理阻尼，\(D_d\) 是虚拟阻尼，\(T\) 是采样周期。这是在以下假设下推导的简化条件：(1) 1-DOF 系统；(2) 环境为纯弹性（\(K_e\)）或纯阻尼（\(D_e\)）；(3) 零阶保持器离散化。对多 DOF 或更复杂的环境模型，需重新分析。

工程含义：

采样率	允许的最大刚度 \(K_d\)（典型值）	后果
1 kHz（Franka）	~3000 N/m	满足大多数任务需求
500 Hz（UR 内环）	~1000 N/m	中等刚度任务可行
125 Hz（UR 外环）	~200 N/m	只能做低刚度柔顺

反事实推理：如果不考虑采样对无源性的影响，在 125 Hz 的导纳控制器上设定 \(K_d = 2000\) N/m 会怎样？系统会在接触时发生高频振荡——控制器在每个采样间隔内注入的能量超过了阻尼耗散的能量。这在实际工程中是 UR 机器人做力控制时最常见的"抖动"故障根因之一。

3.7 接触稳定性的频域直觉

为了对接触稳定性建立更深的直觉，我们画出机器人阻抗 \(Z_r(j\omega)\) 和环境阻抗 \(Z_e(j\omega)\) 在复平面上的 Nyquist 图。

稳定性的频域条件（简化版）：对所有频率 \(\omega\)，\(Z_r(j\omega) + Z_e(j\omega)\) 不应包围原点。

对于二阶阻抗 \(Z_r = M_d j\omega + D_d + K_d/(j\omega)\)：

\[\text{Re}[Z_r(j\omega)] = D_d \geq 0 \quad \text{(无论频率多少)}\]

\[\text{Im}[Z_r(j\omega)] = M_d \omega - K_d/\omega\]

实部总是非负（因为 \(D_d > 0\)）→ Nyquist 图始终在右半平面 → 无源性保证。

阻抗 Z_r(jw) 的 Nyquist 图（复平面）：

  Im[Z]  ^
         |
         |     * (w -> inf: M_d*w 主导, Im 正无穷)
         |    /
         |   /  曲线始终在右半平面
         |  /   (Re = D_d > 0)
  -------+--*-----------> Re[Z]
         | D_d  (w = w_n 时 Im = 0)
         |  \
         |   \  曲线向下
         |    \
         |     * (w -> 0: -K_d/w 主导, Im 负无穷)
         |

  关键点:
    w = 0:    Z = D_d - j*inf  (底部)
    w = w_n:  Z = D_d + j*0    (实轴上, Im 为零)
    w = inf:  Z = D_d + j*inf  (顶部)

  整条曲线的实部 = D_d (常数!), 虚部从 -inf 到 +inf
  -> 始终在右半平面 -> 无源 -> 与任何被动环境耦合都稳定

环境阻抗的 Nyquist 图叠加：

环境 \(Z_e(j\omega) = D_e + K_e/(j\omega) = D_e - jK_e/\omega\) 的 Nyquist 图是一条从 \((D_e, -\infty)\) 到 \((D_e, 0)\) 的垂直半直线（也在右半平面）。

\(Z_r + Z_e\) 的 Nyquist 图是两者的"矢量和"，其实部为 \(D_d + D_e > 0\)——始终在右半平面——所以 \(Z_r + Z_e\) 不可能包围原点，耦合系统保证稳定。这就是 Colgate-Hogan 定理在频域中的直观体现。

从 Nyquist 图看离散化的危险：

离散化（零阶保持 + 采样延迟）等效于在 \(Z_r\) 上增加一个相位滞后：

\[Z_r^{disc}(j\omega) \approx Z_r(j\omega) \cdot e^{-j\omega T/2}\]

相位滞后使 Nyquist 曲线"旋转"——当 \(\omega T/2 > \pi/2\) 时，曲线可能进入左半平面——无源性被破坏。这解释了为什么采样率必须远高于力控带宽：保证在有效频率范围内相位旋转不超过 \(90°\)。

\[\omega_{max} T / 2 < \pi/4 \implies f_{control} > 4 f_{bandwidth}\]

这就是 F02 第 2.4 节"采样率 \(\geq 10 \times\) 力控带宽"经验法则的理论基础。

这个频域分析提供了一个重要的工程直觉：阻尼 \(D_d\) 是稳定性的"守护者"——只要 \(D_d > 0\)，连续时间阻抗控制就是无源的。离散化带来的问题本质上是"虚拟阻尼"在采样间隔内的"缺失"。

⚠️ 常见陷阱

🧠 思维陷阱：认为二端口网络模型只是"花哨的理论"，工程中不需要

新手想法："我只需要调 K 和 D，二端口网络模型太理论了，跳过吧"

实际上：二端口模型给出了**耦合稳定性**的唯一严格判据。不用它你无法回答一个致命的工程问题："我的控制器在自由空间稳定，碰到这个特定环境时还稳定吗？"Colgate-Hogan 1988 定理表明，只有当机器人端口的阻抗是无源的，耦合系统才对**任意**无源环境稳定。不理解这个定理，你只能靠试错来判断参数是否安全——在真机上试错的代价可能是损坏设备

正确做法：至少理解无源性⇒耦合稳定的核心结论，用它作为参数选择的安全检查

💡 概念误区：认为 \(Z_r\) 和 \(Z_e\) 可以独立设计

新手想法："机器人的阻抗我来设计，环境的阻抗是给定的，互不影响"

实际上：耦合稳定性取决于 \(Z_r + Z_e\) 的**联合行为**。一个在柔软环境（\(K_e = 100\) N/m）下完美的控制器，在刚性环境（\(K_e = 10^6\) N/m）下可能不稳定——因为环境刚度改变了闭环特征方程。F02 第 2.3 节将定量分析这个耦合效应

练习

1. ⭐⭐ 二端口推导：对一个简单的 1-DOF 阻抗控制系统（\(Z_r = Ms + D + K/s\)），推导其与弹簧环境（\(Z_e = K_e/s\)）耦合后的闭环特征方程。分析稳定性条件。
2. ⭐⭐ 无源性验证：证明标准阻抗 \(Z(s) = Ms + D + K/s\)（\(M, D, K > 0\)）是正实的（即 \(\text{Re}[Z(j\omega)] \geq 0, \forall \omega\)）。提示：代入 \(s = j\omega\) 并取实部。
3. ⭐⭐⭐ 离散化影响：取 \(M_d = 1\) kg, \(D_d = 20\) Ns/m, \(K_d = 500\) N/m。分别以 \(T = 1\) ms 和 \(T = 8\) ms 离散化阻抗控制器（前向欧拉），用 Matlab/Python 仿真接触刚性墙壁（\(K_e = 10^5\) N/m）的响应，对比两个采样率下的稳定性。

---

4. 力传感器与无力传感器方案 ⭐⭐

4.1 力传感方案全景

力控制需要力的信息——但力信息的来源有多种：

传感方案	原理	带宽	精度	成本	代表平台
六维腕力传感器	应变片/压电晶体	~1 kHz	\(\pm\)0.5 N	$3000-15000	所有通用平台
关节力矩传感器	应变片（谐波减速器输出侧）	~3 kHz	\(\pm\)0.1 Nm	内置	Franka, KUKA iiwa
电流反推	电机电流→力矩（\(\tau = K_t i\)）	~10 kHz	\(\pm\)5%（含摩擦）	$0	MIT Cheetah, QDD 臂
动量观测器	广义动量偏差→外力估计	~500 Hz	\(\pm\)1 N	$0（纯软件）	无传感器平台
触觉传感器皮肤	电容/电阻分布式	~100 Hz	空间分辨率	$500-5000	研究级

4.2 六维力/力矩传感器（F/T Sensor）

六维力/力矩传感器测量 6 个分量：\([f_x, f_y, f_z, \tau_x, \tau_y, \tau_z]^T\)。

工作原理（应变片型）：

机械结构：
  +----------------+
  |  法兰连接面     | <-- 连接机器人末端关节
  |                |
  |  弹性体(十字梁)  | <-- 产生微小形变
  |    +-- 应变片组  | <-- 电阻随形变变化
  |                |
  |  法兰连接面     | <-- 连接末端工具
  +----------------+

信号处理链：
  应变 -> 电桥电压 -> AD转换(16-24bit) -> 标定矩阵 -> 6D力/力矩

  F_6D = C_cal * V_raw    (C_cal: 6xN 标定矩阵)

关键工程问题：

问题	影响	解决方案
零偏漂移	温度变化导致零点偏移 ~0.5N/degC	定期零偏标定 + 温度补偿
交叉耦合	x 方向力影响 z 方向读数 ~2%	标定矩阵补偿（出厂标定 + 现场标定）
工具重力	安装末端工具后，重力产生恒定 F/T 偏移	在线重力补偿：\(f_{ext} = f_{raw} - R_{sensor}^T m_{tool} g\)
高频噪声	电机振动/电磁干扰	低通滤波（2 阶 Butterworth，截止频率 30-100 Hz）
安装位置	安装在腕关节 vs 末端工具之间	影响测量坐标系和干扰源

F/T 传感器的频率响应设计：

传感器本身的机械带宽通常高达数 kHz，但实际有效带宽受限于：

有效带宽 = min(传感器机械带宽, ADC采样率/2, 数字滤波截止频率, 控制环频率)

典型瓶颈：
  ATI Mini45:  机械带宽 ~6 kHz, ADC 7.5 kHz
  → 有效带宽 ~3 kHz (满足 1kHz 控制)

  低成本 F/T:  机械带宽 ~1 kHz, ADC 1 kHz
  → 有效带宽 ~200 Hz (仅满足导纳控制)

4.3 关节力矩传感器——Franka 与 KUKA iiwa 的方案

Franka Panda 和 KUKA iiwa 在**每个关节**都内置了力矩传感器（安装在谐波减速器输出侧）。

优势： - 无需额外传感器硬件 - 可感知任意连杆上的外力（不仅是末端） - 7 个关节力矩传感器提供冗余信息

从关节力矩反推末端外力：

\[\hat{f}_{ext} = (J^T)^+ (\tau_{measured} - \tau_{model})\]

其中 \(\tau_{model} = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)\) 是动力学模型预测的力矩。

类比：这就像你闭着眼用手臂推东西——你通过感受肌肉（"关节力矩传感器"）的用力程度和你对自己手臂重量/运动的预期（"动力学模型"）之差，来推断外物施加了多大的力。模型越准，力估计越准。

精度瓶颈：模型误差（摩擦、惯性参数不准、柔性关节动态）直接传递到力估计误差。Franka 出厂标定后的精度约为 \(\pm\)0.5 N，但随使用磨损可能退化。

关节力矩传感器 vs 腕力传感器的设计权衡：

维度	关节力矩传感器	腕力传感器
测量位置	每个关节（分布式）	末端法兰（集中式）
信息量	7 个标量（7-DOF）	6D 矢量
外力定位	可感知任意连杆上的外力	只能感知末端外力
对模型依赖	高——需要精确的 \(M, C, g\)	低——直接测量
摩擦影响	高——摩擦力矩混入测量	低——传感器在减速器输出侧
安装复杂度	出厂集成	需额外安装+标定
碰撞检测	全身碰撞检测	仅末端碰撞

"不是X而是Y"：关节力矩传感器和腕力传感器**不是**替代关系，而是**互补关系**。在需要全身碰撞检测（安全场景）和精确末端力测量（精密装配）的任务中，最佳方案是**同时使用两者**——关节力矩用于碰撞检测和粗力估计，腕力传感器用于精确末端力控。Franka 的官方推荐就是在需要高精度力控时外接 ATI F/T 传感器。

4.4 无力传感器方案——动量观测器

当没有任何力传感器时，可以用**广义动量观测器**（Generalized Momentum Observer）纯软件估计外力。

原理（De Luca & Mattone 2003, Haddadin et al. 2017）：

定义广义动量：

\[p = M(q)\dot{q}\]

其时间导数为：

\[\dot{p} = M(q)\ddot{q} + \dot{M}(q)\dot{q} = \tau + J^T f_{ext} - C(q,\dot{q})\dot{q} + \dot{M}(q)\dot{q} - g(q)\]

利用 \(\dot{M} = C + C^T\) 的性质（Christoffel 符号对称性），简化为：

\[\dot{p} = \tau + C^T(q,\dot{q})\dot{q} - g(q) + J^T f_{ext}\]

定义观测器残差：

\[r = K_I \left( p - p(0) - \int_0^t (\tau + C^T\dot{q} - g + r) \, d\tau' \right)\]

其中 \(K_I > 0\) 是观测器增益。可以证明 \(r \to J^T f_{ext}\)（指数收敛，时间常数 \(\sim 1/K_I\)）。

这里的 \(p(0)\) 是开启观测器时的初始广义动量。若机器人不是静止启动，不减去 \(p(0)\) 会把初始运动量误认为外力残差，导致刚启用时出现假碰撞。

工程实现要点：

// 动量观测器实现（简化版，1kHz 循环）
// 输入: M (惯量矩阵), C (科氏力矩阵), g (重力), q_dot, tau_measured
// 输出: r (外力矩估计, 约等于 J^T * f_ext)

// 初始化时记录一次；允许机器人非静止启动
Eigen::VectorXd p0 = M_initial * q_dot_initial;

Eigen::VectorXd p_current = M * q_dot;  // 当前广义动量
Eigen::VectorXd beta = C.transpose() * q_dot - g;  // 已知力矩项

// 积分更新（矩形积分，实时安全）
integral += (tau_measured + beta + r_prev) * dt;

// 残差计算
r = K_I * (p_current - p0 - integral);

// 外力估计（需要 Jacobian 伪逆）
// f_ext_hat = (J^T).pseudoInverse() * r;

局限性：

误差源	量级	对策
关节摩擦	0.5-2 Nm（库仑摩擦）	摩擦辨识+补偿（Stribeck 模型）
惯性参数误差	5-15%	SysId 标定（Swevers 法）
速度估计噪声	数值微分放大高频噪声	观测器 + 低通滤波
收敛延迟	\(\sim 1/K_I\)（10-50 ms）	\(K_I\) 越大越快但噪声放大

4.5 力传感器选型决策树

你的机器人有关节力矩传感器吗？
|-- 是（Franka/iiwa）
|     |-- 需要精确末端力测量（< 0.5N）？
|     |     |-- 是 -> 额外加六维 F/T 传感器
|     |     +-- 否 -> 用关节力矩反推
|     +-- 控制范式 -> 阻抗控制（直接力矩接口）
|
|-- 否，但有位置/速度接口（UR/Fanuc/ABB）
|     |-- 预算允许 F/T 传感器？
|     |     |-- 是 -> 六维 F/T + 导纳控制
|     |     +-- 否 -> 动量观测器 + 导纳控制（精度受限）
|     +-- 控制范式 -> 导纳控制（位置输出型）
|
+-- QDD 准直驱（MIT Cheetah 风格）
      +-- 电流反推力矩 + 阻抗控制
      +-- 优势：高带宽（>1kHz）、低成本
      +-- 劣势：力估计精度差（含齿轮/摩擦不确定性）

4.6 工业传感方案对比——三大协作臂平台

维度	Franka Emika Panda	KUKA LBR iiwa	UR e-Series
关节力矩传感器	7 个（谐波输出侧）	7 个（谐波输出侧）	无
腕力传感器	无（需外接）	无（需外接）	集成 6D F/T（TCP）
力控接口	FCI 1kHz 力矩	FRI 1kHz 力矩	位置/速度接口 + force_mode
天然范式	阻抗控制	阻抗控制	导纳控制
力估计精度	\(\pm\)0.5 N（关节反推）	\(\pm\)0.5 N（关节反推）	\(\pm\)2 N（F/T 传感器）
力控带宽	~200 Hz	~200 Hz	~50 Hz（外环导纳）
ROS2 驱动	franka_ros2	iiwa_ros2	Universal_Robots_ROS2_Driver

"不是X而是Y"：UR 的 force_mode **不是**真正的阻抗控制，而是一个内置的导纳控制器——它读取内置 F/T 传感器的力，计算位置修正，然后喂给内环位置控制器。理解这一点对正确使用 UR 做力控至关重要。

⚠️ 常见陷阱

⚠️ 编程陷阱：F/T 传感器安装后不做零偏标定就使用

错误做法：装好传感器直接读取数据，认为"出厂已标定"

现象：即使无外力接触，力读数也有 1-5 N 的持续偏差；导纳控制器在无人推的时候缓慢飘移

根本原因：传感器的零偏会因安装方式、温度、电缆弯曲而变化。出厂标定只保证量程和线性度，不保证零偏

正确做法：每次上电后在无接触状态下执行零偏标定（记录 100 帧取均值作为 bias），并在运行前重复检查

💡 概念误区：认为动量观测器可以完全替代 F/T 传感器

新手想法："动量观测器不需要额外硬件就能估计外力，为什么还要花钱买 F/T？"

实际上：动量观测器依赖精确的动力学模型（\(M(q), C(q,\dot{q}), g(q)\)），而关节摩擦（Coulomb + 粘滞）很难精确建模。典型摩擦模型误差 0.5-2 Nm，映射到末端约 2-10 N 的力估计偏差。对于精度要求 \(< 1\) N 的任务（精密装配、质量检测），动量观测器**精度不够**，必须使用硬件 F/T 传感器

练习

1. ⭐ 重力补偿：一个末端安装了 0.5 kg 工具的 F/T 传感器。机器人当前姿态使工具重心在传感器坐标系 z 轴正下方。写出重力补偿公式，计算补偿前后 z 方向力读数的差异。
2. ⭐⭐ 观测器调参：动量观测器增益 \(K_I\) 选大了会怎样？选小了呢？如果关节摩擦辨识误差为 0.5 Nm，\(K_I = 50\) 时外力估计的稳态偏差约为多少？
3. ⭐⭐ 方案选型：你要为一个 UR5e 搭建一个打磨系统（法向力精度要求 \(\pm\)1 N）。分析是否需要外接 F/T 传感器，还是 UR 内置 F/T + force_mode 就足够。

---

5. 力控技术栈全景分类 ⭐⭐

5.1 完整分类树

经过前 4 节的铺垫，我们现在可以画出力控技术的完整分类树。这棵树将贯穿 F01-F10 全系列：

柔顺控制 (Compliance Control)
|
|-- 被动柔顺 (Passive Compliance) ——无需传感/控制
|   |-- RCC (Remote Center Compliance, Whitney 1977)
|   |     +-- 弹性元件使末端绕远程中心旋转，用于自对准插入
|   |-- SEA (Series Elastic Actuator)
|   |     +-- 弹簧串联于电机和负载之间 (ANYmal/DLR LWR/Franka)
|   +-- 柔性末端执行器 (软体夹爪/气动手指)
|
|-- 主动柔顺 (Active Compliance) ——传感+控制实现
|   |-- 直接力控 (PI 力跟踪)              <-- F03.1
|   |-- 刚度控制 (Salisbury 1980)         <-- F03.3
|   |-- 阻抗控制 (Hogan 1985)             <-- F03.4, F04
|   |     |-- 关节空间阻抗                 <-- F03.4
|   |     |-- 笛卡尔空间阻抗               <-- F03.4, F04
|   |     +-- 操作空间阻抗 (Khatib '87)    <-- F03.4
|   |-- 导纳控制 (Hogan 对偶)              <-- F05
|   |     |-- 位置型导纳                   <-- F05
|   |     +-- 速度型导纳                   <-- F05
|   |-- 混合力/位控 (Raibert-Craig '81)    <-- F03.2
|   |-- 并行力/位控 (Chiaverini '93)       <-- F03 并行力/位控
|   +-- 全身控制 WBC (Sentis-Khatib '05)   <-- F07
|
|-- 变柔顺 (Variable Compliance) ——参数在线调节
|   |-- 规则变阻抗 (状态机切换 K/D)        <-- F06
|   |-- 学习变阻抗
|   |     |-- DMP + 变 K/D (Buchli-Schaal PI^2)  <-- F09
|   |     |-- GMM/GMR 编码 (Calinon/Billard)     <-- F09
|   |     +-- RL 输出阻抗参数 (VICES/SERL)       <-- F09
|   +-- 自适应阻抗 (估计环境刚度 -> 调 K_d)      <-- F06
|
+-- 学习型柔顺 (Learned Compliance) ——端到端或混合
    |-- 策略+柔顺底层 (Diffusion Policy + CRISP)  <-- F09
    |-- 残差力矩+基层阻抗 (Residual RL)            <-- F09
    +-- 端到端力矩输出 (仅仿真, 实机不推荐)         <-- F09

5.2 被动柔顺 vs 主动柔顺——工程设计哲学

特性	被动柔顺	主动柔顺
传感器	不需要	需要（F/T 或关节力矩）
带宽	极高（机械响应 ~kHz）	受限于控制环（~1 kHz）
可调性	固定（由机械结构决定）	在线可调（软件参数）
能量	纯被动（不注入能量）	可能注入能量（需保证无源性）
成本	低（一个弹簧）	高（传感器+实时计算）
精度	低（不能精确控力）	高（闭环力跟踪）
可靠性	极高（无软件故障）	取决于软件/硬件质量

当代最佳实践——三层柔顺设计：

层级 1: 机械被动层 (SEA/弹性法兰)
  -> 提供固有安全（冲击吸收）和粗力估计
  -> 带宽低 (~100 Hz) 但无延迟
  -> 即使控制器崩溃也安全

层级 2: 主动控制层 (阻抗/导纳)
  -> 提供精确力跟踪和可编程刚度
  -> 带宽 ~1 kHz
  -> 可在线调参适应不同任务

层级 3: 学习策略层 (RL/Diffusion)
  -> 提供任务级自适应
  -> 带宽 10-50 Hz
  -> 自动发现最优刚度策略

Franka Panda 就是这三层的典范:
  SEA(谐波减速器弹性) + 阻抗控制(libfranka) + 学习(CRISP)

SEA vs QDD——被动柔顺的两条路线：

SEA (Series Elastic Actuator):
  电机 --[齿轮]-- 弹簧 --[负载]
  优势: 高力估计精度、天然碰撞保护、低带宽
  代表: ANYdrive (K_s~320 Nm/rad), Franka (K_s~3000 Nm/rad)

QDD (Quasi-Direct Drive):
  电机 --[低比减速器/直连]-- 负载
  优势: 高力矩带宽 (>1 kHz)、低惯量、高反驱性
  代表: MIT Cheetah, Unitree Go2

选择原则:
  人机协作/安全优先 -> SEA (ANYmal, Franka)
  高速动态运动优先 -> QDD (MIT Cheetah, Unitree Go2)

**SEA 的弹簧刚度选择**是一个深刻的工程权衡：

• 弹簧越**软**（低 \(K_s\)）：力估计精度越高（相同力下形变大→更好的信噪比）、碰撞保护越好（弹簧吸收冲击能量），但力控带宽越低（弹簧谐振频率 \(\omega_s = \sqrt{K_s/m}\) 限制了可控频率范围）
• 弹簧越**硬**（高 \(K_s\)）：力控带宽越高，但力估计精度降低、碰撞保护能力下降

典型设计点：ANYdrive 选择 \(K_s \approx 320\) Nm/rad（软弹簧，优先安全），Franka 选择 \(K_s \approx 3000\) Nm/rad（硬弹簧，优先带宽）。这个选择直接决定了平台适合的力控应用范围。

5.3 控制频率层级——为什么力环必须快

层级	频率	典型实现	为什么这个频率
电流/力矩环 FOC	10-40 kHz	驱动器 MCU (STM32/TI C2000)	电机电感时间常数 ~100 us
关节扭矩环	1-4 kHz	Franka/iiwa 内部	SEA 弹簧动态带宽
笛卡尔阻抗/导纳	1 kHz	libfranka FCI / KUKA FRI	接触瞬态 ~ms 级
WBC (QP)	200-1000 Hz	mc_rtc / WBIC	QP 求解时间 0.5-2 ms
MPC	30-500 Hz	OCS2 / Crocoddyl / mujoco_mpc	SQP/DDP 迭代
RL / 视觉策略	10-50 Hz	PyTorch GPU	GPU 推理+感知延迟
任务规划	1-10 Hz	BT.CPP / FSM	人类节奏

本质洞察：接触瞬态发生在微秒到毫秒时间尺度，但学习策略的决策周期是 20-100ms。阻抗控制器作为"低延迟安全层"桥接两者——它以 1kHz 处理接触动力学，同时接受低频策略更新的期望位姿/阻抗参数。这正是 CRISP（TUM, 2025）的核心架构洞见："高频 C++ 柔顺底层 + 低频 Python GPU 策略"。

⚠️ 常见陷阱

🧠 思维陷阱：认为"阻抗控制 > 导纳控制"，选型时总是优先阻抗控制

新手想法："阻抗控制是直接控力矩，更高级、更精确，能用阻抗就用阻抗"

实际上：选型**不是**由"哪个更好"决定，而是由**硬件接口**决定。95% 以上的工业臂（UR/Fanuc/ABB/Yaskawa）只有位置/速度接口，**只能**用导纳控制。即使硬件支持力矩接口，导纳控制在不需要精确动力学模型、利用已调好的位置环等场景下仍有优势

正确思维：先看硬件能力，再选控制范式——这是一个**约束满足**问题，不是"哪个更好"的偏好问题

练习

1. ⭐ 分类练习：将以下算法归入分类树的正确位置：(a) libfranka cartesian_impedance_control，(b) ros2_controllers admittance_controller，(c) MIT Cheetah 的 WBIC，(d) SERL，(e) Whitney 的 RCC。
2. ⭐⭐ 频率选择：一个接触任务的接触动力学带宽约为 200 Hz。根据 Nyquist 定理，力控环的最低采样率应为多少？如果你的控制器只能跑在 500 Hz，这会对哪些类型的接触任务产生影响？
3. ⭐⭐ 三层设计：为一个 UR5e + 六维 F/T 传感器的打磨系统设计三层柔顺架构。每层的具体参数如何选择？

---

6. 工业应用场景地图 ⭐⭐

6.1 力控的四大工业应用场景

应用场景	典型力/力矩	关键挑战	推荐控制方案	代表行业
打磨/抛光	法向 10-50 N	恒力跟踪曲面	并行力/位 或 导纳	汽车/航空
精密装配	插入力 5-20 N	peg-in-hole 对准	阻抗（低K）+ 搜索	电子/汽车
人机协作	引导力 2-10 N	安全 + 透明度	导纳（低M_d）	仓储/医疗
质量检测	接触力 0.5-5 N	高精度力测量	直接力控 + F/T	半导体

6.2 打磨/抛光——恒力跟踪

任务描述：机械臂持砂纸/抛光盘，沿工件表面运动，保持恒定法向力 \(f_n = f_{ref}\)。

工程挑战： - 工件表面是曲面 → 法向方向随位置变化 - 工件尺寸有公差 → 标称表面与真实表面有偏差 - 砂纸/抛光盘磨损 → 接触刚度随时间变化

推荐方案：并行力/位控（Chiaverini 1993） - 法向：力 PI 跟踪 \(f_{ref}\)（典型 10-30 N） - 切向：位置 PD 跟踪打磨路径 - 不需要精确曲面模型（力控自动适应）

控制框图（打磨）：
  +--------------+     +-----------+
  | 路径规划      | x_d | 位置 PD    | F_pos
  | (x/y 切向)   |---->|           |----> +
  +--------------+     +-----------+     |    +----------+
                                         +--->| J^T x    | -> tau -> 机器人
  +--------------+     +-----------+     |    | 关节力矩 |
  | 力参考       | f_d | 力 PI      | F_force  +----------+
  | (z 法向)     |---->|           |----> +
  | f_ref=20N    |     | + integral|
  +--------------+     +-----------+
                            ^ f_measured
                       (F/T 传感器)

打磨力控参数工程手册（来自工业实践）：

不同打磨工序对力控参数的要求差异巨大，以下是三类典型工艺的完整参数：

工艺	法向力 \(f_n\)	切向速度	砂纸粒度	环境刚度 \(K_e\)	控制器 \(K_d\)	\(D_d\)	力精度要求
粗磨（去毛刺）	30-50 N	50-200 mm/s	P80-P120	\(10^5\) N/m	200 N/m	\(2\sqrt{K_d M_d}\)	\(\pm 5\) N
精磨（表面处理）	10-20 N	20-50 mm/s	P240-P400	\(5\times10^4\) N/m	100 N/m	\(2\sqrt{K_d M_d}\)	\(\pm 2\) N
抛光（镜面加工）	5-10 N	100-500 mm/s	P1000+	\(10^4\) N/m	50 N/m	\(2\sqrt{K_d M_d}\)	\(\pm 0.5\) N

力控 PI 调参经验（打磨专用）：

力 PI 控制器的参数选择依赖于环境刚度和期望带宽的折中：

\[K_{fp} = \frac{1}{K_e} \cdot \frac{\omega_{fc}}{3}, \quad K_{fi} = K_{fp} \cdot \frac{\omega_{fc}}{5}\]

其中 \(\omega_{fc}\) 是期望的力控闭环带宽（rad/s）。典型值：粗磨 \(\omega_{fc} = 2\pi \times 10\) rad/s，精磨 \(\omega_{fc} = 2\pi \times 5\) rad/s。

反事实推理：如果打磨任务中不用力控而用纯位置控制会怎样？工件表面高度不均匀性为 \(\pm 0.5\) mm，位置控制器刚度 \(K_p = 2000\) N/m，则法向力波动 \(\Delta f = K_p \times 0.5 \text{mm} = 1\) N。看似可接受，但这没有考虑工件装夹误差（\(\pm 2\) mm）和砂纸磨损（接触点后退 \(\sim 1\) mm），实际力波动可达 \(\pm 6\) N——对精磨和抛光完全不可接受。力控的价值：将力波动从"工件精度决定"降低到"控制器精度决定"。

6.3 精密装配——peg-in-hole

任务描述：将销轴（peg）插入孔（hole），间隙通常 0.05-0.5mm。

工程挑战： - 间隙极小 → 位置控制无法保证对准 - 初始对准误差 → 产生巨大侧向力/力矩 - 接触状态复杂 → 粘附/滑动/楔紧多种模式

推荐方案：阻抗控制（低 \(K_d\)）+ 螺旋搜索策略

装配策略状态机：
  [自由空间] K_d=1000 N/m, 快速接近
       | 检测到接触力 > 2N
       v
  [搜索阶段] K_d=100 N/m, 螺旋搜索(r=1mm, v=5mm/s)
       | z 方向位移 > 0.5mm (孔口对准)
       v
  [插入阶段] K_d=50 N/m, z 方向速度控制 2mm/s
       | 插入深度达到目标
       v
  [完成] K_d=500 N/m, 保持位置

装配力控参数工程手册（来自工业实践）：

peg-in-hole 装配任务的关键参数取决于间隙比（间隙/直径）和零件材料：

装配类型	间隙 (mm)	间隙比	搜索力限 \(f_{max}\)	插入刚度 \(K_{z}\)	横向刚度 \(K_{xy}\)	搜索速度	旋转柔顺 \(K_{rot}\)
粗间隙（H7/f6）	0.1-0.5	\(\sim\)0.01	30 N	100 N/m	200 N/m	5 mm/s	5 Nm/rad
精密间隙（H7/g6）	0.01-0.1	\(\sim\)0.001	15 N	50 N/m	100 N/m	2 mm/s	2 Nm/rad
过盈配合	\(<\) 0	负	100 N	500 N/m	1000 N/m	1 mm/s	10 Nm/rad

螺旋搜索策略参数（Archimedean 螺线）：

\[x(t) = r_0 + v_r t \cdot \cos(\omega_s t), \quad y(t) = r_0 + v_r t \cdot \sin(\omega_s t)\]

• 螺线初始半径 \(r_0 = 0\)（从中心开始）
• 径向扩展速率 \(v_r = 0.5\)-\(2\) mm/rev（每圈扩展）
• 旋转角速度 \(\omega_s = 2\pi \times 1\)-\(3\) Hz
• 最大搜索半径 \(r_{max} = 3\)-\(5\) mm（超过则报错——可能工件定位失败）
• 对准判据：\(\Delta z > 0.3\) mm（z 方向位移突然增大，说明已进入孔口）

异常检测与安全保护：

在装配过程中必须监测力/力矩信号以检测异常状态：

异常状态	力/力矩特征	检测条件	动作
楔紧（wedging）	\(f_z\) 突增 + \(\tau_{xy}\) 突增	\(f_z > 2 f_{max}\) 且 \(\|\tau_{xy}\| > 2\) Nm	回退 2mm → 重新搜索
交叉对位（cross-threading）	\(\tau_z\) 异常大	$	\tau_z
零件缺失	搜索超时无对准	搜索时间 \(> t_{max}\)（典型 10 s）	报警 → 人工检查
过度倾斜	$	\tau_{xy}	$ 持续偏大

与被动柔顺（RCC）的对比：

维度	RCC（被动）	阻抗控制（主动）
适应范围	固定几何（需匹配远程中心距离）	任意几何（软件可调）
间隙要求	> 0.1mm	> 0.02mm（更精确）
搜索策略	无（靠物理引导）	可编程螺旋/随机搜索
状态监测	无（盲插）	力/力矩监测+异常检测
成本	极低	高（传感器+控制器）

6.4 人机协作——力引导示教

任务描述：操作者手动引导机械臂运动（"拖动示教"），机器人记录轨迹用于后续回放。

推荐方案：导纳控制（或重力补偿模式 \(K_d = 0\)）

\[\Delta x = Y(s) \cdot f_{ext} = \frac{1}{M_d s + D_d} f_{ext}\]

参数选择： - \(M_d = 1-5\) kg（虚拟质量，决定"推动的惯性感"） - \(D_d = 20-100\) Ns/m（虚拟阻尼，决定"推动的粘滞感"） - \(K_d = 0\)（无弹簧→操作者松手后机器人停在当前位置）

最简实现——重力补偿模式：

\[\tau = g(q) - D_{joint} \dot{q}\]

这是阻抗控制在 \(K_d = 0, M_d = M(q)\)（真实惯量）时的退化形式——机器人只补偿重力，加一点关节阻尼防止飘动（\(-D_{joint}\dot{q}\) 产生阻碍运动的力矩，耗散动能）。Franka 的 gravity_compensation 模式就是这个。

6.5 碰撞安全——Haddadin 四阶段框架

力控制的另一个核心应用是碰撞安全——确保机器人在意外碰撞时不伤害人类。Haddadin et al. 2017（TUM，IEEE RAM）提出了碰撞处理的四阶段框架，已成为协作机器人安全设计的事实标准。

碰撞处理四阶段：

Phase 1: 碰撞前 (Pre-collision)
  目标: 降低碰撞冲击能量
  手段:
    - 速度限制: v_max 随距离人递减 (ISO/TS 15066 SSM)
    - 惯量整形: 降低末端有效质量 m_eff
    - 零空间避障: 让肘关节远离人体
  关键参数:
    - ISO/TS 15066 按 29 个身体区域分别规定准静态/瞬态力和压力阈值
    - 示例: 前额准静态 130 N / 瞬态 260 N; 胸部准静态 140 N / 瞬态 280 N
    - 实际限值取决于碰撞的具体身体部位和接触类型（准静态夹持 vs 瞬态碰撞）
    - 完整阈值表见 ISO/TS 15066:2016 Annex A（Table A.2）

Phase 2: 碰撞检测 (Collision detection)
  方法:
    - 动量观测器: r = K_I(p - p(0) - integral(tau + beta + r))
    - 检测条件: ||r|| > r_threshold (典型 3-5 Nm)
    - 检测延迟: < 5 ms (关键指标!)
  备选方法:
    - 外部 3D 视觉 (latency ~30 ms, 太慢用于保护)
    - 触觉皮肤 (latency ~5 ms, 仅覆盖局部)

Phase 3: 碰撞隔离 (Collision isolation)
  目标: 判断碰撞发生在哪个连杆
  方法: 根据 r 的分布推断碰撞连杆编号
    - 如果 r 集中在关节 4-7 -> 末端附近碰撞
    - 如果 r 分布在关节 1-3 -> 基座附近碰撞
  意义: 不同位置的碰撞需要不同的反应策略

Phase 4: 碰撞反应 (Collision reaction)
  策略矩阵:
  +----------------+------------------+------------------+
  | 碰撞类型       | 反应策略          | 参数              |
  +----------------+------------------+------------------+
  | 瞬态碰撞       | 反射运动(弹开)    | v_reflex = 50mm/s |
  | 准静态夹持     | 零力矩+后退      | tau -> 0, 后退5mm  |
  | 持续接触       | 切换到超低刚度    | K_d -> 20 N/m     |
  +----------------+------------------+------------------+

碰撞能量与速度的工程计算：

碰撞过程中传递给人体的能量为：

\[E_{collision} = \frac{1}{2} m_{eff} v^2 \cdot \frac{m_{human}}{m_{eff} + m_{human}}\]

其中 \(m_{eff} = 1/(n^T \Lambda^{-1} n)\) 是末端有效质量（取决于碰撞方向 \(n\) 和构型），\(m_{human}\) 是人体等效质量（头部约 4.4 kg，ISO/TS 15066）。

对 Franka Panda（\(m_{eff} \approx 2\)-\(5\) kg），以前额瞬态碰撞限值（\(F_{tr} = 260\) N，准静态 \(F_{qs} = 130\) N）为例，具体限速取决于碰撞身体部位和接触类型（参见 ISO/TS 15066 Annex A），大约要求：

\[v_{max} \leq 250 \text{ mm/s} \quad \text{（依构型和碰撞方向而变）}\]

这个限速直接约束了协作场景下的生产节拍——速度越快碰撞能量越大。阻抗控制的 \(M_d\) 参数可以通过降低有效质量来部分缓解这个矛盾，但受限于物理惯量不能真正"消除"质量。

"不是X而是Y"：碰撞安全**不是**简单的"限速+力限幅"，而是一个从运动规划（Phase 1）到反射反应（Phase 4）的完整流水线。仅靠力限幅无法处理高速瞬态碰撞——因为在传感器检测到碰撞力之前，冲击力峰值已经过去了。所以 Phase 1 的运动规划（限速+惯量整形）才是真正的第一道防线。

6.6 质量检测——精密力测量

任务描述：以极低接触力（0.5-5 N）扫描工件表面，测量形状偏差或检测缺陷。

工程挑战： - 接触力极小 → 力传感器噪声成为瓶颈（典型 F/T 噪声 ~0.1 N） - 不允许损伤表面 → 力过冲绝对禁止 - 高空间分辨率 → 扫描速度慢（1-5 mm/s），每个接触点的力必须精确稳定

推荐方案：直接力控 PI + F/T 传感器

参数	典型值	理由
目标力 \(f_d\)	1-3 N	足够检测接触但不损伤
\(K_{fp}\)	0.002 m/N	低增益防止力过冲
\(K_{fi}\)	0.001 m/(N\(\cdot\)s)	低积分消除稳态偏差
F/T 滤波截止	10-20 Hz	低噪声优先于高带宽
接近速度	1 mm/s	极低速减小冲击
阻尼比	\(\zeta = 1.5\)（过阻尼）	绝对禁止力过冲

半导体行业案例：晶圆边缘检测中，接触力限制为 \(< 0.5\) N（防划伤），位置精度要求 \(\pm 10\) \(\mu\)m。这要求 F/T 传感器分辨率 \(< 0.05\) N——只有高端压电式传感器（如 ATI Nano17，分辨率 0.006 N）能满足。

6.7 力控应用与硬件平台的匹配

应用场景 -> 力控方案 -> 硬件平台匹配：

打磨/抛光（恒力 10-30N）
  -> 并行力/位控 或 导纳控制
  -> UR5e + F/T 或 Fanuc CRX（工业首选，成本低）

精密装配（间隙 < 0.1mm）
  -> 阻抗控制（低 K_d）+ 搜索
  -> Franka Panda 或 KUKA iiwa（力矩接口+高精度）

人机协作（安全+自然交互）
  -> 导纳控制（低 M_d/D_d）
  -> 任何协作臂（UR/Franka/ABB YuMi）

质量检测（精密力测量 < 5N）
  -> 直接力控 PI + 高精度 F/T
  -> Franka + ATI Nano17 或 KUKA iiwa（高精度场景）

学习型接触操作（SERL/Diffusion）
  -> 阻抗底层 + RL/IL 策略
  -> Franka（CRISP 框架的首选平台）

工业场景总结——选型速查表：

决策因素	打磨	装配	协作	检测	学习型
力精度要求	\(\pm 2\)-\(5\) N	\(\pm 1\) N	\(\pm 2\) N	\(\pm 0.1\) N	由策略决定
力控带宽	10-30 Hz	20-50 Hz	5-20 Hz	5-10 Hz	10-50 Hz
位置精度	\(\pm 1\) mm	\(\pm 0.05\) mm	\(\pm 5\) mm	\(\pm 0.01\) mm	\(\pm 1\) mm
安全等级	中	中	极高	高	中
成本敏感度	高	中	中	低	低
首选平台	UR/Fanuc	Franka/iiwa	UR/ABB	Franka+ATI	Franka

⚠️ 常见陷阱

💡 概念误区：认为打磨任务用大刚度 \(K_d\) 才能保持恒力

新手想法："目标力 20 N，刚度越大偏差越小，力就越恒定"

实际上：打磨场景中接触面不是完美平面——存在曲率变化和表面粗糙度。高 \(K_d\) 意味着 0.1 mm 的表面起伏产生 \(K_d \times 0.1\text{mm}\) 的力波动。如果 \(K_d = 5000\) N/m，则力波动 \(\pm 0.5\) N；如果用力控外环（PI 跟踪目标力），\(K_d\) 只需保证接触即可，目标力由外环维持。工业打磨通常用 \(K_d = 50\)-\(500\) N/m（远低于直觉预期）配合力控外环

正确做法：打磨用"低 \(K_d\)（保持贴合）+ 力控外环（维持目标力）"，而非"高 \(K_d\) 依赖弹簧回复力"

⚠️ 编程陷阱：碰撞检测阈值设置不当

错误做法：用固定的力阈值 \(f_{th} = 10\) N 做碰撞检测，不考虑任务力

现象：在打磨任务中（正常力 20 N），碰撞检测持续误触发导致机器人频繁急停；或者阈值设太高，真正碰撞时无法及时检测

根本原因：碰撞检测阈值必须高于**任务正常力范围**。如果正常接触力 15-25 N，碰撞阈值至少要 35-50 N 才能避免误触发

正确做法：碰撞阈值 = 任务最大正常力 \(\times\) 安全裕度（通常 1.5-2 倍），或使用力变化率 \(\dot{f}\) 而非绝对值作为检测指标

练习

1. ⭐ 场景分析：一个汽车装配线需要将门锁螺栓（直径 6mm，间隙 0.3mm）自动拧入车门。分析约束空间，选择力控方案，推荐硬件平台。
2. ⭐⭐ 参数设计：为人机协作拖动示教设计导纳参数。要求：(a) 操作者单手推力约 5N 时末端速度达 100mm/s，(b) 松手后 0.5s 内停止。计算 \(M_d\) 和 \(D_d\)。
3. ⭐⭐ 跨场景对比：同一个 Franka Panda，分别做打磨（恒力 20N）和拖动示教（零力跟踪）。写出两个场景下阻抗参数的具体数值选择，解释为什么差异巨大。
4. ⭐⭐ 碰撞安全计算：Franka Panda 末端有效质量 \(m_{eff} = 3\) kg，以速度 \(v = 200\) mm/s 碰撞人体头部（等效质量 4.4 kg，碰撞持续 5 ms）。计算碰撞力峰值。是否满足 ISO/TS 15066 头部限制（130 N）？如果不满足，最大允许速度是多少？
5. ⭐⭐⭐ 打磨参数计算：一个铝合金工件（\(K_e = 5 \times 10^5\) N/m）需要精磨（目标法向力 15 N，精度 \(\pm 2\) N）。(a) 选择阻抗参数 \(K_d, D_d\)；(b) 计算闭环带宽和阻尼比；(c) 检查 \(K_d \ll K_e\) 是否满足——如果不满足意味着什么？(d) 计算稳态位置偏差 \(\tilde{x}_{ss}\)。
6. ⭐⭐⭐ 跨章综合——装配状态机：为 Franka Panda 的 M6 螺栓插入任务设计完整的力控状态机。要求：(a) 画出状态转移图（包含搜索/插入/异常处理至少 5 个状态）；(b) 每个状态给出具体的阻抗参数和切换条件；(c) 分析如果搜索阶段 \(K_d\) 设置过高（如 2000 N/m）会发生什么。这道题综合了 F01 的约束分析、二端口理论和工业应用知识。

---

7. 与足式方向的力控对偶 ⭐⭐⭐

7.1 固定基座 vs 浮动基座——力控的根本差异

回顾 F02 的操作空间动力学和本章的力控框架，一个自然的问题是：这些理论如何推广到足式机器人？

回顾 足式/90_WBC分层优化与TSID（WBC）：浮动基座的全身动力学方程为 \(M(q)\ddot{q} + h(q,\dot{q}) = S^T\tau + J_c^T\lambda\)，其中 \(M\) 是广义质量矩阵，\(h\) 包含科氏力和重力，\(S\) 是选择矩阵（欠驱动的浮动基座前 6 维为零），\(J_c\) 是接触雅可比，\(\lambda\) 是接触力。在那里我们用它构建了 WBC 的 QP 框架——以 \((\ddot{q}, \lambda)\) 为决策变量最小化任务跟踪误差，同时满足动力学约束和摩擦锥约束。

机械臂力控和足式 WBC 的力控对偶关系：

维度	机械臂力控	足式 WBC
基座	固定	浮动（欠驱动）
接触	末端-环境（1 个接触）	足端-地面（2-4 个接触）
控制目标	末端阻抗行为	质心运动 + 足端力分配
力的角色	控制末端与环境的交互力	控制地面反力来驱动质心
数学形式	\(\tau = J^T(-K_d\tilde{x} - D_d\dot{x}) + g\)	\(\tau = S(M\dot{v}_{des}+h-J_c^T\lambda_{opt})\)
约束	力矩限制、力限制	摩擦锥、CoP/ZMP、力矩限制
优化器	显式控制律（无需 QP）	QP 求解器（qpOASES/ProxQP）
代表框架	libfranka 阻抗控制	TSID / mc_rtc / WBIC

7.2 Khatib 操作空间控制——从机械臂到全身控制的桥梁

Khatib 1987 的操作空间框架是连接机械臂力控和足式 WBC 的关键桥梁。

机械臂版本（固定基座，1 个末端）：

\[\tau = J^T \Lambda (\ddot{x}_d - K_d\tilde{x} - D_d\dot{\tilde{x}}) + J^T\mu + J^T p + (I - J^T\bar{J}^T)\tau_{null}\]

Sentis-Khatib 2005 全身控制版本（浮动基座，多个末端）：

\[\tau = \sum_{k=1}^{N_{task}} N_{k-1}^T J_k^T F_k + N_{N}^T \tau_{null}\]

其中 \(N_k\) 是前 \(k\) 个任务的累积零空间投影。

QP 形式的 WBC（当代主流，对应 足式/90_WBC分层优化与TSID 的 QP 力控框架）：

\[\min_{\ddot{q}, \lambda, \tau} \sum_k w_k \|J_k\ddot{q} + \dot{J}_k\dot{q} - \ddot{x}_{k,des}\|^2\]

\[\text{s.t.} \quad M\ddot{q} + h = S^T\tau + J_c^T\lambda \quad \text{(动力学)}$$ $$\quad\quad\quad A_{fc}\lambda \leq 0 \quad \text{(摩擦锥)}\]

若采用 \(S=[0_{n\times 6}\ I_n]\) 选择受驱关节行，则由动力学的受驱部分得到

\[\boxed{\tau = S(M\ddot{q}+h-J_c^T\lambda)}\]

浮动基座的前 6 行没有执行器，不能用 \(\tau\) 平衡；它们必须由接触力 \(\lambda\) 和基座加速度共同满足。这也是 WBC-QP 必须同时优化 \(\ddot{q}\) 与接触力的原因。 $\(\quad\quad\quad \tau_{min} \leq \tau \leq \tau_{max} \quad \text{(力矩限制)}\)$

本质洞察：机械臂的阻抗控制和足式的 WBC-QP 在本质上解决同一个问题——在满足物理约束（动力学、摩擦、力矩限制）的前提下，实现期望的力-运动行为。区别在于：机械臂有 1 个接触点和固定基座，约束简单到可以用显式公式解；足式有多个接触点和浮动基座，约束复杂到必须用 QP 在线求解。

7.3 阻抗参数在固定基座与浮动基座中的物理意义差异

一个容易被忽视但非常重要的区别是：阻抗参数 \((K_d, D_d, M_d)\) 在固定基座和浮动基座系统中的**物理意义不同**。

固定基座：阻抗参数描述的是末端执行器相对于**固定世界坐标系**的力-运动关系。\(K_d = 500\) N/m 意味着末端偏离平衡位置 1mm 时产生 0.5N 的回复力。参考系是固定的、毫无歧义的。

浮动基座：末端的运动是"相对于什么"的？有三种可能的参考系：

参考系选择	定义	物理含义	适用场景
世界系	\(\tilde{x} = x_{ee}^{world} - x_d^{world}\)	末端在惯性系中的绝对偏差	需要绝对定位的操作任务
质心系	\(\tilde{x} = x_{ee}^{CoM} - x_d^{CoM}\)	末端相对于质心的偏差	行走时的手臂运动
基座系	\(\tilde{x} = x_{ee}^{base} - x_d^{base}\)	末端相对于躯干的偏差	遥操作/抓取

反事实推理：如果在浮动基座上使用世界系阻抗而不考虑基座运动，当四足机器人身体前倾时，手臂会"试图"保持世界坐标中的位置——这意味着手臂必须相对于身体向后运动来补偿身体前倾，可能超出关节极限。使用基座系阻抗则避免了这个问题——手臂始终保持相对于身体的姿态。

这个参考系选择问题目前没有公认的最优方案（见第 8.3 节开放问题表），是浮动基座力控的一个活跃研究方向。

7.4 统一视角：力控是交互控制的核心

从更高的抽象层次看，无论是机械臂打磨（末端-工件交互）、四足行走（足端-地面交互）、还是人形操作（同时有足端-地面和手端-物体交互），本质上都是在解决同一类问题：

交互控制问题：在不完全已知的环境中，通过调控接触力和运动，实现期望的任务目标，同时保证安全和稳定。

交互控制的统一框架：
  +-------------------------------------+
  |           任务层（做什么）            |
  |  打磨: 恒力跟踪  /  行走: 步态      |
  +------------------+------------------+
                     | 期望力/运动
  +------------------v------------------+
  |      力-运动关系层（怎么做）          |
  |  机械臂: 阻抗控制                    |
  |  足式:   WBC-QP                     |
  |  人形:   多接触 WBC + 末端阻抗       |
  +------------------+------------------+
                     | 关节力矩
  +------------------v------------------+
  |        执行层（硬件）                 |
  |  力矩接口 / 位置接口 / QDD / SEA    |
  +-------------------------------------+

这个统一视角将在 F07（浮动基座 WBC）和 F08（MPC+WBC 联合力控）中展开。

练习

1. ⭐⭐ 对偶分析：写出机械臂阻抗控制和四足 WBC 的控制律（简化到 1D），标注两者在数学结构上的相似性和差异。
2. ⭐⭐⭐ 跨章综合：结合 F02（操作空间动力学）和 F03（经典力控算法）的框架，设计一个 7-DOF 机械臂的两优先级控制器：(1) 主任务——末端阻抗控制（接触面打磨），(2) 零空间任务——关节中心化。写出完整控制律。
3. ⭐⭐⭐ 统一建模：一个人形机器人（如 Talos）同时需要足端力控（维持站立平衡）和手端力控（擦桌子）。画出它的 WBC-QP 问题的完整框架，标注各个约束和目标项。这道题综合了 足式/90_WBC分层优化与TSID（WBC）和本章的力控知识。

⚠️ 常见陷阱

🧠 思维陷阱：认为机械臂力控和足式 WBC 是完全不同的领域

新手想法："机械臂力控和四足机器人的 WBC 看起来完全不同——一个用阻抗公式，一个解 QP——没什么关系"

实际上：两者在**数学本质**上解决同一类问题——在满足物理约束下实现期望的力-运动行为。区别仅在于约束复杂度：机械臂有 1 个接触点和固定基座，约束可用显式公式解；足式有多接触和浮动基座，约束必须用 QP 在线求解。Khatib 1987 的操作空间框架和 Sentis-Khatib 2005 的多优先级 WBC 就是这座桥梁

正确理解：掌握了操作空间动力学（F02），就同时拥有了理解机械臂阻抗控制和足式 WBC 的统一数学基础

---

8. 力控历史脉络与学术演进 ⭐⭐

8.1 四十年学术脉络

力控制的发展可以划分为四个时代：

开创期（1976-1985）——问题定义与理论奠基：

年份	里程碑	贡献者	核心洞察
1976	主动柔顺概念	Paul & Shimano (Stanford)	第一次在论文中讨论力反馈的必要性
1977	RCC 被动柔顺	Whitney (Draper Lab)	纯机械解决方案，至今仍在工业中使用
1980	主动刚度控制	Salisbury (Stanford)	\(\tau = J^T K_x(x_d - x)\)——阻抗控制的前身
1981	约束空间分析	Mason (MIT)	Natural/Artificial Constraints 的形式化
1981	混合力/位控制	Raibert & Craig (CMU)	选择矩阵 \(S\) 将空间分解为力和位置子空间
1985	阻抗控制三部曲	Hogan (MIT)	"控制关系而非变量"——力控的统一理论

成熟期（1986-1999）——算法多样化：

年份	里程碑	贡献者	核心算法
1987	操作空间动力学	Khatib (Stanford)	\(\Lambda, \mu, p\) 和动力学一致伪逆 \(\bar{J}\)
1988	耦合稳定性定理	Colgate & Hogan (MIT/NWU)	无源性→接触稳定性
1988	任务帧形式化	De Schutter (KU Leuven)	柔顺帧+每轴独立配置
1993	并行力/位控	Chiaverini & Sciavicco (Naples)	不需选择矩阵
1999	力控综述	Chiaverini, Siciliano, Villani	教学标准参考

系统化期（2000-2018）——无源性与工程落地：

年份	里程碑	贡献者	核心进展
2003	柔性关节阻抗	Albu-Schaffer, Ott (DLR)	双质量模型无源化→Franka/iiwa 理论基础
2005	多优先级 WBC	Sentis & Khatib (Stanford)	从操作空间到全身控制
2016	Springer Handbook 2nd ed. Ch.9	Villani & De Schutter	力控教学黄金参考
2016	变阻抗稳定性	Kronander & Billard (EPFL)	能量罐 + KB 条件
2017	碰撞安全四阶段	Haddadin et al. (TUM)	检测→隔离→分类→反应

学习型力控期（2019-今）——AI 与力控的融合：

年份	里程碑	贡献者	核心创新
2019	VICES 变阻抗 RL	Stulp et al.	RL 输出 \(K_d, D_d\) 而非力矩
2023	Diffusion Policy	Chi et al. (Columbia/Toyota)	扩散模型生成动作序列
2024	SERL/HIL-SERL	Luo et al. (Berkeley)	25 分钟真机学习接触任务
2025	CRISP	TUM MIRMI	高频 C++ 底层 + 低频 Python 策略

8.2 关键实验室演化链

MIT (Hogan 1985, 阻抗理论)
  +--> Stanford (Khatib 1987, 操作空间)
        +--> DLR (Albu-Schaffer/Ott 2003-2008, 柔性关节无源化)
              +--> EPFL LASA (Billard 2014-2016, 变阻抗稳定性)
                    +--> TUM MIRMI (Haddadin 2017-2025, 碰撞安全+CRISP)
                          +--> Berkeley RAIL (Levine 2024, SERL 学习型力控)

教学要点：每一代的进步都建立在前一代的局限性之上。Hogan 的理论没有处理柔性关节→DLR 解决了；DLR 的方案是固定参数→EPFL 做了变阻抗；变阻抗的手动设计不可扩展→Berkeley 用 RL 自动学习。理解这个演化链，就理解了力控领域"为什么这样发展"的逻辑。

8.3 前沿工作与开放问题

CRISP（TUM MIRMI, 2025）：定义了"高频 C++ 柔顺底层 + 低频 Python GPU 策略"的新范式，已支持 Franka FR3、KUKA iiwa14、Kinova Gen3 三种臂。已与 Diffusion Policy、ACT、SmolVLA 集成。

CRISP 架构的核心设计决策值得深入理解：

CRISP 双频架构：

高频层 (C++, 1 kHz):
  - 笛卡尔阻抗控制（接收 x_d, K_d, D_d）
  - 零空间关节中心化
  - 力矩安全限幅
  - 接触过渡平滑
  -> 保证实时安全，即使策略层崩溃

低频层 (Python, 10-50 Hz):
  - Diffusion Policy / ACT / SmolVLA
  - 输出: (x_d, K_d, D_d) 而非 tau
  - GPU 推理 + 视觉感知
  -> 负责任务智能，不直接接触硬件

接口约定:
  策略层不允许发送 tau——只能发送 (x_d, K_d, D_d)
  这确保了即使策略输出 K_d = 10000 N/m（超出安全范围），
  底层会自动截断到 K_s 以下——安全由底层保证

"不是X而是Y"：CRISP 架构的创新**不是**把 RL 和控制简单串联，而是**在架构层面用接口约束保证了安全性**——策略层永远无法绕过阻抗控制层直接发送力矩。这与 legged_gym 等端到端框架（RL 直接输出关节力矩）形成鲜明对比。

Diffusion Policy（Chi et al., RSS 2023）+ 底层阻抗：策略生成 10 Hz 目标位姿轨迹，底层阻抗控制器 1 kHz 柔顺跟踪——学习不需要替代控制，而是与控制共生。

开放问题：

问题	当前状态	难度	活跃研究组
阻抗参数自动调节	RL/贝叶斯优化，无公认最优方案	⭐⭐⭐	EPFL LASA, TUM
变阻抗无源性 vs 学习自由度	能量罐/KB 条件，但过于保守	⭐⭐⭐⭐	DLR, EPFL
浮动基座阻抗定义	相对于什么参考系？无共识	⭐⭐⭐	Stanford, MIT
接触模式切换平滑化	sim-to-real 核心瓶颈	⭐⭐⭐⭐	Berkeley, TUM
多模态感知融合力控	视觉+触觉+力联合推理	⭐⭐⭐	MIT CSAIL, CMU
可变形物体力控	线性阻抗模型不适用	⭐⭐⭐⭐	IIT, KTH

⚠️ 常见陷阱

🧠 思维陷阱：认为"最新 = 最好"，忽视经典方法的工程价值

新手想法："2025 年了还学 1985 年的阻抗控制？直接用 RL 不就行了？"

实际上：学习型力控（SERL/Diffusion Policy）目前都依赖阻抗控制作为底层——SERL 的 25 分钟 PCB 插入是在 Franka 的阻抗控制器上运行的；Diffusion Policy 生成的动作轨迹也由底层阻抗控制器柔顺跟踪。没有稳定的阻抗底层，学习策略无法安全地执行接触任务。经典力控理论是学习型方法的"地基"，不是被替代的"旧方法"

正确理解：力控的演进是**累积式**的——Hogan 的理论仍然是底层，RL/IL 是上层策略

向后预告——变阻抗控制（Variable Impedance）：本章讨论的阻抗参数 \(K_d, D_d, M_d\) 都是**固定的**。但真实任务往往需要在线调节——接近时高刚度定位、接触瞬间低刚度缓冲、操作中变刚度适应。参数时变会引发一个深刻的稳定性问题：刚度跳变可能向系统**注入能量**，破坏无源性。F06 将系统化地解决这个问题——能量罐方法、Kronander-Billard 阻尼下界条件、以及前沿的 CBF 方法。

练习

1. ⭐ 文献精读：阅读 Hogan 1985 Part I（约 8 页），摘录三个你认为最重要的原始观点。与本章的讲解做对比——你觉得有哪些 Hogan 的原始洞察在后续 40 年被"稀释"了？
2. ⭐⭐ 综述导图：精读 Abu-Dakka & Saveriano 2020 综述的 Section 2-3（阻抗/导纳基础 + 变阻抗分类），画出完整的技术分类树，标注每个分支的代表论文和年份。
3. ⭐⭐ 跨方向对比：SLAM 中的因子图最小二乘和力控中的 QP-WBC 在数学上有什么结构性相似点？提示：都是稀疏块 KKT 系统。为什么 SLAM 用 Cholesky 分解而 WBC 用 active-set/interior-point？

---

本章常见误解汇总

误解	正确理解
"降低位置控制增益 \(K_p\) 就能安全接触"	降低 \(K_p\) 同时恶化自由空间精度；接触任务必须切换到力控范式
"阻抗控制是柔软版的位置控制"	阻抗控制的设计意图是建立力-运动关系，而非跟踪位置；两者是不同的控制哲学
"阻抗控制一定优于导纳控制"	选型由硬件接口决定（力矩接口→阻抗，位置接口→导纳），不是"优劣"问题
"F/T 传感器出厂标定后可直接使用"	零偏随温度/安装变化；每次上电必须重新标定零偏
"动量观测器可以完全替代 F/T 传感器"	动量观测器精度受限于动力学模型误差和关节摩擦辨识质量，精度通常为 \(\pm 2\)-\(10\) N
"力控制中 \(K_d\) 越大力越恒定"	高 \(K_d\) 在不平整表面上放大力波动；打磨等恒力任务应使用低 \(K_d\) 配合力控外环
"二端口网络模型只是理论，工程中不需要"	二端口模型给出了接触稳定性的唯一严格判据（Colgate-Hogan 定理）
"机械臂力控和足式 WBC 无关"	两者在数学本质上解决同一类问题；Khatib 操作空间框架是统一桥梁
"2025 年了不需要学 1985 年的阻抗控制"	学习型力控（SERL/Diffusion Policy/CRISP）全部依赖阻抗控制作为底层安全保证
"接触力飙升一定是代码 bug"	这是位置控制在接触场景中的正常物理行为（\(f = K_e \cdot \Delta x\)），不是软件缺陷

---

本章小结

术语速查表

术语	英文	一句话定义
机械阻抗	Mechanical Impedance, \(Z(s)\)	速度输入→力输出的传递函数；\(Z = F/V\)
机械导纳	Mechanical Admittance, \(Y(s)\)	力输入→速度输出的传递函数；\(Y = V/F = Z^{-1}\)
阻抗控制	Impedance Control	让机器人末端表现为可编程的弹簧-阻尼器-质量系统
导纳控制	Admittance Control	读取外力→计算位置修正→喂给位置控制器
无源性	Passivity	系统不能凭空产生能量的性质；\(\int_0^T f^T v \, dt \geq -V(0)\)
耦合稳定性	Coupled Stability	两个系统通过端口交互时整体系统的稳定性
二端口网络	Two-port Network	用阻抗/导纳矩阵描述两个端口间力-速度关系的模型
自然约束	Natural Constraint	由接触几何决定的物理约束（如法向不能穿透）
人工约束	Artificial Constraint	由工程师选择、用控制实现的约束
选择矩阵	Selection Matrix, \(S\)	将操作空间分解为力控子空间和位控子空间的对角矩阵
六维力/力矩传感器	F/T Sensor	测量 6D 力和力矩的传感器
动量观测器	Momentum Observer	基于广义动量偏差估计外力的纯软件方法
存储函数	Storage Function	Lyapunov 候选函数，表示系统存储的总能量
操作空间惯量	Operational Space Inertia, \(\Lambda\)	末端笛卡尔空间的等效惯量矩阵；\(\Lambda = (J M^{-1} J^T)^{-1}\)
主动柔顺	Active Compliance	通过传感+控制实现的可编程柔顺行为
被动柔顺	Passive Compliance	通过机械结构（弹簧/弹性体）实现的固定柔顺行为
RCC	Remote Center Compliance	Whitney 1977 提出的被动柔顺装置，用于装配对准
SEA	Series Elastic Actuator	串联弹性执行器，弹簧串联在电机和负载之间
QDD	Quasi-Direct Drive	准直驱，用低速比减速器或直连实现高力矩带宽
UFIC	Unified Force-Impedance Control	Haddadin 2024 提出的统一力-阻抗控制框架

知识点总表

编号	知识点	核心内容	对应节	难度	后续章节
1	位置控制的局限	刚性环境→力不可控→振荡/损坏	§1	⭐	F03 对比
2	接触过渡问题	自由度拓扑突变→冲击力→控制器盲区	§1.2	⭐⭐	F06 碰撞
3	Mason 约束空间	自然/人工约束→选择矩阵 \(S\)	§1.5	⭐	F03 混合控制
4	Hogan 阻抗控制	\(M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x} = f_{ext}\)	§2	⭐⭐	F02 数学, F03 算法
5	阻抗 vs 导纳	因果性对偶, 硬件接口决定选型	§2.4	⭐⭐	F04 阻抗, F05 导纳
6	统一力-阻抗控制	UFIC 连续谱 + 能量罐无源性	§2.6	⭐⭐⭐	F06 能量罐
7	二端口网络	\(Z, Y\) 矩阵, 力-速度对偶	§3	⭐⭐⭐	D05 遥操作
8	能量函数与 Lyapunov	存储函数→无源性→渐近稳定	§3.5	⭐⭐⭐	F02 无源性理论
9	接触稳定性	无源性→耦合稳定（Colgate-Hogan）	§3.4	⭐⭐⭐	F02 无源性理论
10	离散化破坏无源性	采样延迟→相位旋转→能量注入	§3.6	⭐⭐⭐	F04 实时实现
11	力传感器选型	F/T vs 关节力矩 vs 动量观测器	§4	⭐⭐	F04, F05 工程
12	应用场景地图	打磨/装配/协作/检测→方案+平台匹配	§6	⭐⭐	F03-F05 实现
13	碰撞安全四阶段	Haddadin: 碰前→检测→隔离→反应	§6.5	⭐⭐	F06 碰撞安全
14	与足式对偶	阻抗控制↔WBC-QP 的统一视角	§7	⭐⭐⭐	F07 WBC, F08 MPC
15	学术演进脉络	Hogan→DLR→EPFL→TUM→Berkeley	§8	⭐⭐	F09 学习型力控

---

累积项目：Mini-ForceControl 模块 0——力控感知基础

本章在 Mini-ForceControl 综合项目（F10）中的贡献：

本章新增模块：力传感器接口抽象层 - 定义 ForceSensorBase 抽象接口（C++ 纯虚类） - 实现 SimulatedFTSensor（从 MuJoCo 读取接触力） - 实现 MomentumObserver（基于 Pinocchio 的动量观测器） - 统一输出格式：6D 力/力矩 + 时间戳 + 置信度

// 力传感器抽象接口（本章定义）
class ForceSensorBase {
public:
    virtual ~ForceSensorBase() = default;

    // 读取 6D 力/力矩，输出到约定控制坐标系
    virtual Eigen::Matrix<double,6,1> readWrench() = 0;

    // 获取传感器置信度 [0,1]
    virtual double getConfidence() const = 0;

    // 零偏标定
    virtual void calibrateZeroBias() = 0;
};

// MuJoCo 仿真力传感器（本章实现）
class SimulatedFTSensor : public ForceSensorBase {
public:
    SimulatedFTSensor(mjModel* model, mjData* data,
                      int force_sensor_id, int torque_sensor_id,
                      int site_id, double sign = 1.0)
        : model_(model), data_(data),
          force_sensor_id_(force_sensor_id),
          torque_sensor_id_(torque_sensor_id),
          site_id_(site_id),
          sign_(sign) {}

    Eigen::Matrix<double,6,1> readWrench() override {
        // MuJoCo 的 force/torque site sensor 是两个 3D 传感器，
        // sensordata 默认在 site 局部系 S 表达，不是 world wrench。
        Eigen::Vector3d f_S = Eigen::Map<const Eigen::Vector3d>(
            &data_->sensordata[model_->sensor_adr[force_sensor_id_]]);
        Eigen::Vector3d tau_S = Eigen::Map<const Eigen::Vector3d>(
            &data_->sensordata[model_->sensor_adr[torque_sensor_id_]]);

        Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor>> R_WS(
            &data_->site_xmat[9 * site_id_]);  // site S -> world W

        Eigen::Matrix<double,6,1> wrench;
        wrench.head<3>() = sign_ * (R_WS * f_S);
        wrench.tail<3>() = sign_ * (R_WS * tau_S);
        return wrench - zero_bias_;
    }

    double getConfidence() const override { return 1.0; }  // 仿真：完美置信度

    void calibrateZeroBias() override {
        // 在无接触时记录多帧取平均
        zero_bias_ = readWrenchRaw();
    }

private:
    mjModel* model_;
    mjData* data_;
    int force_sensor_id_;
    int torque_sensor_id_;
    int site_id_;
    double sign_;
    Eigen::Matrix<double,6,1> zero_bias_ = Eigen::Matrix<double,6,1>::Zero();
    Eigen::Matrix<double,6,1> readWrenchRaw();
};

MuJoCo 的 site force/torque 传感器符号约定必须用单元测试锁定：在 ee_site 的 +X/+Y/+Z 方向施加已知外力，验证读数在转换到控制帧后的轴向与符号是否符合控制器定义；若相反，只能在传感器封装层用 sign_ 统一修正，不要在各控制器里分别取负号。

后续章节扩展： - F02 → 添加 OperationalSpaceDynamics 类（\(\Lambda, \mu, p\) 计算） - F03 → 添加 ImpedanceController / HybridController / ParallelController - F04 → 添加 FrankaImpedanceController（libfranka 实时回调集成） - F05 → 添加 AdmittanceController（ros2_control 插件）

---

本章与后续章节的关系

后续章节	关系	本章铺垫的知识点
F02 数学基础	深入展开	操作空间动力学（\(\Lambda, \mu, p\)）、无源性理论的严格推导、Lyapunov 稳定性
F03 经典力控算法	算法实现	Hogan 阻抗方程→关节/笛卡尔/操作空间三种实现；混合力/位控→选择矩阵
F04 笛卡尔阻抗工程	工程落地	阻抗控制概念→libfranka 实时回调；参数调节→Franka 真机实验
F05 导纳控制与 ROS2	工程落地	导纳控制概念→ros2_control 插件实现；UR 平台力控
F06 变阻抗无源性	理论深化	无源性基础→变阻抗时能量注入问题→能量罐/KB 条件
F07 浮动基座 WBC	理论推广	操作空间→多优先级 WBC；阻抗控制→QP 力控
F08 MPC+WBC 联合力控	系统集成	力控频率层级→MPC 上层+WBC 下层的双层架构
F09 学习型力控	前沿拓展	CRISP 架构→学习策略+阻抗底层的共生范式
D05 遥操作	跨方向	二端口网络→遥操作透明度与稳定性分析

---

延伸阅读

资料	类型	难度	推荐理由
Hogan 1985, "Impedance Control Parts I-III", ASME J. DSMC 107:1-24	论文	⭐⭐	必读原著，建立阻抗控制的第一性原理
Villani & De Schutter 2016, Springer Handbook Ch.9	教材章节	⭐⭐	力控教学黄金参考，分类清晰
Siciliano et al., "Robotics: Modelling, Planning and Control" Ch.9	教材	⭐⭐	力控的标准教材，推导完整
Abu-Dakka & Saveriano 2020, "Variable Impedance Control Review", Frontiers 7:590681	综述	⭐⭐⭐	变阻抗 40 年综述，图 1 分类树极好
Colgate & Hogan 1988, "Robust Control of Dynamically Interacting Systems"	论文	⭐⭐⭐	耦合稳定性定理——力控稳定性的理论基石
Haddadin & Shahriari 2024, "Unified Force-Impedance Control", IJRR 43(13):2112-2141	论文	⭐⭐⭐	统一力-阻抗框架，能量罐无源性保证
Chiaverini et al. 1999, "Survey of Robot Interaction Control", IEEE/ASME T-Mech 4(3)	综述	⭐⭐	经典力控方案的分类与实验对比
Song et al. 2019, "A Tutorial Survey and Comparison of Impedance Control on Robotic Manipulation", Robotica 37(5):801-836	综述	⭐⭐	阻抗控制各实现方案的系统对比与仿真验证
De Luca & Mattone 2003, "Sensorless Robot Collision Detection and Hybrid Force/Motion Control"	论文	⭐⭐	动量观测器的原始论文，碰撞检测基础
libfranka cartesian_impedance_control.cpp	代码	⭐⭐	最简洁的笛卡尔阻抗实现（约 130 行）
ros2_control admittance_controller	代码	⭐⭐	标准导纳控制器 ROS2 实现，支持多平台

---

研究实践建议

初学者（第一周）

1. 必读原著：Hogan 1985 Part I（约 8 页），重点理解"控制关系而非变量"的核心哲学
2. 仿真实验：用 MuJoCo 或 PyBullet 搭建一个 1-DOF 弹簧-阻尼器系统，手动实现阻抗方程的时域离散化（前向欧拉），观察不同 \(K_d, D_d\) 下的接触响应
3. 参数扫描：对上述仿真，固定环境刚度 \(K_e = 10^4\) N/m，扫描 \(K_d \in [10, 5000]\) N/m，观察接触力峰值和稳态误差的变化趋势

进阶者（第二周）

1. Franka 真机：运行 libfranka 的 cartesian_impedance_control 示例，让末端接触桌面，用示波器观察力和位置信号
2. 导纳控制对比：在 UR5e 上实现简单的导纳控制器（读取 F/T → 积分出 \(\Delta x\) → 命令位置），对比与 Franka 阻抗控制的力控带宽差异
3. 论文精读：Villani & De Schutter 2016（Springer Handbook Ch.9），重点关注力控分类树和实验对比

研究者（第三周）

1. UFIC 复现：精读 Haddadin & Shahriari 2024，在 MuJoCo 中复现统一力-阻抗控制器，验证能量罐在参数切换时的能量约束效果
2. 开放问题调研：选择第 8.3 节开放问题中的一个方向（如"变阻抗无源性 vs 学习自由度"），精读 3-5 篇最新论文，撰写 2 页综述
3. 跨方向实践：将本章的阻抗控制概念应用于足式场景——在四足仿真中实现单脚的阻抗行为，观察与固定基座阻抗控制的差异

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
接触时机械臂剧烈振荡	阻抗刚度 \(K_d\) 过高 + 环境刚度大	1. 降低 \(K_d\)（先降 10x） 2. 检查采样率是否足够 3. 验证无源性条件	F01 3.5, F02
力传感器读数跳变	接线松动 / 标定矩阵错误 / EMI	1. 检查物理连接 2. 重新零偏标定 3. 加低通滤波（50Hz 截止）	F01 4.2
导纳控制"飘移"（松手后缓慢移动）	\(K_d = 0\) 时无弹簧回复力 + 力传感器零偏	1. 检查 F/T 零偏标定 2. 加微小 \(K_d\)（如 5 N/m） 3. 增大 \(D_d\)	F01 2.4, F05
动量观测器力估计持续偏差	关节摩擦未补偿 / 惯性参数不准	1. 运行摩擦辨识实验 2. 检查 URDF 惯性参数 3. 降低 \(K_I\) 减少噪声放大	F01 4.4
装配任务中螺栓楔紧无法插入	搜索阶段 \(K_d\) 过高 / 螺旋搜索半径不足	1. 降低 \(K_d\) 至 50-100 N/m 2. 增大搜索半径至 3-5mm 3. 加入旋转柔顺 \(K_{rot} < 5\) Nm/rad	F01 6.3
碰撞检测在打磨任务中持续误触发	碰撞阈值低于任务正常力范围	1. 将阈值设为最大正常力 \(\times 1.5\)-\(2\) 2. 改用力变化率 \(\dot{f}\) 而非绝对值检测 3. 加入任务上下文感知	F01 6.5
阻抗控制在自由空间位置跟踪不准	\(K_d\) 太低，末端被重力拉偏	1. 检查重力补偿 \(g(q)\) 是否正确 2. 适当提高 \(K_d\) 3. 加前馈项	F01 2.3, F03

---

API 速查表

本章涉及的核心 API 和接口：

API / 接口	所属库	功能	典型调用
franka::Torques callback	libfranka (C++)	Franka 1kHz 力矩回调接口	robot.control(callback)
franka::RobotState	libfranka (C++)	读取关节角/速度/力矩/外力估计	state.tau_J, state.O_F_ext_hat_K
pinocchio::rnea()	Pinocchio (C++)	逆动力学：计算 \(M\ddot{q}+C\dot{q}+g\)	pin::rnea(model, data, q, v, a)
pinocchio::computeJointJacobians()	Pinocchio (C++)	计算所有关节的雅可比矩阵	pin::computeJointJacobians(model, data, q)
pinocchio::crba()	Pinocchio (C++)	计算惯量矩阵 \(M(q)\)	pin::crba(model, data, q)
admittance_controller	ros2_control (C++)	ROS2 标准导纳控制器插件	YAML 参数配置 + controller_manager 加载
force_mode()	URScript	UR 内置导纳控制模式	force_mode(task_frame, selection, wrench, type, limits)
mj_step()	MuJoCo (C)	MuJoCo 仿真步进	mj_step(model, data)
data->sensordata[]	MuJoCo (C)	读取传感器数据（含力/力矩传感器）	data->sensordata[sensor_adr[id]]

---

版本信息速查

库/平台	推荐版本	备注
libfranka	\(\geq\) 0.13.x	Franka FR3 需 0.13+；Franka Panda 兼容 0.8-0.10
Pinocchio	\(\geq\) 3.x	C++17，CMake 集成
MuJoCo	\(\geq\) 3.x	免费开源，C API + Python 绑定
ros2_control	Humble / Iron / Jazzy	与 ROS2 发行版对应
URScript	5.x	UR e-Series 内置脚本语言
Eigen	\(\geq\) 3.4	C++ 线性代数库，Pinocchio 依赖